cm/cmdr-joystick
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Reordered layout buttons. Added smoother functionality. Code cleanup

Browse Source
This commit is contained in:
Christoffer Martinsson 2023-08-02 20:26:30 +02:00
parent 9a87974c71
commit 3c073cd9fd
3 changed files with 407 additions and 325 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
rp2040/Cargo.toml
View File

@ -22,6 +22,7 @@ usb-device = "0.2"
pio = "0.2.0" pio = "0.2.0"
defmt = { version = "0.3", optional = true } defmt = { version = "0.3", optional = true }
libm = "0.2.7" libm = "0.2.7"
dyn-smooth = "0.2.0"
[features] [features]
# This is the set of features we enable by default # This is the set of features we enable by default

160
rp2040/src/layout.rs
View File

@ -24,31 +24,31 @@ pub enum ButtonType {
B14 = 13, B14 = 13,
B15 = 14, B15 = 14,
B16 = 15, B16 = 15,
B17 = 16, FnL = 16,
B18 = 17, FnR = 17,
B19 = 18, ModeL = 18,
B20 = 19, ModeR = 19,
FnL = 20, Hat1U = 20,
FnR = 21, Hat1R = 21,
ModeL = 22, Hat1D = 22,
ModeR = 23, Hat1L = 23,
Hat1U = 24, Hat1B = 24,
Hat1L = 25, Hat2U = 25,
Hat1R = 26, Hat2R = 26,
Hat1D = 27, Hat2D = 27,
Hat2U = 28, Hat2L = 28,
Hat2L = 29, Hat2B = 29,
Hat2R = 30, Hat3U = 30,
Hat2D = 31, Hat3R = 31,
Hat3U = 32, Hat3D = 32,
Hat3L = 33, Hat3L = 33,
Hat3R = 34, Hat3B = 34,
Hat3D = 35, Hat4U = 35,
Hat4U = 36, Hat4R = 36,
Hat4L = 37, Hat4D = 37,
Hat4R = 38, Hat4L = 38,
Hat4D = 39, Hat4B = 39,
NotConnected = 40, NoEventIndicated = 40,
} }
// Button index map: // Button index map:
@ -72,25 +72,25 @@ pub const MAP: [[ButtonType; NUMBER_OF_KEYS]; 4] = [
// ----------------------------------------- // -----------------------------------------
ButtonType::FnL, // 0 ButtonType::FnL, // 0
ButtonType::B1, // 1 ButtonType::B1, // 1
ButtonType::B6, // 2 ButtonType::B5, // 2
ButtonType::FnR, // 3 ButtonType::FnR, // 3
ButtonType::NotConnected, // 4 ButtonType::NoEventIndicated, // 4
ButtonType::B2, // 5 ButtonType::B2, // 5
ButtonType::B3, // 6 ButtonType::B3, // 6
ButtonType::ModeL, // 7 ButtonType::ModeL, // 7
ButtonType::B4, // 8 ButtonType::B4, // 8
ButtonType::NotConnected, // 9 ButtonType::NoEventIndicated, // 9
ButtonType::B7, // 10 ButtonType::B6, // 10
ButtonType::B8, // 11 ButtonType::B7, // 11
ButtonType::ModeR, // 12 ButtonType::ModeR, // 12
ButtonType::B9, // 13 ButtonType::B8, // 13
ButtonType::NotConnected, // 14 ButtonType::NoEventIndicated, // 14
ButtonType::B5, // 15 ButtonType::Hat1B, // 15
ButtonType::Hat1U, // 16 ButtonType::Hat1U, // 16
ButtonType::Hat1L, // 17 ButtonType::Hat1L, // 17
ButtonType::Hat1R, // 18 ButtonType::Hat1R, // 18
ButtonType::Hat1D, // 19 ButtonType::Hat1D, // 19
ButtonType::B10, // 20 ButtonType::Hat2B, // 20
ButtonType::Hat2U, // 21 ButtonType::Hat2U, // 21
ButtonType::Hat2L, // 22 ButtonType::Hat2L, // 22
ButtonType::Hat2R, // 23 ButtonType::Hat2R, // 23
@ -101,26 +101,26 @@ pub const MAP: [[ButtonType; NUMBER_OF_KEYS]; 4] = [
// HID Key // Button Index // HID Key // Button Index
// ----------------------------------------- // -----------------------------------------
ButtonType::FnL, // 0 ButtonType::FnL, // 0
ButtonType::B11, // 1 ButtonType::B9, // 1
ButtonType::B6, // 2 ButtonType::B5, // 2
ButtonType::FnR, // 3 ButtonType::FnR, // 3
ButtonType::NotConnected, // 4 ButtonType::NoEventIndicated, // 4
ButtonType::B12, // 5 ButtonType::B10, // 5
ButtonType::B13, // 6 ButtonType::B11, // 6
ButtonType::ModeL, // 7 ButtonType::ModeL, // 7
ButtonType::B14, // 8 ButtonType::B12, // 8
ButtonType::NotConnected, // 9 ButtonType::NoEventIndicated, // 9
ButtonType::B7, // 10 ButtonType::B6, // 10
ButtonType::B8, // 11 ButtonType::B7, // 11
ButtonType::ModeR, // 12 ButtonType::ModeR, // 12
ButtonType::B9, // 13 ButtonType::B8, // 13
ButtonType::NotConnected, // 14 ButtonType::NoEventIndicated, // 14
ButtonType::B15, // 15 ButtonType::Hat3B, // 15
ButtonType::Hat1U, // 16 ButtonType::Hat3U, // 16
ButtonType::Hat1L, // 17 ButtonType::Hat3L, // 17
ButtonType::Hat1R, // 18 ButtonType::Hat3R, // 18
ButtonType::Hat1D, // 19 ButtonType::Hat3D, // 19
ButtonType::B10, // 20 ButtonType::Hat2B, // 20
ButtonType::Hat2U, // 21 ButtonType::Hat2U, // 21
ButtonType::Hat2L, // 22 ButtonType::Hat2L, // 22
ButtonType::Hat2R, // 23 ButtonType::Hat2R, // 23
@ -132,55 +132,55 @@ pub const MAP: [[ButtonType; NUMBER_OF_KEYS]; 4] = [
// ----------------------------------------- // -----------------------------------------
ButtonType::FnL, // 0 ButtonType::FnL, // 0
ButtonType::B1, // 1 ButtonType::B1, // 1
ButtonType::B16, // 2 ButtonType::B13, // 2
ButtonType::FnR, // 3 ButtonType::FnR, // 3
ButtonType::NotConnected, // 4 ButtonType::NoEventIndicated, // 4
ButtonType::B2, // 5 ButtonType::B2, // 5
ButtonType::B3, // 6 ButtonType::B3, // 6
ButtonType::ModeL, // 7 ButtonType::ModeL, // 7
ButtonType::B4, // 8 ButtonType::B4, // 8
ButtonType::NotConnected, // 9 ButtonType::NoEventIndicated, // 9
ButtonType::B17, // 10 ButtonType::B14, // 10
ButtonType::B18, // 11 ButtonType::B15, // 11
ButtonType::ModeR, // 12 ButtonType::ModeR, // 12
ButtonType::B19, // 13 ButtonType::B16, // 13
ButtonType::NotConnected, // 14 ButtonType::NoEventIndicated, // 14
ButtonType::B5, // 15 ButtonType::Hat1B, // 15
ButtonType::Hat3U, // 16 ButtonType::Hat1U, // 16
ButtonType::Hat3L, // 17 ButtonType::Hat1L, // 17
ButtonType::Hat3R, // 18 ButtonType::Hat1R, // 18
ButtonType::Hat3D, // 19 ButtonType::Hat1D, // 19
ButtonType::B20, // 20 ButtonType::Hat4B, // 20
ButtonType::Hat2U, // 21 ButtonType::Hat4U, // 21
ButtonType::Hat2L, // 22 ButtonType::Hat4L, // 22
ButtonType::Hat2R, // 23 ButtonType::Hat4R, // 23
ButtonType::Hat2D, // 24 ButtonType::Hat4D, // 24
], ],
[ [
// Function layer left + right // Function layer left + right
// HID Key // Button Index // HID Key // Button Index
// ----------------------------------------- // -----------------------------------------
ButtonType::FnL, // 0 ButtonType::FnL, // 0
ButtonType::B11, // 1 ButtonType::B9, // 1
ButtonType::B16, // 2 ButtonType::B13, // 2
ButtonType::FnR, // 3 ButtonType::FnR, // 3
ButtonType::NotConnected, // 4 ButtonType::NoEventIndicated, // 4
ButtonType::B12, // 5 ButtonType::B10, // 5
ButtonType::B13, // 6 ButtonType::B11, // 6
ButtonType::ModeL, // 7 ButtonType::ModeL, // 7
ButtonType::B14, // 8 ButtonType::B12, // 8
ButtonType::NotConnected, // 9 ButtonType::NoEventIndicated, // 9
ButtonType::B17, // 10 ButtonType::B14, // 10
ButtonType::B18, // 11 ButtonType::B15, // 11
ButtonType::ModeR, // 12 ButtonType::ModeR, // 12
ButtonType::B19, // 13 ButtonType::B16, // 13
ButtonType::NotConnected, // 14 ButtonType::NoEventIndicated, // 14
ButtonType::B15, // 15 ButtonType::Hat3B, // 15
ButtonType::Hat3U, // 16 ButtonType::Hat3U, // 16
ButtonType::Hat3L, // 17 ButtonType::Hat3L, // 17
ButtonType::Hat3R, // 18 ButtonType::Hat3R, // 18
ButtonType::Hat3D, // 19 ButtonType::Hat3D, // 19
ButtonType::B20, // 20 ButtonType::Hat4B, // 20
ButtonType::Hat4U, // 21 ButtonType::Hat4U, // 21
ButtonType::Hat4L, // 22 ButtonType::Hat4L, // 22
ButtonType::Hat4R, // 23 ButtonType::Hat4R, // 23

479
rp2040/src/main.rs
View File

@ -15,6 +15,7 @@ mod usb_joystick_device;
use button_matrix::ButtonMatrix; use button_matrix::ButtonMatrix;
use core::convert::Infallible; use core::convert::Infallible;
use cortex_m::delay::Delay; use cortex_m::delay::Delay;
use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS};
use embedded_hal::adc::OneShot; use embedded_hal::adc::OneShot;
use embedded_hal::digital::v2::*; use embedded_hal::digital::v2::*;
use embedded_hal::timer::CountDown; use embedded_hal::timer::CountDown;
@ -49,15 +50,20 @@ pub const KEY_ROWS: usize = 5;
pub const KEY_COLS: usize = 5; pub const KEY_COLS: usize = 5;
pub const NUMBER_OF_KEYS: usize = KEY_ROWS * KEY_COLS; pub const NUMBER_OF_KEYS: usize = KEY_ROWS * KEY_COLS;
const HID_AXIS_MIN: u16 = 0; pub const AXIS_MIN: u16 = 0;
const HID_AXIS_MAX: u16 = 4095; pub const AXIS_MAX: u16 = 4095;
const HID_AXIS_CENTER: u16 = HID_AXIS_MAX / 2; pub const AXIS_CENTER: u16 = AXIS_MAX / 2;
const HAT_CENTER: u8 = 0xf; pub const NBR_OF_GIMBAL_AXIS: usize = 4;
const HAT_UP: u8 = 0; pub const GIMBAL_AXIS_LEFT_X: usize = 0;
const HAT_RIGHT: u8 = 2; pub const GIMBAL_AXIS_LEFT_Y: usize = 1;
const HAT_DOWN: u8 = 4; pub const GIMBAL_AXIS_RIGHT_X: usize = 2;
const HAT_LEFT: u8 = 6; 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 // Public types
#[derive(Copy, Clone, Default)] #[derive(Copy, Clone, Default)]
@ -67,14 +73,31 @@ pub struct JoystickButton {
pub fn_mode: u8, pub fn_mode: u8,
} }
#[derive(Copy, Clone, Default)] #[derive(Copy, Clone)]
pub struct JoystickAxis { pub struct GimbalAxis {
pub value: u16, pub value: u16,
pub previous_value: u16, pub idle_value: u16,
pub max: u16, pub max: u16,
pub min: u16, pub min: u16,
pub center: u16, pub center: u16,
pub fn_mode: u8, 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] #[entry]
@ -115,10 +138,10 @@ fn main() -> ! {
let mut adc = Adc::new(pac.ADC, &mut pac.RESETS); let mut adc = Adc::new(pac.ADC, &mut pac.RESETS);
// Configure ADC input pins // Configure ADC input pins
let mut x1_pin = pins.gp26.into_floating_input(); let mut adc_pin_left_x = pins.gp26.into_floating_input();
let mut y1_pin = pins.gp27.into_floating_input(); let mut adc_pin_left_y = pins.gp27.into_floating_input();
let mut x2_pin = pins.gp28.into_floating_input(); let mut adc_pin_right_x = pins.gp28.into_floating_input();
let mut y2_pin = pins.gp29.into_floating_input(); let mut adc_pin_right_y = pins.gp29.into_floating_input();
// Setting up array with pins connected to button rows // Setting up array with pins connected to button rows
let button_matrix_row_pins: &[&dyn InputPin<Error = Infallible>; KEY_ROWS] = &[ let button_matrix_row_pins: &[&dyn InputPin<Error = Infallible>; KEY_ROWS] = &[
@ -151,7 +174,7 @@ fn main() -> ! {
&mut pac.RESETS, &mut pac.RESETS,
)); ));
let mut joystick = UsbHidClassBuilder::new() let mut usb_hid_joystick = UsbHidClassBuilder::new()
.add_device(JoystickConfig::default()) .add_device(JoystickConfig::default())
.build(&usb_bus); .build(&usb_bus);
@ -186,13 +209,24 @@ fn main() -> ! {
let mut status_led_count_down = timer.count_down(); let mut status_led_count_down = timer.count_down();
status_led_count_down.start(250.millis()); status_led_count_down.start(250.millis());
// Create variables to track all modes // Create variable to track modes
let mut fn_mode: u8; let mut fn_mode: u8 = 0;
let mut alt_l_mode: bool;
let mut alt_r_mode: bool;
// Create joystick axis array // 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 // Initialize button matrix
button_matrix.init_pins(); button_matrix.init_pins();
@ -211,25 +245,23 @@ fn main() -> ! {
} }
loop { loop {
// if status_led_count_down.wait().is_ok() { if status_led_count_down.wait().is_ok() {
// update_status_led(&mut status_led, &caps_lock_active, &gui_lock_state); update_status_led(&mut status_led, &fn_mode);
// } }
if usb_hid_report_count_down.wait().is_ok() { if usb_hid_report_count_down.wait().is_ok() {
let pressed_keys = button_matrix.buttons_pressed(); 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() { for (index, key) in pressed_keys.iter().enumerate() {
buttons[index].pressed = *key; buttons[index].pressed = *key;
} }
match joystick.device().write_report(&get_joystick_report( match usb_hid_joystick.device().write_report(&get_joystick_report(
&mut buttons, &mut buttons,
axis, &mut axis,
fn_mode, fn_mode,
alt_l_mode,
alt_r_mode,
)) { )) {
Err(UsbHidError::WouldBlock) => {} Err(UsbHidError::WouldBlock) => {}
Ok(_) => {} Ok(_) => {}
@ -243,41 +275,26 @@ fn main() -> ! {
if usb_tick_count_down.wait().is_ok() { if usb_tick_count_down.wait().is_ok() {
button_matrix.scan_matrix(&mut delay); button_matrix.scan_matrix(&mut delay);
axis[0].value = apply_calibration( // Read ADC values and smooth them
adc.read(&mut x1_pin).unwrap(), smoother[GIMBAL_AXIS_LEFT_X].tick(adc.read(&mut adc_pin_left_x).unwrap());
HID_AXIS_MIN, smoother[GIMBAL_AXIS_LEFT_Y].tick(adc.read(&mut adc_pin_left_y).unwrap());
HID_AXIS_MAX, smoother[GIMBAL_AXIS_RIGHT_X].tick(adc.read(&mut adc_pin_right_x).unwrap());
HID_AXIS_CENTER, smoother[GIMBAL_AXIS_RIGHT_Y].tick(adc.read(&mut adc_pin_right_y).unwrap());
50,
0.2, // Update axis values
); for (index, item) in axis.iter_mut().enumerate() {
axis[1].value = apply_calibration( item.value = calculate_axis_value(
adc.read(&mut y1_pin).unwrap(), smoother[index].value() as u16,
HID_AXIS_MIN, item.min,
HID_AXIS_MAX, item.max,
HID_AXIS_CENTER, item.center,
50, item.deadzone,
0.2, item.expo,
);
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]) {} if usb_dev.poll(&mut [&mut usb_hid_joystick]) {}
} }
} }
@ -291,19 +308,14 @@ fn main() -> ! {
/// # Arguments /// # Arguments
/// * `status_led` - Reference to status LED /// * `status_led` - Reference to status LED
/// * `caps_lock_active` - Is capslock active /// * `caps_lock_active` - Is capslock active
fn update_status_led<P, SM, I>( fn update_status_led<P, SM, I>(status_led: &mut Ws2812StatusLed<P, SM, I>, fn_mode: &u8)
status_led: &mut Ws2812StatusLed<P, SM, I>, where
caps_lock_active: &bool,
gui_lock_state: &u8,
) where
P: PIOExt + FunctionConfig, P: PIOExt + FunctionConfig,
I: PinId, I: PinId,
Function<P>: ValidPinMode<I>, Function<P>: ValidPinMode<I>,
SM: StateMachineIndex, SM: StateMachineIndex,
{ {
if *caps_lock_active { if *fn_mode & 0x10 == 0x10 {
status_led.update(StatusMode::Warning);
} else if *gui_lock_state != 0 {
status_led.update(StatusMode::Activity); status_led.update(StatusMode::Activity);
} else { } else {
status_led.update(StatusMode::Normal); status_led.update(StatusMode::Normal);
@ -316,13 +328,13 @@ fn update_status_led<P, SM, I>(
/// # Arguments /// # Arguments
/// ///
/// * `pressed_keys` - Array of pressed keys /// * `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 // Check how many Fn keys are pressed
let mut fn_mode: u8 = 0; let mut fn_mode: u8 = 0;
let mut fn_l_active: bool = false; let mut fn_l_active: bool = false;
let mut fn_r_active: bool = false; let mut fn_r_active: bool = false;
let mut alt_l_mode: bool = false; let mut alt_l_active: bool = false;
let mut alt_r_mode: bool = false; let mut alt_r_active: bool = false;
for (index, key) in pressed_keys.iter().enumerate() { for (index, key) in pressed_keys.iter().enumerate() {
if *key && layout::MAP[0][index] == layout::ButtonType::FnL { 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; fn_r_active = true;
} }
if *key && layout::MAP[0][index] == layout::ButtonType::ModeL { 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 { 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 = 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) /// 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 /// * `alt_r_mode` - Is right alt mode active
fn get_joystick_report( fn get_joystick_report(
matrix_keys: &mut [JoystickButton; NUMBER_OF_KEYS], matrix_keys: &mut [JoystickButton; NUMBER_OF_KEYS],
axis: [JoystickAxis; 4], axis: &mut [GimbalAxis; 4],
fn_mode: u8, fn_mode: u8,
alt_l_mode: bool,
alt_r_mode: bool,
) -> JoystickReport { ) -> JoystickReport {
let mut x: u16 = axis[2].value; // right gimbal normal mode let mut x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value;
let mut y: u16 = axis[3].value; // right gimbal normal mode let mut y: u16 = axis[GIMBAL_AXIS_RIGHT_Y].value;
let z: u16 = axis[0].value; // left gimbal normal mode let z: u16 = axis[GIMBAL_AXIS_LEFT_X].value;
let mut rx: u16 = HID_AXIS_CENTER; // right gimbal alt mode let mut rx: u16 = AXIS_CENTER;
let mut ry: u16 = HID_AXIS_CENTER; // right gimbal alt mode let mut ry: u16 = AXIS_CENTER;
let mut rz: u16 = axis[1].value; // left gimbal normal and alt mode let mut rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value;
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;
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( rz = remap(
axis[1].value, axis[GIMBAL_AXIS_LEFT_Y].value,
HID_AXIS_MIN, AXIS_MIN,
HID_AXIS_MAX, AXIS_MAX,
HID_AXIS_CENTER, AXIS_CENTER,
HID_AXIS_MAX, 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( rz = remap(
axis[1].value, axis[GIMBAL_AXIS_LEFT_Y].value,
HID_AXIS_MIN, AXIS_MIN,
HID_AXIS_MAX, AXIS_MAX,
HID_AXIS_CENTER, AXIS_CENTER,
HID_AXIS_MIN, AXIS_MIN,
); );
} }
if alt_r_mode && (axis[2].fn_mode == 2 || axis[2].fn_mode == 3) { // Right Alt mode active
x = HID_AXIS_CENTER; // Right gimbal control third joystick axis when right Fn mode is active
rx = axis[2].value; 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) { // Set fn mode for pressed button
y = HID_AXIS_CENTER; for key in matrix_keys.iter_mut() {
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
if key.pressed != key.previous_pressed && key.pressed { if key.pressed != key.previous_pressed && key.pressed {
key.fn_mode = fn_mode; key.fn_mode = fn_mode & 0x0F;
} }
key.previous_pressed = key.pressed; 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 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; 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 { 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 /// Remapping values from one range to another
/// ///
/// # Arguments /// # Arguments
@ -477,7 +606,13 @@ fn get_joystick_report(
/// * `out_min` - Lower bound of the value's target range /// * `out_min` - Lower bound of the value's target range
/// * `out_max` - Upper 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 { 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 /// Constrain a value to a given range
@ -495,57 +630,3 @@ fn constrain<T: PartialOrd>(value: T, out_min: T, out_max: T) -> T {
value 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
}