//! Project: CMtec CMDR joystick 25 //! Date: 2023-08-01 //! Author: Christoffer Martinsson //! Email: cm@cmtec.se //! License: Please refer to LICENSE in root directory // // HW Button index map: // --------------------------------------------------------------- // | 0 L| 1 U| 25 U | | 2 | | 26 U | 4 U| 3 L| // --------------------------------------------------------------- // | | 5 | 6 | 7 | | 12 | 11 | 10 | | // | | // | | 8 | | 13 | | // | | 9 | | 14 | | // | X1/Y1 X2/Y2 | // | | 16 | | 21 | | // | | 19 | 15 | 17 | | 24 | 20 | 22 | | // | | 18 | | 23 | | // --------------------------------------------------------------- // // USB HID joystick map : // --------------------------------------------------------------- // | Fn L| B2 U| B26 U| | B3 | | B28 U| B5 U| B4 L| // --------------------------------------------------------------- // | | TH | B6 | B8 | | B13 | B12 | B11 | | // | | // | | B9 | | B14 | | // | | B10 | | B15 | | // | X1/Y1 X2/Y2 | // | | H1U | | H2U | | // | | H1L | B18 | H1R | | H2L | B19 | H2R | | // | | H1D | | H2D | | // --------------------------------------------------------------- // // USB HID joystick map (Fn): // --------------------------------------------------------------- // | Fn L| B21 U| B27 U| | B3 | | B28 U| B5 U| B22 L| // --------------------------------------------------------------- // | | TH | B7 | B8 | | B13 | B24 | B23 | | // | | // | | B29 | | B31 | | // | | B30 | | B32 | | // | X1/Y1 X2/Y2 | // | | H3U | | H4U | | // | | H3L | B20 | H3R | | H4L | B25 | H4R | | // | | H3D | | H4D | | // --------------------------------------------------------------- // // Config Layer (holding CONFIG button) // --------------------------------------------------------------- // |BOOT L| CAL U| | CONFIG | | - | - | // --------------------------------------------------------------- // | | - | - | - | | - | - | - | | // | | // | | - | | - | | // | | - | | - | | // | -/- -/- | // | | - | | - | | // | | - | - | - | | - | - | - | | // | | - | | - | | // --------------------------------------------------------------- #![no_std] #![no_main] mod button_matrix; mod status_led; 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 eeprom24x::{Eeprom24x, SlaveAddr}; use embedded_hal::adc::OneShot; use embedded_hal::digital::v2::*; use embedded_hal::timer::CountDown; use fugit::{ExtU32, RateExtU32}; use libm::powf; use panic_halt as _; use rp2040_hal::{ adc::Adc, gpio::{Function, FunctionConfig, PinId, ValidPinMode}, i2c::I2C, pio::StateMachineIndex, }; use status_led::{StatusMode, Ws2812StatusLed}; use usb_device::class_prelude::*; use usb_device::prelude::*; use usb_joystick_device::{JoystickConfig, JoystickReport}; use usbd_human_interface_device::prelude::*; use waveshare_rp2040_zero::entry; use waveshare_rp2040_zero::{ hal::{ clocks::{init_clocks_and_plls, Clock}, pac, pio::PIOExt, timer::Timer, watchdog::Watchdog, Sio, }, Pins, XOSC_CRYSTAL_FREQ, }; // Public constants pub const BUTTON_ROWS: usize = 5; pub const BUTTON_COLS: usize = 5; pub const NUMBER_OF_BUTTONS: usize = BUTTON_ROWS * BUTTON_COLS; pub const AXIS_MIN: u16 = 0; pub const AXIS_MAX: u16 = 4095; pub const AXIS_CENTER: u16 = (AXIS_MIN + AXIS_MAX) / 2; 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; pub const GIMBAL_MODE_M10: u8 = 0; pub const GIMBAL_MODE_M7: u8 = 1; // Analog smoothing 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; pub const DEBOUNCE: u8 = 10; pub const RELEASE_RIMEOUT: u16 = 30; // => 300ms // Public types #[derive(Copy, Clone, Default)] pub struct Button { pub pressed: bool, pub previous_pressed: bool, pub usb_changed: 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 usb_release_timeout: u16, } #[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 deadzone: (u16, u16, u16), pub expo: bool, pub trim: i16, pub hold: u16, pub hold_pending: bool, } impl Default for GimbalAxis { fn default() -> Self { GimbalAxis { value: AXIS_CENTER, previous_value: AXIS_CENTER, idle_value: AXIS_CENTER, max: AXIS_MAX, min: AXIS_MIN, center: AXIS_CENTER, deadzone: (100, 50, 100), expo: true, trim: 0, hold: AXIS_CENTER, hold_pending: false, } } } #[entry] fn main() -> ! { // Grab our singleton objects let mut pac = pac::Peripherals::take().unwrap(); // Set up the watchdog driver - needed by the clock setup code let mut watchdog = Watchdog::new(pac.WATCHDOG); // Configure clocks and PLLs let clocks = init_clocks_and_plls( XOSC_CRYSTAL_FREQ, pac.XOSC, pac.CLOCKS, pac.PLL_SYS, pac.PLL_USB, &mut pac.RESETS, &mut watchdog, ) .ok() .unwrap(); let core = pac::CorePeripherals::take().unwrap(); // The single-cycle I/O block controls our GPIO pins let sio = Sio::new(pac.SIO); // Set the pins to their default state let pins = Pins::new( pac.IO_BANK0, pac.PADS_BANK0, sio.gpio_bank0, &mut pac.RESETS, ); let i2c = I2C::i2c1( pac.I2C1, pins.gp14.into_mode(), // sda pins.gp15.into_mode(), // scl 400.kHz(), &mut pac.RESETS, 125_000_000.Hz(), ); let i2c_address = SlaveAddr::Alternative(false, false, false); let mut eeprom = Eeprom24x::new_24x32(i2c, i2c_address); // Enable adc let mut adc = Adc::new(pac.ADC, &mut pac.RESETS); // Configure ADC input pins // Have not figured out hov to store the adc pins in an array yet // TODO: Find a way to store adc pins in an array let mut adc_pin_left_x = pins.gp29.into_floating_input(); let mut adc_pin_left_y = pins.gp28.into_floating_input(); let mut adc_pin_right_x = pins.gp27.into_floating_input(); let mut adc_pin_right_y = pins.gp26.into_floating_input(); // Setting up array with pins connected to button rows let button_matrix_row_pins: &[&dyn InputPin; BUTTON_ROWS] = &[ &pins.gp6.into_pull_up_input(), &pins.gp8.into_pull_up_input(), &pins.gp4.into_pull_up_input(), &pins.gp7.into_pull_up_input(), &pins.gp5.into_pull_up_input(), ]; // Setting up array with pins connected to button columns let button_matrix_col_pins: &mut [&mut dyn OutputPin; BUTTON_COLS] = &mut [ &mut pins.gp9.into_push_pull_output(), &mut pins.gp10.into_push_pull_output(), &mut pins.gp11.into_push_pull_output(), &mut pins.gp12.into_push_pull_output(), &mut pins.gp13.into_push_pull_output(), ]; // Create button matrix object that scans all buttons let mut button_matrix: ButtonMatrix = ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, DEBOUNCE); // Initialize button matrix button_matrix.init_pins(); // Setup extra buttons (connected to TX/RX pins) let left_extra_button = pins.gp1.into_pull_up_input(); let right_extra_button = pins.gp0.into_pull_up_input(); // Create status LED let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS); let mut status_led = Ws2812StatusLed::new( pins.neopixel.into_mode(), &mut pio, sm0, clocks.peripheral_clock.freq(), ); let mut delay = Delay::new(core.SYST, clocks.system_clock.freq().to_Hz()); // Scan matrix to get initial state and check if bootloader should be entered // This is done by holding button 0 pressed while power on the unit for _ in 0..10 { button_matrix.scan_matrix(&mut delay); } if button_matrix.buttons_pressed()[0] { status_led.update(StatusMode::Bootloader); let gpio_activity_pin_mask: u32 = 0; let disable_interface_mask: u32 = 0; rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask); } // Create timers let timer = Timer::new(pac.TIMER, &mut pac.RESETS); let mut status_led_count_down = timer.count_down(); status_led_count_down.start(250.millis()); let mut scan_count_down = timer.count_down(); scan_count_down.start(200u32.micros()); let mut data_process_count_down = timer.count_down(); data_process_count_down.start(1200u32.micros()); let mut usb_update_count_down = timer.count_down(); usb_update_count_down.start(10.millis()); let mut usb_activity: bool = false; let mut usb_active: bool = false; let mut calibration_active: 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 + 2] = [Button::default(); NUMBER_OF_BUTTONS + 2]; let mut gimbal_mode: u8; // Set up usb button layout buttons[0].usb_button = 0; buttons[1].usb_button = 2; buttons[1].usb_button_sec_enable = true; buttons[1].usb_button_sec = 21; buttons[1].usb_button_sec_trigger_index = 0; buttons[2].usb_button = 3; buttons[3].usb_button = 4; buttons[3].usb_button_sec_enable = true; buttons[3].usb_button_sec = 22; buttons[3].usb_button_sec_trigger_index = 0; buttons[4].usb_button = 5; buttons[5].usb_button = 0; buttons[6].usb_button = 7; buttons[7].usb_button = 8; buttons[8].usb_button = 9; buttons[9].usb_button = 10; buttons[10].usb_button = 11; buttons[10].usb_button_sec_enable = true; buttons[10].usb_button_sec = 23; buttons[10].usb_button_sec_trigger_index = 0; buttons[11].usb_button = 12; buttons[11].usb_button_sec_enable = true; buttons[11].usb_button_sec = 24; buttons[11].usb_button_sec_trigger_index = 0; buttons[12].usb_button = 13; buttons[13].usb_button = 14; buttons[14].usb_button = 15; buttons[15].usb_button = 18; buttons[15].usb_button_sec_enable = true; buttons[15].usb_button_sec = 20; buttons[15].usb_button_sec_trigger_index = 0; buttons[20].usb_button = 19; buttons[20].usb_button_sec_enable = true; buttons[20].usb_button_sec = 25; buttons[20].usb_button_sec_trigger_index = 0; buttons[25].usb_button = 26; buttons[25].usb_button_sec_enable = true; buttons[25].usb_button_sec = 27; buttons[25].usb_button_sec_trigger_index = 0; buttons[26].usb_button = 28; // 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); // 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), ]; // Configure USB let usb_bus = UsbBusAllocator::new(waveshare_rp2040_zero::hal::usb::UsbBus::new( pac.USBCTRL_REGS, pac.USBCTRL_DPRAM, clocks.usb_clock, true, &mut pac.RESETS, )); let mut usb_hid_joystick = UsbHidClassBuilder::new() .add_device(JoystickConfig::default()) .build(&usb_bus); let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x1209, 0x0002)) .manufacturer("CMtec") .product("CMDR Joystick 25") .serial_number("0001") .build(); // Read calibration data from eeprom for (index, item) in axis.iter_mut().enumerate() { item.min = eeprom.read_byte((index as u32 * 6) + 2).unwrap() as u16; item.min <<= 8; item.min |= eeprom.read_byte((index as u32 * 6) + 1).unwrap() as u16; item.max = eeprom.read_byte((index as u32 * 6) + 4).unwrap() as u16; item.max <<= 8; item.max |= eeprom.read_byte((index as u32 * 6) + 3).unwrap() as u16; item.center = eeprom.read_byte((index as u32 * 6) + 6).unwrap() as u16; item.center <<= 8; item.center |= eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16; } gimbal_mode = eeprom.read_byte(25).unwrap(); loop { // Take care of USB HID poll requests if usb_dev.poll(&mut [&mut usb_hid_joystick]) { usb_active = true; } if scan_count_down.wait().is_ok() { button_matrix.scan_matrix(&mut delay); let mut left_x: u16 = adc.read(&mut adc_pin_left_x).unwrap(); let mut left_y: u16 = adc.read(&mut adc_pin_left_y).unwrap(); let mut right_x: u16 = adc.read(&mut adc_pin_right_x).unwrap(); let mut right_y: u16 = adc.read(&mut adc_pin_right_y).unwrap(); if gimbal_mode == GIMBAL_MODE_M10 { // Invert X1 and Y2 axis (M10 gimbals) left_x = AXIS_MAX - left_x; right_y = AXIS_MAX - right_y; } else if gimbal_mode == GIMBAL_MODE_M7 { // Invert Y1 and X2 axis (M7 gimbals) left_y = AXIS_MAX - left_y; right_x = AXIS_MAX - right_x; } smoother[GIMBAL_AXIS_LEFT_X].tick(left_x as i32); smoother[GIMBAL_AXIS_LEFT_Y].tick(left_y as i32); smoother[GIMBAL_AXIS_RIGHT_X].tick(right_x as i32); smoother[GIMBAL_AXIS_RIGHT_Y].tick(right_y as i32); } if status_led_count_down.wait().is_ok() { update_status_led( &mut status_led, &usb_active, &calibration_active, &throttle_hold_enable, ); } if data_process_count_down.wait().is_ok() { // Update pressed keys status for (index, key) in button_matrix.buttons_pressed().iter().enumerate() { buttons[index].pressed = *key; } // Updated extra buttons buttons[25].pressed = left_extra_button.is_low().unwrap(); buttons[26].pressed = right_extra_button.is_low().unwrap(); // Secondary way to enter bootloader (pressing all left hands buttons except the hat if buttons[0].pressed && buttons[2].pressed { status_led.update(StatusMode::Bootloader); let gpio_activity_pin_mask: u32 = 0; let disable_interface_mask: u32 = 0; rp2040_hal::rom_data::reset_to_usb_boot( gpio_activity_pin_mask, disable_interface_mask, ); } // // ON/OFF switch for Throttle hold mode throttle_hold_enable = axis[GIMBAL_AXIS_LEFT_Y].hold != AXIS_CENTER; // Calibration of center position (pressing all right hands buttons except // the hat switch) if buttons[1].pressed && buttons[2].pressed { for (index, item) in axis.iter_mut().enumerate() { item.center = smoother[index].value() as u16; item.min = item.center; item.max = item.center; } calibration_active = true; } // Calibration of min and max position if calibration_active { for (index, item) in axis.iter_mut().enumerate() { if (smoother[index].value() as u16) < item.min { item.min = smoother[index].value() as u16; } else if (smoother[index].value() as u16) > item.max { item.max = smoother[index].value() as u16; } } } if calibration_active && buttons[8].pressed { gimbal_mode = GIMBAL_MODE_M10; for (index, item) in axis.iter_mut().enumerate() { item.center = smoother[index].value() as u16; item.min = item.center; item.max = item.center; } } else if calibration_active && buttons[9].pressed { gimbal_mode = GIMBAL_MODE_M7; for (index, item) in axis.iter_mut().enumerate() { item.center = smoother[index].value() as u16; item.min = item.center; item.max = item.center; } } // Save calibration data to eeprom (pressing right hat switch) else if calibration_active && buttons[20].pressed { let mut eeprom_data: [u8; 25] = [0; 25]; for (index, item) in axis.iter_mut().enumerate() { eeprom_data[index * 6] = item.min as u8; eeprom_data[(index * 6) + 1] = (item.min >> 8) as u8; eeprom_data[(index * 6) + 2] = item.max as u8; eeprom_data[(index * 6) + 3] = (item.max >> 8) as u8; eeprom_data[(index * 6) + 4] = item.center as u8; eeprom_data[(index * 6) + 5] = (item.center >> 8) as u8; } eeprom_data[24] = gimbal_mode; let _ = eeprom.write_page(0x01, &eeprom_data); calibration_active = false; } // Process 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, &expo_lut, ); } // Process throttle hold value let unprocessed_value = axis[GIMBAL_AXIS_LEFT_Y].value; if throttle_hold_enable && axis[GIMBAL_AXIS_LEFT_Y].value < AXIS_CENTER && !axis[GIMBAL_AXIS_LEFT_Y].hold_pending { axis[GIMBAL_AXIS_LEFT_Y].value = remap( axis[GIMBAL_AXIS_LEFT_Y].value, AXIS_MIN, AXIS_CENTER, AXIS_MIN, axis[GIMBAL_AXIS_LEFT_Y].hold, ); } else if throttle_hold_enable && axis[GIMBAL_AXIS_LEFT_Y].value > AXIS_CENTER && !axis[GIMBAL_AXIS_LEFT_Y].hold_pending { axis[GIMBAL_AXIS_LEFT_Y].value = remap( axis[GIMBAL_AXIS_LEFT_Y].value, AXIS_CENTER, AXIS_MAX, axis[GIMBAL_AXIS_LEFT_Y].hold, AXIS_MAX, ); } 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_enable { axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold; } // Generate led activity when gimbal is moved from idle position for item in axis.iter_mut() { if item.value != item.previous_value { usb_activity = true; } item.previous_value = item.value; } // Make sure usb will be updated during timeout countdown for key in buttons.iter() { if key.usb_release_timeout != 0 { usb_activity = true; } } // Generate led activity when a button is pressed for (index, key) in buttons.iter_mut().enumerate() { if key.pressed != key.previous_pressed { key.usb_changed = true; key.usb_changed_to_pressed = key.pressed; usb_activity = true; } // Set throttle_hold_value if key.pressed != key.previous_pressed && key.pressed && index == 5 && unprocessed_value != AXIS_CENTER { axis[GIMBAL_AXIS_LEFT_Y].hold = axis[GIMBAL_AXIS_LEFT_Y].value; axis[GIMBAL_AXIS_LEFT_Y].hold_pending = true; } else if key.pressed != key.previous_pressed && key.pressed && index == 5 && unprocessed_value == AXIS_CENTER { axis[GIMBAL_AXIS_LEFT_Y].hold = AXIS_CENTER; axis[GIMBAL_AXIS_LEFT_Y].hold_pending = true; } key.previous_pressed = key.pressed; } // Reset channel locks when calibration is active if calibration_active { for axis in axis.iter_mut() { axis.hold = 0; } } } // 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)) { Err(UsbHidError::WouldBlock) => {} Ok(_) => {} Err(e) => { status_led.update(StatusMode::Error); core::panic!("Failed to write joystick report: {:?}", e) } }; usb_activity = false; } } } /// Update status LED colour based on function layer and capslock /// /// USB mode = green /// Activity = flashing blue /// Error = steady red (ERROR) /// /// # Arguments /// * `status_led` - Reference to status LED /// * `activity` - Reference to bool that indicates if there is activity /// * `usb_active` - Reference to bool that indicates if USB is active /// * `axis_idle` - Reference to bool that indicates if all axis are in idle position /// * `safety_check` - Reference to bool that indicates if safety check has passed fn update_status_led( status_led: &mut Ws2812StatusLed, usb_active: &bool, calibration_active: &bool, throttle_hold_enable: &bool, ) where P: PIOExt + FunctionConfig, I: PinId, Function

