diff --git a/rp2040/Cargo.toml b/rp2040/Cargo.toml index 0270364..4aebbcd 100644 --- a/rp2040/Cargo.toml +++ b/rp2040/Cargo.toml @@ -19,9 +19,9 @@ smart-leds-trait = "0.2.1" ws2812-pio = "0.6.0" usbd-human-interface-device = "0.4.2" usb-device = "0.2" -packed_struct = { version = "0.10", default-features = false } pio = "0.2.0" defmt = { version = "0.3", optional = true } +libm = "0.2.7" [features] # This is the set of features we enable by default diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 12bc3f1..998a84c 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -8,7 +8,6 @@ #![no_main] mod button_matrix; -// mod fmt; mod layout; mod status_led; mod usb_joystick_device; @@ -16,11 +15,14 @@ mod usb_joystick_device; use button_matrix::ButtonMatrix; use core::convert::Infallible; use cortex_m::delay::Delay; +use embedded_hal::adc::OneShot; use embedded_hal::digital::v2::*; use embedded_hal::timer::CountDown; use fugit::ExtU32; +use libm::powf; use panic_halt as _; use rp2040_hal::{ + adc::Adc, gpio::{Function, FunctionConfig, PinId, ValidPinMode}, pio::StateMachineIndex, }; @@ -47,14 +49,34 @@ 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; + +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; + // Public types #[derive(Copy, Clone, Default)] -pub struct KeyboardButton { +pub struct JoystickButton { pub pressed: bool, pub previous_pressed: bool, pub fn_mode: u8, } +#[derive(Copy, Clone, Default)] +pub struct JoystickAxis { + pub value: u16, + pub previous_value: u16, + pub max: u16, + pub min: u16, + pub center: u16, + pub fn_mode: u8, +} + #[entry] fn main() -> ! { // Grab our singleton objects @@ -89,6 +111,15 @@ fn main() -> ! { &mut pac.RESETS, ); + // Enable adc + 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(); + // Setting up array with pins connected to button rows let button_matrix_row_pins: &[&dyn InputPin; KEY_ROWS] = &[ &pins.gp9.into_pull_up_input(), @@ -139,8 +170,8 @@ fn main() -> ! { clocks.peripheral_clock.freq(), ); - // Create keyboard button array - let mut buttons: [KeyboardButton; NUMBER_OF_KEYS] = [KeyboardButton::default(); NUMBER_OF_KEYS]; + // Create joystick button array + let mut buttons: [JoystickButton; NUMBER_OF_KEYS] = [JoystickButton::default(); NUMBER_OF_KEYS]; // Create timers/delays let timer = Timer::new(pac.TIMER, &mut pac.RESETS); @@ -155,8 +186,13 @@ fn main() -> ! { let mut status_led_count_down = timer.count_down(); status_led_count_down.start(250.millis()); - // Create variables to track caps lock and fn mode + // Create variables to track all modes let mut fn_mode: u8; + let mut alt_l_mode: bool; + let mut alt_r_mode: bool; + + // Create joystick axis array + let mut axis: [JoystickAxis; 4]; // Initialize button matrix button_matrix.init_pins(); @@ -174,8 +210,6 @@ fn main() -> ! { rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask); } - status_led.update(StatusMode::Normal); - loop { // if status_led_count_down.wait().is_ok() { // update_status_led(&mut status_led, &caps_lock_active, &gui_lock_state); @@ -184,23 +218,20 @@ fn main() -> ! { if usb_hid_report_count_down.wait().is_ok() { let pressed_keys = button_matrix.buttons_pressed(); - fn_mode = get_fn_mode(pressed_keys); + (fn_mode, alt_l_mode, alt_r_mode) = get_mode(pressed_keys); for (index, key) in pressed_keys.iter().enumerate() { buttons[index].pressed = *key; } - match joystick - .device() - .write_report(&get_joy_report(&mut buttons, fn_mode)) - { + match joystick.device().write_report(&get_joystick_report( + &mut buttons, + axis, + fn_mode, + alt_l_mode, + alt_r_mode, + )) { Err(UsbHidError::WouldBlock) => {} - Err(UsbHidError::SerializationError) => { - status_led.update(StatusMode::Bootloader); - } - Err(UsbHidError::UsbError(UsbError::BufferOverflow)) => { - status_led.update(StatusMode::Bootloader); - } Ok(_) => {} Err(e) => { status_led.update(StatusMode::Error); @@ -211,6 +242,39 @@ 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, + ); } if usb_dev.poll(&mut [&mut joystick]) {} @@ -246,17 +310,19 @@ fn update_status_led( } } -/// Get current Fn mode (0, 1, 2 or 3) -/// layout::FN_BUTTONS contains the button types +/// 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_fn_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { +fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> (u8, bool, bool) { // 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; for (index, key) in pressed_keys.iter().enumerate() { if *key && layout::MAP[0][index] == layout::ButtonType::FnL { @@ -265,6 +331,12 @@ fn get_fn_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { if *key && layout::MAP[0][index] == layout::ButtonType::FnR { fn_r_active = true; } + if *key && layout::MAP[0][index] == layout::ButtonType::ModeL { + alt_l_mode = true; + } + if *key && layout::MAP[0][index] == layout::ButtonType::ModeR { + alt_r_mode = true; + } } if fn_l_active && fn_r_active { @@ -275,7 +347,7 @@ fn get_fn_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { fn_mode = 1; } - fn_mode + (fn_mode, alt_l_mode, alt_r_mode) } /// Generate keyboard report based on pressed keys and Fn mode (0, 1 or 2) @@ -284,22 +356,56 @@ fn get_fn_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { /// # Arguments /// /// * `matrix_keys` - Array of pressed keys -/// * `fn_mode` - Current function layer -fn get_joy_report( - matrix_keys: &mut [KeyboardButton; NUMBER_OF_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 [JoystickButton; NUMBER_OF_KEYS], + axis: [JoystickAxis; 4], fn_mode: u8, + alt_l_mode: bool, + alt_r_mode: bool, ) -> JoystickReport { - let mut x: u16 = 0x03ff; - let mut y: u16 = 0x03ff; - let mut z: u16 = 0x03ff; - let mut rx: u16 = 0x03ff; - let mut ry: u16 = 0x03ff; - let mut rz: u16 = 0x03ff; + 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 = 0xf; - let mut hat2: u8 = 0xf; - let mut hat3: u8 = 0xf; - let mut hat4: u8 = 0xf; + let mut hat1: u8 = HAT_CENTER; + let mut hat2: u8 = HAT_CENTER; + let mut hat3: u8 = HAT_CENTER; + let mut hat4: u8 = HAT_CENTER; + + if alt_l_mode && (axis[1].fn_mode == 0 || axis[1].fn_mode == 2) { + rz = remap( + axis[1].value, + HID_AXIS_MIN, + HID_AXIS_MAX, + HID_AXIS_CENTER, + HID_AXIS_MAX, + ); + } else if alt_l_mode && (axis[1].fn_mode == 1 || axis[1].fn_mode == 3) { + rz = remap( + axis[1].value, + HID_AXIS_MIN, + HID_AXIS_MAX, + HID_AXIS_CENTER, + HID_AXIS_MIN, + ); + } + + if alt_r_mode && (axis[2].fn_mode == 2 || axis[2].fn_mode == 3) { + x = HID_AXIS_CENTER; + rx = axis[2].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() { @@ -320,29 +426,28 @@ fn get_joy_report( { buttons |= 1 << layout::MAP[fn_mode as usize][index] as usize; } - // Update hat state - if key.pressed + 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 = 0, - layout::ButtonType::Hat1R => hat1 = 2, - layout::ButtonType::Hat1D => hat1 = 4, - layout::ButtonType::Hat1L => hat1 = 6, - layout::ButtonType::Hat2U => hat2 = 0, - layout::ButtonType::Hat2R => hat2 = 2, - layout::ButtonType::Hat2D => hat2 = 4, - layout::ButtonType::Hat2L => hat2 = 6, - layout::ButtonType::Hat3U => hat3 = 0, - layout::ButtonType::Hat3R => hat3 = 2, - layout::ButtonType::Hat3D => hat3 = 4, - layout::ButtonType::Hat3L => hat3 = 6, - layout::ButtonType::Hat4U => hat4 = 0, - layout::ButtonType::Hat4R => hat4 = 2, - layout::ButtonType::Hat4D => hat4 = 4, - layout::ButtonType::Hat4L => hat4 = 6, + 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, _ => {} } } @@ -362,3 +467,85 @@ fn get_joy_report( buttons, } } + +/// Remapping values from one range to another +/// +/// # Arguments +/// * `value` - Value to remap +/// * `in_min` - Lower bound of the value's current range +/// * `in_max` - Upper bound of the value's current range +/// * `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 a value to a given range +/// +/// # Arguments +/// * `value` - Value to constrain +/// * `out_min` - Lower bound of the value's target range +/// * `out_max` - Upper bound of the value's target range +fn constrain(value: T, out_min: T, out_max: T) -> T { + if value < out_min { + out_min + } else if value > out_max { + out_max + } else { + 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 +} diff --git a/rp2040/src/usb_joystick_device.rs b/rp2040/src/usb_joystick_device.rs index 65faf67..2fd20e0 100644 --- a/rp2040/src/usb_joystick_device.rs +++ b/rp2040/src/usb_joystick_device.rs @@ -85,7 +85,7 @@ pub const JOYSTICK_DESCRIPTOR: &[u8] = &[ 0xc0, // End Collection 0x05, 0x09, // Usage Page (Button) 0x19, 0x01, // Usage Minimum (1) - 0x29, 0x10, // Usage Maximum (16) + 0x29, 0x14, // Usage Maximum (20) 0x15, 0x00, // Logical Minimum (0) 0x25, 0x01, // Logical Maximum (1) 0x75, 0x01, // Report Size (1)