diff --git a/.gitignore b/.gitignore index ca385b8..bf04439 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ firmware/.cache/clangd/index firmware/compile_commands.json rp2040/target rp2040/Cargo.lock +firmware/.pio/build diff --git a/README.md b/README.md index 5d9c0b2..ebed1f2 100644 --- a/README.md +++ b/README.md @@ -1,79 +1,73 @@ -# CMDR Joystick 24 +# CMDR Joystick 25 -RC Joystick with 2 hall effect gimbals, 2 hat switches and 24 buttons for use both with simulators and ELRS Rx equipped quads. +RC(ELRS)/USB Joystick with 2 hall effect gimbals, 2 hat switches and 25 buttons for use both with PC simulators and ELRS Rx equipped quads. ## Layout ```cpp -USB Joystick Layer 0 --------------------------------------------------------------- -| FnL | B1 | | B5 | FnR | --------------------------------------------------------------- -| | B2 | B3 | MoL | | MoR | B7 | B6 | | -| | -| | B4 | | B8 | | -| | B17 | | B18 | | -| Z/RZ X/Y | -| | H1U | | H2U | | -| | H1L | H1B | H1R || H2L | H2B | H2R | | -| | H1D | | H2D | | --------------------------------------------------------------- +Button index map: +--------------------------------------------------------------- +| 0 L| 1 U| | 2 | | 3 L| 4 U| +--------------------------------------------------------------- +| | 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 Joystick Layer 1 (FnL) --------------------------------------------------------------- -| FnL | B9 | | B5 | FnR | --------------------------------------------------------------- -| | B10 | B11 | MoL | | MoR | B7 | B6 | | -| | -| | B12 | | B8 | | -| | B19 | | B18 | | -| Z/RZ X/Y | -| | H3U | | H2U | | -| | H3L | H3B | H3R || H2L | H2B | H2R | | -| | H3D | | H2D | | --------------------------------------------------------------- +USB HID joystick map: +--------------------------------------------------------------- +| B1 L| B2 U| | B3 | | B4 L| B5 U| +--------------------------------------------------------------- +| | B6 | B7 | B8/16| |B13/17| B12 | B11 | | +| | +| | B9 | | B14 | | +| | B10 | | B15 | | +| X1/Y1 X2/Y2 | +| | H1U | | H2U | | +| | H1L | B18 | H1R | | H2L | B19 | H2R | | +| | H1D | | H2D | | +--------------------------------------------------------------- +Button (Switch) 7 changes following: +* hat1 => hat3 (button press B20). +Button (switch) 12 changes following: +* B4 => B21 +* B5 => B22 +* B14 => B23 +* B15 => B24 +* hat2 => hat4 (button bpress B25) -USB Joystick Layer 2 (FnR) --------------------------------------------------------------- -| FnL | B1 | | B13 | FnR | --------------------------------------------------------------- -| | B2 | B3 | MoL | | MoR | B15 | B14 | | -| | -| | B4 | | B16 | | -| | B17 | | B20 | | -| Z/RZ X(RX)/Y(RY) | -| | H1U | | H4U | | -| | H1L | H1B | H1R || H4L | H4B | H4R | | -| | H1D | | H4D | | --------------------------------------------------------------- +ELRS channel map (+ = ON, - = OFF, CHxP/M/Z = trim) +--------------------------------------------------------------- +|CH7-L|CH7+U| | - | |CH8-L|CH8+U| +--------------------------------------------------------------- +| | THL | CH9 | CH5 | | CH6 |CH11-|CH11+| | +| | +| |CH10+| |CH12+| | +| |CH10-| |CH12-| | +| CH1/CH2 CH3/CH4 | +| | - | |CH4P | | +| |CH1M |CH12Z|CH1P | |CH3M |CH34Z|CH3P | | +| | - | |CH4M | | +--------------------------------------------------------------- -USB Joystick Layer 3 (FnL + FnR) --------------------------------------------------------------- -| FnL | B9 | | B13 | FnR | --------------------------------------------------------------- -| | B10 | B11 | MoL | | MoR | B15 | B14 | | -| | -| | B12 | | B16 | | -| | B19 | | B20 | | -| Z/RZ X(RX)/Y(RY) | -| | H3U | | H4U | | -| | H3L | H3B | H3R || H4L | H4B | H4R | | -| | H3D | | H4D | | --------------------------------------------------------------- - -ELRS Layer --------------------------------------------------------------- -| CH7 | CH8 | | CH9 | CH10| --------------------------------------------------------------- -| | CH11| - | CH5 | | CH6 | - | CH12| | -| | -| | - | | - | | -| | - | | - | | -| X(CH1)/Y(CH2) X(CH3)/Y(CH4) | -| | - | | - | | -| | - | - | - || - | - | - | | -| | - | | - | | --------------------------------------------------------------- +Config Layer (holding CONFIG button) +--------------------------------------------------------------- +|BOOT L| CAL U| | CONFIG | | USB L|ELRS U| +--------------------------------------------------------------- +| | THL-| THL+| - | | - | - | - | | +| | +| | - | | - | | +| | - | | - | | +| -/- -/- | +| | - | | - | | +| | - | - | - | | - | - | - | | +| | - | | - | | +--------------------------------------------------------------- ``` @@ -81,31 +75,61 @@ ELRS Layer - Ergonomic design (low profile) - Hall effect gimbals -- Supports both USB HID joystick and ELRS Tx module -- Total 6x axis, 4x hat switches and 24x buttons (using Fn mode) implemented in USB HID mode +- Supports both USB HID joystick and ELRS Tx +- Total 6x axis, 4x hat switches and 25x buttons (using Fn mode) implemented in USB HID mode - 12 Channels implemented in ELRS mode (4x axis, 8x buttons) -- Low latency (1.6ms ELRS, 10ms USB) - -## Build environment - -- Cargo (rust embedded) -- Flashing via Cargo - - Pressing boot button on teensy - - Press and hold "top lower right button" when powering the unit ## Hardware -- 1x rp2040zero MCU board -- 2x FrSky M7 or M10 gimbals -- 6x Kailh choc low profile switches -- 6x Cherry MX switches -- 2x Miniature Toggle Switch (M6 shaft, 7mm wide body) -- 2x Alpine RKJXM1015004 hat switches -- 1x PCB (prototype board) +- 2x FrSky M7 or M10 gimbals [M7 datasheet](https://www.frsky-rc.com/product/m7/) +- 6x Kailh choc low profile switches [Brown](http://www.kailh.com/en/Products/Ks/CS/) +- 6x Cherry MX switches [Brown](https://www.cherrymx.de/en/cherry-mx/mx-original/mx-brown.html) +- 2x Miniature Toggle Switch (M6 shaft, 7-8mm wide body. Ex Apem 5636) [Apem 5000 series](https://www.farnell.com/datasheets/2626614.pdf?_ga=2.22934718.461231604.1570510103-1672862477.1542183430) +- 2x Alpine RKJXM1015004 hat switches [pdf](https://www.mouser.se/datasheet/2/15/RKJXM-1662398.pdf) - 1x Bottom case (3D printed) - 1x Top plate (3D printed) -- 2x Hat swith top (3D printed) +- 2x Hat swith top (3D printed) [stl](/mCAD/Hat_Castle_Short_scale_99_99_130.stl) +- 1x Custom PCB (CMDR Joystick 25 rev A) + - ![pcb_top](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_top.png) + - ![pcb_bottom](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_bottom.png) + - Gerber files: [zip](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_gerber.zip) + - Schematics: [pdf](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_schematics.pdf) + - rp2040zero pinout: [jpg](https://www.waveshare.com/w/upload/2/2b/RP2040-Zero-details-7.jpg) + - rp2040zero schematic: [pdf](https://www.waveshare.com/w/upload/4/4c/RP2040_Zero.pdf) + - eeprom 24C32LV: [pdf](https://www.mouser.se/datasheet/2/308/1/NV24C32LV_D-2319484.pdf) + - P-Fet si2372eds: [pdf](https://www.vishay.com/docs/63924/si2371eds.pdf) + - N-Fet 2N7002: [pdf](https://www.mouser.se/datasheet/2/408/T2N7002AK_datasheet_en_20150401-1916411.pdf) + - Small signal diod 1N4148W: [pdf](https://www.diodes.com/assets/Datasheets/BAV16W_1N4148W.pdf) + +- 1x ELRS TX (using a EP1 TCXO Dual receiver reprogrammed as a tramsmitter) + - [Link to EP1](https://www.happymodel.cn/index.php/2022/11/07/2-4g-elrs-ep1-ep2-ep1dual-tcxo-receiver/) + - Reprogramming instructions (using ExpressLRS Configurator): + 1. Select latest release + 2. Device category = Generic target used as base 2.4Ghz + 3. Device = Generic ESP32 2.4GHz Gemini TX + 4. Flash to device + 5. Use following custom settings in 10.0.0.1/hardware.html: + ``` + {"customised":"true","serial_rx":3,"serial_tx":1,"radio_busy":36,"radio_dio1":37,"radio_miso":33,"radio_mosi":32,"radio_nss":27,"radio_rst":26,"radio_sck":25,"radio_busy_2":39,"radio_dio1_2":34,"radio_nss_2":13,"power_rxen":10,"power_txen":14,"power_rxen_2":9,"power_txen_2":15,"power_min":0,"power_high":2,"power_max":2,"power_default":0,"power_control":0,"power_values":[-10,-6,-3],"button":0,"led_rgb":22,"led_rgb_isgrb":true,"screen_type":0} + ``` + +## Software Build environment +##### Rust + +- Cargo (rust embedded) +- Flashing via Cargo + - Press and hold boot button on rp2040zero board while perform a reset + - Press and hold CONF and press BOOT button. + +## References + +- CRSF protocol description (for communicating with ELRS TX): [Link](https://github.com/ExpressLRS/ExpressLRS/wiki/CRSF-Protocol) +- rp2040 datasheet: [pdf](https://datasheets.raspberrypi.com/rp2040/rp2040-datasheet.pdf) ## Calibration -No calibration needed +1. Center both gimbals. +2. Press and hold CONF button and press CAL botton. Status led will start blinking green. +3. Move both gimbals to all corners. +4. Press right hat switch to save calibration data to eeprom. + diff --git a/rp2040/Cargo.toml b/rp2040/Cargo.toml index dfc0c83..8eabb83 100644 --- a/rp2040/Cargo.toml +++ b/rp2040/Cargo.toml @@ -23,6 +23,7 @@ pio = "0.2.0" defmt = { version = "0.3", optional = true } libm = "0.2.7" dyn-smooth = "0.2.0" +eeprom24x = "0.6.0" [features] # This is the set of features we enable by default diff --git a/rp2040/src/button_matrix.rs b/rp2040/src/button_matrix.rs index 2d20098..3ccf895 100644 --- a/rp2040/src/button_matrix.rs +++ b/rp2040/src/button_matrix.rs @@ -62,10 +62,10 @@ impl<'a, const R: usize, const C: usize, const N: usize> ButtonMatrix<'a, R, C, pub fn scan_matrix(&mut self, delay: &mut Delay) { for col_index in 0..self.cols.len() { self.cols[col_index].set_low().unwrap(); - delay.delay_us(10); + delay.delay_us(1); self.process_column(col_index); self.cols[col_index].set_high().unwrap(); - delay.delay_us(10); + delay.delay_us(1); } } diff --git a/rp2040/src/elrs.rs b/rp2040/src/elrs.rs new file mode 100644 index 0000000..4d08f81 --- /dev/null +++ b/rp2040/src/elrs.rs @@ -0,0 +1,194 @@ +//! 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/layout.rs b/rp2040/src/layout.rs deleted file mode 100644 index ef43076..0000000 --- a/rp2040/src/layout.rs +++ /dev/null @@ -1,261 +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 crate::NUMBER_OF_BUTTONS; - -#[allow(dead_code)] -#[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, - B2 = 1, - B3 = 2, - B4 = 3, - B5 = 4, - B6 = 5, - B7 = 6, - B8 = 7, - B9 = 8, - B10 = 9, - B11 = 10, - B12 = 11, - B13 = 12, - B14 = 13, - B15 = 14, - B16 = 15, - B17 = 16, - B18 = 17, - B19 = 18, - B20 = 19, - FnL = 20, - FnR = 21, - ModeL = 22, - ModeR = 23, - Hat1U = 24, - Hat1R = 25, - Hat1D = 26, - Hat1L = 27, - Hat1B = 28, - Hat2U = 29, - Hat2R = 30, - Hat2D = 31, - Hat2L = 32, - Hat2B = 33, - Hat3U = 34, - Hat3R = 35, - Hat3D = 36, - Hat3L = 37, - Hat3B = 38, - Hat4U = 39, - Hat4R = 40, - Hat4D = 41, - Hat4L = 42, - Hat4B = 43, - NoEventIndicated = 44, -} -#[warn(dead_code)] -// Button index map: -// -------------------------------------------------------------- -// | 0 | 1 | | 3 | 4 | (2) -// -------------------------------------------------------------- -// | | 5 | 6 | 7 | | 12 | 11 | 10 | | -// | | -// | | 8 | | 13 | | -// | | 9 | | 14 | | -// | X1/Y1 X2/Y2 | -// | | 16 | | 21 | | -// | | 17 | 15 | 18 || 22 | 20 | 23 | | -// | | 19 | | 24 | | -// -------------------------------------------------------------- -// -/// Button map to HID key (four function 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. -/// Hat button 1-4 = HID B21-B24. -pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ - [ - // Function layer 0 - // HID Key // Button Index - // ----------------------------------------- - HidButton::FnL, // 0 - HidButton::B1, // 1 - HidButton::NoEventIndicated, // 2 Not connected to any button - HidButton::FnR, // 3 - HidButton::B6, // 4 - HidButton::B2, // 5 - HidButton::B3, // 6 - HidButton::ModeL, // 7 - HidButton::B4, // 8 - HidButton::B5, // 9 - HidButton::B7, // 10 - HidButton::B8, // 11 - HidButton::ModeR, // 12 - HidButton::B9, // 13 - HidButton::B10, // 14 - HidButton::Hat1B, // 15 button 21 - HidButton::Hat1U, // 16 - HidButton::Hat1R, // 17 - HidButton::Hat1D, // 18 - HidButton::Hat1L, // 19 - HidButton::Hat2B, // 20 button 22 - HidButton::Hat2U, // 21 - HidButton::Hat2R, // 22 - HidButton::Hat2D, // 23 - HidButton::Hat2L, // 24 - ], - [ - // Function layer 1 (left Fn button pressed) - // HID Key // Button Index - // ----------------------------------------- - HidButton::FnL, // 0 - HidButton::B11, // 1 - HidButton::NoEventIndicated, // 2 Not connected to any button - HidButton::FnR, // 3 - HidButton::B6, // 4 - HidButton::B12, // 5 - HidButton::B13, // 6 - HidButton::ModeL, // 7 - HidButton::B14, // 8 - HidButton::B15, // 9 - HidButton::B7, // 10 - HidButton::B8, // 11 - HidButton::ModeR, // 12 - HidButton::B9, // 13 - HidButton::B10, // 14 - HidButton::Hat3B, // 15 button 23 - HidButton::Hat3U, // 16 - HidButton::Hat3R, // 17 - HidButton::Hat3D, // 18 - HidButton::Hat3L, // 19 - HidButton::Hat2B, // 20 button 22 - HidButton::Hat2U, // 21 - HidButton::Hat2R, // 22 - HidButton::Hat2D, // 23 - HidButton::Hat2L, // 24 - ], - [ - // Function layer 2 (right Fn button pressed) - // HID Key // Button Index - // ----------------------------------------- - HidButton::FnL, // 0 - HidButton::B1, // 1 - HidButton::NoEventIndicated, // 2 Not connected to any button - HidButton::FnR, // 3 - HidButton::B16, // 4 - HidButton::B2, // 5 - HidButton::B3, // 6 - HidButton::ModeL, // 7 - HidButton::B4, // 8 - HidButton::B5, // 9 - HidButton::B17, // 10 - HidButton::B18, // 11 - HidButton::ModeR, // 12 - HidButton::B19, // 13 - HidButton::B20, // 14 - HidButton::Hat1B, // 15 button 21 - HidButton::Hat1U, // 16 - HidButton::Hat1R, // 17 - HidButton::Hat1D, // 18 - HidButton::Hat1L, // 19 - HidButton::Hat4B, // 20 button 24 - HidButton::Hat4U, // 21 - HidButton::Hat4R, // 22 - HidButton::Hat4D, // 23 - HidButton::Hat4L, // 24 - ], - [ - // Function layer 3 (left + right Fn button pressed) - // HID Key // Button Index - // ----------------------------------------- - HidButton::FnL, // 0 - HidButton::B11, // 1 - HidButton::NoEventIndicated, // 2 Not connected to any button - HidButton::FnR, // 3 - HidButton::B16, // 4 - HidButton::B12, // 5 - HidButton::B13, // 6 - HidButton::ModeL, // 7 - HidButton::B14, // 8 - HidButton::B15, // 9 - HidButton::B17, // 10 - HidButton::B18, // 11 - HidButton::ModeR, // 12 - HidButton::B19, // 13 - HidButton::B20, // 14 - HidButton::Hat3B, // 15 button 23 - HidButton::Hat3U, // 16 - HidButton::Hat3R, // 17 - HidButton::Hat3D, // 18 - HidButton::Hat3L, // 19 - HidButton::Hat4B, // 20 button 24 - HidButton::Hat4U, // 21 - HidButton::Hat4R, // 22 - HidButton::Hat4D, // 23 - 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 -]; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 057eb15..1cbc06a 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -1,14 +1,78 @@ -//! Project: CMtec CMDR joystick 24 +//! Project: CMtec CMDR joystick 25 //! Date: 2023-08-01 //! Author: Christoffer Martinsson //! Email: cm@cmtec.se //! License: Please refer to LICENSE in root directory +// +// Button index map: +// --------------------------------------------------------------- +// | 0 L| 1 U| | 2 | | 3 L| 4 U| +// --------------------------------------------------------------- +// | | 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: +// --------------------------------------------------------------- +// | B1 L| B2 U| | B3 | | B4 L| B5 U| +// --------------------------------------------------------------- +// | | B6 | B7 | B8/16| |B13/17| B12 | B11 | | +// | | +// | | B9 | | B14 | | +// | | B10 | | B15 | | +// | X1/Y1 X2/Y2 | +// | | H1U | | H2U | | +// | | H1L | B18 | H1R | | H2L | B19 | H2R | | +// | | H1D | | H2D | | +// --------------------------------------------------------------- +// Button (Switch) 7 changes following: +// * hat1 => hat3 (button press B20). +// Button (switch) 12 changes following: +// * B4 => B21 +// * B5 => B22 +// * B14 => B23 +// * B15 => B24 +// * hat2 => hat4 (button bpress B25) +// +// ELRS channel map (+ = ON, - = OFF, CHxP/M/Z = trim) +// --------------------------------------------------------------- +// |CH7-L|CH7+U| | - | |CH8-L|CH8+U| +// --------------------------------------------------------------- +// | | THL | CH9 | CH5 | | CH6 |CH11-|CH11+| | +// | | +// | |CH10+| |CH12+| | +// | |CH10-| |CH12-| | +// | CH1/CH2 CH3/CH4 | +// | | - | |CH4P | | +// | |CH1M |CH12Z|CH1P | |CH3M |CH34Z|CH3P | | +// | | - | |CH4M | | +// --------------------------------------------------------------- +// +// Config Layer (holding CONFIG button) +// --------------------------------------------------------------- +// |BOOT L| CAL U| | CONFIG | | USB L|ELRS U| +// --------------------------------------------------------------- +// | | THL-| THL+| - | | - | - | - | | +// | | +// | | - | | - | | +// | | - | | - | | +// | -/- -/- | +// | | - | | - | | +// | | - | - | - | | - | - | - | | +// | | - | | - | | +// --------------------------------------------------------------- #![no_std] #![no_main] mod button_matrix; -mod layout; +mod elrs; mod status_led; mod usb_joystick_device; @@ -16,16 +80,20 @@ 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 elrs::Elrs; use embedded_hal::adc::OneShot; use embedded_hal::digital::v2::*; use embedded_hal::timer::CountDown; -use fugit::ExtU32; +use fugit::{ExtU32, RateExtU32}; use libm::powf; use panic_halt as _; use rp2040_hal::{ adc::Adc, - gpio::{Function, FunctionConfig, PinId, ValidPinMode}, + gpio::{Function, FunctionConfig, FunctionUart, PinId, ValidPinMode}, + i2c::I2C, pio::StateMachineIndex, + uart::{DataBits, StopBits, UartConfig, UartPeripheral}, }; use status_led::{StatusMode, Ws2812StatusLed}; use usb_device::class_prelude::*; @@ -52,25 +120,49 @@ 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_MAX / 2; +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; 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 fn_mode: u8, + 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, + 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)] @@ -81,9 +173,11 @@ pub struct GimbalAxis { pub max: u16, pub min: u16, pub center: u16, - pub fn_mode: u8, pub deadzone: (u16, u16, u16), - pub expo: f32, + pub expo: bool, + pub trim: i16, + pub hold: u16, + pub hold_pending: bool, } impl Default for GimbalAxis { @@ -95,9 +189,11 @@ impl Default for GimbalAxis { max: AXIS_MAX, min: AXIS_MIN, center: AXIS_CENTER, - fn_mode: 0, - deadzone: (50, 50, 50), - expo: 0.2, + deadzone: (100, 50, 100), + expo: true, + trim: 0, + hold: 0, + hold_pending: false, } } } @@ -136,6 +232,31 @@ 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 + 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); @@ -149,25 +270,28 @@ fn main() -> ! { // Setting up array with pins connected to button rows let button_matrix_row_pins: &[&dyn InputPin; BUTTON_ROWS] = &[ - &pins.gp11.into_pull_up_input(), - &pins.gp13.into_pull_up_input(), - &pins.gp9.into_pull_up_input(), - &pins.gp12.into_pull_up_input(), - &pins.gp10.into_pull_up_input(), + &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; BUTTON_COLS] = &mut [ - &mut pins.gp4.into_push_pull_output(), - &mut pins.gp5.into_push_pull_output(), - &mut pins.gp6.into_push_pull_output(), - &mut pins.gp7.into_push_pull_output(), - &mut pins.gp8.into_push_pull_output(), + &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(), ]; + 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, 5); + ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, DEBOUNCE); // Initialize button matrix button_matrix.init_pins(); @@ -181,9 +305,10 @@ fn main() -> ! { 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 - let mut delay = Delay::new(core.SYST, clocks.system_clock.freq().to_Hz()); for _ in 0..10 { button_matrix.scan_matrix(&mut delay); } @@ -197,48 +322,104 @@ fn main() -> ! { // Create timers let timer = Timer::new(pac.TIMER, &mut pac.RESETS); - let mut usb_hid_report_count_down = timer.count_down(); - usb_hid_report_count_down.start(10.millis()); + 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(1.millis()); + scan_count_down.start(200u32.micros()); - let mut status_led_count_down = timer.count_down(); - status_led_count_down.start(50.millis()); + let mut data_process_count_down = timer.count_down(); + data_process_count_down.start(1200u32.micros()); - let mut elrs_count_down = timer.count_down(); - elrs_count_down.start(1660u32.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 mode: u8 = 0; let mut safety_check: bool = false; - let mut activity: 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: 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 gimbal_mode: u8; - // 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 = (50, 0, 50); - axis[GIMBAL_AXIS_LEFT_Y].expo = 0.0; + // Set up usb button layout + buttons[0].usb_button = 1; + buttons[1].usb_button = 2; + buttons[2].usb_button = 3; + buttons[3].usb_button = 4; + buttons[3].usb_button_sec_enable = true; + buttons[3].usb_button_sec = 21; + buttons[3].usb_button_sec_trigger_index = 12; + buttons[4].usb_button = 5; + buttons[4].usb_button_sec_enable = true; + buttons[4].usb_button_sec = 22; + buttons[4].usb_button_sec_trigger_index = 12; + buttons[5].usb_button = 6; + buttons[6].usb_button = 7; + buttons[7].usb_button = 8; + buttons[7].usb_button_sec = 16; + buttons[7].usb_button_toggle_enable = true; + buttons[8].usb_button = 9; + buttons[9].usb_button = 10; + buttons[10].usb_button = 11; + buttons[11].usb_button = 12; + buttons[12].usb_button = 13; + buttons[12].usb_button_sec = 17; + buttons[12].usb_button_toggle_enable = true; + buttons[13].usb_button = 14; + buttons[13].usb_button_sec_enable = true; + buttons[13].usb_button_sec = 23; + buttons[13].usb_button_sec_trigger_index = 12; + buttons[14].usb_button = 15; + buttons[14].usb_button_sec_enable = true; + buttons[14].usb_button_sec = 24; + buttons[14].usb_button_sec_trigger_index = 12; - // Manual calibation values - // TODO: add external EEPROM and make calibration routine - axis[GIMBAL_AXIS_LEFT_X].center = AXIS_CENTER; - axis[GIMBAL_AXIS_LEFT_X].max = AXIS_MAX - 450; - axis[GIMBAL_AXIS_LEFT_X].min = AXIS_MIN + 500; - axis[GIMBAL_AXIS_LEFT_Y].center = AXIS_CENTER + 105; - axis[GIMBAL_AXIS_LEFT_Y].max = AXIS_MAX - 250; - axis[GIMBAL_AXIS_LEFT_Y].min = AXIS_MIN + 500; - axis[GIMBAL_AXIS_RIGHT_X].center = AXIS_CENTER - 230; - axis[GIMBAL_AXIS_RIGHT_X].max = AXIS_MAX - 700; - axis[GIMBAL_AXIS_RIGHT_X].min = AXIS_MIN + 350; - axis[GIMBAL_AXIS_RIGHT_Y].center = AXIS_CENTER - 68; - axis[GIMBAL_AXIS_RIGHT_Y].max = AXIS_MAX - 700; - axis[GIMBAL_AXIS_RIGHT_Y].min = AXIS_MIN + 450; + // 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; + + // 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 - // TODO: Find a way to store dynamic smoother in the axis struct let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [ DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), @@ -261,38 +442,158 @@ fn main() -> ! { let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x1209, 0x0002)) .manufacturer("CMtec") - .product("CMDR Joystick") + .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 { - // Temporary way to enter bootloader ------------------------- - // TODO: Remove this after testing - 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); + // 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); - // Have not figured out hov to store the adc pins in an array yet - // so we have to read them one by one - // TODO: Find a way to store adc pins in an array - 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()); + 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, + &elrs_active, + &elrs.connected(), + &safety_check, + &calibration_active, + &throttle_hold, + ); + } + + // 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; + } + + // 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 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 { + throttle_hold = true; + axis[GIMBAL_AXIS_LEFT_Y].hold = 0; + } else if buttons[5].pressed && buttons[2].pressed && throttle_hold { + throttle_hold = false; + } + + // 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, @@ -301,41 +602,43 @@ fn main() -> ! { item.center, item.deadzone, item.expo, + &expo_lut, ); } - 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; + // Process throttle hold value + if throttle_hold + && 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 + && 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 && 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 { + axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold; } - // 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) - { + // Set idle state + for key in buttons.iter_mut() { + if key.pressed { idle = false; } } @@ -343,51 +646,65 @@ fn main() -> ! { // Generate led activity when gimbal is moved from idle position for item in axis.iter_mut() { if item.value != item.previous_value { - activity = true; + 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 - // 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) + 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 && index == 5 { - activity = true; + axis[GIMBAL_AXIS_LEFT_Y].hold = axis[GIMBAL_AXIS_LEFT_Y].value; + 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; + } + for button in buttons.iter_mut() { + button.elrs_lock_state = false; + } + } } - if usb_dev.poll(&mut [&mut usb_hid_joystick]) { - usb_active = true; + 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(); + } } - 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, - )) { + // 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) => { @@ -395,15 +712,8 @@ fn main() -> ! { core::panic!("Failed to write joystick report: {:?}", e) } }; + usb_activity = false; } - - // 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 {} } } @@ -422,190 +732,147 @@ fn main() -> ! { /// * `safety_check` - Reference to bool that indicates if safety check has passed fn update_status_led( status_led: &mut Ws2812StatusLed, - activity: &mut bool, usb_active: &bool, - axis_idle: &bool, + elrs_active: &bool, + elrs_connected: &bool, safety_check: &bool, + calibration_active: &bool, + throttle_hold: &bool, ) where P: PIOExt + FunctionConfig, I: PinId, Function

