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

Merge branch 'main' of https://git.cmtec.se/cm/cmdr-joystick

Browse Source
This commit is contained in:
Christoffer Martinsson 2024-09-21 10:38:21 +02:00
commit abc5b70e00
9 changed files with 1092 additions and 672 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -2,3 +2,4 @@ firmware/.cache/clangd/index
firmware/compile_commands.json firmware/compile_commands.json
rp2040/target rp2040/target
rp2040/Cargo.lock rp2040/Cargo.lock
firmware/.pio/build

182
README.md
View File

@ -1,79 +1,73 @@
# CMDR Joystick 24 # CMDR Joystick 25
RC Joystick with 2 hall effect gimbals, 2 hat switches and 24 buttons for use both with simulators and ELRS Rx equipped quads. RC(ELRS)/USB Joystick with 2 hall effect gimbals, 2 hat switches and 25 buttons for use both with PC simulators and ELRS Rx equipped quads.
## Layout ## Layout
```cpp ```cpp
USB Joystick Layer 0 Button index map:
-------------------------------------------------------------- ---------------------------------------------------------------
| FnL | B1 | | B5 | FnR | | 0 L| 1 U| | 2 | | 3 L| 4 U|
-------------------------------------------------------------- ---------------------------------------------------------------
| | B2 | B3 | MoL | | MoR | B7 | B6 | | | | 5 | 6 | 7 | | 12 | 11 | 10 | |
| | | |
| | B4 | | B8 | | | | 8 | | 13 | |
| | B17 | | B18 | | | | 9 | | 14 | |
| Z/RZ X/Y | | 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 | | | | H1U | | H2U | |
| | H1L | H1B | H1R || H2L | H2B | H2R | | | | H1L | B18 | H1R | | H2L | B19 | H2R | |
| | H1D | | H2D | | | | 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 1 (FnL) ELRS channel map (+ = ON, - = OFF, CHxP/M/Z = trim)
-------------------------------------------------------------- ---------------------------------------------------------------
| FnL | B9 | | B5 | FnR | |CH7-L|CH7+U| | - | |CH8-L|CH8+U|
-------------------------------------------------------------- ---------------------------------------------------------------
| | B10 | B11 | MoL | | MoR | B7 | B6 | | | | THL | CH9 | CH5 | | CH6 |CH11-|CH11+| |
| | | |
| | B12 | | B8 | | | |CH10+| |CH12+| |
| | B19 | | B18 | | | |CH10-| |CH12-| |
| Z/RZ X/Y | | CH1/CH2 CH3/CH4 |
| | H3U | | H2U | | | | - | |CH4P | |
| | H3L | H3B | H3R || H2L | H2B | H2R | | | |CH1M |CH12Z|CH1P | |CH3M |CH34Z|CH3P | |
| | H3D | | H2D | | | | - | |CH4M | |
-------------------------------------------------------------- ---------------------------------------------------------------
USB Joystick Layer 2 (FnR) Config Layer (holding CONFIG button)
-------------------------------------------------------------- ---------------------------------------------------------------
| FnL | B1 | | B13 | FnR | |BOOT L| CAL U| | CONFIG | | USB L|ELRS U|
-------------------------------------------------------------- ---------------------------------------------------------------
| | B2 | B3 | MoL | | MoR | B15 | B14 | | | | THL-| THL+| - | | - | - | - | |
| |
| | B4 | | B16 | |
| | B17 | | B20 | |
| Z/RZ X(RX)/Y(RY) |
| | H1U | | H4U | |
| | H1L | H1B | H1R || H4L | H4B | H4R | |
| | H1D | | H4D | |
--------------------------------------------------------------
USB Joystick Layer 3 (FnL + FnR)
--------------------------------------------------------------
| FnL | B9 | | B13 | FnR |
--------------------------------------------------------------
| | B10 | B11 | MoL | | MoR | B15 | B14 | |
| |
| | B12 | | B16 | |
| | B19 | | B20 | |
| Z/RZ X(RX)/Y(RY) |
| | H3U | | H4U | |
| | H3L | H3B | H3R || H4L | H4B | H4R | |
| | H3D | | H4D | |
--------------------------------------------------------------
ELRS Layer
--------------------------------------------------------------
| CH7 | CH8 | | CH9 | CH10|
--------------------------------------------------------------
| | CH11| - | CH5 | | CH6 | - | CH12| |
| | | |
| | - | | - | | | | - | | - | |
| | - | | - | | | | - | | - | |
| X(CH1)/Y(CH2) X(CH3)/Y(CH4) | | -/- -/- |
| | - | | - | | | | - | | - | |
| | - | - | - || - | - | - | | | | - | - | - | | - | - | - | |
| | - | | - | | | | - | | - | |
-------------------------------------------------------------- ---------------------------------------------------------------
``` ```
@ -81,31 +75,61 @@ ELRS Layer
- Ergonomic design (low profile) - Ergonomic design (low profile)
- Hall effect gimbals - Hall effect gimbals
- Supports both USB HID joystick and ELRS Tx module - Supports both USB HID joystick and ELRS Tx
- Total 6x axis, 4x hat switches and 24x buttons (using Fn mode) implemented in USB HID mode - 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) - 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 ## Hardware
- 1x rp2040zero MCU board - 2x FrSky M7 or M10 gimbals [M7 datasheet](https://www.frsky-rc.com/product/m7/)
- 2x FrSky M7 or M10 gimbals - 6x Kailh choc low profile switches [Brown](http://www.kailh.com/en/Products/Ks/CS/)
- 6x Kailh choc low profile switches - 6x Cherry MX switches [Brown](https://www.cherrymx.de/en/cherry-mx/mx-original/mx-brown.html)
- 6x Cherry MX switches - 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 Miniature Toggle Switch (M6 shaft, 7mm wide body) - 2x Alpine RKJXM1015004 hat switches [pdf](https://www.mouser.se/datasheet/2/15/RKJXM-1662398.pdf)
- 2x Alpine RKJXM1015004 hat switches
- 1x PCB (prototype board)
- 1x Bottom case (3D printed) - 1x Bottom case (3D printed)
- 1x Top plate (3D printed) - 1x Top plate (3D printed)
- 2x Hat swith top (3D printed) - 2x Hat swith top (3D printed) [stl](/mCAD/Hat_Castle_Short_scale_99_99_130.stl)
- 1x Custom PCB (CMDR Joystick 25 rev A)
- ![pcb_top](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_top.png)
- ![pcb_bottom](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_board_bottom.png)
- Gerber files: [zip](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_gerber.zip)
- Schematics: [pdf](/eCAD/cmdr-joystick/cmdr-joystick_rev_a_schematics.pdf)
- rp2040zero pinout: [jpg](https://www.waveshare.com/w/upload/2/2b/RP2040-Zero-details-7.jpg)
- rp2040zero schematic: [pdf](https://www.waveshare.com/w/upload/4/4c/RP2040_Zero.pdf)
- eeprom 24C32LV: [pdf](https://www.mouser.se/datasheet/2/308/1/NV24C32LV_D-2319484.pdf)
- P-Fet si2372eds: [pdf](https://www.vishay.com/docs/63924/si2371eds.pdf)
- N-Fet 2N7002: [pdf](https://www.mouser.se/datasheet/2/408/T2N7002AK_datasheet_en_20150401-1916411.pdf)
- Small signal diod 1N4148W: [pdf](https://www.diodes.com/assets/Datasheets/BAV16W_1N4148W.pdf)
- 1x ELRS TX (using a EP1 TCXO Dual receiver reprogrammed as a tramsmitter)
- [Link to EP1](https://www.happymodel.cn/index.php/2022/11/07/2-4g-elrs-ep1-ep2-ep1dual-tcxo-receiver/)
- Reprogramming instructions (using ExpressLRS Configurator):
1. Select latest release
2. Device category = Generic target used as base 2.4Ghz
3. Device = Generic ESP32 2.4GHz Gemini TX
4. Flash to device
5. Use following custom settings in 10.0.0.1/hardware.html:
```
{"customised":"true","serial_rx":3,"serial_tx":1,"radio_busy":36,"radio_dio1":37,"radio_miso":33,"radio_mosi":32,"radio_nss":27,"radio_rst":26,"radio_sck":25,"radio_busy_2":39,"radio_dio1_2":34,"radio_nss_2":13,"power_rxen":10,"power_txen":14,"power_rxen_2":9,"power_txen_2":15,"power_min":0,"power_high":2,"power_max":2,"power_default":0,"power_control":0,"power_values":[-10,-6,-3],"button":0,"led_rgb":22,"led_rgb_isgrb":true,"screen_type":0}
```
## Software Build environment
##### Rust
- Cargo (rust embedded)
- Flashing via Cargo
- Press and hold boot button on rp2040zero board while perform a reset
- Press and hold CONF and press BOOT button.
## References
- CRSF protocol description (for communicating with ELRS TX): [Link](https://github.com/ExpressLRS/ExpressLRS/wiki/CRSF-Protocol)
- rp2040 datasheet: [pdf](https://datasheets.raspberrypi.com/rp2040/rp2040-datasheet.pdf)
## Calibration ## Calibration
No calibration needed 1. Center both gimbals.
2. Press and hold CONF button and press CAL botton. Status led will start blinking green.
3. Move both gimbals to all corners.
4. Press right hat switch to save calibration data to eeprom.

1
rp2040/Cargo.toml
View File

@ -23,6 +23,7 @@ pio = "0.2.0"
defmt = { version = "0.3", optional = true } defmt = { version = "0.3", optional = true }
libm = "0.2.7" libm = "0.2.7"
dyn-smooth = "0.2.0" dyn-smooth = "0.2.0"
eeprom24x = "0.6.0"
[features] [features]
# This is the set of features we enable by default # This is the set of features we enable by default

4
rp2040/src/button_matrix.rs
View File

@ -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) { pub fn scan_matrix(&mut self, delay: &mut Delay) {
for col_index in 0..self.cols.len() { for col_index in 0..self.cols.len() {
self.cols[col_index].set_low().unwrap(); self.cols[col_index].set_low().unwrap();
delay.delay_us(10); delay.delay_us(1);
self.process_column(col_index); self.process_column(col_index);
self.cols[col_index].set_high().unwrap(); self.cols[col_index].set_high().unwrap();
delay.delay_us(10); delay.delay_us(1);
} }
} }

194
rp2040/src/elrs.rs Normal file
View File

@ -0,0 +1,194 @@
//! Project: CMtec CMDR joystick 24
//! Date: 2023-08-01
//! Author: Christoffer Martinsson
//! Email: cm@cmtec.se
//! License: Please refer to LICENSE in root directory
use embedded_hal::serial::Read;
use rp2040_hal::uart::{Enabled, State, UartDevice, UartPeripheral, ValidUartPinout};
const CRSF_CRC8TAB: [u8; 256] = [
0x00, 0xD5, 0x7F, 0xAA, 0xFE, 0x2B, 0x81, 0x54, 0x29, 0xFC, 0x56, 0x83, 0xD7, 0x02, 0xA8, 0x7D,
0x52, 0x87, 0x2D, 0xF8, 0xAC, 0x79, 0xD3, 0x06, 0x7B, 0xAE, 0x04, 0xD1, 0x85, 0x50, 0xFA, 0x2F,
0xA4, 0x71, 0xDB, 0x0E, 0x5A, 0x8F, 0x25, 0xF0, 0x8D, 0x58, 0xF2, 0x27, 0x73, 0xA6, 0x0C, 0xD9,
0xF6, 0x23, 0x89, 0x5C, 0x08, 0xDD, 0x77, 0xA2, 0xDF, 0x0A, 0xA0, 0x75, 0x21, 0xF4, 0x5E, 0x8B,
0x9D, 0x48, 0xE2, 0x37, 0x63, 0xB6, 0x1C, 0xC9, 0xB4, 0x61, 0xCB, 0x1E, 0x4A, 0x9F, 0x35, 0xE0,
0xCF, 0x1A, 0xB0, 0x65, 0x31, 0xE4, 0x4E, 0x9B, 0xE6, 0x33, 0x99, 0x4C, 0x18, 0xCD, 0x67, 0xB2,
0x39, 0xEC, 0x46, 0x93, 0xC7, 0x12, 0xB8, 0x6D, 0x10, 0xC5, 0x6F, 0xBA, 0xEE, 0x3B, 0x91, 0x44,
0x6B, 0xBE, 0x14, 0xC1, 0x95, 0x40, 0xEA, 0x3F, 0x42, 0x97, 0x3D, 0xE8, 0xBC, 0x69, 0xC3, 0x16,
0xEF, 0x3A, 0x90, 0x45, 0x11, 0xC4, 0x6E, 0xBB, 0xC6, 0x13, 0xB9, 0x6C, 0x38, 0xED, 0x47, 0x92,
0xBD, 0x68, 0xC2, 0x17, 0x43, 0x96, 0x3C, 0xE9, 0x94, 0x41, 0xEB, 0x3E, 0x6A, 0xBF, 0x15, 0xC0,
0x4B, 0x9E, 0x34, 0xE1, 0xB5, 0x60, 0xCA, 0x1F, 0x62, 0xB7, 0x1D, 0xC8, 0x9C, 0x49, 0xE3, 0x36,
0x19, 0xCC, 0x66, 0xB3, 0xE7, 0x32, 0x98, 0x4D, 0x30, 0xE5, 0x4F, 0x9A, 0xCE, 0x1B, 0xB1, 0x64,
0x72, 0xA7, 0x0D, 0xD8, 0x8C, 0x59, 0xF3, 0x26, 0x5B, 0x8E, 0x24, 0xF1, 0xA5, 0x70, 0xDA, 0x0F,
0x20, 0xF5, 0x5F, 0x8A, 0xDE, 0x0B, 0xA1, 0x74, 0x09, 0xDC, 0x76, 0xA3, 0xF7, 0x22, 0x88, 0x5D,
0xD6, 0x03, 0xA9, 0x7C, 0x28, 0xFD, 0x57, 0x82, 0xFF, 0x2A, 0x80, 0x55, 0x01, 0xD4, 0x7E, 0xAB,
0x84, 0x51, 0xFB, 0x2E, 0x7A, 0xAF, 0x05, 0xD0, 0xAD, 0x78, 0xD2, 0x07, 0x53, 0x86, 0x2C, 0xF9,
];
const CONNECTION_TIMEOUT: u16 = 500;
const INIT_TIMEOUT: u16 = 1000;
pub struct Elrs<S: State, D: UartDevice, P>
where
S: State,
D: UartDevice,
P: ValidUartPinout<D>,
{
uart: UartPeripheral<S, D, P>,
elsr_init_done: bool,
elrs_init_counter: u16,
elrs_connected: bool,
elrs_connected_timeout: u16,
rx_buffer_index: usize,
}
impl<S, D, P> Elrs<S, D, P>
where
S: State,
D: UartDevice,
P: ValidUartPinout<D>,
{
pub fn new(uart: UartPeripheral<S, D, P>) -> Self {
let elsr_init_done = false;
let elrs_init_counter = 0;
let elrs_connected = false;
let elrs_connected_timeout = CONNECTION_TIMEOUT;
let rx_buffer_index = 0;
Self {
uart,
elsr_init_done,
elrs_init_counter,
elrs_connected,
elrs_connected_timeout,
rx_buffer_index,
}
}
}
impl<D, P> Elrs<Enabled, D, P>
where
D: UartDevice,
P: ValidUartPinout<D>,
{
pub fn send(&mut self, data: [u16; 12]) {
if self.elsr_init_done {
self.uart
.write_full_blocking(&self.prepare_crsf_data_packet(data));
return;
}
if self.elrs_init_counter < INIT_TIMEOUT {
self.uart
.write_full_blocking(&self.prepare_crsf_data_packet(data));
self.elrs_init_counter += 1;
} else if self.elrs_init_counter < INIT_TIMEOUT + 5 {
self.uart
// Setting Packet Rate to 150Hz
// 0 = 50Hz LoRa, 1 = 100Hz Full, 2 = 150Hz LoRa, 3 = 250Hz LoRa,
// 4 = 333H Full, 5 = 500Hz LoRa, 6 = 250Hz DejaVu, 7 = 500Hz DejaVu,
// 8 = 500Hz FLRC, 9 = 1000Hz FLRC
.write_full_blocking(&self.prepare_crsf_cmd_packet(0x01, 0x00));
self.elrs_init_counter += 1;
} else if self.elrs_init_counter < INIT_TIMEOUT + 10 {
self.uart
// Setting Power to 10mW
// 0 = 10mW, 1 = 25mW, 2 = 50mW, 3 = 100mW,
// 4 = 200mW, 5 = 500mW, 6 = 1000mW, 7 = 2000mW
.write_full_blocking(&self.prepare_crsf_cmd_packet(0x06, 0x00));
self.elrs_init_counter += 1;
} else {
self.elsr_init_done = true;
}
self.check_link_frame();
}
pub fn connected(&self) -> bool {
self.elrs_connected
}
pub fn reset(&mut self) {
self.elsr_init_done = false;
self.elrs_init_counter = 0;
}
fn check_link_frame(&mut self) {
if self.elrs_connected_timeout == 0 {
self.elrs_connected = false;
} else {
self.elrs_connected_timeout -= 1;
}
let mut rx_byte: u8;
while self.uart.uart_is_readable() {
match self.uart.read() {
Ok(byte) => {
rx_byte = byte;
}
Err(_) => {
continue;
}
}
// Simple RX telemetry link frame detection: 0xEA, 0xXX, 0x14
if (rx_byte == 0xEA && self.rx_buffer_index == 0) || self.rx_buffer_index == 1 {
self.rx_buffer_index += 1;
} else if rx_byte == 0x14 && self.rx_buffer_index == 2 {
self.elrs_connected = true;
self.elrs_connected_timeout = CONNECTION_TIMEOUT;
self.rx_buffer_index = 0;
} else {
self.rx_buffer_index = 0;
}
}
}
fn prepare_crsf_data_packet(&self, data: [u16; 12]) -> [u8; 26] {
let mut packet: [u8; 26] = [0; 26];
let mut crc: u8 = 0;
packet[0] = 0xEE;
packet[1] = 0x18;
packet[2] = 0x16;
packet[3] = data[0] as u8;
packet[4] = ((data[0] >> 8) | (data[1] << 3)) as u8;
packet[5] = ((data[1] >> 5) | (data[2] << 6)) as u8;
packet[6] = (data[2] >> 2) as u8;
packet[7] = ((data[2] >> 10) | (data[3] << 1)) as u8;
packet[8] = ((data[3] >> 7) | (data[4] << 4)) as u8;
packet[9] = ((data[4] >> 4) | (data[5] << 7)) as u8;
packet[10] = (data[5] >> 1) as u8;
packet[11] = ((data[5] >> 9) | (data[6] << 2)) as u8;
packet[12] = ((data[6] >> 6) | (data[7] << 5)) as u8;
packet[13] = (data[7] >> 3) as u8;
packet[14] = data[8] as u8;
packet[15] = ((data[8] >> 8) | (data[9] << 3)) as u8;
packet[16] = ((data[9] >> 5) | (data[10] << 6)) as u8;
packet[17] = (data[10] >> 2) as u8;
packet[18] = ((data[10] >> 10) | (data[11] << 1)) as u8;
packet[19] = (data[11] >> 7) as u8;
// Channel 13-16 are not used in ELRS
for i in 2..25 {
crc = CRSF_CRC8TAB[(crc ^ packet[i]) as usize];
}
packet[25] = crc;
packet
}
fn prepare_crsf_cmd_packet(&self, command: u8, value: u8) -> [u8; 8] {
let mut packet: [u8; 8] = [0; 8];
let mut crc: u8 = 0;
packet[0] = 0xEE;
packet[1] = 0x06;
packet[2] = 0x2D;
packet[3] = 0xEE;
packet[4] = 0xEA;
packet[5] = command;
packet[6] = value;
for i in 2..7 {
crc = CRSF_CRC8TAB[(crc ^ packet[i]) as usize];
}
packet[7] = crc;
packet
}
}

261
rp2040/src/layout.rs
View File

@ -1,261 +0,0 @@
//! Project: CMtec CMDR joystick 24
//! Date: 2023-08-01
//! Author: Christoffer Martinsson
//! Email: cm@cmtec.se
//! License: Please refer to LICENSE in root directory
use crate::NUMBER_OF_BUTTONS;
#[allow(dead_code)]
#[derive(Debug, PartialEq, PartialOrd, Copy, Clone)]
pub enum ElrsButton {
CH1 = 0,
CH2 = 1,
CH3 = 2,
CH4 = 3,
CH5 = 4,
CH6 = 5,
CH7 = 6,
CH8 = 7,
CH9 = 8,
CH10 = 9,
CH11 = 10,
CH12 = 11,
CH5ON = 12,
CH5OFF = 13,
CH6ON = 14,
CH6OFF = 15,
CH7ON = 16,
CH7OFF = 17,
CH8ON = 18,
CH8OFF = 19,
CH9ON = 20,
CH9OFF = 21,
CH10ON = 22,
CH10OFF = 23,
CH11ON = 24,
CH11OFF = 25,
CH12ON = 26,
CH12OFF = 27,
NoEventIndicated = 28,
}
#[derive(Debug, PartialEq, PartialOrd, Copy, Clone)]
pub enum HidButton {
B1 = 0,
B2 = 1,
B3 = 2,
B4 = 3,
B5 = 4,
B6 = 5,
B7 = 6,
B8 = 7,
B9 = 8,
B10 = 9,
B11 = 10,
B12 = 11,
B13 = 12,
B14 = 13,
B15 = 14,
B16 = 15,
B17 = 16,
B18 = 17,
B19 = 18,
B20 = 19,
FnL = 20,
FnR = 21,
ModeL = 22,
ModeR = 23,
Hat1U = 24,
Hat1R = 25,
Hat1D = 26,
Hat1L = 27,
Hat1B = 28,
Hat2U = 29,
Hat2R = 30,
Hat2D = 31,
Hat2L = 32,
Hat2B = 33,
Hat3U = 34,
Hat3R = 35,
Hat3D = 36,
Hat3L = 37,
Hat3B = 38,
Hat4U = 39,
Hat4R = 40,
Hat4D = 41,
Hat4L = 42,
Hat4B = 43,
NoEventIndicated = 44,
}
#[warn(dead_code)]
// Button index map:
// --------------------------------------------------------------
// | 0 | 1 | | 3 | 4 | (2)
// --------------------------------------------------------------
// | | 5 | 6 | 7 | | 12 | 11 | 10 | |
// | |
// | | 8 | | 13 | |
// | | 9 | | 14 | |
// | X1/Y1 X2/Y2 |
// | | 16 | | 21 | |
// | | 17 | 15 | 18 || 22 | 20 | 23 | |
// | | 19 | | 24 | |
// --------------------------------------------------------------
//
/// Button map to HID key (four function layers)
/// Please make sure to set FnL, FnR, ModeL and ModeR at the same position for all layers
/// alt. only set these at function layer 0 and set NoEventIndicated in layer 1-3.
/// Hat button 1-4 = HID B21-B24.
pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [
[
// Function layer 0
// HID Key // Button Index
// -----------------------------------------
HidButton::FnL, // 0
HidButton::B1, // 1
HidButton::NoEventIndicated, // 2 Not connected to any button
HidButton::FnR, // 3
HidButton::B6, // 4
HidButton::B2, // 5
HidButton::B3, // 6
HidButton::ModeL, // 7
HidButton::B4, // 8
HidButton::B5, // 9
HidButton::B7, // 10
HidButton::B8, // 11
HidButton::ModeR, // 12
HidButton::B9, // 13
HidButton::B10, // 14
HidButton::Hat1B, // 15 button 21
HidButton::Hat1U, // 16
HidButton::Hat1R, // 17
HidButton::Hat1D, // 18
HidButton::Hat1L, // 19
HidButton::Hat2B, // 20 button 22
HidButton::Hat2U, // 21
HidButton::Hat2R, // 22
HidButton::Hat2D, // 23
HidButton::Hat2L, // 24
],
[
// Function layer 1 (left Fn button pressed)
// HID Key // Button Index
// -----------------------------------------
HidButton::FnL, // 0
HidButton::B11, // 1
HidButton::NoEventIndicated, // 2 Not connected to any button
HidButton::FnR, // 3
HidButton::B6, // 4
HidButton::B12, // 5
HidButton::B13, // 6
HidButton::ModeL, // 7
HidButton::B14, // 8
HidButton::B15, // 9
HidButton::B7, // 10
HidButton::B8, // 11
HidButton::ModeR, // 12
HidButton::B9, // 13
HidButton::B10, // 14
HidButton::Hat3B, // 15 button 23
HidButton::Hat3U, // 16
HidButton::Hat3R, // 17
HidButton::Hat3D, // 18
HidButton::Hat3L, // 19
HidButton::Hat2B, // 20 button 22
HidButton::Hat2U, // 21
HidButton::Hat2R, // 22
HidButton::Hat2D, // 23
HidButton::Hat2L, // 24
],
[
// Function layer 2 (right Fn button pressed)
// HID Key // Button Index
// -----------------------------------------
HidButton::FnL, // 0
HidButton::B1, // 1
HidButton::NoEventIndicated, // 2 Not connected to any button
HidButton::FnR, // 3
HidButton::B16, // 4
HidButton::B2, // 5
HidButton::B3, // 6
HidButton::ModeL, // 7
HidButton::B4, // 8
HidButton::B5, // 9
HidButton::B17, // 10
HidButton::B18, // 11
HidButton::ModeR, // 12
HidButton::B19, // 13
HidButton::B20, // 14
HidButton::Hat1B, // 15 button 21
HidButton::Hat1U, // 16
HidButton::Hat1R, // 17
HidButton::Hat1D, // 18
HidButton::Hat1L, // 19
HidButton::Hat4B, // 20 button 24
HidButton::Hat4U, // 21
HidButton::Hat4R, // 22
HidButton::Hat4D, // 23
HidButton::Hat4L, // 24
],
[
// Function layer 3 (left + right Fn button pressed)
// HID Key // Button Index
// -----------------------------------------
HidButton::FnL, // 0
HidButton::B11, // 1
HidButton::NoEventIndicated, // 2 Not connected to any button
HidButton::FnR, // 3
HidButton::B16, // 4
HidButton::B12, // 5
HidButton::B13, // 6
HidButton::ModeL, // 7
HidButton::B14, // 8
HidButton::B15, // 9
HidButton::B17, // 10
HidButton::B18, // 11
HidButton::ModeR, // 12
HidButton::B19, // 13
HidButton::B20, // 14
HidButton::Hat3B, // 15 button 23
HidButton::Hat3U, // 16
HidButton::Hat3R, // 17
HidButton::Hat3D, // 18
HidButton::Hat3L, // 19
HidButton::Hat4B, // 20 button 24
HidButton::Hat4U, // 21
HidButton::Hat4R, // 22
HidButton::Hat4D, // 23
HidButton::Hat4L, // 24
],
];
pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [
// Function layer 0
// HID Key // Button Index
// -----------------------------------------
ElrsButton::CH7OFF, // 0
ElrsButton::CH7ON, // 1
ElrsButton::NoEventIndicated, // 2 Not connected to any button
ElrsButton::CH8OFF, // 3
ElrsButton::CH8ON, // 4
ElrsButton::CH9ON, // 5
ElrsButton::CH9OFF, // 6
ElrsButton::CH5, // 7
ElrsButton::CH10ON, // 8
ElrsButton::CH10OFF, // 9
ElrsButton::CH11ON, // 10
ElrsButton::CH11OFF, // 11
ElrsButton::CH6, // 12
ElrsButton::CH12ON, // 13
ElrsButton::CH12OFF, // 14
ElrsButton::NoEventIndicated, // 15
ElrsButton::NoEventIndicated, // 16
ElrsButton::NoEventIndicated, // 17
ElrsButton::NoEventIndicated, // 18
ElrsButton::NoEventIndicated, // 19
ElrsButton::NoEventIndicated, // 20
ElrsButton::NoEventIndicated, // 21
ElrsButton::NoEventIndicated, // 22
ElrsButton::NoEventIndicated, // 23
ElrsButton::NoEventIndicated, // 24
];

1035
rp2040/src/main.rs
View File

@ -1,14 +1,78 @@
//! Project: CMtec CMDR joystick 24 //! Project: CMtec CMDR joystick 25
//! Date: 2023-08-01 //! Date: 2023-08-01
//! Author: Christoffer Martinsson //! Author: Christoffer Martinsson
//! Email: cm@cmtec.se //! Email: cm@cmtec.se
//! License: Please refer to LICENSE in root directory //! License: Please refer to LICENSE in root directory
//
// Button index map:
// ---------------------------------------------------------------
// | 0 L| 1 U| | 2 | | 3 L| 4 U|
// ---------------------------------------------------------------
// | | 5 | 6 | 7 | | 12 | 11 | 10 | |
// | |
// | | 8 | | 13 | |
// | | 9 | | 14 | |
// | X1/Y1 X2/Y2 |
// | | 16 | | 21 | |
// | | 19 | 15 | 17 | | 24 | 20 | 22 | |
// | | 18 | | 23 | |
// ---------------------------------------------------------------
//
// USB HID joystick map:
// ---------------------------------------------------------------
// | B1 L| B2 U| | B3 | | B4 L| B5 U|
// ---------------------------------------------------------------
// | | B6 | B7 | B8/16| |B13/17| B12 | B11 | |
// | |
// | | B9 | | B14 | |
// | | B10 | | B15 | |
// | X1/Y1 X2/Y2 |
// | | H1U | | H2U | |
// | | H1L | B18 | H1R | | H2L | B19 | H2R | |
// | | H1D | | H2D | |
// ---------------------------------------------------------------
// Button (Switch) 7 changes following:
// * hat1 => hat3 (button press B20).
// Button (switch) 12 changes following:
// * B4 => B21
// * B5 => B22
// * B14 => B23
// * B15 => B24
// * hat2 => hat4 (button bpress B25)
//
// ELRS channel map (+ = ON, - = OFF, CHxP/M/Z = trim)
// ---------------------------------------------------------------
// |CH7-L|CH7+U| | - | |CH8-L|CH8+U|
// ---------------------------------------------------------------
// | | THL | CH9 | CH5 | | CH6 |CH11-|CH11+| |
// | |
// | |CH10+| |CH12+| |
// | |CH10-| |CH12-| |
// | CH1/CH2 CH3/CH4 |
// | | - | |CH4P | |
// | |CH1M |CH12Z|CH1P | |CH3M |CH34Z|CH3P | |
// | | - | |CH4M | |
// ---------------------------------------------------------------
//
// Config Layer (holding CONFIG button)
// ---------------------------------------------------------------
// |BOOT L| CAL U| | CONFIG | | USB L|ELRS U|
// ---------------------------------------------------------------
// | | THL-| THL+| - | | - | - | - | |
// | |
// | | - | | - | |
// | | - | | - | |
// | -/- -/- |
// | | - | | - | |
// | | - | - | - | | - | - | - | |
// | | - | | - | |
// ---------------------------------------------------------------
#![no_std] #![no_std]
#![no_main] #![no_main]
mod button_matrix; mod button_matrix;
mod layout; mod elrs;
mod status_led; mod status_led;
mod usb_joystick_device; mod usb_joystick_device;
@ -16,16 +80,20 @@ use button_matrix::ButtonMatrix;
use core::convert::Infallible; use core::convert::Infallible;
use cortex_m::delay::Delay; use cortex_m::delay::Delay;
use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS}; use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS};
use eeprom24x::{Eeprom24x, SlaveAddr};
use elrs::Elrs;
use embedded_hal::adc::OneShot; use embedded_hal::adc::OneShot;
use embedded_hal::digital::v2::*; use embedded_hal::digital::v2::*;
use embedded_hal::timer::CountDown; use embedded_hal::timer::CountDown;
use fugit::ExtU32; use fugit::{ExtU32, RateExtU32};
use libm::powf; use libm::powf;
use panic_halt as _; use panic_halt as _;
use rp2040_hal::{ use rp2040_hal::{
adc::Adc, adc::Adc,
gpio::{Function, FunctionConfig, PinId, ValidPinMode}, gpio::{Function, FunctionConfig, FunctionUart, PinId, ValidPinMode},
i2c::I2C,
pio::StateMachineIndex, pio::StateMachineIndex,
uart::{DataBits, StopBits, UartConfig, UartPeripheral},
}; };
use status_led::{StatusMode, Ws2812StatusLed}; use status_led::{StatusMode, Ws2812StatusLed};
use usb_device::class_prelude::*; use usb_device::class_prelude::*;
@ -52,25 +120,49 @@ pub const NUMBER_OF_BUTTONS: usize = BUTTON_ROWS * BUTTON_COLS;
pub const AXIS_MIN: u16 = 0; pub const AXIS_MIN: u16 = 0;
pub const AXIS_MAX: u16 = 4095; 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 NBR_OF_GIMBAL_AXIS: usize = 4;
pub const GIMBAL_AXIS_LEFT_X: usize = 0; pub const GIMBAL_AXIS_LEFT_X: usize = 0;
pub const GIMBAL_AXIS_LEFT_Y: usize = 1; pub const GIMBAL_AXIS_LEFT_Y: usize = 1;
pub const GIMBAL_AXIS_RIGHT_X: usize = 2; pub const GIMBAL_AXIS_RIGHT_X: usize = 2;
pub const GIMBAL_AXIS_RIGHT_Y: usize = 3; 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. // Analog smoothing settings.
pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS; pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
pub const SAMPLE_FREQ: i32 = 1000 << 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 SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;
pub const DEBOUNCE: u8 = 10;
pub const RELEASE_RIMEOUT: u16 = 30; // => 300ms
// Public types // Public types
#[derive(Copy, Clone, Default)] #[derive(Copy, Clone, Default)]
pub struct Button { pub struct Button {
pub pressed: bool, pub pressed: bool,
pub previous_pressed: bool, pub previous_pressed: bool,
pub fn_mode: u8, pub usb_changed: bool,
pub usb_changed_to_pressed: bool,
pub usb_button: usize,
pub usb_button_sec_enable: bool,
pub usb_button_sec: usize,
pub usb_button_sec_trigger_index: usize,
pub usb_button_sec_pressed: bool,
pub usb_button_toggle_enable: bool,
pub usb_release_timeout: u16,
pub elrs_changed: bool,
pub elrs_changed_to_pressed: bool,
pub elrs_channel: usize,
pub elrs_lock_enable: bool,
pub elrs_lock_state_on: bool,
pub elrs_lock_state: bool,
} }
#[derive(Copy, Clone)] #[derive(Copy, Clone)]
@ -81,9 +173,11 @@ pub struct GimbalAxis {
pub max: u16, pub max: u16,
pub min: u16, pub min: u16,
pub center: u16, pub center: u16,
pub fn_mode: u8,
pub deadzone: (u16, u16, u16), pub deadzone: (u16, u16, u16),
pub expo: f32, pub expo: bool,
pub trim: i16,
pub hold: u16,
pub hold_pending: bool,
} }
impl Default for GimbalAxis { impl Default for GimbalAxis {
@ -95,9 +189,11 @@ impl Default for GimbalAxis {
max: AXIS_MAX, max: AXIS_MAX,
min: AXIS_MIN, min: AXIS_MIN,
center: AXIS_CENTER, center: AXIS_CENTER,
fn_mode: 0, deadzone: (100, 50, 100),
deadzone: (50, 50, 50), expo: true,
expo: 0.2, trim: 0,
hold: 0,
hold_pending: false,
} }
} }
} }
@ -136,6 +232,31 @@ fn main() -> ! {
&mut pac.RESETS, &mut pac.RESETS,
); );
// Set up UART on GP0 and GP1 (Pico pins 1 and 2)
let uart_pins = (
pins.gp0.into_mode::<FunctionUart>(),
pins.gp1.into_mode::<FunctionUart>(),
);
let elrs_uart = UartPeripheral::new(pac.UART0, uart_pins, &mut pac.RESETS)
.enable(
UartConfig::new(400000.Hz(), DataBits::Eight, None, StopBits::One),
clocks.peripheral_clock.freq(),
)
.unwrap();
let i2c = I2C::i2c1(
pac.I2C1,
pins.gp14.into_mode(), // sda
pins.gp15.into_mode(), // scl
400.kHz(),
&mut pac.RESETS,
125_000_000.Hz(),
);
let i2c_address = SlaveAddr::Alternative(false, false, false);
let mut eeprom = Eeprom24x::new_24x32(i2c, i2c_address);
// Enable adc // Enable adc
let mut adc = Adc::new(pac.ADC, &mut pac.RESETS); let mut adc = Adc::new(pac.ADC, &mut pac.RESETS);
@ -149,25 +270,28 @@ fn main() -> ! {
// Setting up array with pins connected to button rows // Setting up array with pins connected to button rows
let button_matrix_row_pins: &[&dyn InputPin<Error = Infallible>; BUTTON_ROWS] = &[ let button_matrix_row_pins: &[&dyn InputPin<Error = Infallible>; BUTTON_ROWS] = &[
&pins.gp11.into_pull_up_input(), &pins.gp6.into_pull_up_input(),
&pins.gp13.into_pull_up_input(), &pins.gp8.into_pull_up_input(),
&pins.gp9.into_pull_up_input(), &pins.gp4.into_pull_up_input(),
&pins.gp12.into_pull_up_input(), &pins.gp7.into_pull_up_input(),
&pins.gp10.into_pull_up_input(), &pins.gp5.into_pull_up_input(),
]; ];
// Setting up array with pins connected to button columns // Setting up array with pins connected to button columns
let button_matrix_col_pins: &mut [&mut dyn OutputPin<Error = Infallible>; BUTTON_COLS] = &mut [ let button_matrix_col_pins: &mut [&mut dyn OutputPin<Error = Infallible>; BUTTON_COLS] = &mut [
&mut pins.gp4.into_push_pull_output(), &mut pins.gp9.into_push_pull_output(),
&mut pins.gp5.into_push_pull_output(), &mut pins.gp10.into_push_pull_output(),
&mut pins.gp6.into_push_pull_output(), &mut pins.gp11.into_push_pull_output(),
&mut pins.gp7.into_push_pull_output(), &mut pins.gp12.into_push_pull_output(),
&mut pins.gp8.into_push_pull_output(), &mut pins.gp13.into_push_pull_output(),
]; ];
let mut elrs_en_pin = pins.gp2.into_push_pull_output();
let mut elrs = Elrs::new(elrs_uart);
// Create button matrix object that scans all buttons // Create button matrix object that scans all buttons
let mut button_matrix: ButtonMatrix<BUTTON_ROWS, BUTTON_COLS, NUMBER_OF_BUTTONS> = let mut button_matrix: ButtonMatrix<BUTTON_ROWS, BUTTON_COLS, NUMBER_OF_BUTTONS> =
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 // Initialize button matrix
button_matrix.init_pins(); button_matrix.init_pins();
@ -181,9 +305,10 @@ fn main() -> ! {
clocks.peripheral_clock.freq(), 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 // 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 // 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 { for _ in 0..10 {
button_matrix.scan_matrix(&mut delay); button_matrix.scan_matrix(&mut delay);
} }
@ -197,48 +322,104 @@ fn main() -> ! {
// Create timers // Create timers
let timer = Timer::new(pac.TIMER, &mut pac.RESETS); let timer = Timer::new(pac.TIMER, &mut pac.RESETS);
let mut usb_hid_report_count_down = timer.count_down(); let mut status_led_count_down = timer.count_down();
usb_hid_report_count_down.start(10.millis()); status_led_count_down.start(250.millis());
let mut scan_count_down = timer.count_down(); 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(); let mut data_process_count_down = timer.count_down();
status_led_count_down.start(50.millis()); data_process_count_down.start(1200u32.micros());
let mut elrs_count_down = timer.count_down(); let mut elrs_start_count_down = timer.count_down();
elrs_count_down.start(1660u32.micros()); 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 safety_check: bool = false;
let mut activity: bool = false; let mut usb_activity: bool = false;
let mut idle: bool = false; let mut idle: bool = false;
let mut usb_active: bool = false; let mut usb_active: bool = false;
let mut elrs_active: bool = false;
let _elrs_connected: bool = false;
let mut calibration_active: bool = false;
let mut throttle_hold: bool = false;
let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS]; let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS];
let mut buttons: [Button; NUMBER_OF_BUTTONS] = [Button::default(); NUMBER_OF_BUTTONS]; let mut buttons: [Button; NUMBER_OF_BUTTONS] = [Button::default(); NUMBER_OF_BUTTONS];
let mut gimbal_mode: u8;
// Set up left gimbal Y axis as full range without return to center spring // Set up usb button layout
axis[GIMBAL_AXIS_LEFT_Y].idle_value = AXIS_MIN; buttons[0].usb_button = 1;
axis[GIMBAL_AXIS_LEFT_Y].deadzone = (50, 0, 50); buttons[1].usb_button = 2;
axis[GIMBAL_AXIS_LEFT_Y].expo = 0.0; buttons[2].usb_button = 3;
buttons[3].usb_button = 4;
buttons[3].usb_button_sec_enable = true;
buttons[3].usb_button_sec = 21;
buttons[3].usb_button_sec_trigger_index = 12;
buttons[4].usb_button = 5;
buttons[4].usb_button_sec_enable = true;
buttons[4].usb_button_sec = 22;
buttons[4].usb_button_sec_trigger_index = 12;
buttons[5].usb_button = 6;
buttons[6].usb_button = 7;
buttons[7].usb_button = 8;
buttons[7].usb_button_sec = 16;
buttons[7].usb_button_toggle_enable = true;
buttons[8].usb_button = 9;
buttons[9].usb_button = 10;
buttons[10].usb_button = 11;
buttons[11].usb_button = 12;
buttons[12].usb_button = 13;
buttons[12].usb_button_sec = 17;
buttons[12].usb_button_toggle_enable = true;
buttons[13].usb_button = 14;
buttons[13].usb_button_sec_enable = true;
buttons[13].usb_button_sec = 23;
buttons[13].usb_button_sec_trigger_index = 12;
buttons[14].usb_button = 15;
buttons[14].usb_button_sec_enable = true;
buttons[14].usb_button_sec = 24;
buttons[14].usb_button_sec_trigger_index = 12;
// Manual calibation values // Set up elrs button layout
// TODO: add external EEPROM and make calibration routine buttons[0].elrs_channel = 7;
axis[GIMBAL_AXIS_LEFT_X].center = AXIS_CENTER; buttons[0].elrs_lock_enable = true;
axis[GIMBAL_AXIS_LEFT_X].max = AXIS_MAX - 450; buttons[1].elrs_channel = 7;
axis[GIMBAL_AXIS_LEFT_X].min = AXIS_MIN + 500; buttons[1].elrs_lock_enable = true;
axis[GIMBAL_AXIS_LEFT_Y].center = AXIS_CENTER + 105; buttons[1].elrs_lock_state_on = true;
axis[GIMBAL_AXIS_LEFT_Y].max = AXIS_MAX - 250; buttons[3].elrs_channel = 8;
axis[GIMBAL_AXIS_LEFT_Y].min = AXIS_MIN + 500; buttons[3].elrs_lock_enable = true;
axis[GIMBAL_AXIS_RIGHT_X].center = AXIS_CENTER - 230; buttons[4].elrs_channel = 8;
axis[GIMBAL_AXIS_RIGHT_X].max = AXIS_MAX - 700; buttons[4].elrs_lock_enable = true;
axis[GIMBAL_AXIS_RIGHT_X].min = AXIS_MIN + 350; buttons[4].elrs_lock_state_on = true;
axis[GIMBAL_AXIS_RIGHT_Y].center = AXIS_CENTER - 68; buttons[6].elrs_channel = 9;
axis[GIMBAL_AXIS_RIGHT_Y].max = AXIS_MAX - 700; buttons[7].elrs_channel = 5;
axis[GIMBAL_AXIS_RIGHT_Y].min = AXIS_MIN + 450; buttons[8].elrs_channel = 10;
buttons[8].elrs_lock_enable = true;
buttons[9].elrs_channel = 10;
buttons[9].elrs_lock_enable = true;
buttons[9].elrs_lock_state_on = true;
buttons[10].elrs_channel = 11;
buttons[10].elrs_lock_enable = true;
buttons[11].elrs_channel = 11;
buttons[11].elrs_lock_enable = true;
buttons[11].elrs_lock_state_on = true;
buttons[12].elrs_channel = 6;
buttons[13].elrs_channel = 12;
buttons[13].elrs_lock_enable = true;
buttons[14].elrs_channel = 12;
buttons[14].elrs_lock_enable = true;
buttons[14].elrs_lock_state_on = true;
// Table for gimbal expo curve lookup insded of doing floating point math for every analog read
let expo_lut: [u16; AXIS_MAX as usize + 1] = generate_expo_lut(0.3);
// Create dynamic smoother array for gimbal axis // 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] = [ let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [
DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
@ -261,38 +442,158 @@ fn main() -> ! {
let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x1209, 0x0002)) let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x1209, 0x0002))
.manufacturer("CMtec") .manufacturer("CMtec")
.product("CMDR Joystick") .product("CMDR Joystick 25")
.serial_number("0001") .serial_number("0001")
.build(); .build();
loop { // Read calibration data from eeprom
// Temporary way to enter bootloader ------------------------- for (index, item) in axis.iter_mut().enumerate() {
// TODO: Remove this after testing item.min = eeprom.read_byte((index as u32 * 6) + 2).unwrap() as u16;
if button_matrix.buttons_pressed()[0] item.min <<= 8;
&& button_matrix.buttons_pressed()[1] item.min |= eeprom.read_byte((index as u32 * 6) + 1).unwrap() as u16;
&& button_matrix.buttons_pressed()[5] item.max = eeprom.read_byte((index as u32 * 6) + 4).unwrap() as u16;
&& button_matrix.buttons_pressed()[6] item.max <<= 8;
&& button_matrix.buttons_pressed()[8] item.max |= eeprom.read_byte((index as u32 * 6) + 3).unwrap() as u16;
&& button_matrix.buttons_pressed()[9] item.center = eeprom.read_byte((index as u32 * 6) + 6).unwrap() as u16;
{ item.center <<= 8;
status_led.update(StatusMode::Bootloader); item.center |= eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16;
let gpio_activity_pin_mask: u32 = 0; }
let disable_interface_mask: u32 = 0; gimbal_mode = eeprom.read_byte(25).unwrap();
rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask);
loop {
// Take care of USB HID poll requests
if usb_dev.poll(&mut [&mut usb_hid_joystick]) {
usb_active = true;
} }
// -----------------------------------------------------------
if scan_count_down.wait().is_ok() { if scan_count_down.wait().is_ok() {
button_matrix.scan_matrix(&mut delay); button_matrix.scan_matrix(&mut delay);
// Have not figured out hov to store the adc pins in an array yet let mut left_x: u16 = adc.read(&mut adc_pin_left_x).unwrap();
// so we have to read them one by one let mut left_y: u16 = adc.read(&mut adc_pin_left_y).unwrap();
// TODO: Find a way to store adc pins in an array let mut right_x: u16 = adc.read(&mut adc_pin_right_x).unwrap();
smoother[GIMBAL_AXIS_LEFT_X].tick(adc.read(&mut adc_pin_left_x).unwrap()); let mut right_y: u16 = adc.read(&mut adc_pin_right_y).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());
if gimbal_mode == GIMBAL_MODE_M10 {
// Invert X1 and Y2 axis (M10 gimbals)
left_x = AXIS_MAX - left_x;
right_y = AXIS_MAX - right_y;
} else if gimbal_mode == GIMBAL_MODE_M7 {
// Invert Y1 and X2 axis (M7 gimbals)
left_y = AXIS_MAX - left_y;
right_x = AXIS_MAX - right_x;
}
smoother[GIMBAL_AXIS_LEFT_X].tick(left_x as i32);
smoother[GIMBAL_AXIS_LEFT_Y].tick(left_y as i32);
smoother[GIMBAL_AXIS_RIGHT_X].tick(right_x as i32);
smoother[GIMBAL_AXIS_RIGHT_Y].tick(right_y as i32);
}
if status_led_count_down.wait().is_ok() {
update_status_led(
&mut status_led,
&usb_active,
&elrs_active,
&elrs.connected(),
&safety_check,
&calibration_active,
&throttle_hold,
);
}
// Check if all axis are in idle position and no buttons are pressed
if idle && !safety_check && elrs_active {
safety_check = true;
}
if data_process_count_down.wait().is_ok() {
// Update pressed keys status
for (index, key) in button_matrix.buttons_pressed().iter().enumerate() {
buttons[index].pressed = *key;
}
// Secondary way to enter bootloader (pressing all left hands buttons except the hat
if buttons[0].pressed && buttons[2].pressed {
status_led.update(StatusMode::Bootloader);
let gpio_activity_pin_mask: u32 = 0;
let disable_interface_mask: u32 = 0;
rp2040_hal::rom_data::reset_to_usb_boot(
gpio_activity_pin_mask,
disable_interface_mask,
);
}
// ON/OFF switch for ELRS radio
if buttons[4].pressed && buttons[2].pressed && !elrs_active {
safety_check = false;
elrs_active = true;
} else if buttons[3].pressed && buttons[2].pressed && elrs_active {
elrs_active = false;
}
// ON/OFF switch for Throttle hold mode
if buttons[6].pressed && buttons[2].pressed && !throttle_hold {
throttle_hold = true;
axis[GIMBAL_AXIS_LEFT_Y].hold = 0;
} else if buttons[5].pressed && buttons[2].pressed && throttle_hold {
throttle_hold = false;
}
// Calibration of center position (pressing all right hands buttons except
// the hat switch)
if buttons[1].pressed && buttons[2].pressed {
for (index, item) in axis.iter_mut().enumerate() {
item.center = smoother[index].value() as u16;
item.min = item.center;
item.max = item.center;
}
calibration_active = true;
}
// Calibration of min and max position
if calibration_active {
for (index, item) in axis.iter_mut().enumerate() {
if (smoother[index].value() as u16) < item.min {
item.min = smoother[index].value() as u16;
} else if (smoother[index].value() as u16) > item.max {
item.max = smoother[index].value() as u16;
}
}
}
if calibration_active && buttons[8].pressed {
gimbal_mode = GIMBAL_MODE_M10;
for (index, item) in axis.iter_mut().enumerate() {
item.center = smoother[index].value() as u16;
item.min = item.center;
item.max = item.center;
}
} else if calibration_active && buttons[9].pressed {
gimbal_mode = GIMBAL_MODE_M7;
for (index, item) in axis.iter_mut().enumerate() {
item.center = smoother[index].value() as u16;
item.min = item.center;
item.max = item.center;
}
}
// Save calibration data to eeprom (pressing right hat switch)
else if calibration_active && buttons[20].pressed {
let mut eeprom_data: [u8; 25] = [0; 25];
for (index, item) in axis.iter_mut().enumerate() {
eeprom_data[index * 6] = item.min as u8;
eeprom_data[(index * 6) + 1] = (item.min >> 8) as u8;
eeprom_data[(index * 6) + 2] = item.max as u8;
eeprom_data[(index * 6) + 3] = (item.max >> 8) as u8;
eeprom_data[(index * 6) + 4] = item.center as u8;
eeprom_data[(index * 6) + 5] = (item.center >> 8) as u8;
}
eeprom_data[24] = gimbal_mode;
let _ = eeprom.write_page(0x01, &eeprom_data);
calibration_active = false;
}
// Process axis values
for (index, item) in axis.iter_mut().enumerate() { for (index, item) in axis.iter_mut().enumerate() {
item.value = calculate_axis_value( item.value = calculate_axis_value(
smoother[index].value() as u16, smoother[index].value() as u16,
@ -301,41 +602,43 @@ fn main() -> ! {
item.center, item.center,
item.deadzone, item.deadzone,
item.expo, item.expo,
&expo_lut,
); );
} }
let pressed_keys = button_matrix.buttons_pressed(); // Process throttle hold value
mode = get_mode(pressed_keys); if throttle_hold
&& axis[GIMBAL_AXIS_LEFT_Y].value < AXIS_CENTER
// Update pressed keys status && !axis[GIMBAL_AXIS_LEFT_Y].hold_pending
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)
{ {
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;
}
// Set idle state
for key in buttons.iter_mut() {
if key.pressed {
idle = false; idle = false;
} }
} }
@ -343,51 +646,65 @@ fn main() -> ! {
// Generate led activity when gimbal is moved from idle position // Generate led activity when gimbal is moved from idle position
for item in axis.iter_mut() { for item in axis.iter_mut() {
if item.value != item.previous_value { if item.value != item.previous_value {
activity = true; usb_activity = true;
} }
item.previous_value = item.value; 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 // Generate led activity when a button is pressed
// FnL, FnR, and ModeR are excluded
for (index, key) in buttons.iter_mut().enumerate() { for (index, key) in buttons.iter_mut().enumerate() {
if (usb_active if key.pressed != key.previous_pressed {
&& key.pressed != key.previous_pressed key.usb_changed = true;
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnL key.usb_changed_to_pressed = key.pressed;
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnR key.elrs_changed = true;
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR) key.elrs_changed_to_pressed = key.pressed;
|| (!usb_active usb_activity = true;
&& key.pressed != key.previous_pressed }
&& layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated) // Set throttle_hold_value
if key.pressed != key.previous_pressed && key.pressed && throttle_hold && index == 5
{ {
activity = true; axis[GIMBAL_AXIS_LEFT_Y].hold = axis[GIMBAL_AXIS_LEFT_Y].value;
axis[GIMBAL_AXIS_LEFT_Y].hold_pending = true;
} }
key.previous_pressed = key.pressed; key.previous_pressed = key.pressed;
} }
// Reset channel locks when calibration is active
if calibration_active {
for axis in axis.iter_mut() {
axis.hold = 0;
}
for button in buttons.iter_mut() {
button.elrs_lock_state = false;
}
}
} }
if usb_dev.poll(&mut [&mut usb_hid_joystick]) { if elrs_update_count_down.wait().is_ok() {
usb_active = true; // Send ELRS data
if elrs_active {
elrs_en_pin.set_high().unwrap();
elrs.send(get_elrs_channels(&mut buttons, &mut axis));
} else {
elrs_en_pin.set_low().unwrap();
elrs.reset();
}
} }
if status_led_count_down.wait().is_ok() {
update_status_led(
&mut status_led,
&mut activity,
&usb_active,
&idle,
&safety_check,
);
}
if usb_hid_report_count_down.wait().is_ok() && activity {
// Dont send USB HID joystick report if there is no activity // Dont send USB HID joystick report if there is no activity
// This is to avoid preventing the computer from going to sleep // This is to avoid preventing the computer from going to sleep
match usb_hid_joystick.device().write_report(&get_joystick_report( if usb_update_count_down.wait().is_ok() && usb_activity {
&mut buttons, match usb_hid_joystick
&mut axis, .device()
&mode, .write_report(&get_joystick_report(&mut buttons, &mut axis))
)) { {
Err(UsbHidError::WouldBlock) => {} Err(UsbHidError::WouldBlock) => {}
Ok(_) => {} Ok(_) => {}
Err(e) => { Err(e) => {
@ -395,15 +712,8 @@ fn main() -> ! {
core::panic!("Failed to write joystick report: {:?}", e) core::panic!("Failed to write joystick report: {:?}", e)
} }
}; };
usb_activity = false;
} }
// Check if all axis are in idle position and no buttons are pressed
if idle && !safety_check && !usb_active {
safety_check = true;
}
// TODO: Implement ELRS
if elrs_count_down.wait().is_ok() && !usb_active && safety_check {}
} }
} }
@ -422,191 +732,148 @@ fn main() -> ! {
/// * `safety_check` - Reference to bool that indicates if safety check has passed /// * `safety_check` - Reference to bool that indicates if safety check has passed
fn update_status_led<P, SM, I>( fn update_status_led<P, SM, I>(
status_led: &mut Ws2812StatusLed<P, SM, I>, status_led: &mut Ws2812StatusLed<P, SM, I>,
activity: &mut bool,
usb_active: &bool, usb_active: &bool,
axis_idle: &bool, elrs_active: &bool,
elrs_connected: &bool,
safety_check: &bool, safety_check: &bool,
calibration_active: &bool,
throttle_hold: &bool,
) where ) where
P: PIOExt + FunctionConfig, P: PIOExt + FunctionConfig,
I: PinId, I: PinId,
Function<P>: ValidPinMode<I>, Function<P>: ValidPinMode<I>,
SM: StateMachineIndex, SM: StateMachineIndex,
{ {
if !usb_active && !*safety_check { if *calibration_active {
status_led.update(StatusMode::ActivityFlash);
} else if *elrs_active && !*safety_check {
status_led.update(StatusMode::Warning); status_led.update(StatusMode::Warning);
} else if *activity && status_led.get_mode() != StatusMode::Activity { } else if !*usb_active && !*elrs_active {
status_led.update(StatusMode::NormalFlash);
} else if *usb_active && !*elrs_active && *throttle_hold {
status_led.update(StatusMode::Activity); status_led.update(StatusMode::Activity);
} else if *activity && status_led.get_mode() == StatusMode::Activity { } else if *usb_active && !*elrs_active && !*throttle_hold {
status_led.update(StatusMode::Off);
*activity = false;
} else if !*axis_idle && status_led.get_mode() != StatusMode::Activity {
status_led.update(StatusMode::Activity);
} else if *usb_active && status_led.get_mode() != StatusMode::Normal {
status_led.update(StatusMode::Normal); status_led.update(StatusMode::Normal);
} else if status_led.get_mode() != StatusMode::Other { } else if *elrs_active && *elrs_connected {
status_led.update(StatusMode::Other); status_led.update(StatusMode::Other);
} else if *elrs_active && !*elrs_connected {
status_led.update(StatusMode::OtherFlash);
} }
} }
/// Get current Fn mode (0, 1, 2 or 3 and alt l/r mode) /// Generate keyboard report based on pressed keys and Fn mode (0, 1, 2 or 3)
/// layout::MAP contains the button types
///
/// # Arguments
///
/// * `pressed_keys` - Array of pressed keys
fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 {
// Check how many Fn keys are pressed
let mut mode: u8 = 0;
let mut fn_l_active: bool = false;
let mut fn_r_active: bool = false;
let mut alt_l_active: bool = false;
let mut alt_r_active: bool = false;
for (index, key) in pressed_keys.iter().enumerate() {
if *key && layout::HID_MAP[0][index] == layout::HidButton::FnL {
fn_l_active = true;
}
if *key && layout::HID_MAP[0][index] == layout::HidButton::FnR {
fn_r_active = true;
}
if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeL {
alt_l_active = true;
}
if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeR {
alt_r_active = true;
}
}
if fn_l_active && fn_r_active {
mode = 3;
} else if fn_r_active {
mode = 2;
} else if fn_l_active {
mode = 1;
}
// Set bit 4 and 5 if alt l/r is active
if alt_l_active {
mode |= 0x10;
}
if alt_r_active {
mode |= 0x20;
}
mode
}
/// Generate keyboard report based on pressed keys and Fn mode (0, 1 or 2)
/// layout::MAP contains the keycodes for each key in each Fn mode /// layout::MAP contains the keycodes for each key in each Fn mode
/// ///
/// # Arguments /// # Arguments
/// ///
/// * `matrix_keys` - Array of pressed keys /// * `matrix_keys` - Array of pressed keys
/// * `axis` - Array of joystick axis values /// * `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( fn get_joystick_report(
matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], matrix_keys: &mut [Button; NUMBER_OF_BUTTONS],
axis: &mut [GimbalAxis; 4], axis: &mut [GimbalAxis; 4],
mode: &u8,
) -> JoystickReport { ) -> JoystickReport {
let mut x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; 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 z: u16 = axis[GIMBAL_AXIS_LEFT_X].value;
let mut rx: u16 = AXIS_CENTER; let mut rx: u16 = AXIS_CENTER;
let mut ry: u16 = AXIS_CENTER; let mut ry: u16 = AXIS_CENTER;
let mut rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value; let rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value;
let (mut hat1, mut hat_button1) = format_hat_value(0);
// Left Alt mode active (bit 4) let (mut hat2, mut hat_button2) = format_hat_value(0);
// Full range of left gimbal gives half range of joystick axis (center to max) let (mut hat3, mut hat_button3) = format_hat_value(0);
// Left Fn mode = reversed range (center to min) let (mut hat4, mut hat_button4) = format_hat_value(0);
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,
);
}
// Right Alt mode active (bit 5) // Right Alt mode active (bit 5)
// Right gimbal control third joystick axis when right Fn mode is active // Right gimbal control third joystick axis when right Fn mode is active
if mode & 0x20 == 0x20 if matrix_keys[8].pressed {
&& (axis[GIMBAL_AXIS_RIGHT_X].fn_mode == 2 || axis[GIMBAL_AXIS_RIGHT_X].fn_mode == 3)
{
x = AXIS_CENTER; x = AXIS_CENTER;
rx = axis[GIMBAL_AXIS_RIGHT_X].value; rx = axis[GIMBAL_AXIS_RIGHT_X].value;
}
if mode & 0x20 == 0x20
&& (axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 2 || axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 3)
{
y = AXIS_CENTER; 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: let mut hat_left: u8 = 0;
// * bit 1: Up let mut hat_right: u8 = 0;
// * 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];
for (index, key) in matrix_keys.iter_mut().enumerate() { for (index, key) in matrix_keys.iter_mut().enumerate() {
if key.pressed if (15..=19).contains(&index) && key.pressed {
&& layout::HID_MAP[key.fn_mode as usize][index] >= layout::HidButton::Hat1U hat_left |= 1 << (index - 15);
&& layout::HID_MAP[key.fn_mode as usize][index] <= layout::HidButton::Hat4B }
{ if (20..=24).contains(&index) && key.pressed {
hats[(layout::HID_MAP[key.fn_mode as usize][index] as usize hat_right |= 1 << (index - 20);
- layout::HidButton::Hat1U as usize)
/ 5] |= 1
<< ((layout::HID_MAP[key.fn_mode as usize][index] as usize
- layout::HidButton::Hat1U as usize)
- (5 * ((layout::HID_MAP[key.fn_mode as usize][index] as usize
- layout::HidButton::Hat1U as usize)
/ 5)));
} }
} }
// Convert hat switch data to HID code // Convert hat switch data to HID code
let (hat1, hat_button1) = format_hat_value(hats[0]); if matrix_keys[7].pressed {
let (hat2, hat_button2) = format_hat_value(hats[1]); (hat3, hat_button3) = format_hat_value(hat_left);
let (hat3, hat_button3) = format_hat_value(hats[2]); } else {
let (hat4, hat_button4) = format_hat_value(hats[3]); (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 // Update button state for joystick button 21-24 according to hat button 1-4
let mut buttons: u32 = (hat_button1 as u32) << 20 let mut buttons: u32 = (hat_button1 as u32) << 17
| ((hat_button2 as u32) << 21) | ((hat_button2 as u32) << 18)
| ((hat_button3 as u32) << 22) | ((hat_button3 as u32) << 19)
| ((hat_button4 as u32) << 23); | ((hat_button4 as u32) << 24);
// Update button state for joystick button 1-20 // Update button array with Sec button trigger status
for (index, key) in matrix_keys.iter_mut().enumerate() { // Using indexing instead of iterating to be able to iterate inside loop
if key.pressed for index in 0..NUMBER_OF_BUTTONS {
&& layout::HID_MAP[key.fn_mode as usize][index] as usize let mut sec_button_pressed: bool = false;
>= layout::HidButton::B1 as usize for (sec_index, sec_key) in matrix_keys.iter().enumerate() {
&& layout::HID_MAP[key.fn_mode as usize][index] as usize if matrix_keys[index].usb_button_sec_enable
<= layout::HidButton::B20 as usize && matrix_keys[index].usb_button_sec_trigger_index == sec_index
&& sec_key.pressed
{ {
buttons |= 1 << layout::HID_MAP[key.fn_mode as usize][index] as usize; sec_button_pressed = true;
break;
} }
} }
matrix_keys[index].usb_button_sec_pressed = sec_button_pressed;
}
// Update button state for joystick buttons
for key in matrix_keys.iter_mut() {
// Toggle mode button
if key.usb_changed && key.usb_button_toggle_enable {
key.usb_release_timeout = RELEASE_RIMEOUT;
}
if key.pressed
&& key.usb_button != 0
&& key.usb_button_toggle_enable
&& key.usb_release_timeout > 1
{
buttons |= 1 << (key.usb_button - 1);
} else if !key.pressed
&& key.usb_button_sec != 0
&& key.usb_button_toggle_enable
&& key.usb_release_timeout > 1
{
buttons |= 1 << (key.usb_button_sec - 1);
// Sec button mode
} else if key.pressed && key.usb_button_sec != 0 && key.usb_button_sec_pressed {
buttons |= 1 << (key.usb_button_sec - 1);
} else if key.pressed && key.usb_button != 0 && !key.usb_button_toggle_enable {
buttons |= 1 << (key.usb_button - 1);
}
}
// Auto release button when in toggle mode
for key in matrix_keys.iter_mut() {
if key.usb_release_timeout > 0 {
key.usb_release_timeout -= 1;
}
}
// Reset changed flags
for key in matrix_keys.iter_mut() {
key.usb_changed = false;
}
JoystickReport { JoystickReport {
x, x,
@ -627,8 +894,8 @@ fn get_joystick_report(
/// ///
/// # Arguments /// # Arguments
/// * `input` - Hat value coded as /// * `input` - Hat value coded as
/// bit 1-4: direction (U R D L) /// bit 2-5: direction (U L R D)
/// bit 5: button state /// bit 1: button state
/// 0 = not pressed /// 0 = not pressed
/// 1 = pressed /// 1 = pressed
fn format_hat_value(input: u8) -> (u8, u8) { fn format_hat_value(input: u8) -> (u8, u8) {
@ -642,21 +909,21 @@ fn format_hat_value(input: u8) -> (u8, u8) {
const HAT_LEFT: u8 = 6; const HAT_LEFT: u8 = 6;
const HAT_UP_LEFT: u8 = 7; const HAT_UP_LEFT: u8 = 7;
let direction: u8 = match input & 0x0F { let direction: u8 = match input & 0xFE {
1 => HAT_UP, 2 => HAT_UP,
2 => HAT_RIGHT, 4 => HAT_RIGHT,
3 => HAT_UP_RIGHT, 6 => HAT_UP_RIGHT,
4 => HAT_DOWN, 8 => HAT_DOWN,
6 => HAT_DOWN_RIGHT, 12 => HAT_DOWN_RIGHT,
8 => HAT_LEFT, 16 => HAT_LEFT,
12 => HAT_DOWN_LEFT, 24 => HAT_DOWN_LEFT,
9 => HAT_UP_LEFT, 18 => HAT_UP_LEFT,
_ => HAT_CENTER, _ => HAT_CENTER,
}; };
// Alpine hat switch button filter // Alpine hat switch button filter
let mut button_state: u8 = 0; let mut button_state: u8 = 0;
if input & 0x10 == 0x10 && direction == HAT_CENTER { if input & 0x01 == 0x01 && direction == HAT_CENTER {
button_state = 1; button_state = 1;
} }
@ -671,15 +938,24 @@ fn format_hat_value(input: u8) -> (u8, u8) {
/// * `max` - Upper bound of the value's current range /// * `max` - Upper bound of the value's current range
/// * `center` - Center 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) /// * `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( fn calculate_axis_value(
value: u16, value: u16,
min: u16, min: u16,
max: u16, max: u16,
center: u16, center: u16,
deadzone: (u16, u16, u16), deadzone: (u16, u16, u16),
expo: f32, expo: bool,
expo_lut: &[u16; AXIS_MAX as usize + 1],
) -> u16 { ) -> u16 {
if value <= min {
return AXIS_MIN;
}
if value >= max {
return AXIS_MAX;
}
let mut calibrated_value = AXIS_CENTER; let mut calibrated_value = AXIS_CENTER;
if value > (center + deadzone.1) { if value > (center + deadzone.1) {
@ -700,17 +976,8 @@ fn calculate_axis_value(
); );
} }
if expo != 0.0 { if expo && calibrated_value != AXIS_CENTER {
let joystick_x_float = calibrated_value as f32 / AXIS_MAX as f32; calibrated_value = expo_lut[calibrated_value as usize];
// 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,
);
} }
calibrated_value calibrated_value
@ -749,3 +1016,163 @@ fn constrain<T: PartialOrd>(value: T, out_min: T, out_max: T) -> T {
value value
} }
} }
/// Generate exponential lookup table for 12bit values
///
/// # Arguments
/// * `expo` - Exponential curve factor (range 0.0 - 1.0)
fn generate_expo_lut(expo: f32) -> [u16; AXIS_MAX as usize + 1] {
let mut lut: [u16; AXIS_MAX as usize + 1] = [0; AXIS_MAX as usize + 1];
for i in 0..AXIS_MAX + 1 {
let value_float = i as f32 / AXIS_MAX as f32;
// Calculate expo using 9th order polynomial function with 0.5 as center point
let value_exp: f32 =
expo * (0.5 + 256.0 * powf(value_float - 0.5, 9.0)) + (1.0 - expo) * value_float;
lut[i as usize] = constrain((value_exp * AXIS_MAX as f32) as u16, AXIS_MIN, AXIS_MAX);
}
lut
}
/// Get ELRS channel values
///
/// # Arguments
/// * `matrix_keys` - Array of buttons
/// * `axis` - Array of axis
fn get_elrs_channels(
matrix_keys: &mut [Button; NUMBER_OF_BUTTONS],
axis: &mut [GimbalAxis; 4],
) -> [u16; 12] {
let mut channels: [u16; 12] = [ELRS_MIN; 12];
// Check and store trim values
for (index, key) in matrix_keys.iter_mut().enumerate() {
// Left gimbal X
if key.pressed && index == 17 && axis[GIMBAL_AXIS_LEFT_X].trim < ELRS_CENTER as i16 {
axis[GIMBAL_AXIS_LEFT_X].trim += 1;
} else if key.pressed
&& index == 19
&& axis[GIMBAL_AXIS_LEFT_X].trim > (0 - ELRS_CENTER as i16)
{
axis[GIMBAL_AXIS_LEFT_X].trim -= 1;
// Left gimbal Y
} else if key.pressed && index == 16 && axis[GIMBAL_AXIS_LEFT_Y].trim < ELRS_CENTER as i16 {
axis[GIMBAL_AXIS_LEFT_Y].trim += 1;
} else if key.pressed
&& index == 18
&& axis[GIMBAL_AXIS_LEFT_Y].trim > (0 - ELRS_CENTER as i16)
{
axis[GIMBAL_AXIS_LEFT_Y].trim -= 1;
// Right gimbal X
} else if key.pressed && index == 22 && axis[GIMBAL_AXIS_RIGHT_X].trim < ELRS_CENTER as i16
{
axis[GIMBAL_AXIS_RIGHT_X].trim += 1;
} else if key.pressed
&& index == 24
&& axis[GIMBAL_AXIS_RIGHT_X].trim > (0 - ELRS_CENTER as i16)
{
axis[GIMBAL_AXIS_RIGHT_X].trim -= 1;
// Right gimbal Y
} else if key.pressed && index == 21 && axis[GIMBAL_AXIS_RIGHT_Y].trim < ELRS_CENTER as i16
{
axis[GIMBAL_AXIS_RIGHT_Y].trim += 1;
} else if key.pressed
&& index == 23
&& axis[GIMBAL_AXIS_RIGHT_Y].trim > (0 - ELRS_CENTER as i16)
{
axis[GIMBAL_AXIS_RIGHT_Y].trim -= 1;
}
}
// Alpine hat switch filter
let mut hat_left_button_only: bool = true;
let mut hat_right_button_only: bool = true;
for (index, key) in matrix_keys.iter_mut().enumerate() {
if key.pressed && (16..=19).contains(&index) {
hat_left_button_only = false;
}
if key.pressed && (21..=24).contains(&index) {
hat_right_button_only = false;
}
}
// Check and reset trim values
for (index, key) in matrix_keys.iter_mut().enumerate() {
if key.elrs_changed && key.elrs_changed_to_pressed && index == 15 && hat_left_button_only {
axis[GIMBAL_AXIS_LEFT_X].trim = 0;
axis[GIMBAL_AXIS_LEFT_Y].trim = 0;
} else if key.elrs_changed
&& key.elrs_changed_to_pressed
&& index == 20
&& hat_right_button_only
{
axis[GIMBAL_AXIS_RIGHT_X].trim = 0;
axis[GIMBAL_AXIS_RIGHT_Y].trim = 0;
}
}
// Match ELRS channel 1-4 to new min/max values
for (index, item) in axis.iter_mut().enumerate() {
channels[index] = remap(item.value, AXIS_MIN, AXIS_MAX, ELRS_MIN, ELRS_MAX);
}
// Apply trim to ELRS channel 1,3,4
for (index, item) in axis.iter().enumerate() {
if index != GIMBAL_AXIS_LEFT_Y && channels[index] > ELRS_CENTER {
channels[index] = remap(
channels[index],
ELRS_CENTER,
ELRS_MAX,
(ELRS_CENTER as i16 + item.trim) as u16,
ELRS_MAX,
);
} else if index != GIMBAL_AXIS_LEFT_Y && channels[index] < ELRS_CENTER {
channels[index] = remap(
channels[index],
ELRS_MIN,
ELRS_CENTER,
ELRS_MIN,
(ELRS_CENTER as i16 + item.trim) as u16,
);
} else if index != GIMBAL_AXIS_LEFT_Y {
channels[index] = (ELRS_CENTER as i16 + item.trim) as u16;
}
}
// Update button state for ELRS channels
for key in matrix_keys.iter_mut() {
if key.elrs_changed
&& key.elrs_changed_to_pressed
&& key.elrs_lock_enable
&& key.elrs_lock_state_on
&& key.elrs_channel != 0
{
key.elrs_lock_state = true;
} else if key.elrs_changed
&& !key.elrs_changed_to_pressed
&& key.elrs_lock_enable
&& !key.elrs_lock_state_on
&& key.elrs_channel != 0
{
key.elrs_lock_state = false;
} else if key.pressed && !key.elrs_lock_enable && key.elrs_channel != 0 {
channels[key.elrs_channel] = ELRS_MAX;
}
}
// Apply locking to ELRS channels
for key in matrix_keys.iter_mut() {
if key.elrs_lock_enable
&& key.elrs_lock_state
&& key.elrs_lock_state_on
&& key.elrs_channel != 0
{
channels[key.elrs_channel] = ELRS_MAX;
}
}
// Reset changed flags
for key in matrix_keys.iter_mut() {
key.elrs_changed = false;
}
channels
}

46
rp2040/src/status_led.rs
View File

@ -25,11 +25,14 @@ use ws2812_pio::Ws2812Direct;
pub enum StatusMode { pub enum StatusMode {
Off = 0, Off = 0,
Normal = 1, Normal = 1,
Activity = 2, NormalFlash = 2,
Other = 3, Activity = 3,
Warning = 4, ActivityFlash = 4,
Error = 5, Other = 5,
Bootloader = 6, OtherFlash = 6,
Warning = 7,
Error = 8,
Bootloader = 9,
} }
#[warn(dead_code)] #[warn(dead_code)]
@ -91,41 +94,68 @@ where
} }
/// Get current status mode /// Get current status mode
#[allow(dead_code)]
pub fn get_mode(&self) -> StatusMode { pub fn get_mode(&self) -> StatusMode {
self.mode self.mode
} }
#[warn(dead_code)]
/// Update status LED /// Update status LED
/// Depending on the mode, the LED will be set to a different colour /// Depending on the mode, the LED will be set to a different colour
/// ///
/// * OFF = off /// * OFF = off
/// * NORMAL = green /// * NORMAL = green
/// * NORMALFLASH = green (flashing)
/// * ACTIVITY = blue /// * ACTIVITY = blue
/// * ACTIVITYFLASH = blue (flashing)
/// * OTHER = orange /// * OTHER = orange
/// * OTHERFLASH = orange (flashing)
/// * WARNING = red (flashing) /// * WARNING = red (flashing)
/// * ERROR = red /// * ERROR = red
/// * BOOTLOADER = purple /// * BOOTLOADER = purple
/// ///
/// Make sure to call this function regularly to keep the LED flashing /// Make sure to call this function regularly to keep the LED flashing
pub fn update(&mut self, mode: StatusMode) { pub fn update(&mut self, mode: StatusMode) {
let colors: [RGB8; 7] = [ let colors: [RGB8; 10] = [
(0, 0, 0).into(), // Off (0, 0, 0).into(), // Off
(10, 7, 0).into(), // Green (10, 7, 0).into(), // Green
(10, 7, 0).into(), // Green
(10, 4, 10).into(), // Blue (10, 4, 10).into(), // Blue
(10, 4, 10).into(), // Blue
(5, 10, 0).into(), // Orange
(5, 10, 0).into(), // Orange (5, 10, 0).into(), // Orange
(2, 20, 0).into(), // Red (2, 20, 0).into(), // Red
(2, 20, 0).into(), // Red (2, 20, 0).into(), // Red
(0, 10, 10).into(), // Purple (0, 10, 10).into(), // Purple
]; ];
if mode == StatusMode::Warning
|| mode == StatusMode::NormalFlash
|| mode == StatusMode::ActivityFlash
|| mode == StatusMode::OtherFlash
|| mode != self.mode
{
self.mode = 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 self.ws2812_direct
.write([colors[mode as usize]].iter().copied()) .write([colors[mode as usize]].iter().copied())
.unwrap(); .unwrap();
self.state = true; 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 self.ws2812_direct
.write([colors[0]].iter().copied()) .write([colors[0]].iter().copied())
.unwrap(); .unwrap();

18
rp2040/src/usb_joystick_device.rs
View File

@ -68,7 +68,7 @@ impl<T, E> Try for Result<T, E> {
} }
// Based on example device from https://github.com/dlkj/usbd-human-interface-device/blob/main/src/device/joystick.rs // 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] #[rustfmt::skip]
pub const JOYSTICK_DESCRIPTOR: &[u8] = &[ pub const JOYSTICK_DESCRIPTOR: &[u8] = &[
0x05, 0x01, // Usage Page (Generic Desktop) 0x05, 0x01, // Usage Page (Generic Desktop)
@ -90,12 +90,15 @@ pub const JOYSTICK_DESCRIPTOR: &[u8] = &[
0xc0, // End Collection 0xc0, // End Collection
0x05, 0x09, // Usage Page (Button) 0x05, 0x09, // Usage Page (Button)
0x19, 0x01, // Usage Minimum (1) 0x19, 0x01, // Usage Minimum (1)
0x29, 0x18, // Usage Maximum (24) 0x29, 0x19, // Usage Maximum (25)
0x15, 0x00, // Logical Minimum (0) 0x15, 0x00, // Logical Minimum (0)
0x25, 0x01, // Logical Maximum (1) 0x25, 0x01, // Logical Maximum (1)
0x75, 0x01, // Report Size (1) 0x75, 0x01, // Report Size (1)
0x95, 0x18, // Report Count (24) 0x95, 0x19, // Report Count (25)
0x81, 0x02, // Input (Data, Variable, Absolute) 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) 0x15, 0x00, // Logical Minimum (0)
0x25, 0x07, // Logical Maximum (7) 0x25, 0x07, // Logical Maximum (7)
0x35, 0x00, // Physical Minimum (0) 0x35, 0x00, // Physical Minimum (0)
@ -120,7 +123,7 @@ pub struct JoystickReport {
pub rx: u16, // 12bit pub rx: u16, // 12bit
pub ry: u16, // 12bit pub ry: u16, // 12bit
pub rz: u16, // 12bit pub rz: u16, // 12bit
pub buttons: u32, // 24bit pub buttons: u32, // 32bit
pub hat1: u8, // 4bit pub hat1: u8, // 4bit
pub hat2: u8, // 4bit pub hat2: u8, // 4bit
pub hat3: u8, // 4bit pub hat3: u8, // 4bit
@ -133,7 +136,7 @@ pub struct Joystick<'a, B: UsbBus> {
impl<'a, B: UsbBus> Joystick<'a, B> { impl<'a, B: UsbBus> Joystick<'a, B> {
pub fn write_report(&mut self, report: &JoystickReport) -> Result<(), UsbHidError> { 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 // Did not make the packed struct work, so doing it manually
// TODO: make this work with packed struct // 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[9] = report.buttons as u8;
data[10] = (report.buttons >> 8) as u8; data[10] = (report.buttons >> 8) as u8;
data[11] = (report.buttons >> 16) as u8; data[11] = (report.buttons >> 16) as u8;
data[12] = (report.hat1) | (report.hat2 << 4); data[12] = (report.buttons >> 24) as u8;
data[13] = (report.hat3) | (report.hat4 << 4); data[13] = (report.hat1) | (report.hat2 << 4);
data[14] = (report.hat3) | (report.hat4 << 4);
self.interface self.interface
.write_report(&data) .write_report(&data)