From 3c073cd9fdf908d428f0879fb478e28cd676062c Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Wed, 2 Aug 2023 20:26:30 +0200 Subject: [PATCH] Reordered layout buttons. Added smoother functionality. Code cleanup --- rp2040/Cargo.toml | 1 + rp2040/src/layout.rs | 248 +++++++++++----------- rp2040/src/main.rs | 483 +++++++++++++++++++++++++------------------ 3 files changed, 407 insertions(+), 325 deletions(-) diff --git a/rp2040/Cargo.toml b/rp2040/Cargo.toml index 4aebbcd..dfc0c83 100644 --- a/rp2040/Cargo.toml +++ b/rp2040/Cargo.toml @@ -22,6 +22,7 @@ usb-device = "0.2" pio = "0.2.0" defmt = { version = "0.3", optional = true } libm = "0.2.7" +dyn-smooth = "0.2.0" [features] # This is the set of features we enable by default diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index dcd1bd4..2d5dbd1 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -24,31 +24,31 @@ pub enum ButtonType { B14 = 13, B15 = 14, B16 = 15, - B17 = 16, - B18 = 17, - B19 = 18, - B20 = 19, - FnL = 20, - FnR = 21, - ModeL = 22, - ModeR = 23, - Hat1U = 24, - Hat1L = 25, - Hat1R = 26, - Hat1D = 27, - Hat2U = 28, - Hat2L = 29, - Hat2R = 30, - Hat2D = 31, - Hat3U = 32, + FnL = 16, + FnR = 17, + ModeL = 18, + ModeR = 19, + Hat1U = 20, + Hat1R = 21, + Hat1D = 22, + Hat1L = 23, + Hat1B = 24, + Hat2U = 25, + Hat2R = 26, + Hat2D = 27, + Hat2L = 28, + Hat2B = 29, + Hat3U = 30, + Hat3R = 31, + Hat3D = 32, Hat3L = 33, - Hat3R = 34, - Hat3D = 35, - Hat4U = 36, - Hat4L = 37, - Hat4R = 38, - Hat4D = 39, - NotConnected = 40, + Hat3B = 34, + Hat4U = 35, + Hat4R = 36, + Hat4D = 37, + Hat4L = 38, + Hat4B = 39, + NoEventIndicated = 40, } // Button index map: @@ -70,120 +70,120 @@ pub const MAP: [[ButtonType; NUMBER_OF_KEYS]; 4] = [ // Function layer 0 // HID Key // Button Index // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B1, // 1 - ButtonType::B6, // 2 - ButtonType::FnR, // 3 - ButtonType::NotConnected, // 4 - ButtonType::B2, // 5 - ButtonType::B3, // 6 - ButtonType::ModeL, // 7 - ButtonType::B4, // 8 - ButtonType::NotConnected, // 9 - ButtonType::B7, // 10 - ButtonType::B8, // 11 - ButtonType::ModeR, // 12 - ButtonType::B9, // 13 - ButtonType::NotConnected, // 14 - ButtonType::B5, // 15 - ButtonType::Hat1U, // 16 - ButtonType::Hat1L, // 17 - ButtonType::Hat1R, // 18 - ButtonType::Hat1D, // 19 - ButtonType::B10, // 20 - ButtonType::Hat2U, // 21 - ButtonType::Hat2L, // 22 - ButtonType::Hat2R, // 23 - ButtonType::Hat2D, // 24 + ButtonType::FnL, // 0 + ButtonType::B1, // 1 + ButtonType::B5, // 2 + ButtonType::FnR, // 3 + ButtonType::NoEventIndicated, // 4 + ButtonType::B2, // 5 + ButtonType::B3, // 6 + ButtonType::ModeL, // 7 + ButtonType::B4, // 8 + ButtonType::NoEventIndicated, // 9 + ButtonType::B6, // 10 + ButtonType::B7, // 11 + ButtonType::ModeR, // 12 + ButtonType::B8, // 13 + ButtonType::NoEventIndicated, // 14 + ButtonType::Hat1B, // 15 + ButtonType::Hat1U, // 16 + ButtonType::Hat1L, // 17 + ButtonType::Hat1R, // 18 + ButtonType::Hat1D, // 19 + ButtonType::Hat2B, // 20 + ButtonType::Hat2U, // 21 + ButtonType::Hat2L, // 22 + ButtonType::Hat2R, // 23 + ButtonType::Hat2D, // 24 ], [ // Function layer left // HID Key // Button Index // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B11, // 1 - ButtonType::B6, // 2 - ButtonType::FnR, // 3 - ButtonType::NotConnected, // 4 - ButtonType::B12, // 5 - ButtonType::B13, // 6 - ButtonType::ModeL, // 7 - ButtonType::B14, // 8 - ButtonType::NotConnected, // 9 - ButtonType::B7, // 10 - ButtonType::B8, // 11 - ButtonType::ModeR, // 12 - ButtonType::B9, // 13 - ButtonType::NotConnected, // 14 - ButtonType::B15, // 15 - ButtonType::Hat1U, // 16 - ButtonType::Hat1L, // 17 - ButtonType::Hat1R, // 18 - ButtonType::Hat1D, // 19 - ButtonType::B10, // 20 - ButtonType::Hat2U, // 21 - ButtonType::Hat2L, // 22 - ButtonType::Hat2R, // 23 - ButtonType::Hat2D, // 24 + ButtonType::FnL, // 0 + ButtonType::B9, // 1 + ButtonType::B5, // 2 + ButtonType::FnR, // 3 + ButtonType::NoEventIndicated, // 4 + ButtonType::B10, // 5 + ButtonType::B11, // 6 + ButtonType::ModeL, // 7 + ButtonType::B12, // 8 + ButtonType::NoEventIndicated, // 9 + ButtonType::B6, // 10 + ButtonType::B7, // 11 + ButtonType::ModeR, // 12 + ButtonType::B8, // 13 + ButtonType::NoEventIndicated, // 14 + ButtonType::Hat3B, // 15 + ButtonType::Hat3U, // 16 + ButtonType::Hat3L, // 17 + ButtonType::Hat3R, // 18 + ButtonType::Hat3D, // 19 + ButtonType::Hat2B, // 20 + ButtonType::Hat2U, // 21 + ButtonType::Hat2L, // 22 + ButtonType::Hat2R, // 23 + ButtonType::Hat2D, // 24 ], [ // Function layer right // HID Key // Button Index // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B1, // 1 - ButtonType::B16, // 2 - ButtonType::FnR, // 3 - ButtonType::NotConnected, // 4 - ButtonType::B2, // 5 - ButtonType::B3, // 6 - ButtonType::ModeL, // 7 - ButtonType::B4, // 8 - ButtonType::NotConnected, // 9 - ButtonType::B17, // 10 - ButtonType::B18, // 11 - ButtonType::ModeR, // 12 - ButtonType::B19, // 13 - ButtonType::NotConnected, // 14 - ButtonType::B5, // 15 - ButtonType::Hat3U, // 16 - ButtonType::Hat3L, // 17 - ButtonType::Hat3R, // 18 - ButtonType::Hat3D, // 19 - ButtonType::B20, // 20 - ButtonType::Hat2U, // 21 - ButtonType::Hat2L, // 22 - ButtonType::Hat2R, // 23 - ButtonType::Hat2D, // 24 + ButtonType::FnL, // 0 + ButtonType::B1, // 1 + ButtonType::B13, // 2 + ButtonType::FnR, // 3 + ButtonType::NoEventIndicated, // 4 + ButtonType::B2, // 5 + ButtonType::B3, // 6 + ButtonType::ModeL, // 7 + ButtonType::B4, // 8 + ButtonType::NoEventIndicated, // 9 + ButtonType::B14, // 10 + ButtonType::B15, // 11 + ButtonType::ModeR, // 12 + ButtonType::B16, // 13 + ButtonType::NoEventIndicated, // 14 + ButtonType::Hat1B, // 15 + ButtonType::Hat1U, // 16 + ButtonType::Hat1L, // 17 + ButtonType::Hat1R, // 18 + ButtonType::Hat1D, // 19 + ButtonType::Hat4B, // 20 + ButtonType::Hat4U, // 21 + ButtonType::Hat4L, // 22 + ButtonType::Hat4R, // 23 + ButtonType::Hat4D, // 24 ], [ // Function layer left + right // HID Key // Button Index // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B11, // 1 - ButtonType::B16, // 2 - ButtonType::FnR, // 3 - ButtonType::NotConnected, // 4 - ButtonType::B12, // 5 - ButtonType::B13, // 6 - ButtonType::ModeL, // 7 - ButtonType::B14, // 8 - ButtonType::NotConnected, // 9 - ButtonType::B17, // 10 - ButtonType::B18, // 11 - ButtonType::ModeR, // 12 - ButtonType::B19, // 13 - ButtonType::NotConnected, // 14 - ButtonType::B15, // 15 - ButtonType::Hat3U, // 16 - ButtonType::Hat3L, // 17 - ButtonType::Hat3R, // 18 - ButtonType::Hat3D, // 19 - ButtonType::B20, // 20 - ButtonType::Hat4U, // 21 - ButtonType::Hat4L, // 22 - ButtonType::Hat4R, // 23 - ButtonType::Hat4D, // 24 + ButtonType::FnL, // 0 + ButtonType::B9, // 1 + ButtonType::B13, // 2 + ButtonType::FnR, // 3 + ButtonType::NoEventIndicated, // 4 + ButtonType::B10, // 5 + ButtonType::B11, // 6 + ButtonType::ModeL, // 7 + ButtonType::B12, // 8 + ButtonType::NoEventIndicated, // 9 + ButtonType::B14, // 10 + ButtonType::B15, // 11 + ButtonType::ModeR, // 12 + ButtonType::B16, // 13 + ButtonType::NoEventIndicated, // 14 + ButtonType::Hat3B, // 15 + ButtonType::Hat3U, // 16 + ButtonType::Hat3L, // 17 + ButtonType::Hat3R, // 18 + ButtonType::Hat3D, // 19 + ButtonType::Hat4B, // 20 + ButtonType::Hat4U, // 21 + ButtonType::Hat4L, // 22 + ButtonType::Hat4R, // 23 + ButtonType::Hat4D, // 24 ], ]; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 998a84c..dc83e7e 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -15,6 +15,7 @@ mod usb_joystick_device; use button_matrix::ButtonMatrix; use core::convert::Infallible; use cortex_m::delay::Delay; +use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS}; use embedded_hal::adc::OneShot; use embedded_hal::digital::v2::*; use embedded_hal::timer::CountDown; @@ -49,15 +50,20 @@ pub const KEY_ROWS: usize = 5; pub const KEY_COLS: usize = 5; pub const NUMBER_OF_KEYS: usize = KEY_ROWS * KEY_COLS; -const HID_AXIS_MIN: u16 = 0; -const HID_AXIS_MAX: u16 = 4095; -const HID_AXIS_CENTER: u16 = HID_AXIS_MAX / 2; +pub const AXIS_MIN: u16 = 0; +pub const AXIS_MAX: u16 = 4095; +pub const AXIS_CENTER: u16 = AXIS_MAX / 2; -const HAT_CENTER: u8 = 0xf; -const HAT_UP: u8 = 0; -const HAT_RIGHT: u8 = 2; -const HAT_DOWN: u8 = 4; -const HAT_LEFT: u8 = 6; +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; + +// Create an instance with suitable settings. +pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS; +pub const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS; +pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32; // Public types #[derive(Copy, Clone, Default)] @@ -67,14 +73,31 @@ pub struct JoystickButton { pub fn_mode: u8, } -#[derive(Copy, Clone, Default)] -pub struct JoystickAxis { +#[derive(Copy, Clone)] +pub struct GimbalAxis { pub value: u16, - pub previous_value: u16, + pub idle_value: u16, pub max: u16, pub min: u16, pub center: u16, pub fn_mode: u8, + pub deadzone: (u16, u16, u16), + pub expo: f32, +} + +impl Default for GimbalAxis { + fn default() -> Self { + GimbalAxis { + value: AXIS_CENTER, + idle_value: AXIS_CENTER, + max: AXIS_MAX, + min: AXIS_MIN, + center: AXIS_CENTER, + fn_mode: 0, + deadzone: (500, 50, 500), + expo: 0.2, + } + } } #[entry] @@ -115,10 +138,10 @@ fn main() -> ! { let mut adc = Adc::new(pac.ADC, &mut pac.RESETS); // Configure ADC input pins - let mut x1_pin = pins.gp26.into_floating_input(); - let mut y1_pin = pins.gp27.into_floating_input(); - let mut x2_pin = pins.gp28.into_floating_input(); - let mut y2_pin = pins.gp29.into_floating_input(); + let mut adc_pin_left_x = pins.gp26.into_floating_input(); + let mut adc_pin_left_y = pins.gp27.into_floating_input(); + let mut adc_pin_right_x = pins.gp28.into_floating_input(); + let mut adc_pin_right_y = pins.gp29.into_floating_input(); // Setting up array with pins connected to button rows let button_matrix_row_pins: &[&dyn InputPin; KEY_ROWS] = &[ @@ -151,7 +174,7 @@ fn main() -> ! { &mut pac.RESETS, )); - let mut joystick = UsbHidClassBuilder::new() + let mut usb_hid_joystick = UsbHidClassBuilder::new() .add_device(JoystickConfig::default()) .build(&usb_bus); @@ -186,13 +209,24 @@ fn main() -> ! { let mut status_led_count_down = timer.count_down(); status_led_count_down.start(250.millis()); - // Create variables to track all modes - let mut fn_mode: u8; - let mut alt_l_mode: bool; - let mut alt_r_mode: bool; + // Create variable to track modes + let mut fn_mode: u8 = 0; // Create joystick axis array - let mut axis: [JoystickAxis; 4]; + let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS]; + + // 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 = (500, 0, 500); + axis[GIMBAL_AXIS_LEFT_Y].expo = 0.0; + + // Create dynamic smoother array for gimbal axis + let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [ + DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), + DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), + DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), + DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), + ]; // Initialize button matrix button_matrix.init_pins(); @@ -211,25 +245,23 @@ fn main() -> ! { } loop { - // if status_led_count_down.wait().is_ok() { - // update_status_led(&mut status_led, &caps_lock_active, &gui_lock_state); - // } + if status_led_count_down.wait().is_ok() { + update_status_led(&mut status_led, &fn_mode); + } if usb_hid_report_count_down.wait().is_ok() { let pressed_keys = button_matrix.buttons_pressed(); - (fn_mode, alt_l_mode, alt_r_mode) = get_mode(pressed_keys); + fn_mode = get_mode(pressed_keys); for (index, key) in pressed_keys.iter().enumerate() { buttons[index].pressed = *key; } - match joystick.device().write_report(&get_joystick_report( + match usb_hid_joystick.device().write_report(&get_joystick_report( &mut buttons, - axis, + &mut axis, fn_mode, - alt_l_mode, - alt_r_mode, )) { Err(UsbHidError::WouldBlock) => {} Ok(_) => {} @@ -243,41 +275,26 @@ fn main() -> ! { if usb_tick_count_down.wait().is_ok() { button_matrix.scan_matrix(&mut delay); - axis[0].value = apply_calibration( - adc.read(&mut x1_pin).unwrap(), - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - 50, - 0.2, - ); - axis[1].value = apply_calibration( - adc.read(&mut y1_pin).unwrap(), - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - 50, - 0.2, - ); - axis[2].value = apply_calibration( - adc.read(&mut x2_pin).unwrap(), - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - 50, - 0.2, - ); - axis[3].value = apply_calibration( - adc.read(&mut y2_pin).unwrap(), - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - 0, - 0.0, - ); + // Read ADC values and smooth them + 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()); + + // Update 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, + ); + } } - if usb_dev.poll(&mut [&mut joystick]) {} + if usb_dev.poll(&mut [&mut usb_hid_joystick]) {} } } @@ -291,19 +308,14 @@ fn main() -> ! { /// # Arguments /// * `status_led` - Reference to status LED /// * `caps_lock_active` - Is capslock active -fn update_status_led( - status_led: &mut Ws2812StatusLed, - caps_lock_active: &bool, - gui_lock_state: &u8, -) where +fn update_status_led(status_led: &mut Ws2812StatusLed, fn_mode: &u8) +where P: PIOExt + FunctionConfig, I: PinId, Function