: ValidPinMode, SM: StateMachineIndex, { - if !usb_active && !*safety_check { + if *calibration_active { + status_led.update(StatusMode::ActivityFlash); + } else if *elrs_active && !*safety_check { status_led.update(StatusMode::Warning); - } else if *activity && status_led.get_mode() != StatusMode::Activity { + } else if !*usb_active && !*elrs_active { + status_led.update(StatusMode::NormalFlash); + } else if *usb_active && !*elrs_active && *throttle_hold { status_led.update(StatusMode::Activity); - } 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 { + } else if *usb_active && !*elrs_active && !*throttle_hold { status_led.update(StatusMode::Normal); - } else if status_led.get_mode() != StatusMode::Other { + } else if *elrs_active && *elrs_connected { status_led.update(StatusMode::Other); + } else if *elrs_active && !*elrs_connected { + status_led.update(StatusMode::OtherFlash); } } -/// Get current Fn mode (0, 1, 2 or 3 and alt l/r mode) -/// layout::MAP contains the button types -/// -/// # Arguments -/// -/// * `pressed_keys` - Array of pressed keys -fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { - // Check how many Fn keys are pressed - let mut mode: u8 = 0; - let mut fn_l_active: bool = false; - let mut fn_r_active: 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::HID_MAP[0][index] == layout::HidButton::FnL { - fn_l_active = true; - } - if *key && layout::HID_MAP[0][index] == layout::HidButton::FnR { - fn_r_active = true; - } - if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeL { - alt_l_active = true; - } - if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeR { - alt_r_active = true; - } - } - - if fn_l_active && fn_r_active { - mode = 3; - } else if fn_r_active { - mode = 2; - } else if fn_l_active { - mode = 1; - } - - // Set bit 4 and 5 if alt l/r is active - if alt_l_active { - mode |= 0x10; - } - if alt_r_active { - mode |= 0x20; - } - - mode -} - -/// Generate keyboard report based on pressed keys and Fn mode (0, 1 or 2) +/// Generate keyboard report based on pressed keys and Fn mode (0, 1, 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_mode` - Fn mode (0, 1, 2 or 3) -/// * `alt_l_mode` - Is left alt mode active -/// * `alt_r_mode` - Is right alt mode active fn get_joystick_report( matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], axis: &mut [GimbalAxis; 4], - mode: &u8, ) -> JoystickReport { let mut x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; - let mut y: u16 = axis[GIMBAL_AXIS_RIGHT_Y].value; + let mut y: u16 = AXIS_MAX - 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; - - // Left Alt mode active (bit 4) - // Full range of left gimbal gives half range of joystick axis (center to max) - // Left Fn mode = reversed range (center to min) - if mode & 0x10 == 0x10 - && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 0 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 2) - { - rz = remap( - axis[GIMBAL_AXIS_LEFT_Y].value, - AXIS_MIN, - AXIS_MAX, - AXIS_CENTER, - AXIS_MAX, - ); - } else if mode & 0x10 == 0x10 - && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 1 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 3) - { - rz = AXIS_MAX - - remap( - axis[GIMBAL_AXIS_LEFT_Y].value, - AXIS_MIN, - AXIS_MAX, - AXIS_CENTER, - AXIS_MAX, - ); - } + let rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value; + let (mut hat1, mut hat_button1) = format_hat_value(0); + let (mut hat2, mut hat_button2) = format_hat_value(0); + let (mut hat3, mut hat_button3) = format_hat_value(0); + let (mut hat4, mut hat_button4) = format_hat_value(0); // Right Alt mode active (bit 5) // Right gimbal control third joystick axis when right Fn mode is active - if mode & 0x20 == 0x20 - && (axis[GIMBAL_AXIS_RIGHT_X].fn_mode == 2 || axis[GIMBAL_AXIS_RIGHT_X].fn_mode == 3) - { + if matrix_keys[8].pressed { x = AXIS_CENTER; rx = axis[GIMBAL_AXIS_RIGHT_X].value; - } - if 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; + ry = AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value; } - // 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]; + let mut hat_left: u8 = 0; + let mut hat_right: u8 = 0; for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.pressed - && layout::HID_MAP[key.fn_mode as usize][index] >= layout::HidButton::Hat1U - && layout::HID_MAP[key.fn_mode as usize][index] <= layout::HidButton::Hat4B - { - hats[(layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::Hat1U as usize) - / 5] |= 1 - << ((layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::Hat1U as usize) - - (5 * ((layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::Hat1U as usize) - / 5))); + 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 (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]); + if matrix_keys[7].pressed { + (hat3, hat_button3) = format_hat_value(hat_left); + } else { + (hat1, hat_button1) = format_hat_value(hat_left); + } + if matrix_keys[12].pressed { + (hat4, hat_button4) = format_hat_value(hat_right); + } else { + (hat2, hat_button2) = format_hat_value(hat_right); + } // Update button state for joystick button 21-24 according to hat button 1-4 - let mut buttons: u32 = (hat_button1 as u32) << 20 - | ((hat_button2 as u32) << 21) - | ((hat_button3 as u32) << 22) - | ((hat_button4 as u32) << 23); + let mut buttons: u32 = (hat_button1 as u32) << 17 + | ((hat_button2 as u32) << 18) + | ((hat_button3 as u32) << 19) + | ((hat_button4 as u32) << 24); - // Update button state for joystick button 1-20 - for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.pressed - && layout::HID_MAP[key.fn_mode as usize][index] 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::HID_MAP[key.fn_mode as usize][index] as usize; + // 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 { + 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; + } } + matrix_keys[index].usb_button_sec_pressed = sec_button_pressed; + } + + // Update button state for joystick buttons + 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 { @@ -627,8 +894,8 @@ fn get_joystick_report( /// /// # Arguments /// * `input` - Hat value coded as -/// bit 1-4: direction (U R D L) -/// bit 5: button state +/// 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) { @@ -642,21 +909,21 @@ fn format_hat_value(input: u8) -> (u8, u8) { 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, + 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 & 0x10 == 0x10 && direction == HAT_CENTER { + if input & 0x01 == 0x01 && direction == HAT_CENTER { button_state = 1; } @@ -671,15 +938,24 @@ fn format_hat_value(input: u8) -> (u8, u8) { /// * `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 +/// * `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: f32, + 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) { @@ -700,17 +976,8 @@ fn calculate_axis_value( ); } - 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, - ); + if expo && calibrated_value != AXIS_CENTER { + calibrated_value = expo_lut[calibrated_value as usize]; } calibrated_value @@ -749,3 +1016,163 @@ fn constrain(value: T, out_min: T, out_max: T) -> T { 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 +} + +/// 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/status_led.rs b/rp2040/src/status_led.rs index 65f163f..f5a23ec 100644 --- a/rp2040/src/status_led.rs +++ b/rp2040/src/status_led.rs @@ -25,11 +25,14 @@ use ws2812_pio::Ws2812Direct; pub enum StatusMode { Off = 0, Normal = 1, - Activity = 2, - Other = 3, - Warning = 4, - Error = 5, - Bootloader = 6, + NormalFlash = 2, + Activity = 3, + ActivityFlash = 4, + Other = 5, + OtherFlash = 6, + Warning = 7, + Error = 8, + Bootloader = 9, } #[warn(dead_code)] @@ -91,41 +94,68 @@ where } /// Get current status mode + #[allow(dead_code)] pub fn get_mode(&self) -> StatusMode { self.mode } + #[warn(dead_code)] /// Update status LED /// Depending on the mode, the LED will be set to a different colour /// /// * OFF = off /// * NORMAL = green + /// * NORMALFLASH = green (flashing) /// * ACTIVITY = blue + /// * ACTIVITYFLASH = blue (flashing) /// * OTHER = orange + /// * OTHERFLASH = orange (flashing) /// * WARNING = red (flashing) /// * ERROR = red /// * BOOTLOADER = purple /// /// Make sure to call this function regularly to keep the LED flashing pub fn update(&mut self, mode: StatusMode) { - let colors: [RGB8; 7] = [ + let colors: [RGB8; 10] = [ (0, 0, 0).into(), // Off (10, 7, 0).into(), // Green + (10, 7, 0).into(), // Green (10, 4, 10).into(), // Blue + (10, 4, 10).into(), // Blue + (5, 10, 0).into(), // Orange (5, 10, 0).into(), // Orange (2, 20, 0).into(), // Red (2, 20, 0).into(), // Red (0, 10, 10).into(), // Purple ]; - self.mode = mode; + if mode == StatusMode::Warning + || mode == StatusMode::NormalFlash + || mode == StatusMode::ActivityFlash + || mode == StatusMode::OtherFlash + || mode != self.mode + { + self.mode = mode; + } else { + return; + } - if mode == StatusMode::Warning && !self.state { + if (mode == StatusMode::Warning + || mode == StatusMode::NormalFlash + || mode == StatusMode::ActivityFlash + || mode == StatusMode::OtherFlash) + && !self.state + { self.ws2812_direct .write([colors[mode as usize]].iter().copied()) .unwrap(); self.state = true; - } else if mode == StatusMode::Warning || mode == StatusMode::Off { + } else if mode == StatusMode::Warning + || mode == StatusMode::NormalFlash + || mode == StatusMode::ActivityFlash + || mode == StatusMode::OtherFlash + || mode == StatusMode::Off + { self.ws2812_direct .write([colors[0]].iter().copied()) .unwrap(); diff --git a/rp2040/src/usb_joystick_device.rs b/rp2040/src/usb_joystick_device.rs index 1d81a58..f2793d3 100644 --- a/rp2040/src/usb_joystick_device.rs +++ b/rp2040/src/usb_joystick_device.rs @@ -68,7 +68,7 @@ impl Try for Result { } // Based on example device from https://github.com/dlkj/usbd-human-interface-device/blob/main/src/device/joystick.rs -// Updated to 6pc 12bit axis, 24pc buttons and 4pc hat switches +// Updated to 6x 12bit axis, 25x buttons and 4x hat switches #[rustfmt::skip] pub const JOYSTICK_DESCRIPTOR: &[u8] = &[ 0x05, 0x01, // Usage Page (Generic Desktop) @@ -90,12 +90,15 @@ pub const JOYSTICK_DESCRIPTOR: &[u8] = &[ 0xc0, // End Collection 0x05, 0x09, // Usage Page (Button) 0x19, 0x01, // Usage Minimum (1) - 0x29, 0x18, // Usage Maximum (24) + 0x29, 0x19, // Usage Maximum (25) 0x15, 0x00, // Logical Minimum (0) 0x25, 0x01, // Logical Maximum (1) 0x75, 0x01, // Report Size (1) - 0x95, 0x18, // Report Count (24) - 0x81, 0x02, // Input (Data, Variable, Absolute) + 0x95, 0x19, // Report Count (25) + 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) @@ -120,7 +123,7 @@ pub struct JoystickReport { pub rx: u16, // 12bit pub ry: u16, // 12bit pub rz: u16, // 12bit - pub buttons: u32, // 24bit + pub buttons: u32, // 32bit pub hat1: u8, // 4bit pub hat2: u8, // 4bit pub hat3: u8, // 4bit @@ -133,7 +136,7 @@ pub struct Joystick<'a, B: UsbBus> { impl<'a, B: UsbBus> Joystick<'a, B> { pub fn write_report(&mut self, report: &JoystickReport) -> Result<(), UsbHidError> { - let mut data: [u8; 14] = [0; 14]; + let mut data: [u8; 15] = [0; 15]; // Did not make the packed struct work, so doing it manually // TODO: make this work with packed struct @@ -149,8 +152,9 @@ impl<'a, B: UsbBus> Joystick<'a, B> { data[9] = report.buttons as u8; data[10] = (report.buttons >> 8) as u8; data[11] = (report.buttons >> 16) as u8; - data[12] = (report.hat1) | (report.hat2 << 4); - data[13] = (report.hat3) | (report.hat4 << 4); + data[12] = (report.buttons >> 24) as u8; + data[13] = (report.hat1) | (report.hat2 << 4); + data[14] = (report.hat3) | (report.hat4 << 4); self.interface .write_report(&data)