From 19c5fdbe1bfda90ba96d0b9cf427ececd27075cd Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Tue, 15 Aug 2023 08:20:00 +0000 Subject: [PATCH 01/30] Update README.md --- README.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 5d9c0b2..88cdf2f 100644 --- a/README.md +++ b/README.md @@ -101,11 +101,17 @@ ELRS Layer - 6x Cherry MX switches - 2x Miniature Toggle Switch (M6 shaft, 7mm wide body) - 2x Alpine RKJXM1015004 hat switches -- 1x PCB (prototype board) - 1x Bottom case (3D printed) - 1x Top plate (3D printed) - 2x Hat swith top (3D printed) +### 1x PCB + +![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) + + ## Calibration No calibration needed From 0dd402ec9b776a8e857fe89902f3b3b9468a3421 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Tue, 15 Aug 2023 08:21:10 +0000 Subject: [PATCH 02/30] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 88cdf2f..94ecb6c 100644 --- a/README.md +++ b/README.md @@ -107,7 +107,7 @@ ELRS Layer ### 1x PCB -![pcb_top](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_top.png) ![pcb_bottom](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_bottom.png) +![pcb_top](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_top.png width="120") ![pcb_bottom](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_bottom.png width="120") * Gerber files: [zip](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_gerber.zip) From 41e3a4901e7f7d8ffbf8db8293c45915f98ac197 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Tue, 15 Aug 2023 08:26:43 +0000 Subject: [PATCH 03/30] Update README.md --- README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 94ecb6c..11f020f 100644 --- a/README.md +++ b/README.md @@ -105,11 +105,13 @@ ELRS Layer - 1x Top plate (3D printed) - 2x Hat swith top (3D printed) -### 1x PCB +##### 1x PCB -![pcb_top](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_top.png width="120") ![pcb_bottom](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_bottom.png width="120") +![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) + ## Calibration From acd1b949c84c64062bf5092f3a6c5c1b4c6eff83 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Tue, 15 Aug 2023 08:41:55 +0000 Subject: [PATCH 04/30] Update README.md --- README.md | 45 +++++++++++++++++++++------------------------ 1 file changed, 21 insertions(+), 24 deletions(-) diff --git a/README.md b/README.md index 11f020f..63ca5f9 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,13 @@ -# 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 Joystick with 2 hall effect gimbals, 2 hat switches and 25 buttons for use both with simulators and ELRS Rx equipped quads. ## Layout ```cpp USB Joystick Layer 0 -------------------------------------------------------------- -| FnL | B1 | | B5 | FnR | +| FnL | B1 | | B25 | | B5 | FnR | -------------------------------------------------------------- | | B2 | B3 | MoL | | MoR | B7 | B6 | | | | @@ -21,7 +21,7 @@ USB Joystick Layer 0 USB Joystick Layer 1 (FnL) -------------------------------------------------------------- -| FnL | B9 | | B5 | FnR | +| FnL | B9 | | B25 | | B5 | FnR | -------------------------------------------------------------- | | B10 | B11 | MoL | | MoR | B7 | B6 | | | | @@ -35,7 +35,7 @@ USB Joystick Layer 1 (FnL) USB Joystick Layer 2 (FnR) -------------------------------------------------------------- -| FnL | B1 | | B13 | FnR | +| FnL | B1 | | B25 | | B13 | FnR | -------------------------------------------------------------- | | B2 | B3 | MoL | | MoR | B15 | B14 | | | | @@ -49,7 +49,7 @@ USB Joystick Layer 2 (FnR) USB Joystick Layer 3 (FnL + FnR) -------------------------------------------------------------- -| FnL | B9 | | B13 | FnR | +| FnL | B9 | | B25 | | B13 | FnR | -------------------------------------------------------------- | | B10 | B11 | MoL | | MoR | B15 | B14 | | | | @@ -63,16 +63,16 @@ USB Joystick Layer 3 (FnL + FnR) ELRS Layer -------------------------------------------------------------- -| CH7 | CH8 | | CH9 | CH10| +| CH7 OFF | CH7 ON | | -- | | CH8 ON | CH8 OFF| -------------------------------------------------------------- -| | CH11| - | CH5 | | CH6 | - | CH12| | +| | CH9 ON | CH9 OFF | CH5 | | CH10 OFF | CH10 ON | CH12| | | | -| | - | | - | | -| | - | | - | | +| | CH11 ON | | CH12 ON | | +| | CH11 OFF | | CH12 OFF | | | X(CH1)/Y(CH2) X(CH3)/Y(CH4) | -| | - | | - | | -| | - | - | - || - | - | - | | -| | - | | - | | +| | LTU | | RTU | | +| | LTL | LTC | LTR || RTL | RTC | RTR | | +| | LTD | | RTD | | -------------------------------------------------------------- ``` @@ -81,21 +81,12 @@ 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 @@ -112,7 +103,13 @@ ELRS Layer * Gerber files: [zip](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_gerber.zip) * Schematics: [pdf](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_schematics.pdf) +## Software Build environment +Rust +- Cargo (rust embedded) +- Flashing via Cargo + - Pressing boot button on teensy + - Press and hold "top lower right button" when powering the unit ## Calibration From 29db001fe3d541fbf60ee1d0613364981e941431 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sat, 19 Aug 2023 13:12:06 +0200 Subject: [PATCH 05/30] Added one more Joystick button --- rp2040/src/layout.rs | 308 ++++++++++++++++-------------- rp2040/src/main.rs | 12 +- rp2040/src/usb_joystick_device.rs | 16 +- 3 files changed, 176 insertions(+), 160 deletions(-) diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index ef43076..48deebf 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -37,8 +37,19 @@ pub enum ElrsButton { CH11OFF = 25, CH12ON = 26, CH12OFF = 27, - NoEventIndicated = 28, + XY1TU = 28, + XY1TR = 29, + XY1TD = 30, + XY1TL = 31, + XY1TRESET = 32, + XY2TU = 33, + XY2TR = 34, + XY2TD = 35, + XY2TL = 36, + XY2TRESET = 37, + NoEventIndicated = 38, } +#[allow(dead_code)] #[derive(Debug, PartialEq, PartialOrd, Copy, Clone)] pub enum HidButton { B1 = 0, @@ -61,46 +72,47 @@ pub enum HidButton { 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, + B21 = 20, + FnL = 21, + FnR = 22, + ModeL = 23, + ModeR = 24, + Hat1U = 25, + Hat1R = 26, + Hat1D = 27, + Hat1L = 28, + Hat1B = 29, + Hat2U = 30, + Hat2R = 31, + Hat2D = 32, + Hat2L = 33, + Hat2B = 34, + Hat3U = 35, + Hat3R = 36, + Hat3D = 37, + Hat3L = 38, + Hat3B = 39, + Hat4U = 40, + Hat4R = 41, + Hat4D = 42, + Hat4L = 43, + Hat4B = 44, + NoEventIndicated = 45, } #[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 | | -// -------------------------------------------------------------- +// | 0 | 1 | | 2 | | 3 | 4 | +// --------------------------------------------------------------- +// | | 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 @@ -111,121 +123,121 @@ 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 + HidButton::FnL, // 0 + HidButton::B1, // 1 + HidButton::B21, // 2 + 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 22 + HidButton::Hat1U, // 16 + HidButton::Hat1R, // 17 + HidButton::Hat1D, // 18 + HidButton::Hat1L, // 19 + HidButton::Hat2B, // 20 button 23 + 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 + HidButton::FnL, // 0 + HidButton::B11, // 1 + HidButton::B21, // 2 + 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 24 + HidButton::Hat3U, // 16 + HidButton::Hat3R, // 17 + HidButton::Hat3D, // 18 + HidButton::Hat3L, // 19 + HidButton::Hat2B, // 20 button 23 + 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 + HidButton::FnL, // 0 + HidButton::B1, // 1 + HidButton::B21, // 2 + 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 22 + HidButton::Hat1U, // 16 + HidButton::Hat1R, // 17 + HidButton::Hat1D, // 18 + HidButton::Hat1L, // 19 + HidButton::Hat4B, // 20 button 25 + 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 + HidButton::FnL, // 0 + HidButton::B11, // 1 + HidButton::B21, // 2 + 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 24 + HidButton::Hat3U, // 16 + HidButton::Hat3R, // 17 + HidButton::Hat3D, // 18 + HidButton::Hat3L, // 19 + HidButton::Hat4B, // 20 button 25 + HidButton::Hat4U, // 21 + HidButton::Hat4R, // 22 + HidButton::Hat4D, // 23 + HidButton::Hat4L, // 24 ], ]; @@ -235,7 +247,7 @@ pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ // ----------------------------------------- ElrsButton::CH7OFF, // 0 ElrsButton::CH7ON, // 1 - ElrsButton::NoEventIndicated, // 2 Not connected to any button + ElrsButton::NoEventIndicated, // 2 ElrsButton::CH8OFF, // 3 ElrsButton::CH8ON, // 4 ElrsButton::CH9ON, // 5 @@ -248,14 +260,14 @@ pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ 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 + ElrsButton::XY1TRESET, // 15 + ElrsButton::XY1TU, // 16 + ElrsButton::XY1TR, // 17 + ElrsButton::XY1TD, // 18 + ElrsButton::XY1TL, // 19 + ElrsButton::XY2TRESET, // 20 + ElrsButton::XY2TU, // 21 + ElrsButton::XY2TR, // 22 + ElrsButton::XY2TD, // 23 + ElrsButton::XY2TL, // 24 ]; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 057eb15..cd4fada 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -496,7 +496,7 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { 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 @@ -591,10 +591,10 @@ fn get_joystick_report( let (hat4, hat_button4) = format_hat_value(hats[3]); // 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) << 21 + | ((hat_button2 as u32) << 22) + | ((hat_button3 as u32) << 23) + | ((hat_button4 as u32) << 24); // Update button state for joystick button 1-20 for (index, key) in matrix_keys.iter_mut().enumerate() { @@ -602,7 +602,7 @@ fn get_joystick_report( && 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 + <= layout::HidButton::B21 as usize { buttons |= 1 << layout::HID_MAP[key.fn_mode as usize][index] as usize; } diff --git a/rp2040/src/usb_joystick_device.rs b/rp2040/src/usb_joystick_device.rs index 1d81a58..348153f 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) + 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) @@ -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) From 9218f0a6a19451007ffa5cb67f4f9a48f0370cb5 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Fri, 25 Aug 2023 09:38:49 +0200 Subject: [PATCH 06/30] Lowered latency in button scan function --- rp2040/src/button_matrix.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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); } } From d59cbfa7b09e6adadff1172f285c81e2e62fb757 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Fri, 25 Aug 2023 09:39:23 +0200 Subject: [PATCH 07/30] Added ELRS support --- rp2040/src/elrs.rs | 131 ++++++++++++++++++++++++++++++ rp2040/src/layout.rs | 188 +++++++++++++++++++++---------------------- 2 files changed, 225 insertions(+), 94 deletions(-) create mode 100644 rp2040/src/elrs.rs diff --git a/rp2040/src/elrs.rs b/rp2040/src/elrs.rs new file mode 100644 index 0000000..6807deb --- /dev/null +++ b/rp2040/src/elrs.rs @@ -0,0 +1,131 @@ +//! 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 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, +]; + +pub struct Elrs +where + S: State, + D: UartDevice, + P: ValidUartPinout, +{ + uart: UartPeripheral, + elsr_init_done: bool, + elrs_init_counter: u16, +} + +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; + Self { + uart, + elsr_init_done, + elrs_init_counter, + } + } +} + +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 < 500 { + self.uart + .write_full_blocking(&self.prepare_crsf_data_packet(data)); + self.elrs_init_counter += 1; + } else if self.elrs_init_counter < 505 { + self.uart + .write_full_blocking(&self.prepare_crsf_cmd_packet(0x01, 0x00)); + self.elrs_init_counter += 1; + } else if self.elrs_init_counter < 510 { + self.uart + .write_full_blocking(&self.prepare_crsf_cmd_packet(0x06, 0x00)); + self.elrs_init_counter += 1; + } else { + self.elsr_init_done = true; + } + } + + 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 index 48deebf..8b67f80 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -22,31 +22,31 @@ pub enum ElrsButton { 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, + CH6ON = 13, + CH7ON = 14, + CH8ON = 15, + CH9ON = 16, + CH10ON = 17, + CH11ON = 18, + CH12ON = 19, + CH5OFF = 20, + CH6OFF = 21, + CH7OFF = 22, + CH8OFF = 23, + CH9OFF = 24, + CH10OFF = 25, + CH11OFF = 26, CH12OFF = 27, - XY1TU = 28, - XY1TR = 29, - XY1TD = 30, - XY1TL = 31, - XY1TRESET = 32, - XY2TU = 33, - XY2TR = 34, - XY2TD = 35, - XY2TL = 36, - XY2TRESET = 37, + CH1P = 28, + CH2P = 29, + CH3P = 30, + CH4P = 31, + CH1M = 32, + CH2M = 33, + CH3M = 34, + CH4M = 35, + CH12Z = 36, + CH34Z = 37, NoEventIndicated = 38, } #[allow(dead_code)] @@ -77,26 +77,26 @@ pub enum HidButton { FnR = 22, ModeL = 23, ModeR = 24, - Hat1U = 25, - Hat1R = 26, - Hat1D = 27, - Hat1L = 28, - Hat1B = 29, - Hat2U = 30, - Hat2R = 31, - Hat2D = 32, - Hat2L = 33, - Hat2B = 34, - Hat3U = 35, - Hat3R = 36, - Hat3D = 37, - Hat3L = 38, - Hat3B = 39, - Hat4U = 40, - Hat4R = 41, - Hat4D = 42, - Hat4L = 43, - Hat4B = 44, + H1U = 25, + H1R = 26, + H1D = 27, + H1L = 28, + H1B = 29, + H2U = 30, + H2R = 31, + H2D = 32, + H2L = 33, + H2B = 34, + H3U = 35, + H3R = 36, + H3D = 37, + H3L = 38, + H3B = 39, + H4U = 40, + H4R = 41, + H4D = 42, + H4L = 43, + H4B = 44, NoEventIndicated = 45, } #[warn(dead_code)] @@ -138,16 +138,16 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B9, // 13 HidButton::B10, // 14 - HidButton::Hat1B, // 15 button 22 - HidButton::Hat1U, // 16 - HidButton::Hat1R, // 17 - HidButton::Hat1D, // 18 - HidButton::Hat1L, // 19 - HidButton::Hat2B, // 20 button 23 - HidButton::Hat2U, // 21 - HidButton::Hat2R, // 22 - HidButton::Hat2D, // 23 - HidButton::Hat2L, // 24 + HidButton::H1B, // 15 button 22 + HidButton::H1U, // 16 + HidButton::H1R, // 17 + HidButton::H1D, // 18 + HidButton::H1L, // 19 + HidButton::H2B, // 20 button 23 + HidButton::H2U, // 21 + HidButton::H2R, // 22 + HidButton::H2D, // 23 + HidButton::H2L, // 24 ], [ // Function layer 1 (left Fn button pressed) @@ -168,16 +168,16 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B9, // 13 HidButton::B10, // 14 - HidButton::Hat3B, // 15 button 24 - HidButton::Hat3U, // 16 - HidButton::Hat3R, // 17 - HidButton::Hat3D, // 18 - HidButton::Hat3L, // 19 - HidButton::Hat2B, // 20 button 23 - HidButton::Hat2U, // 21 - HidButton::Hat2R, // 22 - HidButton::Hat2D, // 23 - HidButton::Hat2L, // 24 + HidButton::H3B, // 15 button 24 + HidButton::H3U, // 16 + HidButton::H3R, // 17 + HidButton::H3D, // 18 + HidButton::H3L, // 19 + HidButton::H2B, // 20 button 23 + HidButton::H2U, // 21 + HidButton::H2R, // 22 + HidButton::H2D, // 23 + HidButton::H2L, // 24 ], [ // Function layer 2 (right Fn button pressed) @@ -198,16 +198,16 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B19, // 13 HidButton::B20, // 14 - HidButton::Hat1B, // 15 button 22 - HidButton::Hat1U, // 16 - HidButton::Hat1R, // 17 - HidButton::Hat1D, // 18 - HidButton::Hat1L, // 19 - HidButton::Hat4B, // 20 button 25 - HidButton::Hat4U, // 21 - HidButton::Hat4R, // 22 - HidButton::Hat4D, // 23 - HidButton::Hat4L, // 24 + HidButton::H1B, // 15 button 22 + HidButton::H1U, // 16 + HidButton::H1R, // 17 + HidButton::H1D, // 18 + HidButton::H1L, // 19 + HidButton::H4B, // 20 button 25 + HidButton::H4U, // 21 + HidButton::H4R, // 22 + HidButton::H4D, // 23 + HidButton::H4L, // 24 ], [ // Function layer 3 (left + right Fn button pressed) @@ -228,16 +228,16 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B19, // 13 HidButton::B20, // 14 - HidButton::Hat3B, // 15 button 24 - HidButton::Hat3U, // 16 - HidButton::Hat3R, // 17 - HidButton::Hat3D, // 18 - HidButton::Hat3L, // 19 - HidButton::Hat4B, // 20 button 25 - HidButton::Hat4U, // 21 - HidButton::Hat4R, // 22 - HidButton::Hat4D, // 23 - HidButton::Hat4L, // 24 + HidButton::H3B, // 15 button 24 + HidButton::H3U, // 16 + HidButton::H3R, // 17 + HidButton::H3D, // 18 + HidButton::H3L, // 19 + HidButton::H4B, // 20 button 25 + HidButton::H4U, // 21 + HidButton::H4R, // 22 + HidButton::H4D, // 23 + HidButton::H4L, // 24 ], ]; @@ -260,14 +260,14 @@ pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ ElrsButton::CH6, // 12 ElrsButton::CH12ON, // 13 ElrsButton::CH12OFF, // 14 - ElrsButton::XY1TRESET, // 15 - ElrsButton::XY1TU, // 16 - ElrsButton::XY1TR, // 17 - ElrsButton::XY1TD, // 18 - ElrsButton::XY1TL, // 19 - ElrsButton::XY2TRESET, // 20 - ElrsButton::XY2TU, // 21 - ElrsButton::XY2TR, // 22 - ElrsButton::XY2TD, // 23 - ElrsButton::XY2TL, // 24 + ElrsButton::CH12Z, // 15 + ElrsButton::CH2P, // 16 + ElrsButton::CH1P, // 17 + ElrsButton::CH2M, // 18 + ElrsButton::CH1M, // 19 + ElrsButton::CH34Z, // 20 + ElrsButton::CH4P, // 21 + ElrsButton::CH3P, // 22 + ElrsButton::CH4M, // 23 + ElrsButton::CH3M, // 24 ]; From 555645fc544b1b0d342c9e01162fe8ec8f7b3c73 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Fri, 25 Aug 2023 09:40:17 +0200 Subject: [PATCH 08/30] Added ELRS support. Added EEPROM calibration data (not yet activated). Code cleanup --- README.md | 31 ++- rp2040/Cargo.toml | 1 + rp2040/src/main.rs | 614 ++++++++++++++++++++++++++++++++++----------- 3 files changed, 490 insertions(+), 156 deletions(-) diff --git a/README.md b/README.md index 63ca5f9..12633e9 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,7 @@ RC Joystick with 2 hall effect gimbals, 2 hat switches and 25 buttons for use bo ```cpp USB Joystick Layer 0 -------------------------------------------------------------- -| FnL | B1 | | B25 | | B5 | FnR | +| FnL | B1 | | B21 | | B5 | FnR | -------------------------------------------------------------- | | B2 | B3 | MoL | | MoR | B7 | B6 | | | | @@ -21,7 +21,7 @@ USB Joystick Layer 0 USB Joystick Layer 1 (FnL) -------------------------------------------------------------- -| FnL | B9 | | B25 | | B5 | FnR | +| FnL | B9 | | B21 | | B5 | FnR | -------------------------------------------------------------- | | B10 | B11 | MoL | | MoR | B7 | B6 | | | | @@ -35,7 +35,7 @@ USB Joystick Layer 1 (FnL) USB Joystick Layer 2 (FnR) -------------------------------------------------------------- -| FnL | B1 | | B25 | | B13 | FnR | +| FnL | B1 | | B21 | | B13 | FnR | -------------------------------------------------------------- | | B2 | B3 | MoL | | MoR | B15 | B14 | | | | @@ -49,7 +49,7 @@ USB Joystick Layer 2 (FnR) USB Joystick Layer 3 (FnL + FnR) -------------------------------------------------------------- -| FnL | B9 | | B25 | | B13 | FnR | +| FnL | B9 | | B21 | | B13 | FnR | -------------------------------------------------------------- | | B10 | B11 | MoL | | MoR | B15 | B14 | | | | @@ -100,8 +100,17 @@ ELRS Layer ![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) +- 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: ![rp2040zero_pinout](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) + +##### 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](https://github.com/MUSTARDTIGERFPV/rx-as-tx#flashing) ## Software Build environment Rust @@ -111,6 +120,14 @@ Rust - Pressing boot button on teensy - Press and hold "top lower right button" when powering the unit +- 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 all righ hand side buttons except hat switch. Status led will start blinking green. +3. Move both gimbals to all corners. +4. Press right hat switch to save calibration data to eeprom. + +Done! 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/main.rs b/rp2040/src/main.rs index cd4fada..bee0c80 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -8,6 +8,7 @@ #![no_main] mod button_matrix; +mod elrs; mod layout; mod status_led; mod usb_joystick_device; @@ -16,16 +17,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,13 +57,19 @@ 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; @@ -83,7 +94,8 @@ pub struct GimbalAxis { pub center: u16, pub fn_mode: u8, pub deadzone: (u16, u16, u16), - pub expo: f32, + pub expo: bool, + pub trim: i16, } impl Default for GimbalAxis { @@ -96,8 +108,9 @@ impl Default for GimbalAxis { min: AXIS_MIN, center: AXIS_CENTER, fn_mode: 0, - deadzone: (50, 50, 50), - expo: 0.2, + deadzone: (100, 50, 100), + expo: true, + trim: 0, } } } @@ -136,6 +149,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 mut 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::default(); + let mut eeprom = Eeprom24x::new_24x02(i2c, i2c_address); + // Enable adc let mut adc = Adc::new(pac.ADC, &mut pac.RESETS); @@ -165,6 +203,9 @@ fn main() -> ! { &mut pins.gp8.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); @@ -181,9 +222,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); } @@ -201,44 +243,39 @@ fn main() -> ! { usb_hid_report_count_down.start(10.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 elrs_count_down = timer.count_down(); - elrs_count_down.start(1660u32.micros()); + let mut main_count_down = timer.count_down(); + main_count_down.start(1660u32.micros()); + + let mut elrs_start_count_down = timer.count_down(); + elrs_start_count_down.start(2000.millis()); let mut mode: u8 = 0; let mut safety_check: bool = false; let mut activity: bool = false; let mut idle: bool = false; + let mut usb_active: bool = false; + let mut elrs_active: bool = false; + let mut calibration_active: 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 channel_locks: [bool; 12] = [false; 12]; + let mut gimbal_mode: u8 = GIMBAL_MODE_M10; + + let expo_lut: [u16; AXIS_MAX as usize + 1] = generate_expo_lut(0.3); // 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; - - // 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; + axis[GIMBAL_AXIS_LEFT_Y].deadzone = (100, 0, 100); + axis[GIMBAL_AXIS_LEFT_Y].expo = false; // 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,113 +298,59 @@ 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 + // if !calibration_active { + // for (index, item) in axis.iter_mut().enumerate() { + // item.min = eeprom.read_byte((index as u32 * 6) + 1).unwrap() as u16; + // item.min <<= 8; + // item.min |= eeprom.read_byte(index as u32 * 6).unwrap() as u16; + // item.max = eeprom.read_byte((index as u32 * 6) + 3).unwrap() as u16; + // item.max <<= 8; + // item.max = eeprom.read_byte((index as u32 * 6) + 2).unwrap() as u16; + // item.center = eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16; + // item.center <<= 8; + // item.center = eeprom.read_byte((index as u32 * 6) + 4).unwrap() as u16; + // } + // gimbal_mode = eeprom.read_byte(24).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; + } + + // Power up ELRS TX + if elrs_start_count_down.wait().is_ok() { + elrs_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(); - for (index, item) in axis.iter_mut().enumerate() { - item.value = calculate_axis_value( - smoother[index].value() as u16, - item.min, - item.max, - item.center, - item.deadzone, - item.expo, - ); + if 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; } - let pressed_keys = button_matrix.buttons_pressed(); - mode = get_mode(pressed_keys); - - // Update pressed keys status - for (index, key) in pressed_keys.iter().enumerate() { - buttons[index].pressed = *key; - } - - // Update Fn mode for all axis that are in idle position - // This is to avoid the Fn mode switching when moving the gimbal - idle = true; - for item in axis.iter_mut() { - if item.value == item.idle_value { - item.fn_mode = mode & 0x0F; - } else { - idle = false; - } - } - - // Set fn mode for all keys taht are in idle position - // This is to avoid the Fn mode switching when using a button - for (index, key) in buttons.iter_mut().enumerate() { - if !key.pressed { - key.fn_mode = mode & 0x0F; - } else if (usb_active - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnL - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnR - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeL - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR) - || (!usb_active - && layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated) - { - idle = false; - } - } - - // Generate led activity when gimbal is moved from idle position - for item in axis.iter_mut() { - if item.value != item.previous_value { - activity = true; - } - item.previous_value = item.value; - } - - // Generate led activity when a button is pressed - // FnL, FnR, and ModeR are excluded - for (index, key) in buttons.iter_mut().enumerate() { - if (usb_active - && key.pressed != key.previous_pressed - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnL - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnR - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR) - || (!usb_active - && key.pressed != key.previous_pressed - && layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated) - { - activity = true; - } - key.previous_pressed = key.pressed; - } - } - - if usb_dev.poll(&mut [&mut usb_hid_joystick]) { - usb_active = true; + 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() { @@ -375,14 +358,16 @@ fn main() -> ! { &mut status_led, &mut activity, &usb_active, + &elrs_active, &idle, &safety_check, + &calibration_active, ); } + // Dont send USB HID joystick report if there is no activity + // This is to avoid preventing the computer from going to sleep 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, @@ -398,12 +383,184 @@ fn main() -> ! { } // Check if all axis are in idle position and no buttons are pressed - if idle && !safety_check && !usb_active { + if idle && !safety_check && elrs_active { safety_check = true; } - // TODO: Implement ELRS - if elrs_count_down.wait().is_ok() && !usb_active && safety_check {} + if main_count_down.wait().is_ok() { + // Secondary way to enter bootloader (pressing all left hands buttons except the hat + 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, + ); + } + + // Calibration of center position (pressing all right hands buttons except the hat + // switch) + if button_matrix.buttons_pressed()[3] + && button_matrix.buttons_pressed()[4] + && button_matrix.buttons_pressed()[10] + && button_matrix.buttons_pressed()[11] + && button_matrix.buttons_pressed()[13] + && button_matrix.buttons_pressed()[14] + { + 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 && button_matrix.buttons_pressed()[8] { + 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 && button_matrix.buttons_pressed()[9] { + 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 && button_matrix.buttons_pressed()[20] { + // for (index, item) in axis.iter_mut().enumerate() { + // let _ = eeprom.write_byte(index as u32 * 6, item.min as u8); + // let _ = eeprom.write_byte((index as u32 * 6) + 1, (item.min >> 8) as u8); + // let _ = eeprom.write_byte((index as u32 * 6) + 2, item.max as u8); + // let _ = eeprom.write_byte((index as u32 * 6) + 3, (item.max >> 8) as u8); + // let _ = eeprom.write_byte((index as u32 * 6) + 4, item.center as u8); + // let _ = eeprom.write_byte((index as u32 * 6) + 5, (item.center >> 8) as u8); + // } + // let _ = eeprom.write_byte(24, gimbal_mode); + calibration_active = false; + } + + // Process axis values + for (index, item) in axis.iter_mut().enumerate() { + item.value = calculate_axis_value( + smoother[index].value() as u16, + item.min, + item.max, + item.center, + item.deadzone, + item.expo, + &expo_lut, + ); + } + + // Update pressed keys status + for (index, key) in button_matrix.buttons_pressed().iter().enumerate() { + buttons[index].pressed = *key; + } + + // Update Fn mode for all axis that are in idle position + // This is to avoid the Fn mode switching when moving the gimbal + mode = get_mode(button_matrix.buttons_pressed()); + 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 that 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) + || (elrs_active + && layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated) + { + idle = false; + } + } + + // Generate led activity when gimbal is moved from idle position + for item in axis.iter_mut() { + if item.value != item.previous_value { + activity = true; + } + } + + // 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) + || (elrs_active + && key.pressed != key.previous_pressed + && layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated) + { + activity = true; + } + } + + // Reset channel locks when calibration is active + if calibration_active { + for lock_active in channel_locks.iter_mut() { + *lock_active = false; + } + } + + // Send ELRS data + if elrs_active { + elrs_en_pin.set_high().unwrap(); + elrs.send(get_elrs_channels( + &mut buttons, + &mut axis, + &mut channel_locks, + elrs_active, + )); + } else { + elrs_en_pin.set_low().unwrap(); + } + + // Clear axis status + for item in axis.iter_mut() { + item.previous_value = item.value; + } + // Clear key status + for key in buttons.iter_mut() { + key.previous_pressed = key.pressed; + } + } } } @@ -424,26 +581,42 @@ fn update_status_led( status_led: &mut Ws2812StatusLed, activity: &mut bool, usb_active: &bool, + elrs_active: &bool, axis_idle: &bool, safety_check: &bool, + calibration_active: &bool, ) where P: PIOExt + FunctionConfig, I: PinId, Function

