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

Started to add ELRS support. Code cleanup

Browse Source
This commit is contained in:
Christoffer Martinsson 2023-08-09 18:07:32 +02:00
parent 2bb2c8c010
commit b907f7d791
2 changed files with 365 additions and 222 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

272
rp2040/src/layout.rs
View File

@ -6,8 +6,41 @@
use crate::NUMBER_OF_BUTTONS; use crate::NUMBER_OF_BUTTONS;
#[derive(Debug, PartialEq, Copy, Clone)] #[allow(dead_code)]
pub enum ButtonType { #[derive(Debug, PartialEq, PartialOrd, Copy, Clone)]
pub enum ElrsButton {
CH1 = 0,
CH2 = 1,
CH3 = 2,
CH4 = 3,
CH5 = 4,
CH6 = 5,
CH7 = 6,
CH8 = 7,
CH9 = 8,
CH10 = 9,
CH11 = 10,
CH12 = 11,
CH5ON = 12,
CH5OFF = 13,
CH6ON = 14,
CH6OFF = 15,
CH7ON = 16,
CH7OFF = 17,
CH8ON = 18,
CH8OFF = 19,
CH9ON = 20,
CH9OFF = 21,
CH10ON = 22,
CH10OFF = 23,
CH11ON = 24,
CH11OFF = 25,
CH12ON = 26,
CH12OFF = 27,
NoEventIndicated = 28,
}
#[derive(Debug, PartialEq, PartialOrd, Copy, Clone)]
pub enum HidButton {
B1 = 0, B1 = 0,
B2 = 1, B2 = 1,
B3 = 2, B3 = 2,
@ -54,7 +87,7 @@ pub enum ButtonType {
Hat4B = 43, Hat4B = 43,
NoEventIndicated = 44, NoEventIndicated = 44,
} }
#[warn(dead_code)]
// Button index map: // Button index map:
// -------------------------------------------------------------- // --------------------------------------------------------------
// | 0 | 1 | | 3 | 4 | (2) // | 0 | 1 | | 3 | 4 | (2)
@ -73,125 +106,156 @@ pub enum ButtonType {
/// Please make sure to set FnL, FnR, ModeL and ModeR at the same position for all layers /// Please make sure to set FnL, FnR, ModeL and ModeR at the same position for all layers
/// alt. only set these at function layer 0 and set NoEventIndicated in layer 1-3. /// alt. only set these at function layer 0 and set NoEventIndicated in layer 1-3.
/// Hat button 1-4 = HID B21-B24. /// Hat button 1-4 = HID B21-B24.
pub const MAP: [[ButtonType; NUMBER_OF_BUTTONS]; 4] = [ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [
[ [
// Function layer 0 // Function layer 0
// HID Key // Button Index // HID Key // Button Index
// ----------------------------------------- // -----------------------------------------
ButtonType::FnL, // 0 HidButton::FnL, // 0
ButtonType::B1, // 1 HidButton::B1, // 1
ButtonType::NoEventIndicated, // 2 Not connected to any button HidButton::NoEventIndicated, // 2 Not connected to any button
ButtonType::FnR, // 3 HidButton::FnR, // 3
ButtonType::B6, // 4 HidButton::B6, // 4
ButtonType::B2, // 5 HidButton::B2, // 5
ButtonType::B3, // 6 HidButton::B3, // 6
ButtonType::ModeL, // 7 HidButton::ModeL, // 7
ButtonType::B4, // 8 HidButton::B4, // 8
ButtonType::B5, // 9 HidButton::B5, // 9
ButtonType::B7, // 10 HidButton::B7, // 10
ButtonType::B8, // 11 HidButton::B8, // 11
ButtonType::ModeR, // 12 HidButton::ModeR, // 12
ButtonType::B9, // 13 HidButton::B9, // 13
ButtonType::B10, // 14 HidButton::B10, // 14
ButtonType::Hat1B, // 15 button 21 HidButton::Hat1B, // 15 button 21
ButtonType::Hat1U, // 16 HidButton::Hat1U, // 16
ButtonType::Hat1R, // 17 HidButton::Hat1R, // 17
ButtonType::Hat1D, // 18 HidButton::Hat1D, // 18
ButtonType::Hat1L, // 19 HidButton::Hat1L, // 19
ButtonType::Hat2B, // 20 button 22 HidButton::Hat2B, // 20 button 22
ButtonType::Hat2U, // 21 HidButton::Hat2U, // 21
ButtonType::Hat2R, // 22 HidButton::Hat2R, // 22
ButtonType::Hat2D, // 23 HidButton::Hat2D, // 23
ButtonType::Hat2L, // 24 HidButton::Hat2L, // 24
], ],
[ [
// Function layer 1 (left Fn button pressed) // Function layer 1 (left Fn button pressed)
// HID Key // Button Index // HID Key // Button Index
// ----------------------------------------- // -----------------------------------------
ButtonType::FnL, // 0 HidButton::FnL, // 0
ButtonType::B11, // 1 HidButton::B11, // 1
ButtonType::NoEventIndicated, // 2 Not connected to any button HidButton::NoEventIndicated, // 2 Not connected to any button
ButtonType::FnR, // 3 HidButton::FnR, // 3
ButtonType::B6, // 4 HidButton::B6, // 4
ButtonType::B12, // 5 HidButton::B12, // 5
ButtonType::B13, // 6 HidButton::B13, // 6
ButtonType::ModeL, // 7 HidButton::ModeL, // 7
ButtonType::B14, // 8 HidButton::B14, // 8
ButtonType::B15, // 9 HidButton::B15, // 9
ButtonType::B7, // 10 HidButton::B7, // 10
ButtonType::B8, // 11 HidButton::B8, // 11
ButtonType::ModeR, // 12 HidButton::ModeR, // 12
ButtonType::B9, // 13 HidButton::B9, // 13
ButtonType::B10, // 14 HidButton::B10, // 14
ButtonType::Hat3B, // 15 button 23 HidButton::Hat3B, // 15 button 23
ButtonType::Hat3U, // 16 HidButton::Hat3U, // 16
ButtonType::Hat3R, // 17 HidButton::Hat3R, // 17
ButtonType::Hat3D, // 18 HidButton::Hat3D, // 18
ButtonType::Hat3L, // 19 HidButton::Hat3L, // 19
ButtonType::Hat2B, // 20 button 22 HidButton::Hat2B, // 20 button 22
ButtonType::Hat2U, // 21 HidButton::Hat2U, // 21
ButtonType::Hat2R, // 22 HidButton::Hat2R, // 22
ButtonType::Hat2D, // 23 HidButton::Hat2D, // 23
ButtonType::Hat2L, // 24 HidButton::Hat2L, // 24
], ],
[ [
// Function layer 2 (right Fn button pressed) // Function layer 2 (right Fn button pressed)
// HID Key // Button Index // HID Key // Button Index
// ----------------------------------------- // -----------------------------------------
ButtonType::FnL, // 0 HidButton::FnL, // 0
ButtonType::B1, // 1 HidButton::B1, // 1
ButtonType::NoEventIndicated, // 2 Not connected to any button HidButton::NoEventIndicated, // 2 Not connected to any button
ButtonType::FnR, // 3 HidButton::FnR, // 3
ButtonType::B16, // 4 HidButton::B16, // 4
ButtonType::B2, // 5 HidButton::B2, // 5
ButtonType::B3, // 6 HidButton::B3, // 6
ButtonType::ModeL, // 7 HidButton::ModeL, // 7
ButtonType::B4, // 8 HidButton::B4, // 8
ButtonType::B5, // 9 HidButton::B5, // 9
ButtonType::B17, // 10 HidButton::B17, // 10
ButtonType::B18, // 11 HidButton::B18, // 11
ButtonType::ModeR, // 12 HidButton::ModeR, // 12
ButtonType::B19, // 13 HidButton::B19, // 13
ButtonType::B20, // 14 HidButton::B20, // 14
ButtonType::Hat1B, // 15 button 21 HidButton::Hat1B, // 15 button 21
ButtonType::Hat1U, // 16 HidButton::Hat1U, // 16
ButtonType::Hat1R, // 17 HidButton::Hat1R, // 17
ButtonType::Hat1D, // 18 HidButton::Hat1D, // 18
ButtonType::Hat1L, // 19 HidButton::Hat1L, // 19
ButtonType::Hat4B, // 20 button 24 HidButton::Hat4B, // 20 button 24
ButtonType::Hat4U, // 21 HidButton::Hat4U, // 21
ButtonType::Hat4R, // 22 HidButton::Hat4R, // 22
ButtonType::Hat4D, // 23 HidButton::Hat4D, // 23
ButtonType::Hat4L, // 24 HidButton::Hat4L, // 24
], ],
[ [
// Function layer 3 (left + right Fn button pressed) // Function layer 3 (left + right Fn button pressed)
// HID Key // Button Index // HID Key // Button Index
// ----------------------------------------- // -----------------------------------------
ButtonType::FnL, // 0 HidButton::FnL, // 0
ButtonType::B11, // 1 HidButton::B11, // 1
ButtonType::NoEventIndicated, // 2 Not connected to any button HidButton::NoEventIndicated, // 2 Not connected to any button
ButtonType::FnR, // 3 HidButton::FnR, // 3
ButtonType::B16, // 4 HidButton::B16, // 4
ButtonType::B12, // 5 HidButton::B12, // 5
ButtonType::B13, // 6 HidButton::B13, // 6
ButtonType::ModeL, // 7 HidButton::ModeL, // 7
ButtonType::B14, // 8 HidButton::B14, // 8
ButtonType::B15, // 9 HidButton::B15, // 9
ButtonType::B17, // 10 HidButton::B17, // 10
ButtonType::B18, // 11 HidButton::B18, // 11
ButtonType::ModeR, // 12 HidButton::ModeR, // 12
ButtonType::B19, // 13 HidButton::B19, // 13
ButtonType::B20, // 14 HidButton::B20, // 14
ButtonType::Hat3B, // 15 button 23 HidButton::Hat3B, // 15 button 23
ButtonType::Hat3U, // 16 HidButton::Hat3U, // 16
ButtonType::Hat3R, // 17 HidButton::Hat3R, // 17
ButtonType::Hat3D, // 18 HidButton::Hat3D, // 18
ButtonType::Hat3L, // 19 HidButton::Hat3L, // 19
ButtonType::Hat4B, // 20 button 24 HidButton::Hat4B, // 20 button 24
ButtonType::Hat4U, // 21 HidButton::Hat4U, // 21
ButtonType::Hat4R, // 22 HidButton::Hat4R, // 22
ButtonType::Hat4D, // 23 HidButton::Hat4D, // 23
ButtonType::Hat4L, // 24 HidButton::Hat4L, // 24
], ],
]; ];
pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [
// Function layer 0
// HID Key // Button Index
// -----------------------------------------
ElrsButton::CH7OFF, // 0
ElrsButton::CH7ON, // 1
ElrsButton::NoEventIndicated, // 2 Not connected to any button
ElrsButton::CH8OFF, // 3
ElrsButton::CH8ON, // 4
ElrsButton::CH9ON, // 5
ElrsButton::CH9OFF, // 6
ElrsButton::CH5, // 7
ElrsButton::CH10ON, // 8
ElrsButton::CH10OFF, // 9
ElrsButton::CH11ON, // 10
ElrsButton::CH11OFF, // 11
ElrsButton::CH6, // 12
ElrsButton::CH12ON, // 13
ElrsButton::CH12OFF, // 14
ElrsButton::NoEventIndicated, // 15
ElrsButton::NoEventIndicated, // 16
ElrsButton::NoEventIndicated, // 17
ElrsButton::NoEventIndicated, // 18
ElrsButton::NoEventIndicated, // 19
ElrsButton::NoEventIndicated, // 20
ElrsButton::NoEventIndicated, // 21
ElrsButton::NoEventIndicated, // 22
ElrsButton::NoEventIndicated, // 23
ElrsButton::NoEventIndicated, // 24
];

315
rp2040/src/main.rs
View File

@ -69,12 +69,14 @@ pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;
#[derive(Copy, Clone, Default)] #[derive(Copy, Clone, Default)]
pub struct Button { pub struct Button {
pub pressed: bool, pub pressed: bool,
pub previous_pressed: bool,
pub fn_mode: u8, pub fn_mode: u8,
} }
#[derive(Copy, Clone)] #[derive(Copy, Clone)]
pub struct GimbalAxis { pub struct GimbalAxis {
pub value: u16, pub value: u16,
pub previous_value: u16,
pub idle_value: u16, pub idle_value: u16,
pub max: u16, pub max: u16,
pub min: u16, pub min: u16,
@ -88,6 +90,7 @@ impl Default for GimbalAxis {
fn default() -> Self { fn default() -> Self {
GimbalAxis { GimbalAxis {
value: AXIS_CENTER, value: AXIS_CENTER,
previous_value: AXIS_CENTER,
idle_value: AXIS_CENTER, idle_value: AXIS_CENTER,
max: AXIS_MAX, max: AXIS_MAX,
min: AXIS_MIN, min: AXIS_MIN,
@ -169,25 +172,6 @@ fn main() -> ! {
// Initialize button matrix // Initialize button matrix
button_matrix.init_pins(); button_matrix.init_pins();
// 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")
.serial_number("0001")
.build();
// Create status LED // Create status LED
let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS); let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS);
let mut status_led = Ws2812StatusLed::new( let mut status_led = Ws2812StatusLed::new(
@ -197,9 +181,21 @@ fn main() -> ! {
clocks.peripheral_clock.freq(), clocks.peripheral_clock.freq(),
); );
// Create timers/delays // Scan matrix to get initial state and check if bootloader should be entered
let timer = Timer::new(pac.TIMER, &mut pac.RESETS); // This is done by holding button 0 pressed while power on the unit
let mut delay = Delay::new(core.SYST, clocks.system_clock.freq().to_Hz()); let mut delay = Delay::new(core.SYST, clocks.system_clock.freq().to_Hz());
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 usb_hid_report_count_down = timer.count_down(); let mut usb_hid_report_count_down = timer.count_down();
usb_hid_report_count_down.start(10.millis()); usb_hid_report_count_down.start(10.millis());
@ -208,12 +204,16 @@ fn main() -> ! {
scan_count_down.start(1.millis()); scan_count_down.start(1.millis());
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(50.millis());
let mut elrs_count_down = timer.count_down();
elrs_count_down.start(1660u32.micros());
// Create variable to track modes
let mut mode: u8 = 0; let mut mode: u8 = 0;
let mut safety_check: bool = false;
// Create joystick button/axis array let mut activity: bool = false;
let mut idle: bool = false;
let mut usb_active: bool = false;
let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS]; let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS];
let mut buttons: [Button; NUMBER_OF_BUTTONS] = [Button::default(); NUMBER_OF_BUTTONS]; let mut buttons: [Button; NUMBER_OF_BUTTONS] = [Button::default(); NUMBER_OF_BUTTONS];
@ -246,62 +246,41 @@ fn main() -> ! {
DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
]; ];
// Scan matrix to get initial state // Configure USB
for _ in 0..10 { let usb_bus = UsbBusAllocator::new(waveshare_rp2040_zero::hal::usb::UsbBus::new(
button_matrix.scan_matrix(&mut delay); pac.USBCTRL_REGS,
} pac.USBCTRL_DPRAM,
clocks.usb_clock,
true,
&mut pac.RESETS,
));
// Fallback way to enter bootloader let mut usb_hid_joystick = UsbHidClassBuilder::new()
if button_matrix.buttons_pressed()[0] { .add_device(JoystickConfig::default())
status_led.update(StatusMode::Bootloader); .build(&usb_bus);
let gpio_activity_pin_mask: u32 = 0;
let disable_interface_mask: u32 = 0; let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x1209, 0x0002))
rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask); .manufacturer("CMtec")
} .product("CMDR Joystick")
.serial_number("0001")
.build();
loop { loop {
if status_led_count_down.wait().is_ok() { // Temporary way to enter bootloader -------------------------
update_status_led(&mut status_led, &mode); // TODO: Remove this after testing
} if button_matrix.buttons_pressed()[0]
&& button_matrix.buttons_pressed()[1]
if usb_hid_report_count_down.wait().is_ok() { && button_matrix.buttons_pressed()[5]
let pressed_keys = button_matrix.buttons_pressed(); && button_matrix.buttons_pressed()[6]
&& button_matrix.buttons_pressed()[8]
mode = get_mode(pressed_keys); && button_matrix.buttons_pressed()[9]
{
for (index, key) in pressed_keys.iter().enumerate() { status_led.update(StatusMode::Bootloader);
buttons[index].pressed = *key; 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);
if button_matrix.buttons_pressed()[0]
&& button_matrix.buttons_pressed()[1]
&& button_matrix.buttons_pressed()[5]
&& button_matrix.buttons_pressed()[6]
&& button_matrix.buttons_pressed()[8]
&& button_matrix.buttons_pressed()[9]
{
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,
);
}
match usb_hid_joystick.device().write_report(&get_joystick_report(
&mut buttons,
&mut axis,
&mode,
)) {
Err(UsbHidError::WouldBlock) => {}
Ok(_) => {}
Err(e) => {
status_led.update(StatusMode::Error);
core::panic!("Failed to write joystick report: {:?}", e)
}
};
} }
// -----------------------------------------------------------
if scan_count_down.wait().is_ok() { if scan_count_down.wait().is_ok() {
button_matrix.scan_matrix(&mut delay); button_matrix.scan_matrix(&mut delay);
@ -324,32 +303,148 @@ fn main() -> ! {
item.expo, item.expo,
); );
} }
let pressed_keys = button_matrix.buttons_pressed();
mode = get_mode(pressed_keys);
// Update pressed keys status
for (index, key) in pressed_keys.iter().enumerate() {
buttons[index].pressed = *key;
}
// Update Fn mode for all axis that are in idle position
// This is to avoid the Fn mode switching when moving the gimbal
idle = true;
for item in axis.iter_mut() {
if item.value == item.idle_value {
item.fn_mode = mode & 0x0F;
} else {
idle = false;
}
}
// Set fn mode for all keys taht are in idle position
// This is to avoid the Fn mode switching when using a button
for (index, key) in buttons.iter_mut().enumerate() {
if !key.pressed {
key.fn_mode = mode & 0x0F;
} else if (usb_active
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnL
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnR
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeL
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR)
|| (!usb_active
&& layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated)
{
idle = false;
}
}
// Generate led activity when gimbal is moved from idle position
for item in axis.iter_mut() {
if item.value != item.previous_value {
activity = true;
}
item.previous_value = item.value;
}
// Generate led activity when a button is pressed
// FnL, FnR, and ModeR are excluded
for (index, key) in buttons.iter_mut().enumerate() {
if (usb_active
&& key.pressed != key.previous_pressed
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnL
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnR
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR)
|| (!usb_active
&& key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated)
{
activity = true;
}
key.previous_pressed = key.pressed;
}
} }
if usb_dev.poll(&mut [&mut usb_hid_joystick]) {} if usb_dev.poll(&mut [&mut usb_hid_joystick]) {
usb_active = true;
}
if status_led_count_down.wait().is_ok() {
update_status_led(
&mut status_led,
&mut activity,
&usb_active,
&idle,
&safety_check,
);
}
if usb_hid_report_count_down.wait().is_ok() && activity {
// Dont send USB HID joystick report if there is no activity
// This is to avoid preventing the computer from going to sleep
match usb_hid_joystick.device().write_report(&get_joystick_report(
&mut buttons,
&mut axis,
&mode,
)) {
Err(UsbHidError::WouldBlock) => {}
Ok(_) => {}
Err(e) => {
status_led.update(StatusMode::Error);
core::panic!("Failed to write joystick report: {:?}", e)
}
};
}
// Check if all axis are in idle position and no buttons are pressed
if idle && !safety_check && !usb_active {
safety_check = true;
}
// TODO: Implement ELRS
if elrs_count_down.wait().is_ok() && !usb_active && safety_check {}
} }
} }
/// Update status LED colour based on function layer and capslock /// Update status LED colour based on function layer and capslock
/// ///
/// Normal = green (NORMAL) /// USB mode = green
/// Left Alt mode = blue (GUI LOCK) /// ELRS mode = orange
/// Activity = flashing blue
/// Error = steady red (ERROR) /// Error = steady red (ERROR)
/// ///
/// # Arguments /// # Arguments
/// * `status_led` - Reference to status LED /// * `status_led` - Reference to status LED
/// * `caps_lock_active` - Is capslock active /// * `activity` - Reference to bool that indicates if there is activity
fn update_status_led<P, SM, I>(status_led: &mut Ws2812StatusLed<P, SM, I>, fn_mode: &u8) /// * `usb_active` - Reference to bool that indicates if USB is active
where /// * `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>,
activity: &mut bool,
usb_active: &bool,
axis_idle: &bool,
safety_check: &bool,
) where
P: PIOExt + FunctionConfig, P: PIOExt + FunctionConfig,
I: PinId, I: PinId,
Function<P>: ValidPinMode<I>, Function<P>: ValidPinMode<I>,
SM: StateMachineIndex, SM: StateMachineIndex,
{ {
if *fn_mode & 0x10 == 0x10 { if !usb_active && !*safety_check {
status_led.update(StatusMode::Warning);
} else if *activity && status_led.get_mode() != StatusMode::Activity {
status_led.update(StatusMode::Activity); status_led.update(StatusMode::Activity);
} else { } else if *activity && status_led.get_mode() == StatusMode::Activity {
status_led.update(StatusMode::Off);
*activity = false;
} else if !*axis_idle && status_led.get_mode() != StatusMode::Activity {
status_led.update(StatusMode::Activity);
} else if *usb_active && status_led.get_mode() != StatusMode::Normal {
status_led.update(StatusMode::Normal); status_led.update(StatusMode::Normal);
} else if status_led.get_mode() != StatusMode::Other {
status_led.update(StatusMode::Other);
} }
} }
@ -368,16 +463,16 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 {
let mut alt_r_active: 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::HID_MAP[0][index] == layout::HidButton::FnL {
fn_l_active = true; fn_l_active = true;
} }
if *key && layout::MAP[0][index] == layout::ButtonType::FnR { if *key && layout::HID_MAP[0][index] == layout::HidButton::FnR {
fn_r_active = true; fn_r_active = true;
} }
if *key && layout::MAP[0][index] == layout::ButtonType::ModeL { if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeL {
alt_l_active = true; alt_l_active = true;
} }
if *key && layout::MAP[0][index] == layout::ButtonType::ModeR { if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeR {
alt_r_active = true; alt_r_active = true;
} }
} }
@ -423,14 +518,6 @@ fn get_joystick_report(
let mut ry: u16 = AXIS_CENTER; let mut ry: u16 = AXIS_CENTER;
let mut rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value; let mut rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value;
// Update Fn mode for all axis that are in idle position
// This is to avoid the Fn mode switching when moving the gimbal
for item in axis.iter_mut() {
if item.value == item.idle_value {
item.fn_mode = mode & 0x0F;
}
}
// Left Alt mode active (bit 4) // Left Alt mode active (bit 4)
// Full range of left gimbal gives half range of joystick axis (center to max) // Full range of left gimbal gives half range of joystick axis (center to max)
// Left Fn mode = reversed range (center to min) // Left Fn mode = reversed range (center to min)
@ -472,14 +559,6 @@ fn get_joystick_report(
ry = axis[GIMBAL_AXIS_RIGHT_Y].value; ry = axis[GIMBAL_AXIS_RIGHT_Y].value;
} }
// Set fn mode for all keys taht are in idle position
// This is to avoid the Fn mode switching when using a button
for key in matrix_keys.iter_mut() {
if !key.pressed {
key.fn_mode = mode & 0x0F;
}
}
// Generate array for all four hat switches with following structure: // Generate array for all four hat switches with following structure:
// * bit 1: Up // * bit 1: Up
// * bit 2: Right // * bit 2: Right
@ -491,18 +570,16 @@ fn get_joystick_report(
let mut hats: [u8; 4] = [0; 4]; let mut hats: [u8; 4] = [0; 4];
for (index, key) in matrix_keys.iter_mut().enumerate() { for (index, key) in matrix_keys.iter_mut().enumerate() {
if key.pressed if key.pressed
&& layout::MAP[key.fn_mode as usize][index] as usize && layout::HID_MAP[key.fn_mode as usize][index] >= layout::HidButton::Hat1U
>= layout::ButtonType::Hat1U as usize && layout::HID_MAP[key.fn_mode as usize][index] <= layout::HidButton::Hat4B
&& layout::MAP[key.fn_mode as usize][index] as usize
<= layout::ButtonType::Hat4B as usize
{ {
hats[(layout::MAP[key.fn_mode as usize][index] as usize hats[(layout::HID_MAP[key.fn_mode as usize][index] as usize
- layout::ButtonType::Hat1U as usize) - layout::HidButton::Hat1U as usize)
/ 5] |= 1 / 5] |= 1
<< ((layout::MAP[key.fn_mode as usize][index] as usize << ((layout::HID_MAP[key.fn_mode as usize][index] as usize
- layout::ButtonType::Hat1U as usize) - layout::HidButton::Hat1U as usize)
- (5 * ((layout::MAP[key.fn_mode as usize][index] as usize - (5 * ((layout::HID_MAP[key.fn_mode as usize][index] as usize
- layout::ButtonType::Hat1U as usize) - layout::HidButton::Hat1U as usize)
/ 5))); / 5)));
} }
} }
@ -522,10 +599,12 @@ fn get_joystick_report(
// Update button state for joystick button 1-20 // Update button state for joystick button 1-20
for (index, key) in matrix_keys.iter_mut().enumerate() { for (index, key) in matrix_keys.iter_mut().enumerate() {
if key.pressed if key.pressed
&& layout::MAP[key.fn_mode as usize][index] as usize >= layout::ButtonType::B1 as usize && layout::HID_MAP[key.fn_mode as usize][index] as usize
&& layout::MAP[key.fn_mode as usize][index] as usize <= layout::ButtonType::B20 as usize >= layout::HidButton::B1 as usize
&& layout::HID_MAP[key.fn_mode as usize][index] as usize
<= layout::HidButton::B20 as usize
{ {
buttons |= 1 << layout::MAP[key.fn_mode as usize][index] as usize; buttons |= 1 << layout::HID_MAP[key.fn_mode as usize][index] as usize;
} }
} }