From b6d4b5ccee9ad7571130c7da2f258f89298a38ba Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Fri, 26 Jul 2024 12:33:55 +0200 Subject: [PATCH] Updated button and axis struct --- rp2040/src/main.rs | 141 +++++++++++++++++++++++++++------------------ 1 file changed, 85 insertions(+), 56 deletions(-) diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 6190c03..1cbc06a 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -149,16 +149,20 @@ pub struct Button { pub pressed: bool, pub previous_pressed: bool, pub usb_changed: bool, - pub usb_changed_to: bool, + pub usb_changed_to_pressed: 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, + pub elrs_changed: bool, + pub elrs_changed_to_pressed: bool, + pub elrs_channel: usize, + pub elrs_lock_enable: bool, + pub elrs_lock_state_on: bool, + pub elrs_lock_state: bool, } #[derive(Copy, Clone)] @@ -347,10 +351,9 @@ fn main() -> ! { 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 + // Set up usb button layout buttons[0].usb_button = 1; buttons[1].usb_button = 2; buttons[2].usb_button = 3; @@ -383,6 +386,36 @@ fn main() -> ! { buttons[14].usb_button_sec = 24; buttons[14].usb_button_sec_trigger_index = 12; + // Set up elrs button layout + buttons[0].elrs_channel = 7; + buttons[0].elrs_lock_enable = true; + buttons[1].elrs_channel = 7; + buttons[1].elrs_lock_enable = true; + buttons[1].elrs_lock_state_on = true; + buttons[3].elrs_channel = 8; + buttons[3].elrs_lock_enable = true; + buttons[4].elrs_channel = 8; + buttons[4].elrs_lock_enable = true; + buttons[4].elrs_lock_state_on = true; + buttons[6].elrs_channel = 9; + buttons[7].elrs_channel = 5; + buttons[8].elrs_channel = 10; + buttons[8].elrs_lock_enable = true; + buttons[9].elrs_channel = 10; + buttons[9].elrs_lock_enable = true; + buttons[9].elrs_lock_state_on = true; + buttons[10].elrs_channel = 11; + buttons[10].elrs_lock_enable = true; + buttons[11].elrs_channel = 11; + buttons[11].elrs_lock_enable = true; + buttons[11].elrs_lock_state_on = true; + buttons[12].elrs_channel = 6; + buttons[13].elrs_channel = 12; + buttons[13].elrs_lock_enable = true; + buttons[14].elrs_channel = 12; + buttons[14].elrs_lock_enable = true; + buttons[14].elrs_lock_state_on = true; + // 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); @@ -629,9 +662,9 @@ fn main() -> ! { for (index, key) in buttons.iter_mut().enumerate() { if key.pressed != key.previous_pressed { key.usb_changed = true; - key.usb_changed_to = key.pressed; + key.usb_changed_to_pressed = key.pressed; key.elrs_changed = true; - key.elrs_changed_to = key.pressed; + key.elrs_changed_to_pressed = key.pressed; usb_activity = true; } // Set throttle_hold_value @@ -645,8 +678,11 @@ fn main() -> ! { // Reset channel locks when calibration is active if calibration_active { - for lock_active in channel_locks.iter_mut() { - *lock_active = false; + for axis in axis.iter_mut() { + axis.hold = 0; + } + for button in buttons.iter_mut() { + button.elrs_lock_state = false; } } } @@ -655,11 +691,7 @@ fn main() -> ! { // 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.send(get_elrs_channels(&mut buttons, &mut axis)); } else { elrs_en_pin.set_low().unwrap(); elrs.reset(); @@ -810,7 +842,6 @@ fn get_joystick_report( // 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 && key.usb_button != 0 @@ -839,6 +870,11 @@ fn get_joystick_report( } } + // Reset changed flags + for key in matrix_keys.iter_mut() { + key.usb_changed = false; + } + JoystickReport { x, y, @@ -1005,7 +1041,6 @@ fn generate_expo_lut(expo: f32) -> [u16; AXIS_MAX as usize + 1] { fn get_elrs_channels( matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], axis: &mut [GimbalAxis; 4], - channel_locks: &mut [bool; 12], ) -> [u16; 12] { let mut channels: [u16; 12] = [ELRS_MIN; 12]; @@ -1062,14 +1097,17 @@ fn get_elrs_channels( // Check and reset trim values for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.elrs_changed && key.elrs_changed_to && index == 15 && hat_left_button_only { + if key.elrs_changed && key.elrs_changed_to_pressed && index == 15 && hat_left_button_only { axis[GIMBAL_AXIS_LEFT_X].trim = 0; axis[GIMBAL_AXIS_LEFT_Y].trim = 0; - } else if key.elrs_changed && key.elrs_changed_to && index == 20 && hat_right_button_only { + } else if key.elrs_changed + && key.elrs_changed_to_pressed + && index == 20 + && hat_right_button_only + { axis[GIMBAL_AXIS_RIGHT_X].trim = 0; axis[GIMBAL_AXIS_RIGHT_Y].trim = 0; } - key.elrs_changed = false; } // Match ELRS channel 1-4 to new min/max values @@ -1101,49 +1139,40 @@ fn get_elrs_channels( } // Update button state for ELRS channels - for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.pressed && index == 1 { - channel_locks[6] = true; - } else if key.pressed && index == 0 { - channel_locks[6] = 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; + for key in matrix_keys.iter_mut() { + if key.elrs_changed + && key.elrs_changed_to_pressed + && key.elrs_lock_enable + && key.elrs_lock_state_on + && key.elrs_channel != 0 + { + key.elrs_lock_state = true; + } else if key.elrs_changed + && !key.elrs_changed_to_pressed + && key.elrs_lock_enable + && !key.elrs_lock_state_on + && key.elrs_channel != 0 + { + key.elrs_lock_state = false; + } else if key.pressed && !key.elrs_lock_enable && key.elrs_channel != 0 { + channels[key.elrs_channel] = ELRS_MAX; } } // Apply locking to ELRS channels - for (index, lock_active) in channel_locks.iter().enumerate() { - if *lock_active { - channels[index] = ELRS_MAX; + for key in matrix_keys.iter_mut() { + if key.elrs_lock_enable + && key.elrs_lock_state + && key.elrs_lock_state_on + && key.elrs_channel != 0 + { + channels[key.elrs_channel] = ELRS_MAX; } } + // Reset changed flags + for key in matrix_keys.iter_mut() { + key.elrs_changed = false; + } channels }