cm/cmdr-joystick
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added ELRS support. Added EEPROM calibration data (not yet activated). Code cleanup

Browse Source
This commit is contained in:
Christoffer Martinsson 2023-08-25 09:40:17 +02:00
parent d59cbfa7b0
commit 555645fc54
3 changed files with 490 additions and 156 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
README.md
View File

@ -7,7 +7,7 @@ RC Joystick with 2 hall effect gimbals, 2 hat switches and 25 buttons for use bo
```cpp ```cpp
USB Joystick Layer 0 USB Joystick Layer 0
-------------------------------------------------------------- --------------------------------------------------------------
| FnL | B1 | | B25 | | B5 | FnR | | FnL | B1 | | B21 | | B5 | FnR |
-------------------------------------------------------------- --------------------------------------------------------------
| | B2 | B3 | MoL | | MoR | B7 | B6 | | | | B2 | B3 | MoL | | MoR | B7 | B6 | |
| | | |
@ -21,7 +21,7 @@ USB Joystick Layer 0
USB Joystick Layer 1 (FnL) USB Joystick Layer 1 (FnL)
-------------------------------------------------------------- --------------------------------------------------------------
| FnL | B9 | | B25 | | B5 | FnR | | FnL | B9 | | B21 | | B5 | FnR |
-------------------------------------------------------------- --------------------------------------------------------------
| | B10 | B11 | MoL | | MoR | B7 | B6 | | | | B10 | B11 | MoL | | MoR | B7 | B6 | |
| | | |
@ -35,7 +35,7 @@ USB Joystick Layer 1 (FnL)
USB Joystick Layer 2 (FnR) USB Joystick Layer 2 (FnR)
-------------------------------------------------------------- --------------------------------------------------------------
| FnL | B1 | | B25 | | B13 | FnR | | FnL | B1 | | B21 | | B13 | FnR |
-------------------------------------------------------------- --------------------------------------------------------------
| | B2 | B3 | MoL | | MoR | B15 | B14 | | | | B2 | B3 | MoL | | MoR | B15 | B14 | |
| | | |
@ -49,7 +49,7 @@ USB Joystick Layer 2 (FnR)
USB Joystick Layer 3 (FnL + FnR) USB Joystick Layer 3 (FnL + FnR)
-------------------------------------------------------------- --------------------------------------------------------------
| FnL | B9 | | B25 | | B13 | FnR | | FnL | B9 | | B21 | | B13 | FnR |
-------------------------------------------------------------- --------------------------------------------------------------
| | B10 | B11 | MoL | | MoR | B15 | B14 | | | | B10 | B11 | MoL | | MoR | B15 | B14 | |
| | | |
@ -100,8 +100,17 @@ ELRS Layer
![pcb_top](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_top.png) ![pcb_bottom](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_bottom.png) ![pcb_top](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_top.png) ![pcb_bottom](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_bottom.png)
* Gerber files: [zip](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_gerber.zip) - Gerber files: [zip](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_gerber.zip)
* Schematics: [pdf](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_schematics.pdf) - Schematics: [pdf](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_schematics.pdf)
- rp2040zero pinout: ![rp2040zero_pinout](https://www.waveshare.com/w/upload/2/2b/RP2040-Zero-details-7.jpg)
- rp2040zero schematic: [pdf](https://www.waveshare.com/w/upload/4/4c/RP2040_Zero.pdf)
##### 1x ELRS TX
Using a EP1 TCXO Dual receiver reprogrammed as a tramsmitter
- [Link to EP1](https://www.happymodel.cn/index.php/2022/11/07/2-4g-elrs-ep1-ep2-ep1dual-tcxo-receiver/)
- [Reprogramming instructions](https://github.com/MUSTARDTIGERFPV/rx-as-tx#flashing)
## Software Build environment ## Software Build environment
Rust Rust
@ -111,6 +120,14 @@ Rust
- Pressing boot button on teensy - Pressing boot button on teensy
- Press and hold "top lower right button" when powering the unit - Press and hold "top lower right button" when powering the unit
- CRSF protocol description (for communicating with ELRS TX): [Link](https://github.com/ExpressLRS/ExpressLRS/wiki/CRSF-Protocol)
- rp2040 datasheet: [pdf](https://datasheets.raspberrypi.com/rp2040/rp2040-datasheet.pdf)
## Calibration ## Calibration
No calibration needed 1. Center both gimbals.
2. Press all righ hand side buttons except hat switch. Status led will start blinking green.
3. Move both gimbals to all corners.
4. Press right hat switch to save calibration data to eeprom.
Done!

1
rp2040/Cargo.toml
View File

@ -23,6 +23,7 @@ pio = "0.2.0"
defmt = { version = "0.3", optional = true } defmt = { version = "0.3", optional = true }
libm = "0.2.7" libm = "0.2.7"
dyn-smooth = "0.2.0" dyn-smooth = "0.2.0"
eeprom24x = "0.6.0"
[features] [features]
# This is the set of features we enable by default # This is the set of features we enable by default

614
rp2040/src/main.rs
View File

@ -8,6 +8,7 @@
#![no_main] #![no_main]
mod button_matrix; mod button_matrix;
mod elrs;
mod layout; mod layout;
mod status_led; mod status_led;
mod usb_joystick_device; mod usb_joystick_device;
@ -16,16 +17,20 @@ use button_matrix::ButtonMatrix;
use core::convert::Infallible; use core::convert::Infallible;
use cortex_m::delay::Delay; use cortex_m::delay::Delay;
use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS}; use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS};
use eeprom24x::{Eeprom24x, SlaveAddr};
use elrs::Elrs;
use embedded_hal::adc::OneShot; use embedded_hal::adc::OneShot;
use embedded_hal::digital::v2::*; use embedded_hal::digital::v2::*;
use embedded_hal::timer::CountDown; use embedded_hal::timer::CountDown;
use fugit::ExtU32; use fugit::{ExtU32, RateExtU32};
use libm::powf; use libm::powf;
use panic_halt as _; use panic_halt as _;
use rp2040_hal::{ use rp2040_hal::{
adc::Adc, adc::Adc,
gpio::{Function, FunctionConfig, PinId, ValidPinMode}, gpio::{Function, FunctionConfig, FunctionUart, PinId, ValidPinMode},
i2c::I2C,
pio::StateMachineIndex, pio::StateMachineIndex,
uart::{DataBits, StopBits, UartConfig, UartPeripheral},
}; };
use status_led::{StatusMode, Ws2812StatusLed}; use status_led::{StatusMode, Ws2812StatusLed};
use usb_device::class_prelude::*; use usb_device::class_prelude::*;
@ -52,13 +57,19 @@ pub const NUMBER_OF_BUTTONS: usize = BUTTON_ROWS * BUTTON_COLS;
pub const AXIS_MIN: u16 = 0; pub const AXIS_MIN: u16 = 0;
pub const AXIS_MAX: u16 = 4095; pub const AXIS_MAX: u16 = 4095;
pub const AXIS_CENTER: u16 = AXIS_MAX / 2; pub const AXIS_CENTER: u16 = (AXIS_MIN + AXIS_MAX) / 2;
pub const ELRS_MIN: u16 = 172;
pub const ELRS_MAX: u16 = 1811;
pub const ELRS_CENTER: u16 = (ELRS_MIN + ELRS_MAX) / 2;
pub const NBR_OF_GIMBAL_AXIS: usize = 4; pub const NBR_OF_GIMBAL_AXIS: usize = 4;
pub const GIMBAL_AXIS_LEFT_X: usize = 0; pub const GIMBAL_AXIS_LEFT_X: usize = 0;
pub const GIMBAL_AXIS_LEFT_Y: usize = 1; pub const GIMBAL_AXIS_LEFT_Y: usize = 1;
pub const GIMBAL_AXIS_RIGHT_X: usize = 2; pub const GIMBAL_AXIS_RIGHT_X: usize = 2;
pub const GIMBAL_AXIS_RIGHT_Y: usize = 3; pub const GIMBAL_AXIS_RIGHT_Y: usize = 3;
pub const GIMBAL_MODE_M10: u8 = 0;
pub const GIMBAL_MODE_M7: u8 = 1;
// Analog smoothing settings. // Analog smoothing settings.
pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS; pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
@ -83,7 +94,8 @@ pub struct GimbalAxis {
pub center: u16, pub center: u16,
pub fn_mode: u8, pub fn_mode: u8,
pub deadzone: (u16, u16, u16), pub deadzone: (u16, u16, u16),
pub expo: f32, pub expo: bool,
pub trim: i16,
} }
impl Default for GimbalAxis { impl Default for GimbalAxis {
@ -96,8 +108,9 @@ impl Default for GimbalAxis {
min: AXIS_MIN, min: AXIS_MIN,
center: AXIS_CENTER, center: AXIS_CENTER,
fn_mode: 0, fn_mode: 0,
deadzone: (50, 50, 50), deadzone: (100, 50, 100),
expo: 0.2, expo: true,
trim: 0,
} }
} }
} }
@ -136,6 +149,31 @@ fn main() -> ! {
&mut pac.RESETS, &mut pac.RESETS,
); );
// Set up UART on GP0 and GP1 (Pico pins 1 and 2)
let uart_pins = (
pins.gp0.into_mode::<FunctionUart>(),
pins.gp1.into_mode::<FunctionUart>(),
);
let elrs_uart = UartPeripheral::new(pac.UART0, uart_pins, &mut pac.RESETS)
.enable(
UartConfig::new(400000.Hz(), DataBits::Eight, None, StopBits::One),
clocks.peripheral_clock.freq(),
)
.unwrap();
let mut i2c = I2C::i2c1(
pac.I2C1,
pins.gp14.into_mode(), // sda
pins.gp15.into_mode(), // scl
400.kHz(),
&mut pac.RESETS,
125_000_000.Hz(),
);
let i2c_address = SlaveAddr::default();
let mut eeprom = Eeprom24x::new_24x02(i2c, i2c_address);
// Enable adc // Enable adc
let mut adc = Adc::new(pac.ADC, &mut pac.RESETS); let mut adc = Adc::new(pac.ADC, &mut pac.RESETS);
@ -165,6 +203,9 @@ fn main() -> ! {
&mut pins.gp8.into_push_pull_output(), &mut pins.gp8.into_push_pull_output(),
]; ];
let mut elrs_en_pin = pins.gp2.into_push_pull_output();
let mut elrs = Elrs::new(elrs_uart);
// Create button matrix object that scans all buttons // Create button matrix object that scans all buttons
let mut button_matrix: ButtonMatrix<BUTTON_ROWS, BUTTON_COLS, NUMBER_OF_BUTTONS> = let mut button_matrix: ButtonMatrix<BUTTON_ROWS, BUTTON_COLS, NUMBER_OF_BUTTONS> =
ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, 5); ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, 5);
@ -181,9 +222,10 @@ fn main() -> ! {
clocks.peripheral_clock.freq(), clocks.peripheral_clock.freq(),
); );
let mut delay = Delay::new(core.SYST, clocks.system_clock.freq().to_Hz());
// Scan matrix to get initial state and check if bootloader should be entered // Scan matrix to get initial state and check if bootloader should be entered
// This is done by holding button 0 pressed while power on the unit // This is done by holding button 0 pressed while power on the unit
let mut delay = Delay::new(core.SYST, clocks.system_clock.freq().to_Hz());
for _ in 0..10 { for _ in 0..10 {
button_matrix.scan_matrix(&mut delay); button_matrix.scan_matrix(&mut delay);
} }
@ -201,44 +243,39 @@ fn main() -> ! {
usb_hid_report_count_down.start(10.millis()); usb_hid_report_count_down.start(10.millis());
let mut scan_count_down = timer.count_down(); let mut scan_count_down = timer.count_down();
scan_count_down.start(1.millis()); scan_count_down.start(200u32.micros());
let mut status_led_count_down = timer.count_down(); let mut status_led_count_down = timer.count_down();
status_led_count_down.start(50.millis()); status_led_count_down.start(50.millis());
let mut elrs_count_down = timer.count_down(); let mut main_count_down = timer.count_down();
elrs_count_down.start(1660u32.micros()); main_count_down.start(1660u32.micros());
let mut elrs_start_count_down = timer.count_down();
elrs_start_count_down.start(2000.millis());
let mut mode: u8 = 0; let mut mode: u8 = 0;
let mut safety_check: bool = false; let mut safety_check: bool = false;
let mut activity: bool = false; let mut activity: bool = false;
let mut idle: bool = false; let mut idle: bool = false;
let mut usb_active: bool = false; let mut usb_active: bool = false;
let mut elrs_active: bool = false;
let mut calibration_active: bool = false;
let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS]; let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS];
let mut buttons: [Button; NUMBER_OF_BUTTONS] = [Button::default(); NUMBER_OF_BUTTONS]; let mut buttons: [Button; NUMBER_OF_BUTTONS] = [Button::default(); NUMBER_OF_BUTTONS];
let mut channel_locks: [bool; 12] = [false; 12];
let mut gimbal_mode: u8 = GIMBAL_MODE_M10;
let expo_lut: [u16; AXIS_MAX as usize + 1] = generate_expo_lut(0.3);
// Set up left gimbal Y axis as full range without return to center spring // Set up left gimbal Y axis as full range without return to center spring
axis[GIMBAL_AXIS_LEFT_Y].idle_value = AXIS_MIN; axis[GIMBAL_AXIS_LEFT_Y].idle_value = AXIS_MIN;
axis[GIMBAL_AXIS_LEFT_Y].deadzone = (50, 0, 50); axis[GIMBAL_AXIS_LEFT_Y].deadzone = (100, 0, 100);
axis[GIMBAL_AXIS_LEFT_Y].expo = 0.0; axis[GIMBAL_AXIS_LEFT_Y].expo = false;
// Manual calibation values
// TODO: add external EEPROM and make calibration routine
axis[GIMBAL_AXIS_LEFT_X].center = AXIS_CENTER;
axis[GIMBAL_AXIS_LEFT_X].max = AXIS_MAX - 450;
axis[GIMBAL_AXIS_LEFT_X].min = AXIS_MIN + 500;
axis[GIMBAL_AXIS_LEFT_Y].center = AXIS_CENTER + 105;
axis[GIMBAL_AXIS_LEFT_Y].max = AXIS_MAX - 250;
axis[GIMBAL_AXIS_LEFT_Y].min = AXIS_MIN + 500;
axis[GIMBAL_AXIS_RIGHT_X].center = AXIS_CENTER - 230;
axis[GIMBAL_AXIS_RIGHT_X].max = AXIS_MAX - 700;
axis[GIMBAL_AXIS_RIGHT_X].min = AXIS_MIN + 350;
axis[GIMBAL_AXIS_RIGHT_Y].center = AXIS_CENTER - 68;
axis[GIMBAL_AXIS_RIGHT_Y].max = AXIS_MAX - 700;
axis[GIMBAL_AXIS_RIGHT_Y].min = AXIS_MIN + 450;
// Create dynamic smoother array for gimbal axis // Create dynamic smoother array for gimbal axis
// TODO: Find a way to store dynamic smoother in the axis struct
let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [ let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [
DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
@ -261,113 +298,59 @@ fn main() -> ! {
let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x1209, 0x0002)) let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x1209, 0x0002))
.manufacturer("CMtec") .manufacturer("CMtec")
.product("CMDR Joystick") .product("CMDR Joystick 25")
.serial_number("0001") .serial_number("0001")
.build(); .build();
// Read calibration data from eeprom
// if !calibration_active {
// for (index, item) in axis.iter_mut().enumerate() {
// item.min = eeprom.read_byte((index as u32 * 6) + 1).unwrap() as u16;
// item.min <<= 8;
// item.min |= eeprom.read_byte(index as u32 * 6).unwrap() as u16;
// item.max = eeprom.read_byte((index as u32 * 6) + 3).unwrap() as u16;
// item.max <<= 8;
// item.max = eeprom.read_byte((index as u32 * 6) + 2).unwrap() as u16;
// item.center = eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16;
// item.center <<= 8;
// item.center = eeprom.read_byte((index as u32 * 6) + 4).unwrap() as u16;
// }
// gimbal_mode = eeprom.read_byte(24).unwrap();
// }
loop { loop {
// Temporary way to enter bootloader ------------------------- // Take care of USB HID poll requests
// TODO: Remove this after testing if usb_dev.poll(&mut [&mut usb_hid_joystick]) {
if button_matrix.buttons_pressed()[0] usb_active = true;
&& button_matrix.buttons_pressed()[1] }
&& button_matrix.buttons_pressed()[5]
&& button_matrix.buttons_pressed()[6] // Power up ELRS TX
&& button_matrix.buttons_pressed()[8] if elrs_start_count_down.wait().is_ok() {
&& button_matrix.buttons_pressed()[9] elrs_active = true;
{
status_led.update(StatusMode::Bootloader);
let gpio_activity_pin_mask: u32 = 0;
let disable_interface_mask: u32 = 0;
rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask);
} }
// -----------------------------------------------------------
if scan_count_down.wait().is_ok() { if scan_count_down.wait().is_ok() {
button_matrix.scan_matrix(&mut delay); button_matrix.scan_matrix(&mut delay);
// Have not figured out hov to store the adc pins in an array yet let mut left_x: u16 = adc.read(&mut adc_pin_left_x).unwrap();
// so we have to read them one by one let mut left_y: u16 = adc.read(&mut adc_pin_left_y).unwrap();
// TODO: Find a way to store adc pins in an array let mut right_x: u16 = adc.read(&mut adc_pin_right_x).unwrap();
smoother[GIMBAL_AXIS_LEFT_X].tick(adc.read(&mut adc_pin_left_x).unwrap()); let mut right_y: u16 = adc.read(&mut adc_pin_right_y).unwrap();
smoother[GIMBAL_AXIS_LEFT_Y].tick(adc.read(&mut adc_pin_left_y).unwrap());
smoother[GIMBAL_AXIS_RIGHT_X].tick(adc.read(&mut adc_pin_right_x).unwrap());
smoother[GIMBAL_AXIS_RIGHT_Y].tick(adc.read(&mut adc_pin_right_y).unwrap());
for (index, item) in axis.iter_mut().enumerate() { if gimbal_mode == GIMBAL_MODE_M10 {
item.value = calculate_axis_value( // Invert X1 and Y2 axis (M10 gimbals)
smoother[index].value() as u16, left_x = AXIS_MAX - left_x;
item.min, right_y = AXIS_MAX - right_y;
item.max, } else if gimbal_mode == GIMBAL_MODE_M7 {
item.center, // Invert Y1 and X2 axis (M7 gimbals)
item.deadzone, left_y = AXIS_MAX - left_y;
item.expo, right_x = AXIS_MAX - right_x;
);
} }
let pressed_keys = button_matrix.buttons_pressed(); smoother[GIMBAL_AXIS_LEFT_X].tick(left_x as i32);
mode = get_mode(pressed_keys); smoother[GIMBAL_AXIS_LEFT_Y].tick(left_y as i32);
smoother[GIMBAL_AXIS_RIGHT_X].tick(right_x as i32);
// Update pressed keys status smoother[GIMBAL_AXIS_RIGHT_Y].tick(right_y as i32);
for (index, key) in pressed_keys.iter().enumerate() {
buttons[index].pressed = *key;
}
// Update Fn mode for all axis that are in idle position
// This is to avoid the Fn mode switching when moving the gimbal
idle = true;
for item in axis.iter_mut() {
if item.value == item.idle_value {
item.fn_mode = mode & 0x0F;
} else {
idle = false;
}
}
// Set fn mode for all keys taht are in idle position
// This is to avoid the Fn mode switching when using a button
for (index, key) in buttons.iter_mut().enumerate() {
if !key.pressed {
key.fn_mode = mode & 0x0F;
} else if (usb_active
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnL
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnR
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeL
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR)
|| (!usb_active
&& layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated)
{
idle = false;
}
}
// Generate led activity when gimbal is moved from idle position
for item in axis.iter_mut() {
if item.value != item.previous_value {
activity = true;
}
item.previous_value = item.value;
}
// Generate led activity when a button is pressed
// FnL, FnR, and ModeR are excluded
for (index, key) in buttons.iter_mut().enumerate() {
if (usb_active
&& key.pressed != key.previous_pressed
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnL
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnR
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR)
|| (!usb_active
&& key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated)
{
activity = true;
}
key.previous_pressed = key.pressed;
}
}
if usb_dev.poll(&mut [&mut usb_hid_joystick]) {
usb_active = true;
} }
if status_led_count_down.wait().is_ok() { if status_led_count_down.wait().is_ok() {
@ -375,14 +358,16 @@ fn main() -> ! {
&mut status_led, &mut status_led,
&mut activity, &mut activity,
&usb_active, &usb_active,
&elrs_active,
&idle, &idle,
&safety_check, &safety_check,
&calibration_active,
); );
} }
// Dont send USB HID joystick report if there is no activity
// This is to avoid preventing the computer from going to sleep
if usb_hid_report_count_down.wait().is_ok() && activity { if usb_hid_report_count_down.wait().is_ok() && activity {
// Dont send USB HID joystick report if there is no activity
// This is to avoid preventing the computer from going to sleep
match usb_hid_joystick.device().write_report(&get_joystick_report( match usb_hid_joystick.device().write_report(&get_joystick_report(
&mut buttons, &mut buttons,
&mut axis, &mut axis,
@ -398,12 +383,184 @@ fn main() -> ! {
} }
// Check if all axis are in idle position and no buttons are pressed // Check if all axis are in idle position and no buttons are pressed
if idle && !safety_check && !usb_active { if idle && !safety_check && elrs_active {
safety_check = true; safety_check = true;
} }
// TODO: Implement ELRS if main_count_down.wait().is_ok() {
if elrs_count_down.wait().is_ok() && !usb_active && safety_check {} // Secondary way to enter bootloader (pressing all left hands buttons except the hat
if button_matrix.buttons_pressed()[0]
&& button_matrix.buttons_pressed()[1]
&& button_matrix.buttons_pressed()[5]
&& button_matrix.buttons_pressed()[6]
&& button_matrix.buttons_pressed()[8]
&& button_matrix.buttons_pressed()[9]
{
status_led.update(StatusMode::Bootloader);
let gpio_activity_pin_mask: u32 = 0;
let disable_interface_mask: u32 = 0;
rp2040_hal::rom_data::reset_to_usb_boot(
gpio_activity_pin_mask,
disable_interface_mask,
);
}
// Calibration of center position (pressing all right hands buttons except the hat
// switch)
if button_matrix.buttons_pressed()[3]
&& button_matrix.buttons_pressed()[4]
&& button_matrix.buttons_pressed()[10]
&& button_matrix.buttons_pressed()[11]
&& button_matrix.buttons_pressed()[13]
&& button_matrix.buttons_pressed()[14]
{
for (index, item) in axis.iter_mut().enumerate() {
item.center = smoother[index].value() as u16;
item.min = item.center;
item.max = item.center;
}
calibration_active = true;
}
// Calibration of min and max position
if calibration_active {
for (index, item) in axis.iter_mut().enumerate() {
if (smoother[index].value() as u16) < item.min {
item.min = smoother[index].value() as u16;
} else if (smoother[index].value() as u16) > item.max {
item.max = smoother[index].value() as u16;
}
}
}
if calibration_active && button_matrix.buttons_pressed()[8] {
gimbal_mode = GIMBAL_MODE_M10;
for (index, item) in axis.iter_mut().enumerate() {
item.center = smoother[index].value() as u16;
item.min = item.center;
item.max = item.center;
}
} else if calibration_active && button_matrix.buttons_pressed()[9] {
gimbal_mode = GIMBAL_MODE_M7;
for (index, item) in axis.iter_mut().enumerate() {
item.center = smoother[index].value() as u16;
item.min = item.center;
item.max = item.center;
}
}
// Save calibration data to eeprom (pressing right hat switch)
else if calibration_active && button_matrix.buttons_pressed()[20] {
// for (index, item) in axis.iter_mut().enumerate() {
// let _ = eeprom.write_byte(index as u32 * 6, item.min as u8);
// let _ = eeprom.write_byte((index as u32 * 6) + 1, (item.min >> 8) as u8);
// let _ = eeprom.write_byte((index as u32 * 6) + 2, item.max as u8);
// let _ = eeprom.write_byte((index as u32 * 6) + 3, (item.max >> 8) as u8);
// let _ = eeprom.write_byte((index as u32 * 6) + 4, item.center as u8);
// let _ = eeprom.write_byte((index as u32 * 6) + 5, (item.center >> 8) as u8);
// }
// let _ = eeprom.write_byte(24, gimbal_mode);
calibration_active = false;
}
// Process axis values
for (index, item) in axis.iter_mut().enumerate() {
item.value = calculate_axis_value(
smoother[index].value() as u16,
item.min,
item.max,
item.center,
item.deadzone,
item.expo,
&expo_lut,
);
}
// Update pressed keys status
for (index, key) in button_matrix.buttons_pressed().iter().enumerate() {
buttons[index].pressed = *key;
}
// Update Fn mode for all axis that are in idle position
// This is to avoid the Fn mode switching when moving the gimbal
mode = get_mode(button_matrix.buttons_pressed());
idle = true;
for item in axis.iter_mut() {
if item.value == item.idle_value {
item.fn_mode = mode & 0x0F;
} else {
idle = false;
}
}
// Set fn mode for all keys that are in idle position
// This is to avoid the Fn mode switching when using a button
for (index, key) in buttons.iter_mut().enumerate() {
if !key.pressed {
key.fn_mode = mode & 0x0F;
} else if (usb_active
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnL
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnR
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeL
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR)
|| (elrs_active
&& layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated)
{
idle = false;
}
}
// Generate led activity when gimbal is moved from idle position
for item in axis.iter_mut() {
if item.value != item.previous_value {
activity = true;
}
}
// Generate led activity when a button is pressed
// FnL, FnR, and ModeR are excluded
for (index, key) in buttons.iter_mut().enumerate() {
if (usb_active
&& key.pressed != key.previous_pressed
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnL
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnR
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR)
|| (elrs_active
&& key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated)
{
activity = true;
}
}
// Reset channel locks when calibration is active
if calibration_active {
for lock_active in channel_locks.iter_mut() {
*lock_active = false;
}
}
// Send ELRS data
if elrs_active {
elrs_en_pin.set_high().unwrap();
elrs.send(get_elrs_channels(
&mut buttons,
&mut axis,
&mut channel_locks,
elrs_active,
));
} else {
elrs_en_pin.set_low().unwrap();
}
// Clear axis status
for item in axis.iter_mut() {
item.previous_value = item.value;
}
// Clear key status
for key in buttons.iter_mut() {
key.previous_pressed = key.pressed;
}
}
} }
} }
@ -424,26 +581,42 @@ fn update_status_led<P, SM, I>(
status_led: &mut Ws2812StatusLed<P, SM, I>, status_led: &mut Ws2812StatusLed<P, SM, I>,
activity: &mut bool, activity: &mut bool,
usb_active: &bool, usb_active: &bool,
elrs_active: &bool,
axis_idle: &bool, axis_idle: &bool,
safety_check: &bool, safety_check: &bool,
calibration_active: &bool,
) where ) where
P: PIOExt + FunctionConfig, P: PIOExt + FunctionConfig,
I: PinId, I: PinId,
Function<P>: ValidPinMode<I>, Function<P>: ValidPinMode<I>,
SM: StateMachineIndex, SM: StateMachineIndex,
{ {
if !usb_active && !*safety_check { // If calibration is active, flash the status LED green
if *calibration_active && status_led.get_mode() == StatusMode::Normal {
status_led.update(StatusMode::Off);
} else if *calibration_active && status_led.get_mode() != StatusMode::Normal {
status_led.update(StatusMode::Normal);
// If in ELRS mode and safety chack failed, flash status LED red
} else if *elrs_active && !*safety_check {
status_led.update(StatusMode::Warning); status_led.update(StatusMode::Warning);
// If activity occurs, flash status LED blue
} else if *activity && status_led.get_mode() != StatusMode::Activity { } else if *activity && status_led.get_mode() != StatusMode::Activity {
status_led.update(StatusMode::Activity); status_led.update(StatusMode::Activity);
} else if *activity && status_led.get_mode() == StatusMode::Activity { } else if *activity && status_led.get_mode() == StatusMode::Activity {
status_led.update(StatusMode::Off); status_led.update(StatusMode::Off);
*activity = false; *activity = false;
// If no activity but not in idle position, turn status LED steady blue
} else if !*axis_idle && status_led.get_mode() != StatusMode::Activity { } else if !*axis_idle && status_led.get_mode() != StatusMode::Activity {
status_led.update(StatusMode::Activity); status_led.update(StatusMode::Activity);
} else if *usb_active && status_led.get_mode() != StatusMode::Normal { // Else device idle in USB mode, turn status LED steady green
} else if *axis_idle
&& *usb_active
&& !*elrs_active
&& status_led.get_mode() != StatusMode::Normal
{
status_led.update(StatusMode::Normal); status_led.update(StatusMode::Normal);
} else if status_led.get_mode() != StatusMode::Other { // Else device idle in ELRS mode, turn status LED steady orange
} else if *axis_idle && *elrs_active && status_led.get_mode() != StatusMode::Other {
status_led.update(StatusMode::Other); status_led.update(StatusMode::Other);
} }
} }
@ -570,16 +743,16 @@ fn get_joystick_report(
let mut hats: [u8; 4] = [0; 4]; let mut hats: [u8; 4] = [0; 4];
for (index, key) in matrix_keys.iter_mut().enumerate() { for (index, key) in matrix_keys.iter_mut().enumerate() {
if key.pressed if key.pressed
&& layout::HID_MAP[key.fn_mode as usize][index] >= layout::HidButton::Hat1U && layout::HID_MAP[key.fn_mode as usize][index] >= layout::HidButton::H1U
&& layout::HID_MAP[key.fn_mode as usize][index] <= layout::HidButton::Hat4B && layout::HID_MAP[key.fn_mode as usize][index] <= layout::HidButton::H4B
{ {
hats[(layout::HID_MAP[key.fn_mode as usize][index] as usize hats[(layout::HID_MAP[key.fn_mode as usize][index] as usize
- layout::HidButton::Hat1U as usize) - layout::HidButton::H1U as usize)
/ 5] |= 1 / 5] |= 1
<< ((layout::HID_MAP[key.fn_mode as usize][index] as usize << ((layout::HID_MAP[key.fn_mode as usize][index] as usize
- layout::HidButton::Hat1U as usize) - layout::HidButton::H1U as usize)
- (5 * ((layout::HID_MAP[key.fn_mode as usize][index] as usize - (5 * ((layout::HID_MAP[key.fn_mode as usize][index] as usize
- layout::HidButton::Hat1U as usize) - layout::HidButton::H1U as usize)
/ 5))); / 5)));
} }
} }
@ -671,15 +844,24 @@ fn format_hat_value(input: u8) -> (u8, u8) {
/// * `max` - Upper bound of the value's current range /// * `max` - Upper bound of the value's current range
/// * `center` - Center of the value's current range /// * `center` - Center of the value's current range
/// * `deadzone` - Deadzone of the value's current range (min, center, max) /// * `deadzone` - Deadzone of the value's current range (min, center, max)
/// * `expo` - Exponential curve factor /// * `expo` - Exponential curve factor enabled
/// * `expo_lut` - Exponential curve lookup table
fn calculate_axis_value( fn calculate_axis_value(
value: u16, value: u16,
min: u16, min: u16,
max: u16, max: u16,
center: u16, center: u16,
deadzone: (u16, u16, u16), deadzone: (u16, u16, u16),
expo: f32, expo: bool,
expo_lut: &[u16; AXIS_MAX as usize + 1],
) -> u16 { ) -> u16 {
if value <= min {
return AXIS_MIN;
}
if value >= max {
return AXIS_MAX;
}
let mut calibrated_value = AXIS_CENTER; let mut calibrated_value = AXIS_CENTER;
if value > (center + deadzone.1) { if value > (center + deadzone.1) {
@ -700,17 +882,8 @@ fn calculate_axis_value(
); );
} }
if expo != 0.0 { if expo && calibrated_value != AXIS_CENTER {
let joystick_x_float = calibrated_value as f32 / AXIS_MAX as f32; calibrated_value = expo_lut[calibrated_value as usize];
// Calculate expo using 9th order polynomial function with 0.5 as center point
let joystick_x_exp: f32 = expo * (0.5 + 256.0 * powf(joystick_x_float - 0.5, 9.0))
+ (1.0 - expo) * joystick_x_float;
calibrated_value = constrain(
(joystick_x_exp * AXIS_MAX as f32) as u16,
AXIS_MIN,
AXIS_MAX,
);
} }
calibrated_value calibrated_value
@ -749,3 +922,146 @@ fn constrain<T: PartialOrd>(value: T, out_min: T, out_max: T) -> T {
value value
} }
} }
/// Generate exponential lookup table for 12bit values
///
/// # Arguments
/// * `expo` - Exponential curve factor (range 0.0 - 1.0)
fn generate_expo_lut(expo: f32) -> [u16; AXIS_MAX as usize + 1] {
let mut lut: [u16; AXIS_MAX as usize + 1] = [0; AXIS_MAX as usize + 1];
for i in 0..AXIS_MAX + 1 {
let value_float = i as f32 / AXIS_MAX as f32;
// Calculate expo using 9th order polynomial function with 0.5 as center point
let value_exp: f32 =
expo * (0.5 + 256.0 * powf(value_float - 0.5, 9.0)) + (1.0 - expo) * value_float;
lut[i as usize] = constrain((value_exp * AXIS_MAX as f32) as u16, AXIS_MIN, AXIS_MAX);
}
lut
}
/// Get ELRS channel values
///
/// # Arguments
/// * `matrix_keys` - Array of buttons
/// * `axis` - Array of axis
fn get_elrs_channels(
matrix_keys: &mut [Button; NUMBER_OF_BUTTONS],
axis: &mut [GimbalAxis; 4],
channel_locks: &mut [bool; 12],
elrs_active: bool,
) -> [u16; 12] {
let mut channels: [u16; 12] = [ELRS_MIN; 12];
// Check and store trim values
let mut trim_active = false;
for (index, key) in matrix_keys.iter_mut().enumerate() {
if elrs_active
&& key.pressed
&& key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] >= layout::ElrsButton::CH1P
&& layout::ELRS_MAP[index] <= layout::ElrsButton::CH4P
&& axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim
< ELRS_CENTER as i16
{
axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim += 1;
trim_active = true;
} else if elrs_active
&& key.pressed
&& key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] >= layout::ElrsButton::CH1M
&& layout::ELRS_MAP[index] <= layout::ElrsButton::CH4M
&& axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim
> (0 - ELRS_CENTER as i16)
{
axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim -= 1;
trim_active = true;
}
}
// Check and reser trim values
for (index, key) in matrix_keys.iter_mut().enumerate() {
if elrs_active
&& !trim_active
&& key.pressed
&& key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] == layout::ElrsButton::CH12Z
{
axis[GIMBAL_AXIS_LEFT_X].trim = 0;
axis[GIMBAL_AXIS_LEFT_Y].trim = 0;
} else if elrs_active
&& !trim_active
&& key.pressed
&& key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] == layout::ElrsButton::CH34Z
{
axis[GIMBAL_AXIS_RIGHT_X].trim = 0;
axis[GIMBAL_AXIS_RIGHT_Y].trim = 0;
}
}
// Match ELRS channel 1-4 to new min/max values
for (index, item) in axis.iter_mut().enumerate() {
channels[index] = remap(item.value, AXIS_MIN, AXIS_MAX, ELRS_MIN, ELRS_MAX);
}
// Apply trim to ELRS channel 1,3,4
for (index, item) in axis.iter().enumerate() {
if index != GIMBAL_AXIS_LEFT_Y && channels[index] > ELRS_CENTER {
channels[index] = remap(
channels[index],
ELRS_CENTER,
ELRS_MAX,
(ELRS_CENTER as i16 + item.trim) as u16,
ELRS_MAX,
);
} else if index != GIMBAL_AXIS_LEFT_Y && channels[index] < ELRS_CENTER {
channels[index] = remap(
channels[index],
ELRS_MIN,
ELRS_CENTER,
ELRS_MIN,
(ELRS_CENTER as i16 + item.trim) as u16,
);
} else if index != GIMBAL_AXIS_LEFT_Y {
channels[index] = (ELRS_CENTER as i16 + item.trim) as u16;
}
}
// Update locking button state for ELRS channel 5-12
for (index, key) in matrix_keys.iter_mut().enumerate() {
if key.pressed
&& layout::ELRS_MAP[index] as usize >= layout::ElrsButton::CH5ON as usize
&& layout::ELRS_MAP[index] as usize <= layout::ElrsButton::CH12ON as usize
{
channel_locks
[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH5ON as usize + 4] = true;
}
if key.pressed
&& layout::ELRS_MAP[index] as usize >= layout::ElrsButton::CH5OFF as usize
&& layout::ELRS_MAP[index] as usize <= layout::ElrsButton::CH12OFF as usize
{
channel_locks
[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH5OFF as usize + 4] =
false;
}
}
// Update button state for ELRS channel 5-12
for (index, key) in matrix_keys.iter_mut().enumerate() {
if key.pressed
&& layout::ELRS_MAP[index] as usize >= layout::ElrsButton::CH5 as usize
&& layout::ELRS_MAP[index] as usize <= layout::ElrsButton::CH12 as usize
{
channels[layout::ELRS_MAP[index] as usize] = ELRS_MAX;
}
}
// Apply locking to ELRS channel 5-12
for (index, lock_active) in channel_locks.iter().enumerate() {
if *lock_active {
channels[index] = ELRS_MAX;
}
}
channels
}