959 lines
34 KiB
Rust
959 lines
34 KiB
Rust
//! 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<Error = Infallible>; 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<Error = Infallible>; 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<BUTTON_ROWS, BUTTON_COLS, NUMBER_OF_BUTTONS> =
|
|
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<P, SM, I>(
|
|
status_led: &mut Ws2812StatusLed<P, SM, I>,
|
|
usb_active: &bool,
|
|
calibration_active: &bool,
|
|
throttle_hold_enable: &bool,
|
|
) where
|
|
P: PIOExt + FunctionConfig,
|
|
I: PinId,
|
|
Function<P>: ValidPinMode<I>,
|
|
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<T: PartialOrd>(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
|
|
}
|