Reordered layout buttons. Added smoother functionality. Code cleanup
This commit is contained in:
parent
9a87974c71
commit
3c073cd9fd
@ -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
|
||||
|
||||
@ -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
|
||||
],
|
||||
];
|
||||
|
||||
@ -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<Error = Infallible>; 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<P, SM, I>(
|
||||
status_led: &mut Ws2812StatusLed<P, SM, I>,
|
||||
caps_lock_active: &bool,
|
||||
gui_lock_state: &u8,
|
||||
) where
|
||||
fn update_status_led<P, SM, I>(status_led: &mut Ws2812StatusLed<P, SM, I>, fn_mode: &u8)
|
||||
where
|
||||
P: PIOExt + FunctionConfig,
|
||||
I: PinId,
|
||||
Function<P>: ValidPinMode<I>,
|
||||
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<P, SM, I>(
|
||||
/// # 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<T: PartialOrd>(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
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user