diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index ca24622..dd380c0 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -4,7 +4,7 @@ //! Email: cm@cmtec.se //! License: Please refer to LICENSE in root directory // -// Button index map: +// HW Button index map: // --------------------------------------------------------------- // | 0 L| 1 U| | 2 | | 3 L| 4 U| // --------------------------------------------------------------- @@ -18,7 +18,7 @@ // | | 18 | | 23 | | // --------------------------------------------------------------- // -// USB HID joystick map: +// USB HID joystick map (Enabled by pressing HW button 2+4): // --------------------------------------------------------------- // | B1 L| B2 U| | B3 | | B4 L| B5 U| // --------------------------------------------------------------- @@ -31,15 +31,16 @@ // | | H1L | B18 | H1R | | H2L | B19 | H2R | | // | | H1D | | H2D | | // --------------------------------------------------------------- -// Button (Switch) 0 changes following: -// * B1 => B21 -// * B3 => B22 -// * B10 => B23 -// * B11 => B24 +// Press and hold HW button 0 (B1) changes following: +// * B2 => B21 +// * B4 => B22 +// * B11 => B23 +// * B12 => B24 // * hat1 => hat3 (button press B20). // * hat2 => hat4 (button bpress B25) // -// ELRS channel map (+ = ON, - = OFF, CHxP/M/Z = trim) +// ELRS channel map (Enabled by pressing HW button 2+3): +// (+ = ON, - = OFF, CHxP/M/Z = trim) // --------------------------------------------------------------- // |CH7-L|CH7+U| | - | |CH8-L|CH8+U| // --------------------------------------------------------------- @@ -346,7 +347,7 @@ fn main() -> ! { let mut elrs_active: bool = false; let _elrs_connected: bool = false; let mut calibration_active: bool = false; - let mut throttle_hold: bool = false; + let mut throttle_hold_enable: 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]; @@ -505,7 +506,7 @@ fn main() -> ! { &elrs.connected(), &safety_check, &calibration_active, - &throttle_hold, + &throttle_hold_enable, ); } @@ -540,11 +541,11 @@ fn main() -> ! { } // ON/OFF switch for Throttle hold mode - if buttons[6].pressed && buttons[2].pressed && !throttle_hold { - throttle_hold = true; + if buttons[6].pressed && buttons[2].pressed && !throttle_hold_enable { + throttle_hold_enable = true; axis[GIMBAL_AXIS_LEFT_Y].hold = 0; - } else if buttons[5].pressed && buttons[2].pressed && throttle_hold { - throttle_hold = false; + } else if buttons[5].pressed && buttons[2].pressed && throttle_hold_enable { + throttle_hold_enable = false; } // Calibration of center position (pressing all right hands buttons except @@ -614,7 +615,7 @@ fn main() -> ! { } // Process throttle hold value - if throttle_hold + if throttle_hold_enable && axis[GIMBAL_AXIS_LEFT_Y].value < AXIS_CENTER && !axis[GIMBAL_AXIS_LEFT_Y].hold_pending { @@ -625,7 +626,7 @@ fn main() -> ! { AXIS_MIN, axis[GIMBAL_AXIS_LEFT_Y].hold, ); - } else if throttle_hold + } else if throttle_hold_enable && axis[GIMBAL_AXIS_LEFT_Y].value > AXIS_CENTER && !axis[GIMBAL_AXIS_LEFT_Y].hold_pending { @@ -636,10 +637,10 @@ fn main() -> ! { axis[GIMBAL_AXIS_LEFT_Y].hold, AXIS_MAX, ); - } else if throttle_hold && axis[GIMBAL_AXIS_LEFT_Y].value == AXIS_CENTER { + } else if throttle_hold_enable && 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 { + } else if throttle_hold_enable { axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold; } @@ -675,7 +676,10 @@ fn main() -> ! { usb_activity = true; } // Set throttle_hold_value - if key.pressed != key.previous_pressed && key.pressed && throttle_hold && index == 5 + if key.pressed != key.previous_pressed + && key.pressed + && throttle_hold_enable + && index == 5 { axis[GIMBAL_AXIS_LEFT_Y].hold = axis[GIMBAL_AXIS_LEFT_Y].value; axis[GIMBAL_AXIS_LEFT_Y].hold_pending = true; @@ -708,10 +712,11 @@ 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)) - { + match usb_hid_joystick.device().write_report(&get_joystick_report( + &mut buttons, + &mut axis, + &throttle_hold_enable, + )) { Err(UsbHidError::WouldBlock) => {} Ok(_) => {} Err(e) => { @@ -744,7 +749,7 @@ fn update_status_led( elrs_connected: &bool, safety_check: &bool, calibration_active: &bool, - throttle_hold: &bool, + throttle_hold_enable: &bool, ) where P: PIOExt + FunctionConfig, I: PinId, @@ -757,9 +762,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 && *throttle_hold { + } else if *usb_active && !*elrs_active && *throttle_hold_enable { status_led.update(StatusMode::Activity); - } else if *usb_active && !*elrs_active && !*throttle_hold { + } else if *usb_active && !*elrs_active && !*throttle_hold_enable { status_led.update(StatusMode::Normal); } else if *elrs_active && *elrs_connected { status_led.update(StatusMode::Other); @@ -778,27 +783,19 @@ fn update_status_led( fn get_joystick_report( matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], axis: &mut [GimbalAxis; 4], + throttle_hold_enable: &bool, ) -> JoystickReport { - let mut x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; - let mut y: u16 = AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value; + let x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; + let 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 rx: u16 = AXIS_CENTER; + let ry: u16 = AXIS_CENTER; let rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value; let (mut hat1, _hat_button1) = format_hat_value(0); let (mut hat2, _hat_button2) = format_hat_value(0); let (mut hat3, _hat_button3) = format_hat_value(0); let (mut hat4, _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 matrix_keys[8].pressed { - x = AXIS_CENTER; - rx = axis[GIMBAL_AXIS_RIGHT_X].value; - y = AXIS_CENTER; - ry = AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value; - } - // Store hat bits let mut hat_left: u8 = 0; let mut hat_right: u8 = 0; @@ -889,13 +886,14 @@ fn get_joystick_report( if key.pressed && key.usb_button != 0 && key.usb_button_toggle_enable - && key.usb_release_timeout > 1 + && (key.usb_release_timeout > 1 || *throttle_hold_enable) { 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 + && !throttle_hold_enable { buttons |= 1 << (key.usb_button_sec - 1); // Sec button mode