diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs deleted file mode 100644 index 261f111..0000000 --- a/rp2040/src/layout.rs +++ /dev/null @@ -1,347 +0,0 @@ -//! Project: CMtec CMDR joystick 24 -//! Date: 2023-08-01 -//! Author: Christoffer Martinsson -//! Email: cm@cmtec.se -//! License: Please refer to LICENSE in root directory - -use crate::NUMBER_OF_BUTTONS; - -#[allow(dead_code)] -#[derive(Debug, PartialEq, PartialOrd, Copy, Clone)] -pub enum ConfigButton { - CONF = 0, - BOOT = 1, - CAL = 2, - ELRS = 3, - USB = 4, - NoEventIndicated = 5, -} - -#[allow(dead_code)] -#[derive(Debug, PartialEq, PartialOrd, Copy, Clone)] -pub enum ElrsButton { - CH1 = 0, - CH2 = 1, - CH3 = 2, - CH4 = 3, - CH5 = 4, // AUX1/ARM - CH6 = 5, - CH7 = 6, - CH8 = 7, - CH9 = 8, - CH10 = 9, - CH11 = 10, - CH12 = 11, - CH5ON = 12, - CH6ON = 13, - CH7ON = 14, - CH8ON = 15, - CH9ON = 16, - CH10ON = 17, - CH11ON = 18, - CH12ON = 19, - CH5OFF = 20, - CH6OFF = 21, - CH7OFF = 22, - CH8OFF = 23, - CH9OFF = 24, - CH10OFF = 25, - CH11OFF = 26, - CH12OFF = 27, - CH1P = 28, - CH2P = 29, - CH3P = 30, - CH4P = 31, - CH1M = 32, - CH2M = 33, - CH3M = 34, - CH4M = 35, - CH12Z = 36, - CH34Z = 37, - NoEventIndicated = 38, -} -#[allow(dead_code)] -#[derive(Debug, PartialEq, PartialOrd, Copy, Clone)] -pub enum HidButton { - B1 = 0, - B2 = 1, - B3 = 2, - B4 = 3, - B5 = 4, - B6 = 5, - B7 = 6, - B8 = 7, - B9 = 8, - B10 = 9, - B11 = 10, - B12 = 11, - B13 = 12, - B14 = 13, - B15 = 14, - B16 = 15, - B17 = 16, - B18 = 17, - B19 = 18, - B20 = 19, - B21 = 20, - B22 = 21, - B23 = 22, - B24 = 23, - B25 = 24, - B26 = 25, - B27 = 26, - B28 = 27, - Fn = 28, - ModeL = 29, - ModeR = 30, - H1U = 31, - H1R = 32, - H1D = 33, - H1L = 34, - H1B = 35, - H2U = 36, - H2R = 37, - H2D = 38, - H2L = 39, - H2B = 40, - H3U = 41, - H3R = 42, - H3D = 43, - H3L = 44, - H3B = 45, - H4U = 46, - H4R = 47, - H4D = 48, - H4L = 49, - H4B = 50, - NoEventIndicated = 51, -} -#[warn(dead_code)] -// Button index map: -// -------------------------------------------------------------- -// | 0 L| 1 U| | 2 | | 3 L| 4 U| -// --------------------------------------------------------------- -// | | 5 | 6 | 7 | | 12 | 11 | 10 | | -// | | -// | | 8 | | 13 | | -// | | 9 | | 14 | | -// | X1/Y1 X2/Y2 | -// | | 16 | | 21 | | -// | | 17 | 15 | 18 | | 22 | 20 | 23 | | -// | | 19 | | 24 | | -// --------------------------------------------------------------- -// -/// Button map to HID key (four function layers) -/// Please make sure to set Fn, ModeL and ModeR at the same position for all layers -/// alt. only set these at function layer 0 and set NoEventIndicated in layer 1-3. -/// Hat button 1-4 = HID B28-B32. -pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ - [ - // Function layer 0 - // HID Key // Button Index - // ----------------------------------------- - HidButton::B23, // 0 - HidButton::B1, // 1 - HidButton::B27, // 2 - HidButton::B21, // 3 - HidButton::B6, // 4 - HidButton::B2, // 5 - HidButton::B3, // 6 - HidButton::ModeL, // 7 - HidButton::B4, // 8 - HidButton::B5, // 9 - HidButton::B7, // 10 - HidButton::B8, // 11 - HidButton::Fn, // 12 - HidButton::B9, // 13 - HidButton::B10, // 14 - HidButton::H1B, // 15 - HidButton::H1U, // 16 - HidButton::H1R, // 17 - HidButton::H1D, // 18 - HidButton::H1L, // 19 - HidButton::H2B, // 20 - HidButton::H2U, // 21 - HidButton::H2R, // 22 - HidButton::H2D, // 23 - HidButton::H2L, // 24 - ], - [ - // Function layer 1 (Fn button pressed) - // HID Key // Button Index - // ----------------------------------------- - HidButton::B24, // 0 - HidButton::B16, // 1 - HidButton::B28, // 2 - HidButton::B11, // 3 - HidButton::B22, // 4 - HidButton::B12, // 5 - HidButton::B13, // 6 - HidButton::ModeL, // 7 - HidButton::B14, // 8 - HidButton::B15, // 9 - HidButton::B17, // 10 - HidButton::B18, // 11 - HidButton::Fn, // 12 - HidButton::B19, // 13 - HidButton::B20, // 14 - HidButton::H3B, // 15 - HidButton::H3U, // 16 - HidButton::H3R, // 17 - HidButton::H3D, // 18 - HidButton::H3L, // 19 - HidButton::H4B, // 20 - HidButton::H4U, // 21 - HidButton::H4R, // 22 - HidButton::H4D, // 23 - HidButton::H4L, // 24 - ], - [ - // Function layer 2 (Mode R selected) - // HID Key // Button Index - // ----------------------------------------- - HidButton::Fn, // 0 - HidButton::H3U, // 1 - HidButton::NoEventIndicated, // 2 - HidButton::B21, // 3 - HidButton::B6, // 4 - HidButton::B2, // 5 - HidButton::B3, // 6 - HidButton::ModeL, // 7 - HidButton::B4, // 8 - HidButton::B5, // 9 - HidButton::B7, // 10 - HidButton::B8, // 11 - HidButton::ModeR, // 12 - HidButton::B9, // 13 - HidButton::B10, // 14 - HidButton::H1B, // 15 - HidButton::H1U, // 16 - HidButton::H1R, // 17 - HidButton::H1D, // 18 - HidButton::H1L, // 19 - HidButton::H2B, // 20 - HidButton::H2U, // 21 - HidButton::H2R, // 22 - HidButton::H2D, // 23 - HidButton::H2L, // 24 - ], - [ - // Function layer 3 (Mode R selected, Fn button pressed) - // HID Key // Button Index - // ----------------------------------------- - HidButton::Fn, // 0 - HidButton::H3D, // 1 - HidButton::NoEventIndicated, // 2 - HidButton::B11, // 3 - HidButton::B16, // 4 - HidButton::B12, // 5 - HidButton::B13, // 6 - HidButton::ModeL, // 7 - HidButton::B14, // 8 - HidButton::B15, // 9 - HidButton::B17, // 10 - HidButton::B18, // 11 - HidButton::ModeR, // 12 - HidButton::B19, // 13 - HidButton::B20, // 14 - HidButton::H3B, // 15 - HidButton::H3U, // 16 - HidButton::H3R, // 17 - HidButton::H3D, // 18 - HidButton::H3L, // 19 - HidButton::H4B, // 20 - HidButton::H4U, // 21 - HidButton::H4R, // 22 - HidButton::H4D, // 23 - HidButton::H4L, // 24 - ], -]; - -// Button index map: -// -------------------------------------------------------------- -// | 0 L| 1 U| | 2 | | 3 L| 4 U| -// --------------------------------------------------------------- -// | | 5 | 6 | 7 | | 12 | 11 | 10 | | -// | | -// | | 8 | | 13 | | -// | | 9 | | 14 | | -// | X1/Y1 X2/Y2 | -// | | 16 | | 21 | | -// | | 17 | 15 | 18 | | 22 | 20 | 23 | | -// | | 19 | | 24 | | -// --------------------------------------------------------------- -// -pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ - // ELRS Key // Button Index - // ----------------------------------------- - ElrsButton::CH7OFF, // 0 - ElrsButton::CH7ON, // 1 - ElrsButton::NoEventIndicated, // 2 - ElrsButton::CH8OFF, // 3 - ElrsButton::CH8ON, // 4 - ElrsButton::CH9ON, // 5 - ElrsButton::CH9OFF, // 6 - ElrsButton::CH5, // 7 - ElrsButton::CH10ON, // 8 - ElrsButton::CH10OFF, // 9 - ElrsButton::CH11ON, // 10 - ElrsButton::CH11OFF, // 11 - ElrsButton::CH6, // 12 - ElrsButton::CH12ON, // 13 - ElrsButton::CH12OFF, // 14 - ElrsButton::CH12Z, // 15 - ElrsButton::CH2P, // 16 - ElrsButton::CH1P, // 17 - ElrsButton::CH2M, // 18 - ElrsButton::CH1M, // 19 - ElrsButton::CH34Z, // 20 - ElrsButton::CH4P, // 21 - ElrsButton::CH3P, // 22 - ElrsButton::CH4M, // 23 - ElrsButton::CH3M, // 24 -]; - -// Button index map: -// -------------------------------------------------------------- -// | 0 L| 1 U| | 2 | | 3 L| 4 U| -// --------------------------------------------------------------- -// | | 5 | 6 | 7 | | 12 | 11 | 10 | | -// | | -// | | 8 | | 13 | | -// | | 9 | | 14 | | -// | X1/Y1 X2/Y2 | -// | | 16 | | 21 | | -// | | 17 | 15 | 18 | | 22 | 20 | 23 | | -// | | 19 | | 24 | | -// --------------------------------------------------------------- -// -pub const CONFIG_MAP: [ConfigButton; NUMBER_OF_BUTTONS] = [ - // Config Key // Button Index - // ----------------------------------------- - ConfigButton::BOOT, // 0 - ConfigButton::CAL, // 1 - ConfigButton::CONF, // 2 - ConfigButton::USB, // 3 - ConfigButton::ELRS, // 4 - ConfigButton::NoEventIndicated, // 5 - ConfigButton::NoEventIndicated, // 6 - ConfigButton::NoEventIndicated, // 7 - ConfigButton::NoEventIndicated, // 8 - ConfigButton::NoEventIndicated, // 9 - ConfigButton::NoEventIndicated, // 10 - ConfigButton::NoEventIndicated, // 11 - ConfigButton::NoEventIndicated, // 12 - ConfigButton::NoEventIndicated, // 13 - ConfigButton::NoEventIndicated, // 14 - ConfigButton::NoEventIndicated, // 15 - ConfigButton::NoEventIndicated, // 16 - ConfigButton::NoEventIndicated, // 17 - ConfigButton::NoEventIndicated, // 18 - ConfigButton::NoEventIndicated, // 19 - ConfigButton::NoEventIndicated, // 20 - ConfigButton::NoEventIndicated, // 21 - ConfigButton::NoEventIndicated, // 22 - ConfigButton::NoEventIndicated, // 23 - ConfigButton::NoEventIndicated, // 24 -]; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index cc2ad38..84ca5b9 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -3,13 +3,62 @@ //! Author: Christoffer Martinsson //! Email: cm@cmtec.se //! License: Please refer to LICENSE in root directory +// +// Button index map: +// --------------------------------------------------------------- +// | 0 L| 1 U| | 2 | | 3 L| 4 U| +// --------------------------------------------------------------- +// | | 5 | 6 | 7 | | 12 | 11 | 10 | | +// | | +// | | 8 | | 13 | | +// | | 9 | | 14 | | +// | X1/Y1 X2/Y2 | +// | | 16 | | 21 | | +// | | 19 | 15 | 17 | | 24 | 20 | 22 | | +// | | 18 | | 23 | | +// --------------------------------------------------------------- +// +// USB HID joystick map: +// --------------------------------------------------------------- +// | B1 L| B2 U| | B3 | | B4 L| B5 U| +// --------------------------------------------------------------- +// | | B6 | B7 | B8/16| |B13/17| B12 | B11 | | +// | | +// | | B9 | | B14 | | +// | | B10 | | B15 | | +// | X1/Y1 X2/Y2 | +// | | H1U | | H2U | | +// | | H1L | B18 | H1R | | H2L | B19 | H2R | | +// | | H1D | | H2D | | +// --------------------------------------------------------------- +// Button (Switch) 7 changes following: +// * hat1 => hat3 (button press B20). +// Button (switch) 12 changes following: +// * B4 => B21 +// * B5 => B22 +// * B14 => B23 +// * B15 => B24 +// * hat2 => hat4 (button bpress B25) +// +// ELRS channel map (+ = ON, - = OFF, CHxP/M/Z = trim) +// --------------------------------------------------------------- +// |CH7-L|CH7+U| | - | |CH8-L|CH8+U| +// --------------------------------------------------------------- +// | | THL | CH9 | CH5 | | CH6 |CH11-|CH11+| | +// | | +// | |CH10+| |CH12+| | +// | |CH10-| |CH12-| | +// | CH1/CH2 CH3/CH4 | +// | | - | |CH4P | | +// | |CH1M |CH12Z|CH1P | |CH3M |CH34Z|CH3P | | +// | | - | |CH4M | | +// --------------------------------------------------------------- #![no_std] #![no_main] mod button_matrix; mod elrs; -mod layout; mod status_led; mod usb_joystick_device; @@ -78,16 +127,24 @@ pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32; pub const DEBOUNCE: u8 = 10; +pub const RELEASE_RIMEOUT: u16 = 30; + // Public types #[derive(Copy, Clone, Default)] pub struct Button { pub pressed: bool, pub previous_pressed: bool, - pub fn_mode: u8, pub usb_changed: bool, pub usb_changed_to: bool, + pub usb_button: usize, + pub usb_button_sec_enable: bool, + pub usb_button_sec: usize, + pub usb_button_sec_trigger_index: usize, + pub usb_button_sec_pressed: bool, + pub usb_button_toggle_enable: bool, pub elrs_changed: bool, pub elrs_changed_to: bool, + pub usb_release_timeout: u16, } #[derive(Copy, Clone)] @@ -98,10 +155,11 @@ pub struct GimbalAxis { pub max: u16, pub min: u16, pub center: u16, - pub fn_mode: u8, pub deadzone: (u16, u16, u16), pub expo: bool, pub trim: i16, + pub hold: u16, + pub hold_pending: bool, } impl Default for GimbalAxis { @@ -113,10 +171,11 @@ impl Default for GimbalAxis { max: AXIS_MAX, min: AXIS_MIN, center: AXIS_CENTER, - fn_mode: 0, deadzone: (100, 50, 100), expo: true, trim: 0, + hold: 0, + hold_pending: false, } } } @@ -263,7 +322,6 @@ fn main() -> ! { let mut usb_update_count_down = timer.count_down(); usb_update_count_down.start(10.millis()); - let mut mode: u8 = 0; let mut safety_check: bool = false; let mut usb_activity: bool = false; let mut idle: bool = false; @@ -271,20 +329,49 @@ fn main() -> ! { let mut elrs_active: bool = false; let mut elrs_connected: bool = false; let mut calibration_active: bool = false; + let mut throttle_hold: 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; + // Set up button layout + buttons[0].usb_button = 1; + buttons[1].usb_button = 2; + buttons[2].usb_button = 3; + buttons[3].usb_button = 4; + buttons[3].usb_button_sec_enable = true; + buttons[3].usb_button_sec = 21; + buttons[3].usb_button_sec_trigger_index = 12; + buttons[4].usb_button = 5; + buttons[4].usb_button_sec_enable = true; + buttons[4].usb_button_sec = 22; + buttons[4].usb_button_sec_trigger_index = 12; + buttons[5].usb_button = 6; + buttons[6].usb_button = 7; + buttons[7].usb_button = 8; + buttons[7].usb_button_sec = 16; + buttons[7].usb_button_toggle_enable = true; + buttons[8].usb_button = 9; + buttons[9].usb_button = 10; + buttons[10].usb_button = 11; + buttons[11].usb_button = 12; + buttons[12].usb_button = 13; + buttons[12].usb_button_sec = 17; + buttons[12].usb_button_toggle_enable = true; + buttons[13].usb_button = 14; + buttons[13].usb_button_sec_enable = true; + buttons[13].usb_button_sec = 23; + buttons[13].usb_button_sec_trigger_index = 12; + buttons[14].usb_button = 15; + buttons[14].usb_button_sec_enable = true; + buttons[14].usb_button_sec = 24; + buttons[14].usb_button_sec_trigger_index = 12; + // Table for gimbal expo curve lookup insded of doing floating point math for every analog read 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 = (100, 0, 100); - axis[GIMBAL_AXIS_LEFT_Y].expo = false; - // Create dynamic smoother array for gimbal axis let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [ DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), @@ -364,6 +451,7 @@ fn main() -> ! { &elrs.connected(), &safety_check, &calibration_active, + &throttle_hold, ); } @@ -373,8 +461,13 @@ fn main() -> ! { } if data_process_count_down.wait().is_ok() { + // Update pressed keys status + for (index, key) in button_matrix.buttons_pressed().iter().enumerate() { + buttons[index].pressed = *key; + } + // Secondary way to enter bootloader (pressing all left hands buttons except the hat - if button_matrix.buttons_pressed()[0] && button_matrix.buttons_pressed()[2] { + if buttons[0].pressed && buttons[2].pressed { status_led.update(StatusMode::Bootloader); let gpio_activity_pin_mask: u32 = 0; let disable_interface_mask: u32 = 0; @@ -385,22 +478,24 @@ fn main() -> ! { } // ON/OFF switch for ELRS radio - if button_matrix.buttons_pressed()[4] - && button_matrix.buttons_pressed()[2] - && !elrs_active - { + if buttons[4].pressed && buttons[2].pressed && !elrs_active { safety_check = false; elrs_active = true; - } else if button_matrix.buttons_pressed()[3] - && button_matrix.buttons_pressed()[2] - && elrs_active - { + } else if buttons[3].pressed && buttons[2].pressed && elrs_active { elrs_active = false; } - // Calibration of center position (pressing all right hands buttons except the hat - // switch) - if button_matrix.buttons_pressed()[1] && button_matrix.buttons_pressed()[2] { + // ON/OFF switch for Throttle hold mode + if buttons[6].pressed && buttons[2].pressed && !throttle_hold { + throttle_hold = true; + axis[GIMBAL_AXIS_LEFT_Y].hold = 0; + } else if buttons[5].pressed && buttons[2].pressed && throttle_hold { + throttle_hold = false; + } + + // Calibration of center position (pressing all right hands buttons except + // the hat switch) + if buttons[1].pressed && buttons[2].pressed { for (index, item) in axis.iter_mut().enumerate() { item.center = smoother[index].value() as u16; item.min = item.center; @@ -420,14 +515,14 @@ fn main() -> ! { } } - if calibration_active && button_matrix.buttons_pressed()[8] { + if calibration_active && buttons[8].pressed { 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] { + } else if calibration_active && buttons[9].pressed { gimbal_mode = GIMBAL_MODE_M7; for (index, item) in axis.iter_mut().enumerate() { item.center = smoother[index].value() as u16; @@ -436,7 +531,7 @@ fn main() -> ! { } } // Save calibration data to eeprom (pressing right hat switch) - else if calibration_active && button_matrix.buttons_pressed()[20] { + else if calibration_active && buttons[20].pressed { let mut eeprom_data: [u8; 25] = [0; 25]; for (index, item) in axis.iter_mut().enumerate() { eeprom_data[index * 6] = item.min as u8; @@ -464,37 +559,39 @@ fn main() -> ! { ); } - // Update pressed keys status - for (index, key) in button_matrix.buttons_pressed().iter().enumerate() { - buttons[index].pressed = *key; + // Process throttle hold value + if throttle_hold + && axis[GIMBAL_AXIS_LEFT_Y].value < AXIS_CENTER + && !axis[GIMBAL_AXIS_LEFT_Y].hold_pending + { + axis[GIMBAL_AXIS_LEFT_Y].value = remap( + axis[GIMBAL_AXIS_LEFT_Y].value, + AXIS_MIN, + AXIS_CENTER, + AXIS_MIN, + axis[GIMBAL_AXIS_LEFT_Y].hold, + ); + } else if throttle_hold + && axis[GIMBAL_AXIS_LEFT_Y].value > AXIS_CENTER + && !axis[GIMBAL_AXIS_LEFT_Y].hold_pending + { + axis[GIMBAL_AXIS_LEFT_Y].value = remap( + axis[GIMBAL_AXIS_LEFT_Y].value, + AXIS_CENTER, + AXIS_MAX, + axis[GIMBAL_AXIS_LEFT_Y].hold, + AXIS_MAX, + ); + } else if throttle_hold && axis[GIMBAL_AXIS_LEFT_Y].value == AXIS_CENTER { + axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold; + axis[GIMBAL_AXIS_LEFT_Y].hold_pending = false; + } else if throttle_hold { + axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold; } - // 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::Fn - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeL - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR - && index != 2) - || (elrs_active - && layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated - && index != 2) - { + // Set idle state + for key in buttons.iter_mut() { + if key.pressed { idle = false; } } @@ -507,23 +604,28 @@ fn main() -> ! { item.previous_value = item.value; } + // Make sure usb will be updated during timeout countdown + for key in buttons.iter() { + if key.usb_release_timeout != 0 { + usb_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::Fn - && 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) - { + if key.pressed != key.previous_pressed { key.usb_changed = true; key.usb_changed_to = key.pressed; key.elrs_changed = true; key.elrs_changed_to = key.pressed; usb_activity = true; } + // Set throttle_hold_value + if key.pressed != key.previous_pressed && key.pressed && throttle_hold && index == 5 + { + axis[GIMBAL_AXIS_LEFT_Y].hold = axis[GIMBAL_AXIS_LEFT_Y].value; + axis[GIMBAL_AXIS_LEFT_Y].hold_pending = true; + } key.previous_pressed = key.pressed; } @@ -553,11 +655,10 @@ fn main() -> ! { // Dont send USB HID joystick report if there is no activity // This is to avoid preventing the computer from going to sleep if usb_update_count_down.wait().is_ok() && usb_activity { - match usb_hid_joystick.device().write_report(&get_joystick_report( - &mut buttons, - &mut axis, - &mode, - )) { + match usb_hid_joystick + .device() + .write_report(&get_joystick_report(&mut buttons, &mut axis)) + { Err(UsbHidError::WouldBlock) => {} Ok(_) => {} Err(e) => { @@ -590,6 +691,7 @@ fn update_status_led( elrs_connected: &bool, safety_check: &bool, calibration_active: &bool, + throttle_hold: &bool, ) where P: PIOExt + FunctionConfig, I: PinId, @@ -602,7 +704,9 @@ fn update_status_led( status_led.update(StatusMode::Warning); } else if !*usb_active && !*elrs_active { status_led.update(StatusMode::NormalFlash); - } else if *usb_active && !*elrs_active { + } else if *usb_active && !*elrs_active && *throttle_hold { + status_led.update(StatusMode::Activity); + } else if *usb_active && !*elrs_active && !*throttle_hold { status_led.update(StatusMode::Normal); } else if *elrs_active && *elrs_connected { status_led.update(StatusMode::Other); @@ -611,50 +715,6 @@ fn update_status_led( } } -/// Get current Fn mode (0, 1, 2 or 3 and alt l/r mode) -/// layout::MAP contains the button types -/// -/// # Arguments -/// -/// * `pressed_keys` - Array of pressed keys -fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { - // Check how many Fn keys are pressed - let mut mode: u8 = 0; - let mut fn_active: bool = false; - let mut alt_l_active: bool = false; - let mut alt_r_active: bool = false; - - for (index, key) in pressed_keys.iter().enumerate() { - if *key && layout::HID_MAP[0][index] == layout::HidButton::Fn { - fn_active = true; - } - if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeL { - alt_l_active = true; - } - if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeR { - alt_r_active = true; - } - } - - if alt_r_active && fn_active { - mode = 3; - } else if alt_r_active { - mode = 2; - } else if fn_active { - mode = 1; - } - - // Set bit 4 and 5 if mode l/r is active - if alt_l_active { - mode |= 0x10; - } - if alt_r_active { - mode |= 0x20; - } - - mode -} - /// Generate keyboard report based on pressed keys and Fn mode (0, 1, 2 or 3) /// layout::MAP contains the keycodes for each key in each Fn mode /// @@ -662,104 +722,106 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { /// /// * `matrix_keys` - Array of pressed keys /// * `axis` - Array of joystick axis values -/// * `fn_mode` - Fn mode (0, 1, 2 or 3) -/// * `alt_l_mode` - Is left alt mode active -/// * `alt_r_mode` - Is right alt mode active fn get_joystick_report( matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], axis: &mut [GimbalAxis; 4], - mode: &u8, ) -> JoystickReport { let mut x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; let mut y: u16 = AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value; let z: u16 = axis[GIMBAL_AXIS_LEFT_X].value; let mut rx: u16 = AXIS_CENTER; let mut ry: u16 = AXIS_CENTER; - let mut rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value; - - // Left Alt mode active (bit 4) - // Full range of left gimbal gives half range of joystick axis (center to max) - // Left Fn mode = reversed range (center to min) - if mode & 0x10 == 0x10 - && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 0 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 2) - { - rz = remap( - axis[GIMBAL_AXIS_LEFT_Y].value, - AXIS_MIN, - AXIS_MAX, - AXIS_CENTER, - AXIS_MAX, - ); - } else if mode & 0x10 == 0x10 - && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 1 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 3) - { - rz = AXIS_MAX - - remap( - axis[GIMBAL_AXIS_LEFT_Y].value, - AXIS_MIN, - AXIS_MAX, - AXIS_CENTER, - AXIS_MAX, - ); - } + let rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value; + let (mut hat1, mut hat_button1) = format_hat_value(0); + let (mut hat2, mut hat_button2) = format_hat_value(0); + let (mut hat3, mut hat_button3) = format_hat_value(0); + let (mut hat4, mut hat_button4) = format_hat_value(0); // Right Alt mode active (bit 5) // Right gimbal control third joystick axis when right Fn mode is active - if mode & 0x20 == 0x20 && (axis[GIMBAL_AXIS_RIGHT_X].fn_mode == 1) { + if matrix_keys[8].pressed { x = AXIS_CENTER; rx = axis[GIMBAL_AXIS_RIGHT_X].value; - } - if mode & 0x20 == 0x20 && axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 1 { y = AXIS_CENTER; ry = AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value; } - // Generate array for all four hat switches with following structure: - // * bit 1: Up - // * bit 2: Right - // * bit 3: Down - // * bit 4: Left - // * bit 5: Button - // * value 0 = not pressed - // * value 1 = pressed - let mut hats: [u8; 4] = [0; 4]; + let mut hat_left: u8 = 0; + let mut hat_right: u8 = 0; for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.pressed - && 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::H1U as usize) - / 5] |= 1 - << ((layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::H1U as usize) - - (5 * ((layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::H1U as usize) - / 5))); + if (15..=19).contains(&index) && key.pressed { + hat_left |= 1 << (index - 15); + } + if (20..=24).contains(&index) && key.pressed { + hat_right |= 1 << (index - 20); } } // Convert hat switch data to HID code - let (hat1, hat_button1) = format_hat_value(hats[0]); - let (hat2, hat_button2) = format_hat_value(hats[1]); - let (hat3, hat_button3) = format_hat_value(hats[2]); - let (hat4, hat_button4) = format_hat_value(hats[3]); + if matrix_keys[7].pressed { + (hat3, hat_button3) = format_hat_value(hat_left); + } else { + (hat1, hat_button1) = format_hat_value(hat_left); + } + if matrix_keys[12].pressed { + (hat4, hat_button4) = format_hat_value(hat_right); + } else { + (hat2, hat_button2) = format_hat_value(hat_right); + } // Update button state for joystick button 21-24 according to hat button 1-4 - let mut buttons: u32 = (hat_button1 as u32) << 28 - | ((hat_button2 as u32) << 29) - | ((hat_button3 as u32) << 30) - | ((hat_button4 as u32) << 31); + let mut buttons: u32 = (hat_button1 as u32) << 17 + | ((hat_button2 as u32) << 18) + | ((hat_button3 as u32) << 19) + | ((hat_button4 as u32) << 24); - // Update button state for joystick button 1-28 - for (index, key) in matrix_keys.iter_mut().enumerate() { + // Update button array with Sec button trigger status + // Using indexing instead of iterating to be able to iterate inside loop + for index in 0..NUMBER_OF_BUTTONS { + let mut sec_button_pressed: bool = false; + for (sec_index, sec_key) in matrix_keys.iter().enumerate() { + if matrix_keys[index].usb_button_sec_enable + && matrix_keys[index].usb_button_sec_trigger_index == sec_index + && sec_key.pressed + { + sec_button_pressed = true; + break; + } + } + matrix_keys[index].usb_button_sec_pressed = sec_button_pressed; + } + + // Update button state for joystick buttons + for key in matrix_keys.iter_mut() { + // Toggle mode button + if key.usb_changed && key.usb_button_toggle_enable { + key.usb_release_timeout = RELEASE_RIMEOUT; + key.usb_changed = false; + } if key.pressed - && layout::HID_MAP[key.fn_mode as usize][index] as usize - >= layout::HidButton::B1 as usize - && layout::HID_MAP[key.fn_mode as usize][index] as usize - <= layout::HidButton::B28 as usize + && key.usb_button != 0 + && key.usb_button_toggle_enable + && key.usb_release_timeout > 1 { - buttons |= 1 << layout::HID_MAP[key.fn_mode as usize][index] as usize; + buttons |= 1 << (key.usb_button - 1); + } else if !key.pressed + && key.usb_button_sec != 0 + && key.usb_button_toggle_enable + && key.usb_release_timeout > 1 + { + buttons |= 1 << (key.usb_button_sec - 1); + // Sec button mode + } else if key.pressed && key.usb_button_sec != 0 && key.usb_button_sec_pressed { + buttons |= 1 << (key.usb_button_sec - 1); + } else if key.pressed && key.usb_button != 0 && !key.usb_button_toggle_enable { + buttons |= 1 << (key.usb_button - 1); + } + } + + // Auto release button when in toggle mode + for key in matrix_keys.iter_mut() { + if key.usb_release_timeout > 0 { + key.usb_release_timeout -= 1; } } @@ -782,8 +844,8 @@ fn get_joystick_report( /// /// # Arguments /// * `input` - Hat value coded as -/// bit 1-4: direction (U R D L) -/// bit 5: button state +/// bit 2-5: direction (U L R D) +/// bit 1: button state /// 0 = not pressed /// 1 = pressed fn format_hat_value(input: u8) -> (u8, u8) { @@ -797,21 +859,21 @@ fn format_hat_value(input: u8) -> (u8, u8) { const HAT_LEFT: u8 = 6; const HAT_UP_LEFT: u8 = 7; - let direction: u8 = match input & 0x0F { - 1 => HAT_UP, - 2 => HAT_RIGHT, - 3 => HAT_UP_RIGHT, - 4 => HAT_DOWN, - 6 => HAT_DOWN_RIGHT, - 8 => HAT_LEFT, - 12 => HAT_DOWN_LEFT, - 9 => HAT_UP_LEFT, + let direction: u8 = match input & 0xFE { + 2 => HAT_UP, + 4 => HAT_RIGHT, + 6 => HAT_UP_RIGHT, + 8 => HAT_DOWN, + 12 => HAT_DOWN_RIGHT, + 16 => HAT_LEFT, + 24 => HAT_DOWN_LEFT, + 18 => HAT_UP_LEFT, _ => HAT_CENTER, }; // Alpine hat switch button filter let mut button_state: u8 = 0; - if input & 0x10 == 0x10 && direction == HAT_CENTER { + if input & 0x01 == 0x01 && direction == HAT_CENTER { button_state = 1; } @@ -934,47 +996,62 @@ fn get_elrs_channels( 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 key.pressed - && layout::ELRS_MAP[index] >= layout::ElrsButton::CH1P - && layout::ELRS_MAP[index] <= layout::ElrsButton::CH4P - { - if 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; + // Left gimbal X + if key.pressed && index == 17 && axis[GIMBAL_AXIS_LEFT_X].trim < ELRS_CENTER as i16 { + axis[GIMBAL_AXIS_LEFT_X].trim += 1; } else if key.pressed - && layout::ELRS_MAP[index] >= layout::ElrsButton::CH1M - && layout::ELRS_MAP[index] <= layout::ElrsButton::CH4M + && index == 19 + && axis[GIMBAL_AXIS_LEFT_X].trim > (0 - ELRS_CENTER as i16) { - if 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; + axis[GIMBAL_AXIS_LEFT_X].trim -= 1; + // Left gimbal Y + } else if key.pressed && index == 16 && axis[GIMBAL_AXIS_LEFT_Y].trim < ELRS_CENTER as i16 { + axis[GIMBAL_AXIS_LEFT_Y].trim += 1; + } else if key.pressed + && index == 18 + && axis[GIMBAL_AXIS_LEFT_Y].trim > (0 - ELRS_CENTER as i16) + { + axis[GIMBAL_AXIS_LEFT_Y].trim -= 1; + // Right gimbal X + } else if key.pressed && index == 22 && axis[GIMBAL_AXIS_RIGHT_X].trim < ELRS_CENTER as i16 + { + axis[GIMBAL_AXIS_RIGHT_X].trim += 1; + } else if key.pressed + && index == 24 + && axis[GIMBAL_AXIS_RIGHT_X].trim > (0 - ELRS_CENTER as i16) + { + axis[GIMBAL_AXIS_RIGHT_X].trim -= 1; + // Right gimbal Y + } else if key.pressed && index == 21 && axis[GIMBAL_AXIS_RIGHT_Y].trim < ELRS_CENTER as i16 + { + axis[GIMBAL_AXIS_RIGHT_Y].trim += 1; + } else if key.pressed + && index == 23 + && axis[GIMBAL_AXIS_RIGHT_Y].trim > (0 - ELRS_CENTER as i16) + { + axis[GIMBAL_AXIS_RIGHT_Y].trim -= 1; + } + } + + // Alpine hat switch filter + let mut hat_left_button_only: bool = true; + let mut hat_right_button_only: bool = true; + for (index, key) in matrix_keys.iter_mut().enumerate() { + if key.pressed && (16..=19).contains(&index) { + hat_left_button_only = false; + } + if key.pressed && (21..=24).contains(&index) { + hat_right_button_only = false; } } // Check and reset trim values for (index, key) in matrix_keys.iter_mut().enumerate() { - if !trim_active - && key.elrs_changed - && key.elrs_changed_to - && layout::ELRS_MAP[index] == layout::ElrsButton::CH12Z - { + if key.elrs_changed && key.elrs_changed_to && index == 15 && hat_left_button_only { axis[GIMBAL_AXIS_LEFT_X].trim = 0; axis[GIMBAL_AXIS_LEFT_Y].trim = 0; - } else if !trim_active - && key.elrs_changed - && key.elrs_changed_to - && layout::ELRS_MAP[index] == layout::ElrsButton::CH34Z - { + } else if key.elrs_changed && key.elrs_changed_to && index == 20 && hat_right_button_only { axis[GIMBAL_AXIS_RIGHT_X].trim = 0; axis[GIMBAL_AXIS_RIGHT_Y].trim = 0; } @@ -1009,36 +1086,45 @@ fn get_elrs_channels( } } - // Update locking button state for ELRS channel 5-12 + // Update button state for ELRS channels 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 && index == 1 { + channel_locks[6] = true; + } else if key.pressed && index == 0 { + channel_locks[6] = false; } - 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; + if key.pressed && index == 4 { + channel_locks[7] = true; + } else if key.pressed && index == 3 { + channel_locks[7] = false; + } + if key.pressed && index == 8 { + channel_locks[9] = true; + } else if key.pressed && index == 9 { + channel_locks[9] = false; + } + if key.pressed && index == 10 { + channel_locks[10] = true; + } else if key.pressed && index == 11 { + channel_locks[10] = false; + } + if key.pressed && index == 13 { + channel_locks[11] = true; + } else if key.pressed && index == 14 { + channel_locks[11] = false; + } + if key.pressed && index == 7 { + channels[4] = ELRS_MAX; + } + if key.pressed && index == 12 { + channels[5] = ELRS_MAX; + } + if key.pressed && index == 6 { + channels[7] = ELRS_MAX; } } - // 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 + // Apply locking to ELRS channels for (index, lock_active) in channel_locks.iter().enumerate() { if *lock_active { channels[index] = ELRS_MAX; diff --git a/rp2040/src/usb_joystick_device.rs b/rp2040/src/usb_joystick_device.rs index 33595f3..f2793d3 100644 --- a/rp2040/src/usb_joystick_device.rs +++ b/rp2040/src/usb_joystick_device.rs @@ -90,12 +90,15 @@ pub const JOYSTICK_DESCRIPTOR: &[u8] = &[ 0xc0, // End Collection 0x05, 0x09, // Usage Page (Button) 0x19, 0x01, // Usage Minimum (1) - 0x29, 0x20, // Usage Maximum (32) + 0x29, 0x19, // Usage Maximum (25) 0x15, 0x00, // Logical Minimum (0) 0x25, 0x01, // Logical Maximum (1) 0x75, 0x01, // Report Size (1) - 0x95, 0x20, // Report Count (32) - 0x81, 0x02, // Input (Data, Variable, Absolute) + 0x95, 0x19, // Report Count (25) + 0x81, 0x02, // Input (Data, Variable, Absolute) + 0x75, 0x01, // Report Size (1) PADDING + 0x95, 0x07, // Report Count (7) PADDING + 0x81, 0x03, // Input (Const, Variable, Absolute) PADDING 0x15, 0x00, // Logical Minimum (0) 0x25, 0x07, // Logical Maximum (7) 0x35, 0x00, // Physical Minimum (0)