: ValidPinMode, SM: StateMachineIndex, { - if !usb_active && !*safety_check { + // If calibration is active, flash the status LED green + if *calibration_active && status_led.get_mode() == StatusMode::Normal { + status_led.update(StatusMode::Off); + } else if *calibration_active && status_led.get_mode() != StatusMode::Normal { + status_led.update(StatusMode::Normal); + // If in ELRS mode and safety chack failed, flash status LED red + } else if *elrs_active && !*safety_check { status_led.update(StatusMode::Warning); + // If activity occurs, flash status LED blue } else if *activity && status_led.get_mode() != StatusMode::Activity { status_led.update(StatusMode::Activity); } else if *activity && status_led.get_mode() == StatusMode::Activity { status_led.update(StatusMode::Off); *activity = false; + // If no activity but not in idle position, turn status LED steady blue } 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 device idle in USB mode, turn status LED steady green + } else if *axis_idle + && *usb_active + && !*elrs_active + && status_led.get_mode() != StatusMode::Normal + { status_led.update(StatusMode::Normal); - } else if status_led.get_mode() != StatusMode::Other { + // Else device idle in ELRS mode, turn status LED steady orange + } else if *axis_idle && *elrs_active && status_led.get_mode() != StatusMode::Other { status_led.update(StatusMode::Other); } } @@ -570,16 +743,16 @@ fn get_joystick_report( let mut hats: [u8; 4] = [0; 4]; 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 + && layout::HID_MAP[key.fn_mode as usize][index] >= layout::HidButton::H1U + && layout::HID_MAP[key.fn_mode as usize][index] <= layout::HidButton::H4B { hats[(layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::Hat1U as usize) + - layout::HidButton::H1U as usize) / 5] |= 1 << ((layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::Hat1U as usize) + - layout::HidButton::H1U as usize) - (5 * ((layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::Hat1U as usize) + - layout::HidButton::H1U as usize) / 5))); } } @@ -671,15 +844,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 +882,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 +922,146 @@ 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], + channel_locks: &mut [bool; 12], + elrs_active: bool, +) -> [u16; 12] { + let mut channels: [u16; 12] = [ELRS_MIN; 12]; + + // Check and store trim values + let mut trim_active = false; + for (index, key) in matrix_keys.iter_mut().enumerate() { + if elrs_active + && key.pressed + && key.pressed != key.previous_pressed + && layout::ELRS_MAP[index] >= layout::ElrsButton::CH1P + && layout::ELRS_MAP[index] <= layout::ElrsButton::CH4P + && axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim + < ELRS_CENTER as i16 + { + axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim += 1; + trim_active = true; + } else if elrs_active + && key.pressed + && key.pressed != key.previous_pressed + && layout::ELRS_MAP[index] >= layout::ElrsButton::CH1M + && layout::ELRS_MAP[index] <= layout::ElrsButton::CH4M + && axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim + > (0 - ELRS_CENTER as i16) + { + axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim -= 1; + trim_active = true; + } + } + + // Check and reser trim values + for (index, key) in matrix_keys.iter_mut().enumerate() { + if elrs_active + && !trim_active + && key.pressed + && key.pressed != key.previous_pressed + && layout::ELRS_MAP[index] == layout::ElrsButton::CH12Z + { + axis[GIMBAL_AXIS_LEFT_X].trim = 0; + axis[GIMBAL_AXIS_LEFT_Y].trim = 0; + } else if elrs_active + && !trim_active + && key.pressed + && key.pressed != key.previous_pressed + && layout::ELRS_MAP[index] == layout::ElrsButton::CH34Z + { + 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 locking button state for ELRS channel 5-12 + for (index, key) in matrix_keys.iter_mut().enumerate() { + if key.pressed + && layout::ELRS_MAP[index] as usize >= layout::ElrsButton::CH5ON as usize + && layout::ELRS_MAP[index] as usize <= layout::ElrsButton::CH12ON as usize + { + channel_locks + [layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH5ON as usize + 4] = true; + } + if key.pressed + && layout::ELRS_MAP[index] as usize >= layout::ElrsButton::CH5OFF as usize + && layout::ELRS_MAP[index] as usize <= layout::ElrsButton::CH12OFF as usize + { + channel_locks + [layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH5OFF as usize + 4] = + false; + } + } + + // Update button state for ELRS channel 5-12 + for (index, key) in matrix_keys.iter_mut().enumerate() { + if key.pressed + && layout::ELRS_MAP[index] as usize >= layout::ElrsButton::CH5 as usize + && layout::ELRS_MAP[index] as usize <= layout::ElrsButton::CH12 as usize + { + channels[layout::ELRS_MAP[index] as usize] = ELRS_MAX; + } + } + + // Apply locking to ELRS channel 5-12 + for (index, lock_active) in channel_locks.iter().enumerate() { + if *lock_active { + channels[index] = ELRS_MAX; + } + } + + channels +} From 0136c62c2c030309672c762fd41a95a739218f6b Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Fri, 25 Aug 2023 09:50:37 +0200 Subject: [PATCH 09/30] Updated readme --- README.md | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 12633e9..20e7e0b 100644 --- a/README.md +++ b/README.md @@ -102,9 +102,13 @@ ELRS Layer - 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: ![rp2040zero_pinout](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) - + * 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 From 360ed30578871d119f57445c39a8873d33c081cb Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Fri, 25 Aug 2023 09:51:38 +0200 Subject: [PATCH 10/30] Updated readme --- README.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 20e7e0b..cd97ffc 100644 --- a/README.md +++ b/README.md @@ -102,12 +102,12 @@ ELRS Layer - 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) + - 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 From c951a3b85808e8eeff3385d616959703182d7757 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Fri, 25 Aug 2023 09:58:48 +0200 Subject: [PATCH 11/30] Updated readme --- README.md | 28 ++++++++++++---------------- 1 file changed, 12 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index cd97ffc..3497528 100644 --- a/README.md +++ b/README.md @@ -95,26 +95,22 @@ ELRS Layer - 1x Bottom case (3D printed) - 1x Top plate (3D printed) - 2x Hat swith top (3D printed) - -##### 1x PCB +- 1x PCB ![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) + - 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](https://github.com/MUSTARDTIGERFPV/rx-as-tx#flashing) +- 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](https://github.com/MUSTARDTIGERFPV/rx-as-tx#flashing) ## Software Build environment Rust From b7f064489d9305aa44211065f5d939147315bb46 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Fri, 25 Aug 2023 10:00:26 +0200 Subject: [PATCH 12/30] Updated readme --- README.md | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/README.md b/README.md index 3497528..6937d61 100644 --- a/README.md +++ b/README.md @@ -96,9 +96,7 @@ ELRS Layer - 1x Top plate (3D printed) - 2x Hat swith top (3D printed) - 1x PCB - -![pcb_top](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_top.png) ![pcb_bottom](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_bottom.png) - + - ![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) From 2151703d950e31b0620c5eb394022f5934afffe5 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Fri, 25 Aug 2023 10:17:12 +0200 Subject: [PATCH 13/30] Updated readme --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 6937d61..cc1bae1 100644 --- a/README.md +++ b/README.md @@ -87,11 +87,11 @@ ELRS Layer ## Hardware -- 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 +- 2x FrSky M7 or M10 gimbals: [M7 datasheed](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, 7mm wide body) [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) From d7587184401277139cc2afb393aad8edbcc15af6 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Tue, 29 Aug 2023 19:00:33 +0200 Subject: [PATCH 14/30] Updated readme --- README.md | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index cc1bae1..f88bda8 100644 --- a/README.md +++ b/README.md @@ -90,12 +90,12 @@ ELRS Layer - 2x FrSky M7 or M10 gimbals: [M7 datasheed](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, 7mm wide body) [Apem 5000 series](https://www.farnell.com/datasheets/2626614.pdf?_ga=2.22934718.461231604.1570510103-1672862477.1542183430) +- 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) -- 1x PCB +- 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) @@ -108,7 +108,15 @@ ELRS Layer - 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](https://github.com/MUSTARDTIGERFPV/rx-as-tx#flashing) + - 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 json settings: + ´´´ + {"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 From 67fa403316faa1c4dc6549d8619d64c58a76a02a Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Tue, 29 Aug 2023 19:01:21 +0200 Subject: [PATCH 15/30] Updated readme --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index f88bda8..a9eed87 100644 --- a/README.md +++ b/README.md @@ -114,9 +114,9 @@ ELRS Layer 3. Device = Generic ESP32 2.4GHz Gemini TX 4. Flash to device 5. Use following json settings: - ´´´ + ``` {"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 From b9320245a85834eaeca71b44e54d9d778dd66107 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Wed, 30 Aug 2023 07:24:58 +0200 Subject: [PATCH 16/30] Code cleanup --- rp2040/src/layout.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index 8b67f80..4749c28 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -13,7 +13,7 @@ pub enum ElrsButton { CH2 = 1, CH3 = 2, CH4 = 3, - CH5 = 4, + CH5 = 4, // AUX1/ARM CH6 = 5, CH7 = 6, CH8 = 7, From d44e16d7faffc9fd7beadb9c5acb374b01a768b9 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Wed, 30 Aug 2023 07:25:14 +0200 Subject: [PATCH 17/30] Updated readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index a9eed87..0abf8f8 100644 --- a/README.md +++ b/README.md @@ -113,7 +113,7 @@ ELRS Layer 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 json settings: + 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} ``` From 3c2ab2c5e8b45618bd3be35c182a8505b832b7ef Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Wed, 30 Aug 2023 07:26:29 +0200 Subject: [PATCH 18/30] Added flashing status mode to status led --- rp2040/src/status_led.rs | 43 +++++++++++++++++++++++++++++++--------- 1 file changed, 34 insertions(+), 9 deletions(-) diff --git a/rp2040/src/status_led.rs b/rp2040/src/status_led.rs index 65f163f..5108008 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)] @@ -108,24 +111,46 @@ where /// /// 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(); From f2f40a2a7e4a5063651db3d037000a5a9b515921 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Wed, 30 Aug 2023 07:27:14 +0200 Subject: [PATCH 19/30] Updated ELRS support. Code cleanup --- rp2040/src/elrs.rs | 106 ++++++++++++++++- rp2040/src/main.rs | 285 ++++++++++++++++++++++----------------------- 2 files changed, 241 insertions(+), 150 deletions(-) diff --git a/rp2040/src/elrs.rs b/rp2040/src/elrs.rs index 6807deb..8f8e571 100644 --- a/rp2040/src/elrs.rs +++ b/rp2040/src/elrs.rs @@ -34,6 +34,22 @@ where uart: UartPeripheral, elsr_init_done: bool, elrs_init_counter: u16, + elrs_connected: bool, + elrs_connected_timeout: u16, + uplink_rssi_1: u8, + uplink_rssi_2: u8, + uplink_package_success: u8, + uplink_snr: i8, + diversity: u8, + rf_mode: u8, + uplink_tx_power: u8, + downlink_rssi: u8, + downlink_package_success: u8, + downlink_snr: i8, + rx_buffer: [u8; 64], + rx_buffer_index: usize, + rx_sync: bool, + rx_package_size: usize, } impl Elrs @@ -45,10 +61,42 @@ where pub fn new(uart: UartPeripheral) -> Self { let elsr_init_done = false; let elrs_init_counter = 0; + let elrs_connected = false; + let elrs_connected_timeout = 500; + let uplink_rssi_1 = 0; + let uplink_rssi_2 = 0; + let uplink_package_success = 0; + let uplink_snr = 0; + let diversity = 0; + let rf_mode = 0; + let uplink_tx_power = 0; + let downlink_rssi = 0; + let downlink_package_success = 0; + let downlink_snr = 0; + let rx_buffer = [0; 64]; + let rx_buffer_index = 0; + let rx_sync = false; + let rx_package_size = 0; Self { uart, elsr_init_done, elrs_init_counter, + elrs_connected, + elrs_connected_timeout, + uplink_rssi_1, + uplink_rssi_2, + uplink_package_success, + uplink_snr, + diversity, + rf_mode, + uplink_tx_power, + downlink_rssi, + downlink_package_success, + downlink_snr, + rx_buffer, + rx_buffer_index, + rx_sync, + rx_package_size, } } } @@ -64,16 +112,23 @@ where .write_full_blocking(&self.prepare_crsf_data_packet(data)); return; } - if self.elrs_init_counter < 500 { + if self.elrs_init_counter < 5000 { self.uart .write_full_blocking(&self.prepare_crsf_data_packet(data)); self.elrs_init_counter += 1; - } else if self.elrs_init_counter < 505 { + } else if self.elrs_init_counter < 5005 { self.uart - .write_full_blocking(&self.prepare_crsf_cmd_packet(0x01, 0x00)); + // 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, 0x02)); self.elrs_init_counter += 1; - } else if self.elrs_init_counter < 510 { + } else if self.elrs_init_counter < 5010 { 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 { @@ -81,6 +136,49 @@ where } } + pub fn check_link(&mut self) { + let mut buffer: [u8; 640] = [0; 640]; + // let _ = self.uart.read_raw(&mut buffer); + let mut crc: u8 = 0; + + for byte in buffer.iter() { + if *byte == 0xEA && !self.rx_sync { + self.rx_buffer_index = 0; + self.rx_package_size = 0; + self.rx_sync = true; + } else if self.rx_sync && self.rx_package_size == 0 { + self.rx_package_size = *byte as usize; + } else if self.rx_sync && self.rx_buffer_index < self.rx_package_size { + self.rx_buffer[self.rx_buffer_index] = *byte; + self.rx_buffer_index += 1; + } else if self.rx_buffer_index == self.rx_package_size { + for i in 0..self.rx_package_size { + crc = CRSF_CRC8TAB[(crc ^ self.rx_buffer[i]) as usize]; + } + if crc == *byte { + self.elrs_connected = true; + self.elrs_connected_timeout = 500; + } + self.rx_sync = false; + } + } + + if self.elrs_connected_timeout == 0 { + self.elrs_connected = false; + } else { + self.elrs_connected_timeout -= 1; + } + } + + pub fn reset(&mut self) { + self.elsr_init_done = false; + self.elrs_init_counter = 0; + } + + pub fn connected(&self) -> bool { + self.elrs_connected + } + fn prepare_crsf_data_packet(&self, data: [u16; 12]) -> [u8; 26] { let mut packet: [u8; 26] = [0; 26]; let mut crc: u8 = 0; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index bee0c80..a6b1450 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -76,12 +76,18 @@ 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; + // 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: bool, + pub elrs_changed: bool, + pub elrs_changed_to: bool, } #[derive(Copy, Clone)] @@ -162,7 +168,7 @@ fn main() -> ! { ) .unwrap(); - let mut i2c = I2C::i2c1( + let i2c = I2C::i2c1( pac.I2C1, pins.gp14.into_mode(), // sda pins.gp15.into_mode(), // scl @@ -171,8 +177,8 @@ fn main() -> ! { 125_000_000.Hz(), ); - let i2c_address = SlaveAddr::default(); - let mut eeprom = Eeprom24x::new_24x02(i2c, i2c_address); + 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); @@ -187,20 +193,20 @@ 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(); @@ -208,7 +214,7 @@ fn main() -> ! { // 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(); @@ -239,35 +245,39 @@ 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(200u32.micros()); - let mut status_led_count_down = timer.count_down(); - status_led_count_down.start(50.millis()); - - let mut main_count_down = timer.count_down(); - main_count_down.start(1660u32.micros()); + 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 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 mut elrs_connected: bool = false; let mut calibration_active: 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 channel_locks: [bool; 12] = [false; 12]; - let mut gimbal_mode: u8 = GIMBAL_MODE_M10; + let mut gimbal_mode: u8; + // 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); // Set up left gimbal Y axis as full range without return to center spring @@ -303,20 +313,18 @@ fn main() -> ! { .build(); // Read calibration data from eeprom - // if !calibration_active { - // for (index, item) in axis.iter_mut().enumerate() { - // item.min = eeprom.read_byte((index as u32 * 6) + 1).unwrap() as u16; - // item.min <<= 8; - // item.min |= eeprom.read_byte(index as u32 * 6).unwrap() as u16; - // item.max = eeprom.read_byte((index as u32 * 6) + 3).unwrap() as u16; - // item.max <<= 8; - // item.max = eeprom.read_byte((index as u32 * 6) + 2).unwrap() as u16; - // item.center = eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16; - // item.center <<= 8; - // item.center = eeprom.read_byte((index as u32 * 6) + 4).unwrap() as u16; - // } - // gimbal_mode = eeprom.read_byte(24).unwrap(); - // } + for (index, item) in axis.iter_mut().enumerate() { + item.min = eeprom.read_byte((index as u32 * 6) + 2).unwrap() as u16; + item.min <<= 8; + item.min |= eeprom.read_byte((index as u32 * 6) + 1).unwrap() as u16; + item.max = eeprom.read_byte((index as u32 * 6) + 4).unwrap() as u16; + item.max <<= 8; + item.max |= eeprom.read_byte((index as u32 * 6) + 3).unwrap() as u16; + item.center = eeprom.read_byte((index as u32 * 6) + 6).unwrap() as u16; + item.center <<= 8; + item.center |= eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16; + } + gimbal_mode = eeprom.read_byte(25).unwrap(); loop { // Take care of USB HID poll requests @@ -324,11 +332,6 @@ fn main() -> ! { usb_active = true; } - // Power up ELRS TX - if elrs_start_count_down.wait().is_ok() { - elrs_active = true; - } - if scan_count_down.wait().is_ok() { button_matrix.scan_matrix(&mut delay); @@ -356,46 +359,22 @@ fn main() -> ! { if status_led_count_down.wait().is_ok() { update_status_led( &mut status_led, - &mut activity, &usb_active, &elrs_active, - &idle, + &elrs.connected(), &safety_check, &calibration_active, ); } - // Dont send USB HID joystick report if there is no activity - // This is to avoid preventing the computer from going to sleep - if usb_hid_report_count_down.wait().is_ok() && activity { - match usb_hid_joystick.device().write_report(&get_joystick_report( - &mut buttons, - &mut axis, - &mode, - )) { - Err(UsbHidError::WouldBlock) => {} - Ok(_) => {} - Err(e) => { - status_led.update(StatusMode::Error); - core::panic!("Failed to write joystick report: {:?}", e) - } - }; - } - // Check if all axis are in idle position and no buttons are pressed if idle && !safety_check && elrs_active { safety_check = true; } - if main_count_down.wait().is_ok() { + if data_process_count_down.wait().is_ok() { // Secondary way to enter bootloader (pressing all left hands buttons except the hat - 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] - { + if button_matrix.buttons_pressed()[0] && button_matrix.buttons_pressed()[2] { status_led.update(StatusMode::Bootloader); let gpio_activity_pin_mask: u32 = 0; let disable_interface_mask: u32 = 0; @@ -405,15 +384,23 @@ fn main() -> ! { ); } + // ON/OFF switch for ELRS radio + if button_matrix.buttons_pressed()[4] + && button_matrix.buttons_pressed()[2] + && !elrs_active + { + safety_check = false; + elrs_active = true; + } else if button_matrix.buttons_pressed()[3] + && button_matrix.buttons_pressed()[2] + && elrs_active + { + elrs_active = false; + } + // Calibration of center position (pressing all right hands buttons except the hat // switch) - if button_matrix.buttons_pressed()[3] - && button_matrix.buttons_pressed()[4] - && button_matrix.buttons_pressed()[10] - && button_matrix.buttons_pressed()[11] - && button_matrix.buttons_pressed()[13] - && button_matrix.buttons_pressed()[14] - { + if button_matrix.buttons_pressed()[1] && button_matrix.buttons_pressed()[2] { for (index, item) in axis.iter_mut().enumerate() { item.center = smoother[index].value() as u16; item.min = item.center; @@ -450,15 +437,17 @@ fn main() -> ! { } // Save calibration data to eeprom (pressing right hat switch) else if calibration_active && button_matrix.buttons_pressed()[20] { - // for (index, item) in axis.iter_mut().enumerate() { - // let _ = eeprom.write_byte(index as u32 * 6, item.min as u8); - // let _ = eeprom.write_byte((index as u32 * 6) + 1, (item.min >> 8) as u8); - // let _ = eeprom.write_byte((index as u32 * 6) + 2, item.max as u8); - // let _ = eeprom.write_byte((index as u32 * 6) + 3, (item.max >> 8) as u8); - // let _ = eeprom.write_byte((index as u32 * 6) + 4, item.center as u8); - // let _ = eeprom.write_byte((index as u32 * 6) + 5, (item.center >> 8) as u8); - // } - // let _ = eeprom.write_byte(24, gimbal_mode); + 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; } @@ -501,9 +490,11 @@ fn main() -> ! { && 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) + && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR + && index != 2) || (elrs_active - && layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated) + && layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated + && index != 2) { idle = false; } @@ -512,8 +503,9 @@ 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; } // Generate led activity when a button is pressed @@ -528,8 +520,13 @@ fn main() -> ! { && key.pressed != key.previous_pressed && layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated) { - activity = true; + key.usb_changed = true; + key.usb_changed_to = key.pressed; + key.elrs_changed = true; + key.elrs_changed_to = key.pressed; + usb_activity = true; } + key.previous_pressed = key.pressed; } // Reset channel locks when calibration is active @@ -538,7 +535,9 @@ fn main() -> ! { *lock_active = false; } } + } + if elrs_update_count_down.wait().is_ok() { // Send ELRS data if elrs_active { elrs_en_pin.set_high().unwrap(); @@ -546,20 +545,30 @@ fn main() -> ! { &mut buttons, &mut axis, &mut channel_locks, - elrs_active, )); + // elrs.check_link(); } else { elrs_en_pin.set_low().unwrap(); + elrs.reset(); } + } - // Clear axis status - for item in axis.iter_mut() { - item.previous_value = item.value; - } - // Clear key status - for key in buttons.iter_mut() { - key.previous_pressed = key.pressed; - } + // 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, + &mode, + )) { + Err(UsbHidError::WouldBlock) => {} + Ok(_) => {} + Err(e) => { + status_led.update(StatusMode::Error); + core::panic!("Failed to write joystick report: {:?}", e) + } + }; + usb_activity = false; } } } @@ -579,10 +588,9 @@ 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, elrs_active: &bool, - axis_idle: &bool, + elrs_connected: &bool, safety_check: &bool, calibration_active: &bool, ) where @@ -591,33 +599,18 @@ fn update_status_led( Function

: ValidPinMode, SM: StateMachineIndex, { - // If calibration is active, flash the status LED green - if *calibration_active && status_led.get_mode() == StatusMode::Normal { - status_led.update(StatusMode::Off); - } else if *calibration_active && status_led.get_mode() != StatusMode::Normal { - status_led.update(StatusMode::Normal); - // If in ELRS mode and safety chack failed, flash status LED red + if *calibration_active { + status_led.update(StatusMode::ActivityFlash); } else if *elrs_active && !*safety_check { status_led.update(StatusMode::Warning); - // If activity occurs, flash status LED blue - } else if *activity && status_led.get_mode() != StatusMode::Activity { - status_led.update(StatusMode::Activity); - } else if *activity && status_led.get_mode() == StatusMode::Activity { - status_led.update(StatusMode::Off); - *activity = false; - // If no activity but not in idle position, turn status LED steady blue - } else if !*axis_idle && status_led.get_mode() != StatusMode::Activity { - status_led.update(StatusMode::Activity); - // Else device idle in USB mode, turn status LED steady green - } else if *axis_idle - && *usb_active - && !*elrs_active - && status_led.get_mode() != StatusMode::Normal - { + } else if !*usb_active && !*elrs_active { + status_led.update(StatusMode::NormalFlash); + } else if *usb_active && !*elrs_active { status_led.update(StatusMode::Normal); - // Else device idle in ELRS mode, turn status LED steady orange - } else if *axis_idle && *elrs_active && 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); } } @@ -948,55 +941,55 @@ fn get_elrs_channels( matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], axis: &mut [GimbalAxis; 4], channel_locks: &mut [bool; 12], - elrs_active: bool, ) -> [u16; 12] { let mut channels: [u16; 12] = [ELRS_MIN; 12]; // Check and store trim values let mut trim_active = false; for (index, key) in matrix_keys.iter_mut().enumerate() { - if elrs_active - && key.pressed - && key.pressed != key.previous_pressed + if key.pressed && layout::ELRS_MAP[index] >= layout::ElrsButton::CH1P && layout::ELRS_MAP[index] <= layout::ElrsButton::CH4P - && axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim - < ELRS_CENTER as i16 { - axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim += 1; + if axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim + < ELRS_CENTER as i16 + { + axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim += + 1; + } trim_active = true; - } else if elrs_active - && key.pressed - && key.pressed != key.previous_pressed + } else if key.pressed && layout::ELRS_MAP[index] >= layout::ElrsButton::CH1M && layout::ELRS_MAP[index] <= layout::ElrsButton::CH4M - && axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim - > (0 - ELRS_CENTER as i16) { - axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim -= 1; + if axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim + > (0 - ELRS_CENTER as i16) + { + axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim -= + 1; + } trim_active = true; } } - // Check and reser trim values + // Check and reset trim values for (index, key) in matrix_keys.iter_mut().enumerate() { - if elrs_active - && !trim_active - && key.pressed - && key.pressed != key.previous_pressed + if !trim_active + && key.elrs_changed + && key.elrs_changed_to && layout::ELRS_MAP[index] == layout::ElrsButton::CH12Z { axis[GIMBAL_AXIS_LEFT_X].trim = 0; axis[GIMBAL_AXIS_LEFT_Y].trim = 0; - } else if elrs_active - && !trim_active - && key.pressed - && key.pressed != key.previous_pressed + } else if !trim_active + && key.elrs_changed + && key.elrs_changed_to && layout::ELRS_MAP[index] == layout::ElrsButton::CH34Z { axis[GIMBAL_AXIS_RIGHT_X].trim = 0; axis[GIMBAL_AXIS_RIGHT_Y].trim = 0; } + key.elrs_changed = false; } // Match ELRS channel 1-4 to new min/max values From 1c8229a949cf6363fcb1da6e861e1bdeea7e2a6e Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Wed, 30 Aug 2023 17:15:30 +0200 Subject: [PATCH 20/30] ELRS read update --- .gitignore | 1 + rp2040/src/elrs.rs | 34 +++++++++++++++++++--------------- 2 files changed, 20 insertions(+), 15 deletions(-) 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/rp2040/src/elrs.rs b/rp2040/src/elrs.rs index 8f8e571..f6433bd 100644 --- a/rp2040/src/elrs.rs +++ b/rp2040/src/elrs.rs @@ -137,37 +137,41 @@ where } pub fn check_link(&mut self) { - let mut buffer: [u8; 640] = [0; 640]; - // let _ = self.uart.read_raw(&mut buffer); + if self.elrs_connected_timeout == 0 { + self.elrs_connected = false; + } else { + self.elrs_connected_timeout -= 1; + } + + if !self.uart.uart_is_readable() { + return; + } + + let mut tmp_buffer: [u8; 256] = [0; 256]; + let bytes_to_read = self.uart.read_raw(&mut tmp_buffer).unwrap(); let mut crc: u8 = 0; - for byte in buffer.iter() { - if *byte == 0xEA && !self.rx_sync { + for i in 0..bytes_to_read { + if tmp_buffer[i] == 0xEA && !self.rx_sync { self.rx_buffer_index = 0; self.rx_package_size = 0; self.rx_sync = true; } else if self.rx_sync && self.rx_package_size == 0 { - self.rx_package_size = *byte as usize; + self.rx_package_size = tmp_buffer[i] as usize; } else if self.rx_sync && self.rx_buffer_index < self.rx_package_size { - self.rx_buffer[self.rx_buffer_index] = *byte; + self.rx_buffer[self.rx_buffer_index] = tmp_buffer[i]; self.rx_buffer_index += 1; } else if self.rx_buffer_index == self.rx_package_size { - for i in 0..self.rx_package_size { - crc = CRSF_CRC8TAB[(crc ^ self.rx_buffer[i]) as usize]; + for j in 0..self.rx_package_size { + crc = CRSF_CRC8TAB[(crc ^ self.rx_buffer[j]) as usize]; } - if crc == *byte { + if crc == tmp_buffer[i] { self.elrs_connected = true; self.elrs_connected_timeout = 500; } self.rx_sync = false; } } - - if self.elrs_connected_timeout == 0 { - self.elrs_connected = false; - } else { - self.elrs_connected_timeout -= 1; - } } pub fn reset(&mut self) { From 6bae45d53d8676ef9a4d0d5558d08241d42c7a59 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Thu, 31 Aug 2023 10:33:27 +0200 Subject: [PATCH 21/30] Updated readme --- README.md | 29 ++++++++++++++++++++++------- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 0abf8f8..bbdc8c5 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # CMDR Joystick 25 -RC Joystick with 2 hall effect gimbals, 2 hat switches and 25 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 @@ -65,7 +65,7 @@ ELRS Layer -------------------------------------------------------------- | CH7 OFF | CH7 ON | | -- | | CH8 ON | CH8 OFF| -------------------------------------------------------------- -| | CH9 ON | CH9 OFF | CH5 | | CH10 OFF | CH10 ON | CH12| | +| | CH9 ON | CH9 OFF | CH5 | | CH6 | CH10 OFF | CH10 ON | | | | | CH11 ON | | CH12 ON | | | | CH11 OFF | | CH12 OFF | | @@ -75,6 +75,20 @@ ELRS Layer | | LTD | | RTD | | -------------------------------------------------------------- +Config Layer +-------------------------------------------------------------- +| BOOT | CAL | | CONFIG | | USB | ELRS | +-------------------------------------------------------------- +| | -- | -- | -- | | -- | -- | -- | | +| | +| | --- | | --- | | +| | --- | | --- | | +| X(CH1)/Y(CH2) X(CH3)/Y(CH4) | +| | --- | | --- | | +| | --- | --- | --- || --- | --- | --- | | +| | --- | | --- | | +-------------------------------------------------------------- + ``` ## Features @@ -119,12 +133,14 @@ ELRS Layer ``` ## Software Build environment -Rust +##### Rust - Cargo (rust embedded) - Flashing via Cargo - - Pressing boot button on teensy - - Press and hold "top lower right button" when powering the unit + - 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) @@ -132,8 +148,7 @@ Rust ## Calibration 1. Center both gimbals. -2. Press all righ hand side buttons except hat switch. Status led will start blinking green. +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. -Done! From d33cc6a3249b577b237a36129960363188b78e74 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Thu, 31 Aug 2023 10:36:43 +0200 Subject: [PATCH 22/30] Updated readme --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index bbdc8c5..13c795c 100644 --- a/README.md +++ b/README.md @@ -101,7 +101,7 @@ Config Layer ## Hardware -- 2x FrSky M7 or M10 gimbals: [M7 datasheed](https://www.frsky-rc.com/product/m7/) +- 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) @@ -110,7 +110,8 @@ Config Layer - 1x Top plate (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) + - ![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) From f16ecb30dfee34fc13af537a910907e3c7b7b5d7 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Mon, 18 Sep 2023 07:22:11 +0200 Subject: [PATCH 23/30] More updates to ELRS implementation --- rp2040/src/elrs.rs | 125 ++++++++++++++------------------------- rp2040/src/layout.rs | 90 ++++++++++++++++++++++++---- rp2040/src/main.rs | 1 - rp2040/src/status_led.rs | 3 + 4 files changed, 125 insertions(+), 94 deletions(-) diff --git a/rp2040/src/elrs.rs b/rp2040/src/elrs.rs index f6433bd..4d08f81 100644 --- a/rp2040/src/elrs.rs +++ b/rp2040/src/elrs.rs @@ -4,6 +4,7 @@ //! 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] = [ @@ -25,6 +26,9 @@ const CRSF_CRC8TAB: [u8; 256] = [ 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, @@ -36,20 +40,7 @@ where elrs_init_counter: u16, elrs_connected: bool, elrs_connected_timeout: u16, - uplink_rssi_1: u8, - uplink_rssi_2: u8, - uplink_package_success: u8, - uplink_snr: i8, - diversity: u8, - rf_mode: u8, - uplink_tx_power: u8, - downlink_rssi: u8, - downlink_package_success: u8, - downlink_snr: i8, - rx_buffer: [u8; 64], rx_buffer_index: usize, - rx_sync: bool, - rx_package_size: usize, } impl Elrs @@ -62,41 +53,15 @@ where let elsr_init_done = false; let elrs_init_counter = 0; let elrs_connected = false; - let elrs_connected_timeout = 500; - let uplink_rssi_1 = 0; - let uplink_rssi_2 = 0; - let uplink_package_success = 0; - let uplink_snr = 0; - let diversity = 0; - let rf_mode = 0; - let uplink_tx_power = 0; - let downlink_rssi = 0; - let downlink_package_success = 0; - let downlink_snr = 0; - let rx_buffer = [0; 64]; + let elrs_connected_timeout = CONNECTION_TIMEOUT; let rx_buffer_index = 0; - let rx_sync = false; - let rx_package_size = 0; Self { uart, elsr_init_done, elrs_init_counter, elrs_connected, elrs_connected_timeout, - uplink_rssi_1, - uplink_rssi_2, - uplink_package_success, - uplink_snr, - diversity, - rf_mode, - uplink_tx_power, - downlink_rssi, - downlink_package_success, - downlink_snr, - rx_buffer, rx_buffer_index, - rx_sync, - rx_package_size, } } } @@ -112,19 +77,19 @@ where .write_full_blocking(&self.prepare_crsf_data_packet(data)); return; } - if self.elrs_init_counter < 5000 { + 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 < 5005 { + } 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, 0x02)); + .write_full_blocking(&self.prepare_crsf_cmd_packet(0x01, 0x00)); self.elrs_init_counter += 1; - } else if self.elrs_init_counter < 5010 { + } else if self.elrs_init_counter < INIT_TIMEOUT + 10 { self.uart // Setting Power to 10mW // 0 = 10mW, 1 = 25mW, 2 = 50mW, 3 = 100mW, @@ -134,44 +99,12 @@ where } else { self.elsr_init_done = true; } + + self.check_link_frame(); } - pub fn check_link(&mut self) { - if self.elrs_connected_timeout == 0 { - self.elrs_connected = false; - } else { - self.elrs_connected_timeout -= 1; - } - - if !self.uart.uart_is_readable() { - return; - } - - let mut tmp_buffer: [u8; 256] = [0; 256]; - let bytes_to_read = self.uart.read_raw(&mut tmp_buffer).unwrap(); - let mut crc: u8 = 0; - - for i in 0..bytes_to_read { - if tmp_buffer[i] == 0xEA && !self.rx_sync { - self.rx_buffer_index = 0; - self.rx_package_size = 0; - self.rx_sync = true; - } else if self.rx_sync && self.rx_package_size == 0 { - self.rx_package_size = tmp_buffer[i] as usize; - } else if self.rx_sync && self.rx_buffer_index < self.rx_package_size { - self.rx_buffer[self.rx_buffer_index] = tmp_buffer[i]; - self.rx_buffer_index += 1; - } else if self.rx_buffer_index == self.rx_package_size { - for j in 0..self.rx_package_size { - crc = CRSF_CRC8TAB[(crc ^ self.rx_buffer[j]) as usize]; - } - if crc == tmp_buffer[i] { - self.elrs_connected = true; - self.elrs_connected_timeout = 500; - } - self.rx_sync = false; - } - } + pub fn connected(&self) -> bool { + self.elrs_connected } pub fn reset(&mut self) { @@ -179,8 +112,36 @@ where self.elrs_init_counter = 0; } - pub fn connected(&self) -> bool { - self.elrs_connected + 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] { diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index 4749c28..bbb62fb 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -6,6 +6,17 @@ use crate::NUMBER_OF_BUTTONS; +#[allow(dead_code)] +#[derive(Debug, PartialEq, PartialOrd, Copy, Clone)] +pub enum ConfigButton { + CONF = 0, + BOOT = 1, + CAL = 2, + ELRS = 3, + USB = 4, + NoEventIndicated = 5, +} + #[allow(dead_code)] #[derive(Debug, PartialEq, PartialOrd, Copy, Clone)] pub enum ElrsButton { @@ -102,7 +113,7 @@ pub enum HidButton { #[warn(dead_code)] // Button index map: // -------------------------------------------------------------- -// | 0 | 1 | | 2 | | 3 | 4 | +// | 0 L| 1 U| | 2 | | 3 L| 4 U| // --------------------------------------------------------------- // | | 5 | 6 | 7 | | 12 | 11 | 10 | | // | | @@ -138,12 +149,12 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B9, // 13 HidButton::B10, // 14 - HidButton::H1B, // 15 button 22 + HidButton::H1B, // 15 HidButton::H1U, // 16 HidButton::H1R, // 17 HidButton::H1D, // 18 HidButton::H1L, // 19 - HidButton::H2B, // 20 button 23 + HidButton::H2B, // 20 HidButton::H2U, // 21 HidButton::H2R, // 22 HidButton::H2D, // 23 @@ -168,12 +179,12 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B9, // 13 HidButton::B10, // 14 - HidButton::H3B, // 15 button 24 + HidButton::H3B, // 15 HidButton::H3U, // 16 HidButton::H3R, // 17 HidButton::H3D, // 18 HidButton::H3L, // 19 - HidButton::H2B, // 20 button 23 + HidButton::H2B, // 20 HidButton::H2U, // 21 HidButton::H2R, // 22 HidButton::H2D, // 23 @@ -198,12 +209,12 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B19, // 13 HidButton::B20, // 14 - HidButton::H1B, // 15 button 22 + HidButton::H1B, // 15 HidButton::H1U, // 16 HidButton::H1R, // 17 HidButton::H1D, // 18 HidButton::H1L, // 19 - HidButton::H4B, // 20 button 25 + HidButton::H4B, // 20 HidButton::H4U, // 21 HidButton::H4R, // 22 HidButton::H4D, // 23 @@ -228,12 +239,12 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B19, // 13 HidButton::B20, // 14 - HidButton::H3B, // 15 button 24 + HidButton::H3B, // 15 HidButton::H3U, // 16 HidButton::H3R, // 17 HidButton::H3D, // 18 HidButton::H3L, // 19 - HidButton::H4B, // 20 button 25 + HidButton::H4B, // 20 HidButton::H4U, // 21 HidButton::H4R, // 22 HidButton::H4D, // 23 @@ -241,9 +252,22 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ ], ]; +// 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 | | +// | | 17 | 15 | 18 | | 22 | 20 | 23 | | +// | | 19 | | 24 | | +// --------------------------------------------------------------- +// pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ - // Function layer 0 - // HID Key // Button Index + // ELRS Key // Button Index // ----------------------------------------- ElrsButton::CH7OFF, // 0 ElrsButton::CH7ON, // 1 @@ -271,3 +295,47 @@ pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ ElrsButton::CH4M, // 23 ElrsButton::CH3M, // 24 ]; + +// 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 | | +// | | 17 | 15 | 18 | | 22 | 20 | 23 | | +// | | 19 | | 24 | | +// --------------------------------------------------------------- +// +pub const CONFIG_MAP: [ConfigButton; NUMBER_OF_BUTTONS] = [ + // Config Key // Button Index + // ----------------------------------------- + ConfigButton::BOOT, // 0 + ConfigButton::CAL, // 1 + ConfigButton::CONF, // 2 + ConfigButton::USB, // 3 + ConfigButton::ELRS, // 4 + ConfigButton::NoEventIndicated, // 5 + ConfigButton::NoEventIndicated, // 6 + ConfigButton::NoEventIndicated, // 7 + ConfigButton::NoEventIndicated, // 8 + ConfigButton::NoEventIndicated, // 9 + ConfigButton::NoEventIndicated, // 10 + ConfigButton::NoEventIndicated, // 11 + ConfigButton::NoEventIndicated, // 12 + ConfigButton::NoEventIndicated, // 13 + ConfigButton::NoEventIndicated, // 14 + ConfigButton::NoEventIndicated, // 15 + ConfigButton::NoEventIndicated, // 16 + ConfigButton::NoEventIndicated, // 17 + ConfigButton::NoEventIndicated, // 18 + ConfigButton::NoEventIndicated, // 19 + ConfigButton::NoEventIndicated, // 20 + ConfigButton::NoEventIndicated, // 21 + ConfigButton::NoEventIndicated, // 22 + ConfigButton::NoEventIndicated, // 23 + ConfigButton::NoEventIndicated, // 24 +]; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index a6b1450..431c15e 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -546,7 +546,6 @@ fn main() -> ! { &mut axis, &mut channel_locks, )); - // elrs.check_link(); } else { elrs_en_pin.set_low().unwrap(); elrs.reset(); diff --git a/rp2040/src/status_led.rs b/rp2040/src/status_led.rs index 5108008..40abfc7 100644 --- a/rp2040/src/status_led.rs +++ b/rp2040/src/status_led.rs @@ -103,8 +103,11 @@ where /// /// * OFF = off /// * NORMAL = green + /// * NORMALFLASH = green (flashing) /// * ACTIVITY = blue + /// * ACTIVITYFLASH = blue (flashing) /// * OTHER = orange + /// * OTHERFLASH = orange (flashing) /// * WARNING = red (flashing) /// * ERROR = red /// * BOOTLOADER = purple From bf919854bb46da3a2d04c087a42081abab007f7c Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Mon, 8 Jul 2024 23:36:14 +0200 Subject: [PATCH 24/30] Updated to 32 button descriptor --- rp2040/src/layout.rs | 202 +++++++++++++++--------------- rp2040/src/main.rs | 48 +++---- rp2040/src/usb_joystick_device.rs | 9 +- 3 files changed, 126 insertions(+), 133 deletions(-) diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index bbb62fb..dcfec02 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -84,31 +84,37 @@ pub enum HidButton { B19 = 18, B20 = 19, B21 = 20, - FnL = 21, - FnR = 22, - ModeL = 23, - ModeR = 24, - H1U = 25, - H1R = 26, - H1D = 27, - H1L = 28, - H1B = 29, - H2U = 30, - H2R = 31, - H2D = 32, - H2L = 33, - H2B = 34, - H3U = 35, - H3R = 36, - H3D = 37, - H3L = 38, - H3B = 39, - H4U = 40, - H4R = 41, - H4D = 42, - H4L = 43, - H4B = 44, - NoEventIndicated = 45, + B22 = 21, + B23 = 22, + B24 = 23, + B25 = 24, + B26 = 25, + B27 = 26, + B28 = 27, + Fn = 28, + ModeL = 29, + ModeR = 30, + H1U = 31, + H1R = 32, + H1D = 33, + H1L = 34, + H1B = 35, + H2U = 36, + H2R = 37, + H2D = 38, + H2L = 39, + H2B = 40, + H3U = 41, + H3R = 42, + H3D = 43, + H3L = 44, + H3B = 45, + H4U = 46, + H4R = 47, + H4D = 48, + H4L = 49, + H4B = 50, + NoEventIndicated = 51, } #[warn(dead_code)] // Button index map: @@ -126,18 +132,18 @@ pub enum HidButton { // --------------------------------------------------------------- // /// 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 +/// Please make sure to set Fn, 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. +/// Hat button 1-4 = HID B28-B32. pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ [ // Function layer 0 // HID Key // Button Index // ----------------------------------------- - HidButton::FnL, // 0 + HidButton::Fn, // 0 HidButton::B1, // 1 - HidButton::B21, // 2 - HidButton::FnR, // 3 + HidButton::B27, // 2 + HidButton::B21, // 3 HidButton::B6, // 4 HidButton::B2, // 5 HidButton::B3, // 6 @@ -146,7 +152,7 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::B5, // 9 HidButton::B7, // 10 HidButton::B8, // 11 - HidButton::ModeR, // 12 + HidButton::B23, // 12 HidButton::B9, // 13 HidButton::B10, // 14 HidButton::H1B, // 15 @@ -161,74 +167,14 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::H2L, // 24 ], [ - // Function layer 1 (left Fn button pressed) + // Function layer 1 (Fn button pressed) // HID Key // Button Index // ----------------------------------------- - HidButton::FnL, // 0 - HidButton::B11, // 1 - HidButton::B21, // 2 - 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::H3B, // 15 - HidButton::H3U, // 16 - HidButton::H3R, // 17 - HidButton::H3D, // 18 - HidButton::H3L, // 19 - HidButton::H2B, // 20 - HidButton::H2U, // 21 - HidButton::H2R, // 22 - HidButton::H2D, // 23 - HidButton::H2L, // 24 - ], - [ - // Function layer 2 (right Fn button pressed) - // HID Key // Button Index - // ----------------------------------------- - HidButton::FnL, // 0 - HidButton::B1, // 1 - HidButton::B21, // 2 - 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::H1B, // 15 - HidButton::H1U, // 16 - HidButton::H1R, // 17 - HidButton::H1D, // 18 - HidButton::H1L, // 19 - HidButton::H4B, // 20 - HidButton::H4U, // 21 - HidButton::H4R, // 22 - HidButton::H4D, // 23 - HidButton::H4L, // 24 - ], - [ - // Function layer 3 (left + right Fn button pressed) - // HID Key // Button Index - // ----------------------------------------- - HidButton::FnL, // 0 - HidButton::B11, // 1 - HidButton::B21, // 2 - HidButton::FnR, // 3 - HidButton::B16, // 4 + HidButton::Fn, // 0 + HidButton::B16, // 1 + HidButton::B28, // 2 + HidButton::B11, // 3 + HidButton::B22, // 4 HidButton::B12, // 5 HidButton::B13, // 6 HidButton::ModeL, // 7 @@ -236,7 +182,7 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::B15, // 9 HidButton::B17, // 10 HidButton::B18, // 11 - HidButton::ModeR, // 12 + HidButton::B23, // 12 HidButton::B19, // 13 HidButton::B20, // 14 HidButton::H3B, // 15 @@ -250,6 +196,66 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::H4D, // 23 HidButton::H4L, // 24 ], + [ + // Function layer 2 (Mode R selected) + // HID Key // Button Index + // ----------------------------------------- + HidButton::Fn, // 0 + HidButton::H3U, // 1 + HidButton::NoEventIndicated, // 2 + HidButton::B21, // 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::H1B, // 15 + HidButton::H1U, // 16 + HidButton::H1R, // 17 + HidButton::H1D, // 18 + HidButton::H1L, // 19 + HidButton::H2B, // 20 + HidButton::H2U, // 21 + HidButton::H2R, // 22 + HidButton::H2D, // 23 + HidButton::H2L, // 24 + ], + [ + // Function layer 3 (Mode R selected, Fn button pressed) + // HID Key // Button Index + // ----------------------------------------- + HidButton::Fn, // 0 + HidButton::H3D, // 1 + HidButton::NoEventIndicated, // 2 + HidButton::B11, // 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::H3B, // 15 + HidButton::H3U, // 16 + HidButton::H3R, // 17 + HidButton::H3D, // 18 + HidButton::H3L, // 19 + HidButton::H4B, // 20 + HidButton::H4U, // 21 + HidButton::H4R, // 22 + HidButton::H4D, // 23 + HidButton::H4L, // 24 + ], ]; // Button index map: diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 431c15e..cc2ad38 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -487,8 +487,7 @@ fn main() -> ! { 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::Fn && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeL && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR && index != 2) @@ -513,8 +512,7 @@ fn main() -> ! { 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::Fn && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR) || (elrs_active && key.pressed != key.previous_pressed @@ -622,17 +620,13 @@ fn update_status_led( 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 fn_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::Fn { + fn_active = true; } if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeL { alt_l_active = true; @@ -642,15 +636,15 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { } } - if fn_l_active && fn_r_active { + if alt_r_active && fn_active { mode = 3; - } else if fn_r_active { + } else if alt_r_active { mode = 2; - } else if fn_l_active { + } else if fn_active { mode = 1; } - // Set bit 4 and 5 if alt l/r is active + // Set bit 4 and 5 if mode l/r is active if alt_l_active { mode |= 0x10; } @@ -677,7 +671,7 @@ fn get_joystick_report( 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; @@ -711,17 +705,13 @@ fn get_joystick_report( // 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 mode & 0x20 == 0x20 && (axis[GIMBAL_AXIS_RIGHT_X].fn_mode == 1) { 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) - { + if mode & 0x20 == 0x20 && axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 1 { 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: @@ -756,18 +746,18 @@ fn get_joystick_report( let (hat4, hat_button4) = format_hat_value(hats[3]); // Update button state for joystick button 21-24 according to hat button 1-4 - let mut buttons: u32 = (hat_button1 as u32) << 21 - | ((hat_button2 as u32) << 22) - | ((hat_button3 as u32) << 23) - | ((hat_button4 as u32) << 24); + let mut buttons: u32 = (hat_button1 as u32) << 28 + | ((hat_button2 as u32) << 29) + | ((hat_button3 as u32) << 30) + | ((hat_button4 as u32) << 31); - // Update button state for joystick button 1-20 + // Update button state for joystick button 1-28 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::B21 as usize + <= layout::HidButton::B28 as usize { buttons |= 1 << layout::HID_MAP[key.fn_mode as usize][index] as usize; } diff --git a/rp2040/src/usb_joystick_device.rs b/rp2040/src/usb_joystick_device.rs index 348153f..33595f3 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) @@ -123,7 +120,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 From 748ca8aa94d81bce464fbc268c4c5cb290848053 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Wed, 10 Jul 2024 20:52:20 +0200 Subject: [PATCH 25/30] Updated layout --- rp2040/src/layout.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index dcfec02..261f111 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -140,7 +140,7 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ // Function layer 0 // HID Key // Button Index // ----------------------------------------- - HidButton::Fn, // 0 + HidButton::B23, // 0 HidButton::B1, // 1 HidButton::B27, // 2 HidButton::B21, // 3 @@ -152,7 +152,7 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::B5, // 9 HidButton::B7, // 10 HidButton::B8, // 11 - HidButton::B23, // 12 + HidButton::Fn, // 12 HidButton::B9, // 13 HidButton::B10, // 14 HidButton::H1B, // 15 @@ -170,7 +170,7 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ // Function layer 1 (Fn button pressed) // HID Key // Button Index // ----------------------------------------- - HidButton::Fn, // 0 + HidButton::B24, // 0 HidButton::B16, // 1 HidButton::B28, // 2 HidButton::B11, // 3 @@ -182,7 +182,7 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::B15, // 9 HidButton::B17, // 10 HidButton::B18, // 11 - HidButton::B23, // 12 + HidButton::Fn, // 12 HidButton::B19, // 13 HidButton::B20, // 14 HidButton::H3B, // 15 From 197b562a017d9580260c75e5d185662e88e93533 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sat, 20 Jul 2024 17:37:15 +0200 Subject: [PATCH 26/30] Code refactor. Removed layout file. Changed back to 25 button descriptor. Added toggle switch mode. --- rp2040/src/layout.rs | 347 ------------------ rp2040/src/main.rs | 588 +++++++++++++++++------------- rp2040/src/usb_joystick_device.rs | 9 +- 3 files changed, 343 insertions(+), 601 deletions(-) delete mode 100644 rp2040/src/layout.rs diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs deleted file mode 100644 index 261f111..0000000 --- a/rp2040/src/layout.rs +++ /dev/null @@ -1,347 +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 ConfigButton { - CONF = 0, - BOOT = 1, - CAL = 2, - ELRS = 3, - USB = 4, - NoEventIndicated = 5, -} - -#[allow(dead_code)] -#[derive(Debug, PartialEq, PartialOrd, Copy, Clone)] -pub enum ElrsButton { - CH1 = 0, - CH2 = 1, - CH3 = 2, - CH4 = 3, - CH5 = 4, // AUX1/ARM - CH6 = 5, - CH7 = 6, - CH8 = 7, - CH9 = 8, - CH10 = 9, - CH11 = 10, - CH12 = 11, - CH5ON = 12, - CH6ON = 13, - CH7ON = 14, - CH8ON = 15, - CH9ON = 16, - CH10ON = 17, - CH11ON = 18, - CH12ON = 19, - CH5OFF = 20, - CH6OFF = 21, - CH7OFF = 22, - CH8OFF = 23, - CH9OFF = 24, - CH10OFF = 25, - CH11OFF = 26, - CH12OFF = 27, - CH1P = 28, - CH2P = 29, - CH3P = 30, - CH4P = 31, - CH1M = 32, - CH2M = 33, - CH3M = 34, - CH4M = 35, - CH12Z = 36, - CH34Z = 37, - NoEventIndicated = 38, -} -#[allow(dead_code)] -#[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, - B21 = 20, - B22 = 21, - B23 = 22, - B24 = 23, - B25 = 24, - B26 = 25, - B27 = 26, - B28 = 27, - Fn = 28, - ModeL = 29, - ModeR = 30, - H1U = 31, - H1R = 32, - H1D = 33, - H1L = 34, - H1B = 35, - H2U = 36, - H2R = 37, - H2D = 38, - H2L = 39, - H2B = 40, - H3U = 41, - H3R = 42, - H3D = 43, - H3L = 44, - H3B = 45, - H4U = 46, - H4R = 47, - H4D = 48, - H4L = 49, - H4B = 50, - NoEventIndicated = 51, -} -#[warn(dead_code)] -// 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 | | -// | | 17 | 15 | 18 | | 22 | 20 | 23 | | -// | | 19 | | 24 | | -// --------------------------------------------------------------- -// -/// Button map to HID key (four function layers) -/// Please make sure to set Fn, 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 B28-B32. -pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ - [ - // Function layer 0 - // HID Key // Button Index - // ----------------------------------------- - HidButton::B23, // 0 - HidButton::B1, // 1 - HidButton::B27, // 2 - HidButton::B21, // 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::Fn, // 12 - HidButton::B9, // 13 - HidButton::B10, // 14 - HidButton::H1B, // 15 - HidButton::H1U, // 16 - HidButton::H1R, // 17 - HidButton::H1D, // 18 - HidButton::H1L, // 19 - HidButton::H2B, // 20 - HidButton::H2U, // 21 - HidButton::H2R, // 22 - HidButton::H2D, // 23 - HidButton::H2L, // 24 - ], - [ - // Function layer 1 (Fn button pressed) - // HID Key // Button Index - // ----------------------------------------- - HidButton::B24, // 0 - HidButton::B16, // 1 - HidButton::B28, // 2 - HidButton::B11, // 3 - HidButton::B22, // 4 - HidButton::B12, // 5 - HidButton::B13, // 6 - HidButton::ModeL, // 7 - HidButton::B14, // 8 - HidButton::B15, // 9 - HidButton::B17, // 10 - HidButton::B18, // 11 - HidButton::Fn, // 12 - HidButton::B19, // 13 - HidButton::B20, // 14 - HidButton::H3B, // 15 - HidButton::H3U, // 16 - HidButton::H3R, // 17 - HidButton::H3D, // 18 - HidButton::H3L, // 19 - HidButton::H4B, // 20 - HidButton::H4U, // 21 - HidButton::H4R, // 22 - HidButton::H4D, // 23 - HidButton::H4L, // 24 - ], - [ - // Function layer 2 (Mode R selected) - // HID Key // Button Index - // ----------------------------------------- - HidButton::Fn, // 0 - HidButton::H3U, // 1 - HidButton::NoEventIndicated, // 2 - HidButton::B21, // 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::H1B, // 15 - HidButton::H1U, // 16 - HidButton::H1R, // 17 - HidButton::H1D, // 18 - HidButton::H1L, // 19 - HidButton::H2B, // 20 - HidButton::H2U, // 21 - HidButton::H2R, // 22 - HidButton::H2D, // 23 - HidButton::H2L, // 24 - ], - [ - // Function layer 3 (Mode R selected, Fn button pressed) - // HID Key // Button Index - // ----------------------------------------- - HidButton::Fn, // 0 - HidButton::H3D, // 1 - HidButton::NoEventIndicated, // 2 - HidButton::B11, // 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::H3B, // 15 - HidButton::H3U, // 16 - HidButton::H3R, // 17 - HidButton::H3D, // 18 - HidButton::H3L, // 19 - HidButton::H4B, // 20 - HidButton::H4U, // 21 - HidButton::H4R, // 22 - HidButton::H4D, // 23 - HidButton::H4L, // 24 - ], -]; - -// 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 | | -// | | 17 | 15 | 18 | | 22 | 20 | 23 | | -// | | 19 | | 24 | | -// --------------------------------------------------------------- -// -pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ - // ELRS Key // Button Index - // ----------------------------------------- - ElrsButton::CH7OFF, // 0 - ElrsButton::CH7ON, // 1 - ElrsButton::NoEventIndicated, // 2 - 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::CH12Z, // 15 - ElrsButton::CH2P, // 16 - ElrsButton::CH1P, // 17 - ElrsButton::CH2M, // 18 - ElrsButton::CH1M, // 19 - ElrsButton::CH34Z, // 20 - ElrsButton::CH4P, // 21 - ElrsButton::CH3P, // 22 - ElrsButton::CH4M, // 23 - ElrsButton::CH3M, // 24 -]; - -// 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 | | -// | | 17 | 15 | 18 | | 22 | 20 | 23 | | -// | | 19 | | 24 | | -// --------------------------------------------------------------- -// -pub const CONFIG_MAP: [ConfigButton; NUMBER_OF_BUTTONS] = [ - // Config Key // Button Index - // ----------------------------------------- - ConfigButton::BOOT, // 0 - ConfigButton::CAL, // 1 - ConfigButton::CONF, // 2 - ConfigButton::USB, // 3 - ConfigButton::ELRS, // 4 - ConfigButton::NoEventIndicated, // 5 - ConfigButton::NoEventIndicated, // 6 - ConfigButton::NoEventIndicated, // 7 - ConfigButton::NoEventIndicated, // 8 - ConfigButton::NoEventIndicated, // 9 - ConfigButton::NoEventIndicated, // 10 - ConfigButton::NoEventIndicated, // 11 - ConfigButton::NoEventIndicated, // 12 - ConfigButton::NoEventIndicated, // 13 - ConfigButton::NoEventIndicated, // 14 - ConfigButton::NoEventIndicated, // 15 - ConfigButton::NoEventIndicated, // 16 - ConfigButton::NoEventIndicated, // 17 - ConfigButton::NoEventIndicated, // 18 - ConfigButton::NoEventIndicated, // 19 - ConfigButton::NoEventIndicated, // 20 - ConfigButton::NoEventIndicated, // 21 - ConfigButton::NoEventIndicated, // 22 - ConfigButton::NoEventIndicated, // 23 - ConfigButton::NoEventIndicated, // 24 -]; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index cc2ad38..84ca5b9 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -3,13 +3,62 @@ //! 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 | | +// --------------------------------------------------------------- #![no_std] #![no_main] mod button_matrix; mod elrs; -mod layout; mod status_led; mod usb_joystick_device; @@ -78,16 +127,24 @@ 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; + // 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: 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 elrs_changed: bool, pub elrs_changed_to: bool, + pub usb_release_timeout: u16, } #[derive(Copy, Clone)] @@ -98,10 +155,11 @@ pub struct GimbalAxis { pub max: u16, pub min: u16, pub center: u16, - pub fn_mode: u8, pub deadzone: (u16, u16, u16), pub expo: bool, pub trim: i16, + pub hold: u16, + pub hold_pending: bool, } impl Default for GimbalAxis { @@ -113,10 +171,11 @@ impl Default for GimbalAxis { max: AXIS_MAX, min: AXIS_MIN, center: AXIS_CENTER, - fn_mode: 0, deadzone: (100, 50, 100), expo: true, trim: 0, + hold: 0, + hold_pending: false, } } } @@ -263,7 +322,6 @@ fn main() -> ! { 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 usb_activity: bool = false; let mut idle: bool = false; @@ -271,20 +329,49 @@ fn main() -> ! { let mut elrs_active: bool = false; let mut 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 channel_locks: [bool; 12] = [false; 12]; let mut gimbal_mode: u8; + // Set up 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; + // 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); - // 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 = (100, 0, 100); - axis[GIMBAL_AXIS_LEFT_Y].expo = false; - // Create dynamic smoother array for gimbal axis let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [ DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), @@ -364,6 +451,7 @@ fn main() -> ! { &elrs.connected(), &safety_check, &calibration_active, + &throttle_hold, ); } @@ -373,8 +461,13 @@ fn main() -> ! { } 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 button_matrix.buttons_pressed()[0] && button_matrix.buttons_pressed()[2] { + 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; @@ -385,22 +478,24 @@ fn main() -> ! { } // ON/OFF switch for ELRS radio - if button_matrix.buttons_pressed()[4] - && button_matrix.buttons_pressed()[2] - && !elrs_active - { + if buttons[4].pressed && buttons[2].pressed && !elrs_active { safety_check = false; elrs_active = true; - } else if button_matrix.buttons_pressed()[3] - && button_matrix.buttons_pressed()[2] - && elrs_active - { + } else if buttons[3].pressed && buttons[2].pressed && elrs_active { elrs_active = false; } - // Calibration of center position (pressing all right hands buttons except the hat - // switch) - if button_matrix.buttons_pressed()[1] && button_matrix.buttons_pressed()[2] { + // 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; @@ -420,14 +515,14 @@ fn main() -> ! { } } - if calibration_active && button_matrix.buttons_pressed()[8] { + 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 && button_matrix.buttons_pressed()[9] { + } 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; @@ -436,7 +531,7 @@ fn main() -> ! { } } // Save calibration data to eeprom (pressing right hat switch) - else if calibration_active && button_matrix.buttons_pressed()[20] { + 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; @@ -464,37 +559,39 @@ fn main() -> ! { ); } - // Update pressed keys status - for (index, key) in button_matrix.buttons_pressed().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 - mode = get_mode(button_matrix.buttons_pressed()); - 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 that 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::Fn - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeL - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR - && index != 2) - || (elrs_active - && layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated - && index != 2) - { + // Set idle state + for key in buttons.iter_mut() { + if key.pressed { idle = false; } } @@ -507,23 +604,28 @@ fn main() -> ! { 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::Fn - && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR) - || (elrs_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 = key.pressed; key.elrs_changed = true; key.elrs_changed_to = key.pressed; usb_activity = true; } + // Set throttle_hold_value + if key.pressed != key.previous_pressed && key.pressed && throttle_hold && index == 5 + { + 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; } @@ -553,11 +655,10 @@ fn main() -> ! { // 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, - &mode, - )) { + match usb_hid_joystick + .device() + .write_report(&get_joystick_report(&mut buttons, &mut axis)) + { Err(UsbHidError::WouldBlock) => {} Ok(_) => {} Err(e) => { @@ -590,6 +691,7 @@ fn update_status_led( elrs_connected: &bool, safety_check: &bool, calibration_active: &bool, + throttle_hold: &bool, ) where P: PIOExt + FunctionConfig, I: PinId, @@ -602,7 +704,9 @@ fn update_status_led( status_led.update(StatusMode::Warning); } else if !*usb_active && !*elrs_active { status_led.update(StatusMode::NormalFlash); - } else if *usb_active && !*elrs_active { + } else if *usb_active && !*elrs_active && *throttle_hold { + status_led.update(StatusMode::Activity); + } else if *usb_active && !*elrs_active && !*throttle_hold { status_led.update(StatusMode::Normal); } else if *elrs_active && *elrs_connected { status_led.update(StatusMode::Other); @@ -611,50 +715,6 @@ fn update_status_led( } } -/// 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_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::Fn { - fn_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 alt_r_active && fn_active { - mode = 3; - } else if alt_r_active { - mode = 2; - } else if fn_active { - mode = 1; - } - - // Set bit 4 and 5 if mode 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, 2 or 3) /// layout::MAP contains the keycodes for each key in each Fn mode /// @@ -662,104 +722,106 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { /// /// * `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_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 == 1) { + 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 == 1 { y = AXIS_CENTER; 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::H1U - && layout::HID_MAP[key.fn_mode as usize][index] <= layout::HidButton::H4B - { - hats[(layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::H1U as usize) - / 5] |= 1 - << ((layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::H1U as usize) - - (5 * ((layout::HID_MAP[key.fn_mode as usize][index] as usize - - layout::HidButton::H1U 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) << 28 - | ((hat_button2 as u32) << 29) - | ((hat_button3 as u32) << 30) - | ((hat_button4 as u32) << 31); + 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-28 - for (index, key) in matrix_keys.iter_mut().enumerate() { + // 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; + key.usb_changed = false; + } 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::B28 as usize + && key.usb_button != 0 + && key.usb_button_toggle_enable + && key.usb_release_timeout > 1 { - buttons |= 1 << layout::HID_MAP[key.fn_mode as usize][index] as usize; + 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; } } @@ -782,8 +844,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) { @@ -797,21 +859,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; } @@ -934,47 +996,62 @@ fn get_elrs_channels( let mut channels: [u16; 12] = [ELRS_MIN; 12]; // Check and store trim values - let mut trim_active = false; for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.pressed - && layout::ELRS_MAP[index] >= layout::ElrsButton::CH1P - && layout::ELRS_MAP[index] <= layout::ElrsButton::CH4P - { - if axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim - < ELRS_CENTER as i16 - { - axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim += - 1; - } - trim_active = true; + // 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 - && layout::ELRS_MAP[index] >= layout::ElrsButton::CH1M - && layout::ELRS_MAP[index] <= layout::ElrsButton::CH4M + && index == 19 + && axis[GIMBAL_AXIS_LEFT_X].trim > (0 - ELRS_CENTER as i16) { - if axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim - > (0 - ELRS_CENTER as i16) - { - axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim -= - 1; - } - trim_active = true; + 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 !trim_active - && key.elrs_changed - && key.elrs_changed_to - && layout::ELRS_MAP[index] == layout::ElrsButton::CH12Z - { + if key.elrs_changed && key.elrs_changed_to && index == 15 && hat_left_button_only { axis[GIMBAL_AXIS_LEFT_X].trim = 0; axis[GIMBAL_AXIS_LEFT_Y].trim = 0; - } else if !trim_active - && key.elrs_changed - && key.elrs_changed_to - && layout::ELRS_MAP[index] == layout::ElrsButton::CH34Z - { + } else if key.elrs_changed && key.elrs_changed_to && index == 20 && hat_right_button_only { axis[GIMBAL_AXIS_RIGHT_X].trim = 0; axis[GIMBAL_AXIS_RIGHT_Y].trim = 0; } @@ -1009,36 +1086,45 @@ fn get_elrs_channels( } } - // Update locking button state for ELRS channel 5-12 + // Update button state for ELRS channels for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.pressed - && layout::ELRS_MAP[index] as usize >= layout::ElrsButton::CH5ON as usize - && layout::ELRS_MAP[index] as usize <= layout::ElrsButton::CH12ON as usize - { - channel_locks - [layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH5ON as usize + 4] = true; + if key.pressed && index == 1 { + channel_locks[6] = true; + } else if key.pressed && index == 0 { + channel_locks[6] = false; } - if key.pressed - && layout::ELRS_MAP[index] as usize >= layout::ElrsButton::CH5OFF as usize - && layout::ELRS_MAP[index] as usize <= layout::ElrsButton::CH12OFF as usize - { - channel_locks - [layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH5OFF as usize + 4] = - false; + if key.pressed && index == 4 { + channel_locks[7] = true; + } else if key.pressed && index == 3 { + channel_locks[7] = false; + } + if key.pressed && index == 8 { + channel_locks[9] = true; + } else if key.pressed && index == 9 { + channel_locks[9] = false; + } + if key.pressed && index == 10 { + channel_locks[10] = true; + } else if key.pressed && index == 11 { + channel_locks[10] = false; + } + if key.pressed && index == 13 { + channel_locks[11] = true; + } else if key.pressed && index == 14 { + channel_locks[11] = false; + } + if key.pressed && index == 7 { + channels[4] = ELRS_MAX; + } + if key.pressed && index == 12 { + channels[5] = ELRS_MAX; + } + if key.pressed && index == 6 { + channels[7] = ELRS_MAX; } } - // Update button state for ELRS channel 5-12 - for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.pressed - && layout::ELRS_MAP[index] as usize >= layout::ElrsButton::CH5 as usize - && layout::ELRS_MAP[index] as usize <= layout::ElrsButton::CH12 as usize - { - channels[layout::ELRS_MAP[index] as usize] = ELRS_MAX; - } - } - - // Apply locking to ELRS channel 5-12 + // Apply locking to ELRS channels for (index, lock_active) in channel_locks.iter().enumerate() { if *lock_active { channels[index] = ELRS_MAX; diff --git a/rp2040/src/usb_joystick_device.rs b/rp2040/src/usb_joystick_device.rs index 33595f3..f2793d3 100644 --- a/rp2040/src/usb_joystick_device.rs +++ b/rp2040/src/usb_joystick_device.rs @@ -90,12 +90,15 @@ pub const JOYSTICK_DESCRIPTOR: &[u8] = &[ 0xc0, // End Collection 0x05, 0x09, // Usage Page (Button) 0x19, 0x01, // Usage Minimum (1) - 0x29, 0x20, // Usage Maximum (32) + 0x29, 0x19, // Usage Maximum (25) 0x15, 0x00, // Logical Minimum (0) 0x25, 0x01, // Logical Maximum (1) 0x75, 0x01, // Report Size (1) - 0x95, 0x20, // Report Count (32) - 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) From 41ce7d3b4053fce036628678a84886c4bba9444d Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sat, 20 Jul 2024 17:40:08 +0200 Subject: [PATCH 27/30] Removed compiler warnings --- rp2040/src/main.rs | 2 +- rp2040/src/status_led.rs | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 84ca5b9..af1d038 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -327,7 +327,7 @@ fn main() -> ! { let mut idle: bool = false; let mut usb_active: bool = false; let mut elrs_active: bool = false; - let mut elrs_connected: bool = false; + let _elrs_connected: bool = false; let mut calibration_active: bool = false; let mut throttle_hold: bool = false; diff --git a/rp2040/src/status_led.rs b/rp2040/src/status_led.rs index 40abfc7..f5a23ec 100644 --- a/rp2040/src/status_led.rs +++ b/rp2040/src/status_led.rs @@ -94,9 +94,11 @@ 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 From 94b24fa0ac89098ef2f1cbd6757f2c412208ec04 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sat, 20 Jul 2024 20:55:33 +0200 Subject: [PATCH 28/30] Code cleanup --- rp2040/src/main.rs | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index af1d038..6190c03 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -1,4 +1,4 @@ -//! Project: CMtec CMDR joystick 24 +//! Project: CMtec CMDR joystick 25 //! Date: 2023-08-01 //! Author: Christoffer Martinsson //! Email: cm@cmtec.se @@ -53,6 +53,20 @@ // | |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] @@ -127,7 +141,7 @@ 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; +pub const RELEASE_RIMEOUT: u16 = 30; // => 300ms // Public types #[derive(Copy, Clone, Default)] From f3c2895810df2fa785c9263b8a857440791aec32 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sat, 20 Jul 2024 20:55:45 +0200 Subject: [PATCH 29/30] Updated Readme --- README.md | 140 +++++++++++++++++++++++------------------------------- 1 file changed, 60 insertions(+), 80 deletions(-) diff --git a/README.md b/README.md index 13c795c..ebed1f2 100644 --- a/README.md +++ b/README.md @@ -5,89 +5,69 @@ RC(ELRS)/USB Joystick with 2 hall effect gimbals, 2 hat switches and 25 buttons ## Layout ```cpp -USB Joystick Layer 0 --------------------------------------------------------------- -| FnL | B1 | | B21 | | 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 | | B21 | | 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 | | B21 | | 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 | | B21 | | 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 OFF | CH7 ON | | -- | | CH8 ON | CH8 OFF| --------------------------------------------------------------- -| | CH9 ON | CH9 OFF | CH5 | | CH6 | CH10 OFF | CH10 ON | -| | -| | CH11 ON | | CH12 ON | | -| | CH11 OFF | | CH12 OFF | | -| X(CH1)/Y(CH2) X(CH3)/Y(CH4) | -| | LTU | | RTU | | -| | LTL | LTC | LTR || RTL | RTC | RTR | | -| | LTD | | RTD | | --------------------------------------------------------------- - -Config Layer --------------------------------------------------------------- -| BOOT | CAL | | CONFIG | | USB | ELRS | --------------------------------------------------------------- -| | -- | -- | -- | | -- | -- | -- | | -| | -| | --- | | --- | | -| | --- | | --- | | -| X(CH1)/Y(CH2) X(CH3)/Y(CH4) | -| | --- | | --- | | -| | --- | --- | --- || --- | --- | --- | | -| | --- | | --- | | --------------------------------------------------------------- +Config Layer (holding CONFIG button) +--------------------------------------------------------------- +|BOOT L| CAL U| | CONFIG | | USB L|ELRS U| +--------------------------------------------------------------- +| | THL-| THL+| - | | - | - | - | | +| | +| | - | | - | | +| | - | | - | | +| -/- -/- | +| | - | | - | | +| | - | - | - | | - | - | - | | +| | - | | - | | +--------------------------------------------------------------- ``` From b6d4b5ccee9ad7571130c7da2f258f89298a38ba Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Fri, 26 Jul 2024 12:33:55 +0200 Subject: [PATCH 30/30] Updated button and axis struct --- rp2040/src/main.rs | 141 +++++++++++++++++++++++++++------------------ 1 file changed, 85 insertions(+), 56 deletions(-) diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 6190c03..1cbc06a 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -149,16 +149,20 @@ pub struct Button { pub pressed: bool, pub previous_pressed: bool, pub usb_changed: bool, - pub usb_changed_to: 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 elrs_changed: bool, - pub elrs_changed_to: 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)] @@ -347,10 +351,9 @@ fn main() -> ! { 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 channel_locks: [bool; 12] = [false; 12]; let mut gimbal_mode: u8; - // Set up button layout + // Set up usb button layout buttons[0].usb_button = 1; buttons[1].usb_button = 2; buttons[2].usb_button = 3; @@ -383,6 +386,36 @@ fn main() -> ! { buttons[14].usb_button_sec = 24; buttons[14].usb_button_sec_trigger_index = 12; + // 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); @@ -629,9 +662,9 @@ fn main() -> ! { for (index, key) in buttons.iter_mut().enumerate() { if key.pressed != key.previous_pressed { key.usb_changed = true; - key.usb_changed_to = key.pressed; + key.usb_changed_to_pressed = key.pressed; key.elrs_changed = true; - key.elrs_changed_to = key.pressed; + key.elrs_changed_to_pressed = key.pressed; usb_activity = true; } // Set throttle_hold_value @@ -645,8 +678,11 @@ fn main() -> ! { // Reset channel locks when calibration is active if calibration_active { - for lock_active in channel_locks.iter_mut() { - *lock_active = false; + for axis in axis.iter_mut() { + axis.hold = 0; + } + for button in buttons.iter_mut() { + button.elrs_lock_state = false; } } } @@ -655,11 +691,7 @@ fn main() -> ! { // Send ELRS data if elrs_active { elrs_en_pin.set_high().unwrap(); - elrs.send(get_elrs_channels( - &mut buttons, - &mut axis, - &mut channel_locks, - )); + elrs.send(get_elrs_channels(&mut buttons, &mut axis)); } else { elrs_en_pin.set_low().unwrap(); elrs.reset(); @@ -810,7 +842,6 @@ fn get_joystick_report( // Toggle mode button if key.usb_changed && key.usb_button_toggle_enable { key.usb_release_timeout = RELEASE_RIMEOUT; - key.usb_changed = false; } if key.pressed && key.usb_button != 0 @@ -839,6 +870,11 @@ fn get_joystick_report( } } + // Reset changed flags + for key in matrix_keys.iter_mut() { + key.usb_changed = false; + } + JoystickReport { x, y, @@ -1005,7 +1041,6 @@ fn generate_expo_lut(expo: f32) -> [u16; AXIS_MAX as usize + 1] { fn get_elrs_channels( matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], axis: &mut [GimbalAxis; 4], - channel_locks: &mut [bool; 12], ) -> [u16; 12] { let mut channels: [u16; 12] = [ELRS_MIN; 12]; @@ -1062,14 +1097,17 @@ fn get_elrs_channels( // Check and reset trim values for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.elrs_changed && key.elrs_changed_to && index == 15 && hat_left_button_only { + 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 && index == 20 && hat_right_button_only { + } 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; } - key.elrs_changed = false; } // Match ELRS channel 1-4 to new min/max values @@ -1101,49 +1139,40 @@ fn get_elrs_channels( } // Update button state for ELRS channels - for (index, key) in matrix_keys.iter_mut().enumerate() { - if key.pressed && index == 1 { - channel_locks[6] = true; - } else if key.pressed && index == 0 { - channel_locks[6] = false; - } - if key.pressed && index == 4 { - channel_locks[7] = true; - } else if key.pressed && index == 3 { - channel_locks[7] = false; - } - if key.pressed && index == 8 { - channel_locks[9] = true; - } else if key.pressed && index == 9 { - channel_locks[9] = false; - } - if key.pressed && index == 10 { - channel_locks[10] = true; - } else if key.pressed && index == 11 { - channel_locks[10] = false; - } - if key.pressed && index == 13 { - channel_locks[11] = true; - } else if key.pressed && index == 14 { - channel_locks[11] = false; - } - if key.pressed && index == 7 { - channels[4] = ELRS_MAX; - } - if key.pressed && index == 12 { - channels[5] = ELRS_MAX; - } - if key.pressed && index == 6 { - channels[7] = ELRS_MAX; + 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 (index, lock_active) in channel_locks.iter().enumerate() { - if *lock_active { - channels[index] = ELRS_MAX; + 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 }