diff --git a/README.md b/README.md index 63ca5f9..12633e9 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,7 @@ RC Joystick with 2 hall effect gimbals, 2 hat switches and 25 buttons for use bo ```cpp USB Joystick Layer 0 -------------------------------------------------------------- -| FnL | B1 | | B25 | | B5 | FnR | +| FnL | B1 | | B21 | | B5 | FnR | -------------------------------------------------------------- | | B2 | B3 | MoL | | MoR | B7 | B6 | | | | @@ -21,7 +21,7 @@ USB Joystick Layer 0 USB Joystick Layer 1 (FnL) -------------------------------------------------------------- -| FnL | B9 | | B25 | | B5 | FnR | +| FnL | B9 | | B21 | | B5 | FnR | -------------------------------------------------------------- | | B10 | B11 | MoL | | MoR | B7 | B6 | | | | @@ -35,7 +35,7 @@ USB Joystick Layer 1 (FnL) USB Joystick Layer 2 (FnR) -------------------------------------------------------------- -| FnL | B1 | | B25 | | B13 | FnR | +| FnL | B1 | | B21 | | B13 | FnR | -------------------------------------------------------------- | | B2 | B3 | MoL | | MoR | B15 | B14 | | | | @@ -49,7 +49,7 @@ USB Joystick Layer 2 (FnR) USB Joystick Layer 3 (FnL + FnR) -------------------------------------------------------------- -| FnL | B9 | | B25 | | B13 | FnR | +| FnL | B9 | | B21 | | B13 | FnR | -------------------------------------------------------------- | | 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) -* Gerber files: [zip](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_gerber.zip) -* Schematics: [pdf](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_schematics.pdf) +- Gerber files: [zip](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_gerber.zip) +- 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 Rust @@ -111,6 +120,14 @@ Rust - Pressing boot button on teensy - 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 -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! diff --git a/rp2040/Cargo.toml b/rp2040/Cargo.toml index dfc0c83..8eabb83 100644 --- a/rp2040/Cargo.toml +++ b/rp2040/Cargo.toml @@ -23,6 +23,7 @@ pio = "0.2.0" defmt = { version = "0.3", optional = true } libm = "0.2.7" dyn-smooth = "0.2.0" +eeprom24x = "0.6.0" [features] # This is the set of features we enable by default diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index cd4fada..bee0c80 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -8,6 +8,7 @@ #![no_main] mod button_matrix; +mod elrs; mod layout; mod status_led; mod usb_joystick_device; @@ -16,16 +17,20 @@ use button_matrix::ButtonMatrix; use core::convert::Infallible; use cortex_m::delay::Delay; use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS}; +use eeprom24x::{Eeprom24x, SlaveAddr}; +use elrs::Elrs; use embedded_hal::adc::OneShot; use embedded_hal::digital::v2::*; use embedded_hal::timer::CountDown; -use fugit::ExtU32; +use fugit::{ExtU32, RateExtU32}; use libm::powf; use panic_halt as _; use rp2040_hal::{ adc::Adc, - gpio::{Function, FunctionConfig, PinId, ValidPinMode}, + gpio::{Function, FunctionConfig, FunctionUart, PinId, ValidPinMode}, + i2c::I2C, pio::StateMachineIndex, + uart::{DataBits, StopBits, UartConfig, UartPeripheral}, }; use status_led::{StatusMode, Ws2812StatusLed}; 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_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 GIMBAL_AXIS_LEFT_X: usize = 0; pub const GIMBAL_AXIS_LEFT_Y: usize = 1; pub const GIMBAL_AXIS_RIGHT_X: usize = 2; 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. pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS; @@ -83,7 +94,8 @@ pub struct GimbalAxis { pub center: u16, pub fn_mode: u8, pub deadzone: (u16, u16, u16), - pub expo: f32, + pub expo: bool, + pub trim: i16, } impl Default for GimbalAxis { @@ -96,8 +108,9 @@ impl Default for GimbalAxis { min: AXIS_MIN, center: AXIS_CENTER, fn_mode: 0, - deadzone: (50, 50, 50), - expo: 0.2, + deadzone: (100, 50, 100), + expo: true, + trim: 0, } } } @@ -136,6 +149,31 @@ fn main() -> ! { &mut pac.RESETS, ); + // Set up UART on GP0 and GP1 (Pico pins 1 and 2) + let uart_pins = ( + pins.gp0.into_mode::(), + pins.gp1.into_mode::(), + ); + + 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 let mut adc = Adc::new(pac.ADC, &mut pac.RESETS); @@ -165,6 +203,9 @@ fn main() -> ! { &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 let mut button_matrix: ButtonMatrix = ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, 5); @@ -181,9 +222,10 @@ fn main() -> ! { 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 // 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 { button_matrix.scan_matrix(&mut delay); } @@ -201,44 +243,39 @@ fn main() -> ! { usb_hid_report_count_down.start(10.millis()); 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(); status_led_count_down.start(50.millis()); - let mut elrs_count_down = timer.count_down(); - elrs_count_down.start(1660u32.micros()); + let mut main_count_down = timer.count_down(); + 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 safety_check: bool = false; let mut activity: bool = false; let mut idle: 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 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 axis[GIMBAL_AXIS_LEFT_Y].idle_value = AXIS_MIN; - axis[GIMBAL_AXIS_LEFT_Y].deadzone = (50, 0, 50); - axis[GIMBAL_AXIS_LEFT_Y].expo = 0.0; - - // 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; + axis[GIMBAL_AXIS_LEFT_Y].deadzone = (100, 0, 100); + axis[GIMBAL_AXIS_LEFT_Y].expo = false; // 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] = [ 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)) .manufacturer("CMtec") - .product("CMDR Joystick") + .product("CMDR Joystick 25") .serial_number("0001") .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 { - // Temporary way to enter bootloader ------------------------- - // TODO: Remove this after testing - 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); + // Take care of USB HID poll requests + if usb_dev.poll(&mut [&mut usb_hid_joystick]) { + usb_active = true; + } + + // Power up ELRS TX + if elrs_start_count_down.wait().is_ok() { + elrs_active = true; } - // ----------------------------------------------------------- if scan_count_down.wait().is_ok() { button_matrix.scan_matrix(&mut delay); - // Have not figured out hov to store the adc pins in an array yet - // so we have to read them one by one - // TODO: Find a way to store adc pins in an array - smoother[GIMBAL_AXIS_LEFT_X].tick(adc.read(&mut adc_pin_left_x).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()); + let mut left_x: u16 = adc.read(&mut adc_pin_left_x).unwrap(); + let mut left_y: u16 = adc.read(&mut adc_pin_left_y).unwrap(); + let mut right_x: u16 = adc.read(&mut adc_pin_right_x).unwrap(); + let mut right_y: u16 = adc.read(&mut adc_pin_right_y).unwrap(); - 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, - ); + if gimbal_mode == GIMBAL_MODE_M10 { + // Invert X1 and Y2 axis (M10 gimbals) + left_x = AXIS_MAX - left_x; + right_y = AXIS_MAX - right_y; + } else if gimbal_mode == GIMBAL_MODE_M7 { + // Invert Y1 and X2 axis (M7 gimbals) + left_y = AXIS_MAX - left_y; + right_x = AXIS_MAX - right_x; } - let pressed_keys = button_matrix.buttons_pressed(); - mode = get_mode(pressed_keys); - - // Update pressed keys status - 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; + smoother[GIMBAL_AXIS_LEFT_X].tick(left_x as i32); + smoother[GIMBAL_AXIS_LEFT_Y].tick(left_y as i32); + smoother[GIMBAL_AXIS_RIGHT_X].tick(right_x as i32); + smoother[GIMBAL_AXIS_RIGHT_Y].tick(right_y as i32); } if status_led_count_down.wait().is_ok() { @@ -375,14 +358,16 @@ fn main() -> ! { &mut status_led, &mut activity, &usb_active, + &elrs_active, &idle, &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 { - // 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( &mut buttons, &mut axis, @@ -398,12 +383,184 @@ fn main() -> ! { } // 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; } - // TODO: Implement ELRS - if elrs_count_down.wait().is_ok() && !usb_active && safety_check {} + if main_count_down.wait().is_ok() { + // 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( status_led: &mut Ws2812StatusLed, activity: &mut bool, usb_active: &bool, + elrs_active: &bool, axis_idle: &bool, safety_check: &bool, + calibration_active: &bool, ) where P: PIOExt + FunctionConfig, I: PinId, Function

: ValidPinMode, 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); + // If activity occurs, flash status LED blue } else if *activity && status_led.get_mode() != StatusMode::Activity { status_led.update(StatusMode::Activity); } else if *activity && status_led.get_mode() == StatusMode::Activity { status_led.update(StatusMode::Off); *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 { 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); - } 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); } } @@ -570,16 +743,16 @@ fn get_joystick_report( let mut hats: [u8; 4] = [0; 4]; for (index, key) in matrix_keys.iter_mut().enumerate() { 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::Hat4B + && layout::HID_MAP[key.fn_mode as usize][index] >= layout::HidButton::H1U + && layout::HID_MAP[key.fn_mode as usize][index] <= layout::HidButton::H4B { hats[(layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::Hat1U as usize) + - layout::HidButton::H1U as usize) / 5] |= 1 << ((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 - - layout::HidButton::Hat1U as usize) + - layout::HidButton::H1U as usize) / 5))); } } @@ -671,15 +844,24 @@ fn format_hat_value(input: u8) -> (u8, u8) { /// * `max` - Upper bound 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) -/// * `expo` - Exponential curve factor +/// * `expo` - Exponential curve factor enabled +/// * `expo_lut` - Exponential curve lookup table fn calculate_axis_value( value: u16, min: u16, max: u16, center: u16, deadzone: (u16, u16, u16), - expo: f32, + expo: bool, + expo_lut: &[u16; AXIS_MAX as usize + 1], ) -> u16 { + if value <= min { + return AXIS_MIN; + } + if value >= max { + return AXIS_MAX; + } + let mut calibrated_value = AXIS_CENTER; if value > (center + deadzone.1) { @@ -700,17 +882,8 @@ fn calculate_axis_value( ); } - if expo != 0.0 { - let joystick_x_float = calibrated_value as f32 / AXIS_MAX as f32; - // 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, - ); + if expo && calibrated_value != AXIS_CENTER { + calibrated_value = expo_lut[calibrated_value as usize]; } calibrated_value @@ -749,3 +922,146 @@ fn constrain(value: T, out_min: T, out_max: T) -> T { 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 +}