diff --git a/rp2040/src/elrs.rs b/rp2040/src/elrs.rs deleted file mode 100644 index 4d08f81..0000000 --- a/rp2040/src/elrs.rs +++ /dev/null @@ -1,194 +0,0 @@ -//! Project: CMtec CMDR joystick 24 -//! Date: 2023-08-01 -//! Author: Christoffer Martinsson -//! Email: cm@cmtec.se -//! License: Please refer to LICENSE in root directory - -use embedded_hal::serial::Read; -use rp2040_hal::uart::{Enabled, State, UartDevice, UartPeripheral, ValidUartPinout}; - -const CRSF_CRC8TAB: [u8; 256] = [ - 0x00, 0xD5, 0x7F, 0xAA, 0xFE, 0x2B, 0x81, 0x54, 0x29, 0xFC, 0x56, 0x83, 0xD7, 0x02, 0xA8, 0x7D, - 0x52, 0x87, 0x2D, 0xF8, 0xAC, 0x79, 0xD3, 0x06, 0x7B, 0xAE, 0x04, 0xD1, 0x85, 0x50, 0xFA, 0x2F, - 0xA4, 0x71, 0xDB, 0x0E, 0x5A, 0x8F, 0x25, 0xF0, 0x8D, 0x58, 0xF2, 0x27, 0x73, 0xA6, 0x0C, 0xD9, - 0xF6, 0x23, 0x89, 0x5C, 0x08, 0xDD, 0x77, 0xA2, 0xDF, 0x0A, 0xA0, 0x75, 0x21, 0xF4, 0x5E, 0x8B, - 0x9D, 0x48, 0xE2, 0x37, 0x63, 0xB6, 0x1C, 0xC9, 0xB4, 0x61, 0xCB, 0x1E, 0x4A, 0x9F, 0x35, 0xE0, - 0xCF, 0x1A, 0xB0, 0x65, 0x31, 0xE4, 0x4E, 0x9B, 0xE6, 0x33, 0x99, 0x4C, 0x18, 0xCD, 0x67, 0xB2, - 0x39, 0xEC, 0x46, 0x93, 0xC7, 0x12, 0xB8, 0x6D, 0x10, 0xC5, 0x6F, 0xBA, 0xEE, 0x3B, 0x91, 0x44, - 0x6B, 0xBE, 0x14, 0xC1, 0x95, 0x40, 0xEA, 0x3F, 0x42, 0x97, 0x3D, 0xE8, 0xBC, 0x69, 0xC3, 0x16, - 0xEF, 0x3A, 0x90, 0x45, 0x11, 0xC4, 0x6E, 0xBB, 0xC6, 0x13, 0xB9, 0x6C, 0x38, 0xED, 0x47, 0x92, - 0xBD, 0x68, 0xC2, 0x17, 0x43, 0x96, 0x3C, 0xE9, 0x94, 0x41, 0xEB, 0x3E, 0x6A, 0xBF, 0x15, 0xC0, - 0x4B, 0x9E, 0x34, 0xE1, 0xB5, 0x60, 0xCA, 0x1F, 0x62, 0xB7, 0x1D, 0xC8, 0x9C, 0x49, 0xE3, 0x36, - 0x19, 0xCC, 0x66, 0xB3, 0xE7, 0x32, 0x98, 0x4D, 0x30, 0xE5, 0x4F, 0x9A, 0xCE, 0x1B, 0xB1, 0x64, - 0x72, 0xA7, 0x0D, 0xD8, 0x8C, 0x59, 0xF3, 0x26, 0x5B, 0x8E, 0x24, 0xF1, 0xA5, 0x70, 0xDA, 0x0F, - 0x20, 0xF5, 0x5F, 0x8A, 0xDE, 0x0B, 0xA1, 0x74, 0x09, 0xDC, 0x76, 0xA3, 0xF7, 0x22, 0x88, 0x5D, - 0xD6, 0x03, 0xA9, 0x7C, 0x28, 0xFD, 0x57, 0x82, 0xFF, 0x2A, 0x80, 0x55, 0x01, 0xD4, 0x7E, 0xAB, - 0x84, 0x51, 0xFB, 0x2E, 0x7A, 0xAF, 0x05, 0xD0, 0xAD, 0x78, 0xD2, 0x07, 0x53, 0x86, 0x2C, 0xF9, -]; - -const CONNECTION_TIMEOUT: u16 = 500; -const INIT_TIMEOUT: u16 = 1000; - -pub struct Elrs -where - S: State, - D: UartDevice, - P: ValidUartPinout, -{ - uart: UartPeripheral, - elsr_init_done: bool, - elrs_init_counter: u16, - elrs_connected: bool, - elrs_connected_timeout: u16, - rx_buffer_index: usize, -} - -impl Elrs -where - S: State, - D: UartDevice, - P: ValidUartPinout, -{ - pub fn new(uart: UartPeripheral) -> Self { - let elsr_init_done = false; - let elrs_init_counter = 0; - let elrs_connected = false; - let elrs_connected_timeout = CONNECTION_TIMEOUT; - let rx_buffer_index = 0; - Self { - uart, - elsr_init_done, - elrs_init_counter, - elrs_connected, - elrs_connected_timeout, - rx_buffer_index, - } - } -} - -impl Elrs -where - D: UartDevice, - P: ValidUartPinout, -{ - pub fn send(&mut self, data: [u16; 12]) { - if self.elsr_init_done { - self.uart - .write_full_blocking(&self.prepare_crsf_data_packet(data)); - return; - } - if self.elrs_init_counter < INIT_TIMEOUT { - self.uart - .write_full_blocking(&self.prepare_crsf_data_packet(data)); - self.elrs_init_counter += 1; - } else if self.elrs_init_counter < INIT_TIMEOUT + 5 { - self.uart - // Setting Packet Rate to 150Hz - // 0 = 50Hz LoRa, 1 = 100Hz Full, 2 = 150Hz LoRa, 3 = 250Hz LoRa, - // 4 = 333H Full, 5 = 500Hz LoRa, 6 = 250Hz DejaVu, 7 = 500Hz DejaVu, - // 8 = 500Hz FLRC, 9 = 1000Hz FLRC - .write_full_blocking(&self.prepare_crsf_cmd_packet(0x01, 0x00)); - self.elrs_init_counter += 1; - } else if self.elrs_init_counter < INIT_TIMEOUT + 10 { - self.uart - // Setting Power to 10mW - // 0 = 10mW, 1 = 25mW, 2 = 50mW, 3 = 100mW, - // 4 = 200mW, 5 = 500mW, 6 = 1000mW, 7 = 2000mW - .write_full_blocking(&self.prepare_crsf_cmd_packet(0x06, 0x00)); - self.elrs_init_counter += 1; - } else { - self.elsr_init_done = true; - } - - self.check_link_frame(); - } - - pub fn connected(&self) -> bool { - self.elrs_connected - } - - pub fn reset(&mut self) { - self.elsr_init_done = false; - self.elrs_init_counter = 0; - } - - fn check_link_frame(&mut self) { - if self.elrs_connected_timeout == 0 { - self.elrs_connected = false; - } else { - self.elrs_connected_timeout -= 1; - } - - let mut rx_byte: u8; - - while self.uart.uart_is_readable() { - match self.uart.read() { - Ok(byte) => { - rx_byte = byte; - } - Err(_) => { - continue; - } - } - - // Simple RX telemetry link frame detection: 0xEA, 0xXX, 0x14 - if (rx_byte == 0xEA && self.rx_buffer_index == 0) || self.rx_buffer_index == 1 { - self.rx_buffer_index += 1; - } else if rx_byte == 0x14 && self.rx_buffer_index == 2 { - self.elrs_connected = true; - self.elrs_connected_timeout = CONNECTION_TIMEOUT; - self.rx_buffer_index = 0; - } else { - self.rx_buffer_index = 0; - } - } - } - - fn prepare_crsf_data_packet(&self, data: [u16; 12]) -> [u8; 26] { - let mut packet: [u8; 26] = [0; 26]; - let mut crc: u8 = 0; - packet[0] = 0xEE; - packet[1] = 0x18; - packet[2] = 0x16; - packet[3] = data[0] as u8; - packet[4] = ((data[0] >> 8) | (data[1] << 3)) as u8; - packet[5] = ((data[1] >> 5) | (data[2] << 6)) as u8; - packet[6] = (data[2] >> 2) as u8; - packet[7] = ((data[2] >> 10) | (data[3] << 1)) as u8; - packet[8] = ((data[3] >> 7) | (data[4] << 4)) as u8; - packet[9] = ((data[4] >> 4) | (data[5] << 7)) as u8; - packet[10] = (data[5] >> 1) as u8; - packet[11] = ((data[5] >> 9) | (data[6] << 2)) as u8; - packet[12] = ((data[6] >> 6) | (data[7] << 5)) as u8; - packet[13] = (data[7] >> 3) as u8; - packet[14] = data[8] as u8; - packet[15] = ((data[8] >> 8) | (data[9] << 3)) as u8; - packet[16] = ((data[9] >> 5) | (data[10] << 6)) as u8; - packet[17] = (data[10] >> 2) as u8; - packet[18] = ((data[10] >> 10) | (data[11] << 1)) as u8; - packet[19] = (data[11] >> 7) as u8; - // Channel 13-16 are not used in ELRS - for i in 2..25 { - crc = CRSF_CRC8TAB[(crc ^ packet[i]) as usize]; - } - packet[25] = crc; - packet - } - - fn prepare_crsf_cmd_packet(&self, command: u8, value: u8) -> [u8; 8] { - let mut packet: [u8; 8] = [0; 8]; - let mut crc: u8 = 0; - packet[0] = 0xEE; - packet[1] = 0x06; - packet[2] = 0x2D; - packet[3] = 0xEE; - packet[4] = 0xEA; - packet[5] = command; - packet[6] = value; - for i in 2..7 { - crc = CRSF_CRC8TAB[(crc ^ packet[i]) as usize]; - } - packet[7] = crc; - packet - } -} diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index adde844..54ba967 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -6,7 +6,7 @@ // // HW Button index map: // --------------------------------------------------------------- -// | 0 L| 1 U| | 2 | | 3 L| 4 U| +// | 0 L| 1 U| 25 U | | 2 | | 26 U | 4 U| 3 L| // --------------------------------------------------------------- // | | 5 | 6 | 7 | | 12 | 11 | 10 | | // | | @@ -18,11 +18,11 @@ // | | 18 | | 23 | | // --------------------------------------------------------------- // -// USB HID joystick map (Enabled by pressing HW button 2+4): +// USB HID joystick map : // --------------------------------------------------------------- -// | B1 L| B2 U| | B3 | | B4 L| B5 U| +// | Fn L| B2 U| B26 U| | B3 | | B28 U| B5 U| B4 L| // --------------------------------------------------------------- -// | | B6 | B7 | B8/16| |B13/17| B12 | B11 | | +// | | TH | B6 | B8/16| |B13/17| B12 | B11 | | // | | // | | B9 | | B14 | | // | | B10 | | B15 | | @@ -31,34 +31,26 @@ // | | H1L | B18 | H1R | | H2L | B19 | H2R | | // | | H1D | | H2D | | // --------------------------------------------------------------- -// Press and hold HW button 0 (B1) changes following: -// * B2 => B21 -// * B4 => B22 -// * B11 => B23 -// * B12 => B24 -// * hat1 => hat3 (button press B20). -// * hat2 => hat4 (button bpress B25) // -// ELRS channel map (Enabled by pressing HW button 2+3): -// (+ = ON, - = OFF, CHxP/M/Z = trim) +// USB HID joystick map (Fn): // --------------------------------------------------------------- -// |CH7-L|CH7+U| | - | |CH8-L|CH8+U| +// | Fn L| B21 U| B27 U| | B3 | | B28 U| B5 U| B22 L| // --------------------------------------------------------------- -// | | THL | CH9 | CH5 | | CH6 |CH11-|CH11+| | +// | | TH | B7 | B8/16| |B13/17| B24 | B23 | | // | | -// | |CH10+| |CH12+| | -// | |CH10-| |CH12-| | -// | CH1/CH2 CH3/CH4 | -// | | - | |CH4P | | -// | |CH1M |CH12Z|CH1P | |CH3M |CH34Z|CH3P | | -// | | - | |CH4M | | +// | | 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 | | USB L|ELRS U| +// |BOOT L| CAL U| | CONFIG | | - | - | // --------------------------------------------------------------- -// | | THL-| THL+| - | | - | - | - | | +// | | - | - | - | | - | - | - | | // | | // | | - | | - | | // | | - | | - | | @@ -72,7 +64,6 @@ #![no_main] mod button_matrix; -mod elrs; mod status_led; mod usb_joystick_device; @@ -81,7 +72,6 @@ use core::convert::Infallible; use cortex_m::delay::Delay; use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS}; use eeprom24x::{Eeprom24x, SlaveAddr}; -use elrs::Elrs; use embedded_hal::adc::OneShot; use embedded_hal::digital::v2::*; use embedded_hal::timer::CountDown; @@ -90,10 +80,9 @@ use libm::powf; use panic_halt as _; use rp2040_hal::{ adc::Adc, - gpio::{Function, FunctionConfig, FunctionUart, PinId, ValidPinMode}, + gpio::{Function, FunctionConfig, PinId, ValidPinMode}, i2c::I2C, pio::StateMachineIndex, - uart::{DataBits, StopBits, UartConfig, UartPeripheral}, }; use status_led::{StatusMode, Ws2812StatusLed}; use usb_device::class_prelude::*; @@ -122,10 +111,6 @@ pub const AXIS_MIN: u16 = 0; pub const AXIS_MAX: u16 = 4095; pub const AXIS_CENTER: u16 = (AXIS_MIN + AXIS_MAX) / 2; -pub const ELRS_MIN: u16 = 172; -pub const ELRS_MAX: u16 = 1811; -pub const ELRS_CENTER: u16 = (ELRS_MIN + ELRS_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; @@ -157,12 +142,6 @@ pub struct Button { pub usb_button_sec_pressed: bool, pub usb_button_toggle_enable: bool, pub usb_release_timeout: u16, - pub elrs_changed: bool, - pub elrs_changed_to_pressed: bool, - pub elrs_channel: usize, - pub elrs_lock_enable: bool, - pub elrs_lock_state_on: bool, - pub elrs_lock_state: bool, } #[derive(Copy, Clone)] @@ -192,7 +171,7 @@ impl Default for GimbalAxis { deadzone: (100, 50, 100), expo: true, trim: 0, - hold: 0, + hold: AXIS_CENTER, hold_pending: false, } } @@ -232,19 +211,6 @@ fn main() -> ! { &mut pac.RESETS, ); - // Set up UART on GP0 and GP1 (Pico pins 1 and 2) - let uart_pins = ( - pins.gp0.into_mode::(), - pins.gp1.into_mode::(), - ); - - let elrs_uart = UartPeripheral::new(pac.UART0, uart_pins, &mut pac.RESETS) - .enable( - UartConfig::new(400000.Hz(), DataBits::Eight, None, StopBits::One), - clocks.peripheral_clock.freq(), - ) - .unwrap(); - let i2c = I2C::i2c1( pac.I2C1, pins.gp14.into_mode(), // sda @@ -286,9 +252,6 @@ fn main() -> ! { &mut pins.gp13.into_push_pull_output(), ]; - let mut elrs_en_pin = pins.gp2.into_push_pull_output(); - let mut elrs = Elrs::new(elrs_uart); - // Create button matrix object that scans all buttons let mut button_matrix: ButtonMatrix = ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, DEBOUNCE); @@ -296,6 +259,10 @@ fn main() -> ! { // 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( @@ -331,26 +298,16 @@ fn main() -> ! { let mut data_process_count_down = timer.count_down(); data_process_count_down.start(1200u32.micros()); - let mut elrs_start_count_down = timer.count_down(); - elrs_start_count_down.start(2000.millis()); - - let mut elrs_update_count_down = timer.count_down(); - elrs_update_count_down.start(1666u32.micros()); - let mut usb_update_count_down = timer.count_down(); usb_update_count_down.start(10.millis()); - let mut safety_check: bool = false; let mut usb_activity: bool = false; - let mut idle: bool = false; let mut usb_active: bool = false; - let mut elrs_active: bool = false; - let _elrs_connected: 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] = [Button::default(); NUMBER_OF_BUTTONS]; + let mut buttons: [Button; NUMBER_OF_BUTTONS + 2] = [Button::default(); NUMBER_OF_BUTTONS + 2]; let mut gimbal_mode: u8; // Set up usb button layout @@ -365,7 +322,7 @@ fn main() -> ! { buttons[3].usb_button_sec = 22; buttons[3].usb_button_sec_trigger_index = 0; buttons[4].usb_button = 5; - buttons[5].usb_button = 6; + buttons[5].usb_button = 0; buttons[6].usb_button = 7; buttons[7].usb_button = 8; buttons[7].usb_button_sec = 16; @@ -393,36 +350,11 @@ fn main() -> ! { buttons[20].usb_button_sec_enable = true; buttons[20].usb_button_sec = 25; buttons[20].usb_button_sec_trigger_index = 0; - - // Set up elrs button layout - buttons[0].elrs_channel = 7; - buttons[0].elrs_lock_enable = true; - buttons[1].elrs_channel = 7; - buttons[1].elrs_lock_enable = true; - buttons[1].elrs_lock_state_on = true; - buttons[3].elrs_channel = 8; - buttons[3].elrs_lock_enable = true; - buttons[4].elrs_channel = 8; - buttons[4].elrs_lock_enable = true; - buttons[4].elrs_lock_state_on = true; - buttons[6].elrs_channel = 9; - buttons[7].elrs_channel = 5; - buttons[8].elrs_channel = 10; - buttons[8].elrs_lock_enable = true; - buttons[9].elrs_channel = 10; - buttons[9].elrs_lock_enable = true; - buttons[9].elrs_lock_state_on = true; - buttons[10].elrs_channel = 11; - buttons[10].elrs_lock_enable = true; - buttons[11].elrs_channel = 11; - buttons[11].elrs_lock_enable = true; - buttons[11].elrs_lock_state_on = true; - buttons[12].elrs_channel = 6; - buttons[13].elrs_channel = 12; - buttons[13].elrs_lock_enable = true; - buttons[14].elrs_channel = 12; - buttons[14].elrs_lock_enable = true; - buttons[14].elrs_lock_state_on = true; + 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); @@ -502,25 +434,21 @@ fn main() -> ! { update_status_led( &mut status_led, &usb_active, - &elrs_active, - &elrs.connected(), - &safety_check, &calibration_active, &throttle_hold_enable, ); } - // Check if all axis are in idle position and no buttons are pressed - if idle && !safety_check && elrs_active { - safety_check = true; - } - 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); @@ -532,21 +460,8 @@ fn main() -> ! { ); } - // ON/OFF switch for ELRS radio - if buttons[4].pressed && buttons[2].pressed && !elrs_active { - safety_check = false; - elrs_active = true; - } else if buttons[3].pressed && buttons[2].pressed && elrs_active { - elrs_active = false; - } - - // ON/OFF switch for Throttle hold mode - if buttons[6].pressed && buttons[2].pressed && !throttle_hold_enable { - throttle_hold_enable = true; - axis[GIMBAL_AXIS_LEFT_Y].hold = 0; - } else if buttons[5].pressed && buttons[2].pressed && throttle_hold_enable { - throttle_hold_enable = false; - } + // // 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) @@ -615,6 +530,7 @@ fn main() -> ! { } // 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 @@ -644,13 +560,6 @@ fn main() -> ! { axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold; } - // Set idle state - for key in buttons.iter_mut() { - if key.pressed { - idle = false; - } - } - // Generate led activity when gimbal is moved from idle position for item in axis.iter_mut() { if item.value != item.previous_value { @@ -671,18 +580,23 @@ fn main() -> ! { if key.pressed != key.previous_pressed { key.usb_changed = true; key.usb_changed_to_pressed = key.pressed; - key.elrs_changed = true; - key.elrs_changed_to_pressed = key.pressed; usb_activity = true; } // Set throttle_hold_value if key.pressed != key.previous_pressed && key.pressed - && throttle_hold_enable && 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; } @@ -692,20 +606,6 @@ fn main() -> ! { for axis in axis.iter_mut() { axis.hold = 0; } - for button in buttons.iter_mut() { - button.elrs_lock_state = false; - } - } - } - - if elrs_update_count_down.wait().is_ok() { - // Send ELRS data - if elrs_active { - elrs_en_pin.set_high().unwrap(); - elrs.send(get_elrs_channels(&mut buttons, &mut axis)); - } else { - elrs_en_pin.set_low().unwrap(); - elrs.reset(); } } @@ -732,7 +632,6 @@ fn main() -> ! { /// Update status LED colour based on function layer and capslock /// /// USB mode = green -/// ELRS mode = orange /// Activity = flashing blue /// Error = steady red (ERROR) /// @@ -745,9 +644,6 @@ fn main() -> ! { fn update_status_led( status_led: &mut Ws2812StatusLed, usb_active: &bool, - elrs_active: &bool, - elrs_connected: &bool, - safety_check: &bool, calibration_active: &bool, throttle_hold_enable: &bool, ) where @@ -757,19 +653,13 @@ fn update_status_led( SM: StateMachineIndex, { if *calibration_active { - status_led.update(StatusMode::ActivityFlash); - } else if *elrs_active && !*safety_check { status_led.update(StatusMode::Warning); - } else if !*usb_active && !*elrs_active { + } else if !*usb_active { status_led.update(StatusMode::NormalFlash); - } else if *usb_active && !*elrs_active && *throttle_hold_enable { - status_led.update(StatusMode::Activity); - } else if *usb_active && !*elrs_active && !*throttle_hold_enable { - status_led.update(StatusMode::Normal); - } else if *elrs_active && *elrs_connected { + } else if *usb_active && *throttle_hold_enable { status_led.update(StatusMode::Other); - } else if *elrs_active && !*elrs_connected { - status_led.update(StatusMode::OtherFlash); + } else if *usb_active && !*throttle_hold_enable { + status_led.update(StatusMode::Normal); } } @@ -781,16 +671,16 @@ fn update_status_led( /// * `matrix_keys` - Array of pressed keys /// * `axis` - Array of joystick axis values fn get_joystick_report( - matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], + matrix_keys: &mut [Button; NUMBER_OF_BUTTONS + 2], axis: &mut [GimbalAxis; 4], throttle_hold_enable: &bool, ) -> JoystickReport { - let x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; - let y: u16 = AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value; - let z: u16 = axis[GIMBAL_AXIS_LEFT_X].value; - let rx: u16 = AXIS_CENTER; + 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[GIMBAL_AXIS_LEFT_Y].value; + let rz: u16 = AXIS_CENTER; let mut hat1: u8 = 0xf; let mut hat2: u8 = 0xf; let mut hat3: u8 = 0xf; @@ -858,7 +748,7 @@ fn get_joystick_report( // 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 { + 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 @@ -1073,147 +963,3 @@ fn generate_expo_lut(expo: f32) -> [u16; AXIS_MAX as usize + 1] { } lut } - -/// Get ELRS channel values -/// -/// # Arguments -/// * `matrix_keys` - Array of buttons -/// * `axis` - Array of axis -fn get_elrs_channels( - matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], - axis: &mut [GimbalAxis; 4], -) -> [u16; 12] { - let mut channels: [u16; 12] = [ELRS_MIN; 12]; - - // Check and store trim values - for (index, key) in matrix_keys.iter_mut().enumerate() { - // Left gimbal X - if key.pressed && index == 17 && axis[GIMBAL_AXIS_LEFT_X].trim < ELRS_CENTER as i16 { - axis[GIMBAL_AXIS_LEFT_X].trim += 1; - } else if key.pressed - && index == 19 - && axis[GIMBAL_AXIS_LEFT_X].trim > (0 - ELRS_CENTER as i16) - { - axis[GIMBAL_AXIS_LEFT_X].trim -= 1; - // Left gimbal Y - } else if key.pressed && index == 16 && axis[GIMBAL_AXIS_LEFT_Y].trim < ELRS_CENTER as i16 { - axis[GIMBAL_AXIS_LEFT_Y].trim += 1; - } else if key.pressed - && index == 18 - && axis[GIMBAL_AXIS_LEFT_Y].trim > (0 - ELRS_CENTER as i16) - { - axis[GIMBAL_AXIS_LEFT_Y].trim -= 1; - // Right gimbal X - } else if key.pressed && index == 22 && axis[GIMBAL_AXIS_RIGHT_X].trim < ELRS_CENTER as i16 - { - axis[GIMBAL_AXIS_RIGHT_X].trim += 1; - } else if key.pressed - && index == 24 - && axis[GIMBAL_AXIS_RIGHT_X].trim > (0 - ELRS_CENTER as i16) - { - axis[GIMBAL_AXIS_RIGHT_X].trim -= 1; - // Right gimbal Y - } else if key.pressed && index == 21 && axis[GIMBAL_AXIS_RIGHT_Y].trim < ELRS_CENTER as i16 - { - axis[GIMBAL_AXIS_RIGHT_Y].trim += 1; - } else if key.pressed - && index == 23 - && axis[GIMBAL_AXIS_RIGHT_Y].trim > (0 - ELRS_CENTER as i16) - { - axis[GIMBAL_AXIS_RIGHT_Y].trim -= 1; - } - } - - // Alpine hat switch filter - let mut hat_left_button_only: bool = true; - let mut hat_right_button_only: bool = true; - for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.pressed && (16..=19).contains(&index) { - hat_left_button_only = false; - } - if key.pressed && (21..=24).contains(&index) { - hat_right_button_only = false; - } - } - - // Check and reset trim values - for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.elrs_changed && key.elrs_changed_to_pressed && index == 15 && hat_left_button_only { - axis[GIMBAL_AXIS_LEFT_X].trim = 0; - axis[GIMBAL_AXIS_LEFT_Y].trim = 0; - } else if key.elrs_changed - && key.elrs_changed_to_pressed - && index == 20 - && hat_right_button_only - { - axis[GIMBAL_AXIS_RIGHT_X].trim = 0; - axis[GIMBAL_AXIS_RIGHT_Y].trim = 0; - } - } - - // Match ELRS channel 1-4 to new min/max values - for (index, item) in axis.iter_mut().enumerate() { - channels[index] = remap(item.value, AXIS_MIN, AXIS_MAX, ELRS_MIN, ELRS_MAX); - } - - // Apply trim to ELRS channel 1,3,4 - for (index, item) in axis.iter().enumerate() { - if index != GIMBAL_AXIS_LEFT_Y && channels[index] > ELRS_CENTER { - channels[index] = remap( - channels[index], - ELRS_CENTER, - ELRS_MAX, - (ELRS_CENTER as i16 + item.trim) as u16, - ELRS_MAX, - ); - } else if index != GIMBAL_AXIS_LEFT_Y && channels[index] < ELRS_CENTER { - channels[index] = remap( - channels[index], - ELRS_MIN, - ELRS_CENTER, - ELRS_MIN, - (ELRS_CENTER as i16 + item.trim) as u16, - ); - } else if index != GIMBAL_AXIS_LEFT_Y { - channels[index] = (ELRS_CENTER as i16 + item.trim) as u16; - } - } - - // Update button state for ELRS channels - for key in matrix_keys.iter_mut() { - if key.elrs_changed - && key.elrs_changed_to_pressed - && key.elrs_lock_enable - && key.elrs_lock_state_on - && key.elrs_channel != 0 - { - key.elrs_lock_state = true; - } else if key.elrs_changed - && !key.elrs_changed_to_pressed - && key.elrs_lock_enable - && !key.elrs_lock_state_on - && key.elrs_channel != 0 - { - key.elrs_lock_state = false; - } else if key.pressed && !key.elrs_lock_enable && key.elrs_channel != 0 { - channels[key.elrs_channel] = ELRS_MAX; - } - } - - // Apply locking to ELRS channels - for key in matrix_keys.iter_mut() { - if key.elrs_lock_enable - && key.elrs_lock_state - && key.elrs_lock_state_on - && key.elrs_channel != 0 - { - channels[key.elrs_channel] = ELRS_MAX; - } - } - - // Reset changed flags - for key in matrix_keys.iter_mut() { - key.elrs_changed = false; - } - channels -} diff --git a/rp2040/src/usb_joystick_device.rs b/rp2040/src/usb_joystick_device.rs index f2793d3..b13ff26 100644 --- a/rp2040/src/usb_joystick_device.rs +++ b/rp2040/src/usb_joystick_device.rs @@ -90,15 +90,12 @@ pub const JOYSTICK_DESCRIPTOR: &[u8] = &[ 0xc0, // End Collection 0x05, 0x09, // Usage Page (Button) 0x19, 0x01, // Usage Minimum (1) - 0x29, 0x19, // Usage Maximum (25) + 0x29, 0x20, // Usage Maximum (32) 0x15, 0x00, // Logical Minimum (0) 0x25, 0x01, // Logical Maximum (1) 0x75, 0x01, // Report Size (1) - 0x95, 0x19, // Report Count (25) + 0x95, 0x20, // Report Count (32) 0x81, 0x02, // Input (Data, Variable, Absolute) - 0x75, 0x01, // Report Size (1) PADDING - 0x95, 0x07, // Report Count (7) PADDING - 0x81, 0x03, // Input (Const, Variable, Absolute) PADDING 0x15, 0x00, // Logical Minimum (0) 0x25, 0x07, // Logical Maximum (7) 0x35, 0x00, // Physical Minimum (0)