: ValidPinMode, SM: StateMachineIndex, { - if *caps_lock_active { - status_led.update(StatusMode::Warning); - } else if *gui_lock_state != 0 { + if *fn_mode & 0x10 == 0x10 { status_led.update(StatusMode::Activity); } else { status_led.update(StatusMode::Normal); @@ -316,13 +328,13 @@ fn update_status_led( /// # Arguments /// /// * `pressed_keys` - Array of pressed keys -fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> (u8, bool, bool) { +fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { // Check how many Fn keys are pressed let mut fn_mode: u8 = 0; let mut fn_l_active: bool = false; let mut fn_r_active: bool = false; - let mut alt_l_mode: bool = false; - let mut alt_r_mode: 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::MAP[0][index] == layout::ButtonType::FnL { @@ -332,10 +344,10 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> (u8, bool, bool) { fn_r_active = true; } if *key && layout::MAP[0][index] == layout::ButtonType::ModeL { - alt_l_mode = true; + alt_l_active = true; } if *key && layout::MAP[0][index] == layout::ButtonType::ModeR { - alt_r_mode = true; + alt_r_active = true; } } @@ -347,7 +359,15 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> (u8, bool, bool) { fn_mode = 1; } - (fn_mode, alt_l_mode, alt_r_mode) + // Set bit 4 and 5 if alt l/r is active + if alt_l_active { + fn_mode |= 0x10; + } + if alt_r_active { + fn_mode |= 0x20; + } + + fn_mode } /// Generate keyboard report based on pressed keys and Fn mode (0, 1 or 2) @@ -362,95 +382,111 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> (u8, bool, bool) { /// * `alt_r_mode` - Is right alt mode active fn get_joystick_report( matrix_keys: &mut [JoystickButton; NUMBER_OF_KEYS], - axis: [JoystickAxis; 4], + axis: &mut [GimbalAxis; 4], fn_mode: u8, - alt_l_mode: bool, - alt_r_mode: bool, ) -> JoystickReport { - let mut x: u16 = axis[2].value; // right gimbal normal mode - let mut y: u16 = axis[3].value; // right gimbal normal mode - let z: u16 = axis[0].value; // left gimbal normal mode - let mut rx: u16 = HID_AXIS_CENTER; // right gimbal alt mode - let mut ry: u16 = HID_AXIS_CENTER; // right gimbal alt mode - let mut rz: u16 = axis[1].value; // left gimbal normal and alt mode - let mut buttons: u32 = 0; - let mut hat1: u8 = HAT_CENTER; - let mut hat2: u8 = HAT_CENTER; - let mut hat3: u8 = HAT_CENTER; - let mut hat4: u8 = HAT_CENTER; + let mut x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; + let mut y: u16 = 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; - if alt_l_mode && (axis[1].fn_mode == 0 || axis[1].fn_mode == 2) { + // Update Fn mode for all axis + for item in axis.iter_mut() { + if item.value == item.idle_value { + item.fn_mode = fn_mode & 0x0F; + } + } + + // Left Alt mode active + // 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 + && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 0 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 2) + { rz = remap( - axis[1].value, - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - HID_AXIS_MAX, + axis[GIMBAL_AXIS_LEFT_Y].value, + AXIS_MIN, + AXIS_MAX, + AXIS_CENTER, + AXIS_MAX, ); - } else if alt_l_mode && (axis[1].fn_mode == 1 || axis[1].fn_mode == 3) { + } else if fn_mode & 0x10 == 0x10 + && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 1 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 3) + { rz = remap( - axis[1].value, - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - HID_AXIS_MIN, + axis[GIMBAL_AXIS_LEFT_Y].value, + AXIS_MIN, + AXIS_MAX, + AXIS_CENTER, + AXIS_MIN, ); } - if alt_r_mode && (axis[2].fn_mode == 2 || axis[2].fn_mode == 3) { - x = HID_AXIS_CENTER; - rx = axis[2].value; + // Right Alt mode active + // Right gimbal control third joystick axis when right Fn mode is active + if fn_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 + && (axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 2 || axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 3) + { + y = AXIS_CENTER; + ry = axis[GIMBAL_AXIS_RIGHT_Y].value; } - if alt_r_mode && (axis[3].fn_mode == 2 || axis[3].fn_mode == 3) { - y = HID_AXIS_CENTER; - ry = axis[3].value; - } - - // Filter report based on Fn mode and pressed keys - for (index, key) in matrix_keys.iter_mut().enumerate() { - // Set fn mode for the pressed button + // Set fn mode for pressed button + for key in matrix_keys.iter_mut() { if key.pressed != key.previous_pressed && key.pressed { - key.fn_mode = fn_mode; + key.fn_mode = fn_mode & 0x0F; } key.previous_pressed = key.pressed; + } - // Skip key if defined as NoEventIndicated - if layout::MAP[key.fn_mode as usize][index] == layout::ButtonType::NotConnected { - continue; - } - - // Update button state + // 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]; + for (index, key) in matrix_keys.iter_mut().enumerate() { if key.pressed - && layout::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::B20 as usize + && layout::MAP[fn_mode as usize][index] as usize >= layout::ButtonType::Hat1U as usize + && layout::MAP[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))); + } + } + + // 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]); + + // Update button state for joystick button 17-20 according to hat button 1-4 + let mut buttons: u32 = (hat_button1 as u32) << 16 + | ((hat_button2 as u32) << 17) + | ((hat_button3 as u32) << 18) + | ((hat_button4 as u32) << 19); + + // Update button state for joystick button 1-16 + for (index, key) in matrix_keys.iter_mut().enumerate() { + if key.pressed + && layout::MAP[fn_mode as usize][index] as usize >= layout::ButtonType::B1 as usize + && layout::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::B16 as usize { buttons |= 1 << layout::MAP[fn_mode as usize][index] as usize; } - // Update hat state - else if key.pressed - && layout::MAP[fn_mode as usize][index] as usize >= layout::ButtonType::Hat1U as usize - && layout::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::Hat4D as usize - { - match layout::MAP[fn_mode as usize][index] { - layout::ButtonType::Hat1U => hat1 = HAT_UP, - layout::ButtonType::Hat1R => hat1 = HAT_RIGHT, - layout::ButtonType::Hat1D => hat1 = HAT_DOWN, - layout::ButtonType::Hat1L => hat1 = HAT_LEFT, - layout::ButtonType::Hat2U => hat2 = HAT_UP, - layout::ButtonType::Hat2R => hat2 = HAT_RIGHT, - layout::ButtonType::Hat2D => hat2 = HAT_DOWN, - layout::ButtonType::Hat2L => hat2 = HAT_LEFT, - layout::ButtonType::Hat3U => hat3 = HAT_UP, - layout::ButtonType::Hat3R => hat3 = HAT_RIGHT, - layout::ButtonType::Hat3D => hat3 = HAT_DOWN, - layout::ButtonType::Hat3L => hat3 = HAT_RIGHT, - layout::ButtonType::Hat4U => hat4 = HAT_UP, - layout::ButtonType::Hat4R => hat4 = HAT_RIGHT, - layout::ButtonType::Hat4D => hat4 = HAT_DOWN, - layout::ButtonType::Hat4L => hat4 = HAT_LEFT, - _ => {} - } - } } JoystickReport { @@ -468,6 +504,99 @@ fn get_joystick_report( } } +/// Format hat value from 5 switches to USB HID coded value and button state +/// +/// # Arguments +/// * `input` - Hat value coded as +/// bit 1-4: direction (U R D L) +/// bit 5: button state +/// 0 = not pressed +/// 1 = pressed +fn format_hat_value(input: u8) -> (u8, u8) { + const HAT_CENTER: u8 = 0xf; + const HAT_UP: u8 = 0; + const HAT_UP_RIGHT: u8 = 1; + const HAT_RIGHT: u8 = 2; + const HAT_DOWN_RIGHT: u8 = 3; + const HAT_DOWN: u8 = 4; + const HAT_DOWN_LEFT: u8 = 5; + 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, + _ => HAT_CENTER, + }; + + // Alpine hat switch button filter + let mut button_state: u8 = 0; + if input & 0x10 == 0x10 && direction == HAT_CENTER { + button_state = 1; + } + + (direction, button_state) +} + +/// Calculate value for joystick axis +/// +/// # Arguments +/// * `value` - Value to calibrate +/// * `min` - Lower bound of the value's current range +/// * `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 +fn calculate_axis_value( + value: u16, + min: u16, + max: u16, + center: u16, + deadzone: (u16, u16, u16), + expo: f32, +) -> u16 { + let mut calibrated_value = AXIS_CENTER; + + if value > (center + deadzone.1) { + calibrated_value = remap( + value, + center + deadzone.1, + max - deadzone.2, + AXIS_CENTER, + AXIS_MAX, + ); + } else if value < (center - deadzone.1) { + calibrated_value = remap( + value, + min + deadzone.0, + center - deadzone.1, + AXIS_MIN, + AXIS_CENTER, + ); + } + + 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, + ); + } + + calibrated_value +} + /// Remapping values from one range to another /// /// # Arguments @@ -477,7 +606,13 @@ fn get_joystick_report( /// * `out_min` - Lower bound of the value's target range /// * `out_max` - Upper bound of the value's target range fn remap(value: u16, in_min: u16, in_max: u16, out_min: u16, out_max: u16) -> u16 { - (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min + constrain( + (value as i64 - in_min as i64) * (out_max as i64 - out_min as i64) + / (in_max as i64 - in_min as i64) + + out_min as i64, + out_min as i64, + out_max as i64, + ) as u16 } /// Constrain a value to a given range @@ -495,57 +630,3 @@ fn constrain(value: T, out_min: T, out_max: T) -> T { value } } - -fn calibrate_axis(axis: [JoystickAxis; 4]) {} - -fn apply_calibration( - gimbal_value: u16, - min_value: u16, - max_value: u16, - center_value: u16, - deadband_value: u16, - expo_value: f32, -) -> u16 { - let mut calibrated_value = HID_AXIS_CENTER; - - if gimbal_value > (center_value + deadband_value) { - calibrated_value = constrain( - remap( - gimbal_value, - center_value + deadband_value, - max_value, - HID_AXIS_CENTER, - HID_AXIS_MAX, - ), - HID_AXIS_CENTER, - HID_AXIS_MAX, - ); - } else if gimbal_value < (center_value - deadband_value) { - calibrated_value = constrain( - remap( - gimbal_value, - min_value, - center_value - deadband_value, - HID_AXIS_MIN, - HID_AXIS_CENTER, - ), - HID_AXIS_MIN, - HID_AXIS_CENTER, - ); - } - - if expo_value != 0.0 { - let joystick_x_float = calibrated_value as f32 / HID_AXIS_MAX as f32; - /* Calculate expo using 9th order polynomial function with 0.5 as center point */ - let joystick_x_exp: f32 = expo_value * (0.5 + 256.0 * powf(joystick_x_float - 0.5, 9.0)) - + (1.0 - expo_value) * joystick_x_float; - - calibrated_value = constrain( - (joystick_x_exp * HID_AXIS_MAX as f32) as u16, - HID_AXIS_MIN, - HID_AXIS_MAX, - ); - } - - calibrated_value -}