diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 527a75f..df9d5dd 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -147,11 +147,11 @@ fn main() -> ! { // Setting up array with pins connected to button rows let button_matrix_row_pins: &[&dyn InputPin; BUTTON_ROWS] = &[ - &pins.gp9.into_pull_up_input(), - &pins.gp10.into_pull_up_input(), &pins.gp11.into_pull_up_input(), - &pins.gp12.into_pull_up_input(), &pins.gp13.into_pull_up_input(), + &pins.gp9.into_pull_up_input(), + &pins.gp12.into_pull_up_input(), + &pins.gp10.into_pull_up_input(), ]; // Setting up array with pins connected to button columns @@ -212,7 +212,7 @@ fn main() -> ! { status_led_count_down.start(250.millis()); // Create variable to track modes - let mut fn_mode: u8 = 0; + let mut mode: u8 = 0; // Create joystick button/axis array let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS]; @@ -247,13 +247,13 @@ fn main() -> ! { loop { if status_led_count_down.wait().is_ok() { - update_status_led(&mut status_led, &fn_mode); + update_status_led(&mut status_led, &mode); } if usb_hid_report_count_down.wait().is_ok() { let pressed_keys = button_matrix.buttons_pressed(); - fn_mode = get_mode(pressed_keys); + mode = get_mode(pressed_keys); for (index, key) in pressed_keys.iter().enumerate() { buttons[index].pressed = *key; @@ -262,7 +262,7 @@ fn main() -> ! { match usb_hid_joystick.device().write_report(&get_joystick_report( &mut buttons, &mut axis, - &fn_mode, + &mode, )) { Err(UsbHidError::WouldBlock) => {} Ok(_) => {} @@ -331,7 +331,7 @@ where /// * `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 fn_mode: u8 = 0; + let mut mode: u8 = 0; let mut fn_l_active: bool = false; let mut fn_r_active: bool = false; let mut alt_l_active: bool = false; @@ -353,22 +353,22 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { } if fn_l_active && fn_r_active { - fn_mode = 3; + mode = 3; } else if fn_l_active { - fn_mode = 2; + mode = 2; } else if fn_r_active { - fn_mode = 1; + mode = 1; } // Set bit 4 and 5 if alt l/r is active if alt_l_active { - fn_mode |= 0x10; + mode |= 0x10; } if alt_r_active { - fn_mode |= 0x20; + mode |= 0x20; } - fn_mode + mode } /// Generate keyboard report based on pressed keys and Fn mode (0, 1 or 2) @@ -384,7 +384,7 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { fn get_joystick_report( matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], axis: &mut [GimbalAxis; 4], - fn_mode: &u8, + mode: &u8, ) -> JoystickReport { let mut x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; let mut y: u16 = axis[GIMBAL_AXIS_RIGHT_Y].value; @@ -397,14 +397,14 @@ fn get_joystick_report( // This is to avoid the Fn mode switching when moving the gimbal for item in axis.iter_mut() { if item.value == item.idle_value { - item.fn_mode = fn_mode & 0x0F; + item.fn_mode = mode & 0x0F; } } // 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 fn_mode & 0x10 == 0x10 + if mode & 0x10 == 0x10 && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 0 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 2) { rz = remap( @@ -414,7 +414,7 @@ fn get_joystick_report( AXIS_CENTER, AXIS_MAX, ); - } else if fn_mode & 0x10 == 0x10 + } else if mode & 0x10 == 0x10 && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 1 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 3) { rz = remap( @@ -428,13 +428,13 @@ fn get_joystick_report( // Right Alt mode active (bit 5) // Right gimbal control third joystick axis when right Fn mode is active - if fn_mode & 0x20 == 0x20 + if mode & 0x20 == 0x20 && (axis[GIMBAL_AXIS_RIGHT_X].fn_mode == 2 || axis[GIMBAL_AXIS_RIGHT_X].fn_mode == 3) { x = AXIS_CENTER; rx = axis[GIMBAL_AXIS_RIGHT_X].value; } - if fn_mode & 0x20 == 0x20 + if mode & 0x20 == 0x20 && (axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 2 || axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 3) { y = AXIS_CENTER; @@ -445,7 +445,7 @@ fn get_joystick_report( // This is to avoid the Fn mode switching when using a button for key in matrix_keys.iter_mut() { if !key.pressed { - key.fn_mode = fn_mode & 0x0F; + key.fn_mode = mode & 0x0F; } } @@ -460,14 +460,19 @@ fn get_joystick_report( let mut hats: [u8; 4] = [0; 4]; for (index, key) in matrix_keys.iter_mut().enumerate() { if key.pressed - && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize + && layout::MAP[key.fn_mode as usize][index] as usize >= layout::ButtonType::Hat1U as usize - && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize + && layout::MAP[key.fn_mode as usize][index] as usize <= layout::ButtonType::Hat4B as usize { - hats[(index - layout::ButtonType::Hat1U as usize) / 4] |= 1 - << ((index - layout::ButtonType::Hat1U as usize) - + (5 * ((index - layout::ButtonType::Hat1U as usize) / 4))); + hats[(layout::MAP[key.fn_mode as usize][index] as usize + - layout::ButtonType::Hat1U as usize) + / 5] |= 1 + << ((layout::MAP[key.fn_mode as usize][index] as usize + - layout::ButtonType::Hat1U as usize) + - (5 * ((layout::MAP[key.fn_mode as usize][index] as usize + - layout::ButtonType::Hat1U as usize) + / 5))); } } @@ -486,12 +491,10 @@ fn get_joystick_report( // Update button state for joystick button 1-20 for (index, key) in matrix_keys.iter_mut().enumerate() { if key.pressed - && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize - >= layout::ButtonType::B1 as usize - && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize - <= layout::ButtonType::B20 as usize + && layout::MAP[key.fn_mode as usize][index] as usize >= layout::ButtonType::B1 as usize + && layout::MAP[key.fn_mode as usize][index] as usize <= layout::ButtonType::B20 as usize { - buttons |= 1 << layout::MAP[(fn_mode & 0x0F) as usize][index] as usize; + buttons |= 1 << layout::MAP[key.fn_mode as usize][index] as usize; } }