: ValidPinMode, SM: StateMachineIndex, { if *calibration_active { status_led.update(StatusMode::Warning); } else if !*usb_active { status_led.update(StatusMode::NormalFlash); } else if *usb_active && *throttle_hold_enable { status_led.update(StatusMode::Other); } else if *usb_active && !*throttle_hold_enable { status_led.update(StatusMode::Normal); } } /// Generate keyboard report based on pressed keys and Fn mode (0, 1, 2 or 3) /// layout::MAP contains the keycodes for each key in each Fn mode /// /// # Arguments /// /// * `matrix_keys` - Array of pressed keys /// * `axis` - Array of joystick axis values fn get_joystick_report( matrix_keys: &mut [Button; NUMBER_OF_BUTTONS + 2], axis: &mut [GimbalAxis; 4], ) -> JoystickReport { let x: u16 = axis[GIMBAL_AXIS_LEFT_X].value; let y: u16 = AXIS_MAX - axis[GIMBAL_AXIS_LEFT_Y].value; let z: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; let rx: u16 = AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value; let ry: u16 = AXIS_CENTER; let rz: u16 = AXIS_CENTER; let mut hat1: u8 = 0xf; let mut hat2: u8 = 0xf; let mut hat3: u8 = 0xf; let mut hat4: u8 = 0xf; // Store hat bits let mut hat_left: u8 = 0; let mut hat_right: u8 = 0; for (index, key) in matrix_keys.iter_mut().enumerate() { if (15..=19).contains(&index) && key.pressed { hat_left |= 1 << (index - 15); } if (20..=24).contains(&index) && key.pressed { hat_right |= 1 << (index - 20); } } // Convert hat switch data to HID code let (hat_l, hat_button_l) = format_hat_value(hat_left); let (hat_r, hat_button_r) = format_hat_value(hat_right); // Handle sec_button (Fn) for left hat switch let mut sec_button_pressed: bool = false; for (sec_index, sec_key) in matrix_keys.iter().enumerate() { if matrix_keys[15].usb_button_sec_enable && matrix_keys[15].usb_button_sec_trigger_index == sec_index && sec_key.pressed { sec_button_pressed = true; break; } } if matrix_keys[15].usb_changed { matrix_keys[15].usb_button_sec_pressed = sec_button_pressed; } if matrix_keys[15].usb_button_sec != 0 && matrix_keys[15].usb_button_sec_pressed { hat2 = hat_l; } else { hat4 = hat_l; } // Handle sec_button (Fn) for right hat switch let mut sec_button_pressed: bool = false; for (sec_index, sec_key) in matrix_keys.iter().enumerate() { if matrix_keys[20].usb_button_sec_enable && matrix_keys[20].usb_button_sec_trigger_index == sec_index && sec_key.pressed { sec_button_pressed = true; break; } } if matrix_keys[20].usb_changed { matrix_keys[20].usb_button_sec_pressed = sec_button_pressed; } if matrix_keys[20].usb_button_sec != 0 && matrix_keys[20].usb_button_sec_pressed { hat1 = hat_r; } else { hat3 = hat_r; } // Fix button state for center hat press on hat matrix_keys[15].pressed = hat_button_l != 0; matrix_keys[20].pressed = hat_button_r != 0; // Update button array with Sec button trigger status // Using indexing instead of iterating to be able to iterate inside loop for index in 0..NUMBER_OF_BUTTONS + 2 { let mut sec_button_pressed: bool = false; for (sec_index, sec_key) in matrix_keys.iter().enumerate() { if matrix_keys[index].usb_button_sec_enable && matrix_keys[index].usb_button_sec_trigger_index == sec_index && sec_key.pressed { sec_button_pressed = true; break; } } // Only update button when "root key" is pressed (ie. Do not change sec button as soon as Fn // key is pressed if matrix_keys[index].usb_changed { matrix_keys[index].usb_button_sec_pressed = sec_button_pressed; } } // Update button state for joystick buttons let mut buttons: u32 = 0; for key in matrix_keys.iter_mut() { // Toggle mode button if key.usb_changed && key.usb_button_toggle_enable { key.usb_release_timeout = RELEASE_RIMEOUT; } if key.pressed && key.usb_button != 0 && key.usb_button_toggle_enable && key.usb_release_timeout > 1 { 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 { buttons |= 1 << (key.usb_button_sec - 1); // Sec button mode } else if key.pressed && key.usb_button_sec != 0 && key.usb_button_sec_pressed { buttons |= 1 << (key.usb_button_sec - 1); } else if key.pressed && key.usb_button != 0 && !key.usb_button_toggle_enable { buttons |= 1 << (key.usb_button - 1); } } // Auto release button when in toggle mode for key in matrix_keys.iter_mut() { if key.usb_release_timeout > 0 { key.usb_release_timeout -= 1; } } // Reset changed flags for key in matrix_keys.iter_mut() { key.usb_changed = false; } JoystickReport { x, y, z, rx, ry, rz, hat1, hat2, hat3, hat4, buttons, } } /// Format hat value from 5 switches to USB HID coded value and button state /// /// # Arguments /// * `input` - Hat value coded as /// bit 2-5: direction (U L R D) /// bit 1: 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 & 0xFE { 2 => HAT_UP, 4 => HAT_RIGHT, 6 => HAT_UP_RIGHT, 8 => HAT_DOWN, 12 => HAT_DOWN_RIGHT, 16 => HAT_LEFT, 24 => HAT_DOWN_LEFT, 18 => HAT_UP_LEFT, _ => HAT_CENTER, }; // Alpine hat switch button filter let mut button_state: u8 = 0; if input & 0x01 == 0x01 && 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 enabled /// * `expo_lut` - Exponential curve lookup table fn calculate_axis_value( value: u16, min: u16, max: u16, center: u16, deadzone: (u16, u16, u16), expo: bool, expo_lut: &[u16; AXIS_MAX as usize + 1], ) -> u16 { if value <= min { return AXIS_MIN; } if value >= max { return AXIS_MAX; } 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 && calibrated_value != AXIS_CENTER { calibrated_value = expo_lut[calibrated_value as usize]; } calibrated_value } /// 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 { 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 /// /// # 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 } } /// Generate exponential lookup table for 12bit values /// /// # Arguments /// * `expo` - Exponential curve factor (range 0.0 - 1.0) fn generate_expo_lut(expo: f32) -> [u16; AXIS_MAX as usize + 1] { let mut lut: [u16; AXIS_MAX as usize + 1] = [0; AXIS_MAX as usize + 1]; for i in 0..AXIS_MAX + 1 { let value_float = i as f32 / AXIS_MAX as f32; // Calculate expo using 9th order polynomial function with 0.5 as center point let value_exp: f32 = expo * (0.5 + 256.0 * powf(value_float - 0.5, 9.0)) + (1.0 - expo) * value_float; lut[i as usize] = constrain((value_exp * AXIS_MAX as f32) as u16, AXIS_MIN, AXIS_MAX); } lut }