diff --git a/.gitignore b/.gitignore index 841477f..ca385b8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ firmware/.cache/clangd/index firmware/compile_commands.json +rp2040/target +rp2040/Cargo.lock diff --git a/README.md b/README.md index db5dedc..5d9c0b2 100644 --- a/README.md +++ b/README.md @@ -1,53 +1,80 @@ -# CMDR Joystick +# CMDR Joystick 24 -RC Joystick with 2 hall effect gimbals and 8 buttons for use both with simulators and ELRS Rx equipped quads. +RC Joystick with 2 hall effect gimbals, 2 hat switches and 24 buttons for use both with simulators and ELRS Rx equipped quads. ## Layout ```cpp USB Joystick Layer 0 +-------------------------------------------------------------- +| FnL | B1 | | B5 | FnR | +-------------------------------------------------------------- +| | B2 | B3 | MoL | | MoR | B7 | B6 | | +| | +| | B4 | | B8 | | +| | B17 | | B18 | | +| Z/RZ X/Y | +| | H1U | | H2U | | +| | H1L | H1B | H1R || H2L | H2B | H2R | | +| | H1D | | H2D | | +-------------------------------------------------------------- -| B3 | | B1 | -| Fn1 | | B2 | --------------------------------------------- -| | B4 | | B5 | | -| | -| X1,Y1 X2,Y2 | -| | B6 | | B7 | | --------------------------------------------- +USB Joystick Layer 1 (FnL) +-------------------------------------------------------------- +| FnL | B9 | | B5 | FnR | +-------------------------------------------------------------- +| | B10 | B11 | MoL | | MoR | B7 | B6 | | +| | +| | B12 | | B8 | | +| | B19 | | B18 | | +| Z/RZ X/Y | +| | H3U | | H2U | | +| | H3L | H3B | H3R || H2L | H2B | H2R | | +| | H3D | | H2D | | +-------------------------------------------------------------- -USB Joystick Layer 1 (Fn1) +USB Joystick Layer 2 (FnR) +-------------------------------------------------------------- +| FnL | B1 | | B13 | FnR | +-------------------------------------------------------------- +| | B2 | B3 | MoL | | MoR | B15 | B14 | | +| | +| | B4 | | B16 | | +| | B17 | | B20 | | +| Z/RZ X(RX)/Y(RY) | +| | H1U | | H4U | | +| | H1L | H1B | H1R || H4L | H4B | H4R | | +| | H1D | | H4D | | +-------------------------------------------------------------- -| Fn2 | | B1 | -| Fn1 | | B2 | --------------------------------------------- -| | B8 | | B9 | | -| | -| X1,Y1 X3,Y2 | -| | B10 | | B11 | | --------------------------------------------- - -USB Joystick Layer 2 (Fn2) - -| Fn2 | | B16 | -| Fn1 | | B17 | --------------------------------------------- -| | B12 | | B13 | | -| | -| X1,Y1 X3,Y3 | -| | B14 | | B15 | | --------------------------------------------- +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) | +| | - | | - | | +| | - | - | - || - | - | - | | +| | - | | - | | +-------------------------------------------------------------- -| CH6 on | | CH5 on | -| CH6 off | | CH5 off | --------------------------------------------- -| | CH7 | | CH8 | | -| | -| X,Y X,Y | -| CH1,CH2 | CH9 | | CH10 | CH3,CH4 | --------------------------------------------- ``` ## Features @@ -55,40 +82,30 @@ ELRS Layer - Ergonomic design (low profile) - Hall effect gimbals - Supports both USB HID joystick and ELRS Tx module -- Total 6x axis and 15x buttons (using Fn mode) implemented in USB HID mode -- 10 Channels implemented in ELRS mode (4x axis, 6x buttons) -- Low latency (1.6ms ELRS, 5ms USB) +- Total 6x axis, 4x hat switches and 24x 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 -- Platformio - - env: teensylc - - platform: teensy - - board: teensylc - - framework: arduino -- Flashing via Teensy USB bootloader +- Cargo (rust embedded) +- Flashing via Cargo - Pressing boot button on teensy - Press and hold "top lower right button" when powering the unit ## Hardware -- 1x TeensyLC MCU +- 1x rp2040zero MCU board - 2x FrSky M7 or M10 gimbals - 6x Kailh choc low profile switches -- 2x Cherry MX switches -- 1x PCB -- 1x Bottom case -- 1x Top plate -- 2x Gimbal spacers +- 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) ## Calibration -_The button is from here reffered to "top lower left button"_ - -1. Turn off the unit -2. Press and hold the button while powering the unit -3. Release the button and center the two gimbals -4. Press the button again -5. Move the two gimbals to it maximux X and Y -6. Press the button one mo time -7. Done! +No calibration needed diff --git a/eCAD/experiment_board.md b/eCAD/experiment_board.md new file mode 100644 index 0000000..95f3a72 --- /dev/null +++ b/eCAD/experiment_board.md @@ -0,0 +1,42 @@ +# Experiment Board + +## Layout + +* 17 * 17 hole +* 2.5mm mounting hole in each corner +* Cutout in the top for the rp2040zero board +* Painted black + + + ----------------- + ------| |------ + | | + | | + | | +------------------------- ----|-------------------- +| M | o | 3 - G - A | - | | - | o | o | o | o | M | +| o | o | 3 - G - A | - | | - | o | o | o | o | o | +| o | o | 3 - G - A | - | | - | o | o | o | o | o | +| o | o | 3 - G - A | - | | - | o | o | o | o | o | +| o | o | o | o | o | - |-------------------| - | o | o | o | o | o | +| o | o | o | o | o | - | - | - | - | - | - | - | o | o | o | o | o | +| o | B | B | o | o | o | o | o | o | o | o | o | o | o | B | B | o | +| o | B | B | o | o | o | o | o | o | o | o | o | o | o | B | B | o | +| o | B | B | o | o | o | o | o | o | o | o | o | o | o | B | B | o | +| o | B | B | o | o | o | B | B | o | B | B | o | o | o | B | B | o | +| o | B | B | o | o | o | B | B | o | B | B | o | o | o | B | B | o | +| o | B | B | o | o | o | o | o | o | o | o | o | o | o | B | B | o | +| o | B | B | o | o | o | o | o | o | o | o | o | o | o | B | B | o | +| o | o | o | o | o | o | o | o | o | o | o | o | o | o | o | o | o | +| o | o | o | o | o | o | o | o | o | o | o | o | o | o | o | o | o | +| o | o | H | H | H | H | H | H | o | H | H | H | H | H | H | o | o | +| M | o | o | o | o | o | o | o | o | o | o | o | o | o | o | o | M | +--------------------------------------------------------------------- + +## Mounting + +* The rp2040zero board is mounted upside down +* 4pc 4x1 pin header (3-G-A) mounted for the analog channels +* 2pc 2x7 pin header (BB) mounted for the left/right buttons mounted in the lid +* 2pc 2x2 pin header (BB) mounted for the left/right buttons mouted in the bottom chassis +* 2pc 1x6 pin header (H) mounted for the left/right hat switches diff --git a/rp2040/.cargo/config b/rp2040/.cargo/config new file mode 100644 index 0000000..ed10a10 --- /dev/null +++ b/rp2040/.cargo/config @@ -0,0 +1,42 @@ +# +# Cargo Configuration for the https://github.com/rp-rs/rp-hal.git repository. +# +# Copyright (c) The RP-RS Developers, 2021 +# +# You might want to make a similar file in your own repository if you are +# writing programs for Raspberry Silicon microcontrollers. +# +# This file is MIT or Apache-2.0 as per the repository README.md file +# + +[build] +# Set the default target to match the Cortex-M0+ in the RP2040 +target = "thumbv6m-none-eabi" + +# Target specific options +[target.thumbv6m-none-eabi] +# Pass some extra options to rustc, some of which get passed on to the linker. +# +# * linker argument --nmagic turns off page alignment of sections (which saves +# flash space) +# * linker argument -Tlink.x tells the linker to use link.x as the linker +# script. This is usually provided by the cortex-m-rt crate, and by default +# the version in that crate will include a file called `memory.x` which +# describes the particular memory layout for your specific chip. +# * inline-threshold=5 makes the compiler more aggressive and inlining functions +# * no-vectorize-loops turns off the loop vectorizer (seeing as the M0+ doesn't +# have SIMD) +rustflags = [ + "-C", "link-arg=--nmagic", + "-C", "link-arg=-Tlink.x", + "-C", "inline-threshold=5", + "-C", "no-vectorize-loops", +] + +# This runner will make a UF2 file and then copy it to a mounted RP2040 in USB +# Bootloader mode: +runner = "elf2uf2-rs -d" + +# This runner will find a supported SWD debug probe and flash your RP2040 over +# SWD: +# runner = "probe-run --chip RP2040" diff --git a/rp2040/Cargo.toml b/rp2040/Cargo.toml new file mode 100644 index 0000000..dfc0c83 --- /dev/null +++ b/rp2040/Cargo.toml @@ -0,0 +1,59 @@ +[package] +name = "rp2040" +version = "0.1.0" +edition = "2021" + +[dependencies] +cortex-m = "0.7.2" +waveshare-rp2040-zero = "0.6.0" +rp2040-boot2 = { version = "0.2.0", optional = true } +rp2040-hal = { version = "0.8.0" } +cortex-m-rt = { version = "0.7", optional = true } +panic-halt= "0.2.0" +embedded-hal ="0.2.5" +log = { version = "0.4", optional = true } +fugit = "0.3.5" +nb = "1.0.0" +smart-leds = "0.3.0" +smart-leds-trait = "0.2.1" +ws2812-pio = "0.6.0" +usbd-human-interface-device = "0.4.2" +usb-device = "0.2" +pio = "0.2.0" +defmt = { version = "0.3", optional = true } +libm = "0.2.7" +dyn-smooth = "0.2.0" + +[features] +# This is the set of features we enable by default +default = ["boot2", "rt", "critical-section-impl", "rom-func-cache"] + +# critical section that is safe for multicore use +critical-section-impl = ["rp2040-hal/critical-section-impl"] + +# 2nd stage bootloaders for rp2040 +boot2 = ["rp2040-boot2"] + +# Minimal startup / runtime for Cortex-M microcontrollers +rt = ["cortex-m-rt","rp2040-hal/rt"] + +# This enables a fix for USB errata 5: USB device fails to exit RESET state on busy USB bus. +# Only required for RP2040 B0 and RP2040 B1, but it doesn't hurt to enable it +rp2040-e5 = ["rp2040-hal/rp2040-e5"] + +# Memoize(cache) ROM function pointers on first use to improve performance +rom-func-cache = ["rp2040-hal/rom-func-cache"] + +# Disable automatic mapping of language features (like floating point math) to ROM functions +disable-intrinsics = ["rp2040-hal/disable-intrinsics"] + +# This enables ROM functions for f64 math that were not present in the earliest RP2040s +rom-v2-intrinsics = ["rp2040-hal/rom-v2-intrinsics"] + +defmt = ["dep:defmt", "usb-device/defmt"] + +[[bin]] +name = "rp2040" +test = false +bench = false + diff --git a/rp2040/memory.x b/rp2040/memory.x new file mode 100644 index 0000000..4077aab --- /dev/null +++ b/rp2040/memory.x @@ -0,0 +1,15 @@ +MEMORY { + BOOT2 : ORIGIN = 0x10000000, LENGTH = 0x100 + FLASH : ORIGIN = 0x10000100, LENGTH = 2048K - 0x100 + RAM : ORIGIN = 0x20000000, LENGTH = 256K +} + +EXTERN(BOOT2_FIRMWARE) + +SECTIONS { + /* ### Boot loader */ + .boot2 ORIGIN(BOOT2) : + { + KEEP(*(.boot2)); + } > BOOT2 +} INSERT BEFORE .text; diff --git a/rp2040/pico-load b/rp2040/pico-load new file mode 100755 index 0000000..2a31890 --- /dev/null +++ b/rp2040/pico-load @@ -0,0 +1,12 @@ +#!/bin/bash +sudo umount /mnt/usb +while [ ! -f /mnt/usb/INFO_UF2.TXT ]; do + sudo mount /dev/sda1 /mnt/usb -o umask=000 + sleep 1 +done +set -e +# cargo build --release --example waveshare_rp2040_zero_neopixel_rainbow +cargo run --release +# elf2uf2-rs $1 +# cp $1.uf2 /mnt/usb +sudo umount /mnt/usb diff --git a/rp2040/src/button_matrix.rs b/rp2040/src/button_matrix.rs new file mode 100644 index 0000000..2d20098 --- /dev/null +++ b/rp2040/src/button_matrix.rs @@ -0,0 +1,98 @@ +//! 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 core::convert::Infallible; +use cortex_m::delay::Delay; +use embedded_hal::digital::v2::*; + +/// Button matrix driver +/// +/// # Example +/// ``` +/// let button_matrix: ButtonMatrix<4, 6, 48> = ButtonMatrix::new(row_pins, col_pins, 5); +/// ``` +pub struct ButtonMatrix<'a, const R: usize, const C: usize, const N: usize> { + rows: &'a [&'a dyn InputPin; R], + cols: &'a mut [&'a mut dyn OutputPin; C], + pressed: [bool; N], + debounce: u8, + debounce_counter: [u8; N], +} + +impl<'a, const R: usize, const C: usize, const N: usize> ButtonMatrix<'a, R, C, N> { + /// Creates a new button matrix. + /// + /// # Arguments + /// + /// * `rows` - An array of references to the row pins. + /// * `cols` - An array of references to the column pins. + /// * `debounce` - The debounce time in number of scans. + pub fn new( + rows: &'a [&'a dyn InputPin; R], + cols: &'a mut [&'a mut dyn OutputPin; C], + debounce: u8, + ) -> Self { + Self { + rows, + cols, + pressed: [false; N], + debounce, + debounce_counter: [0; N], + } + } + + /// Initializes the button matrix. + /// This should be called once before scanning the matrix. + pub fn init_pins(&mut self) { + for col in self.cols.iter_mut() { + col.set_high().unwrap(); + } + } + + /// Scans the button matrix and updates the pressed state of each button. + /// This should be called at regular intervals. + /// Allow at least 5 times the delay compared to the needed button latency. + /// + /// # Arguments + /// + /// * `delay` - A mutable reference to a delay object. + 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); + self.process_column(col_index); + self.cols[col_index].set_high().unwrap(); + delay.delay_us(10); + } + } + + /// Processes a column of the button matrix. + /// + /// # Arguments + /// + /// * `col_index` - The index of the column to process. + fn process_column(&mut self, col_index: usize) { + for row_index in 0..self.rows.len() { + let button_index: usize = col_index + (row_index * C); + let current_state = self.rows[row_index].is_low().unwrap(); + + if current_state == self.pressed[button_index] { + self.debounce_counter[button_index] = 0; + continue; + } + + self.debounce_counter[button_index] += 1; + if self.debounce_counter[button_index] >= self.debounce { + self.pressed[button_index] = current_state; + } + } + } + + /// Returns an array of booleans indicating whether each button is pressed. + pub fn buttons_pressed(&mut self) -> [bool; N] { + self.pressed + } +} diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs new file mode 100644 index 0000000..ef43076 --- /dev/null +++ b/rp2040/src/layout.rs @@ -0,0 +1,261 @@ +//! Project: CMtec CMDR joystick 24 +//! Date: 2023-08-01 +//! Author: Christoffer Martinsson +//! Email: cm@cmtec.se +//! License: Please refer to LICENSE in root directory + +use crate::NUMBER_OF_BUTTONS; + +#[allow(dead_code)] +#[derive(Debug, PartialEq, PartialOrd, Copy, Clone)] +pub enum ElrsButton { + CH1 = 0, + CH2 = 1, + CH3 = 2, + CH4 = 3, + CH5 = 4, + CH6 = 5, + CH7 = 6, + CH8 = 7, + CH9 = 8, + CH10 = 9, + CH11 = 10, + CH12 = 11, + CH5ON = 12, + CH5OFF = 13, + CH6ON = 14, + CH6OFF = 15, + CH7ON = 16, + CH7OFF = 17, + CH8ON = 18, + CH8OFF = 19, + CH9ON = 20, + CH9OFF = 21, + CH10ON = 22, + CH10OFF = 23, + CH11ON = 24, + CH11OFF = 25, + CH12ON = 26, + CH12OFF = 27, + NoEventIndicated = 28, +} +#[derive(Debug, PartialEq, PartialOrd, Copy, Clone)] +pub enum HidButton { + B1 = 0, + B2 = 1, + B3 = 2, + B4 = 3, + B5 = 4, + B6 = 5, + B7 = 6, + B8 = 7, + B9 = 8, + B10 = 9, + B11 = 10, + B12 = 11, + B13 = 12, + B14 = 13, + B15 = 14, + B16 = 15, + B17 = 16, + B18 = 17, + B19 = 18, + B20 = 19, + FnL = 20, + FnR = 21, + ModeL = 22, + ModeR = 23, + Hat1U = 24, + Hat1R = 25, + Hat1D = 26, + Hat1L = 27, + Hat1B = 28, + Hat2U = 29, + Hat2R = 30, + Hat2D = 31, + Hat2L = 32, + Hat2B = 33, + Hat3U = 34, + Hat3R = 35, + Hat3D = 36, + Hat3L = 37, + Hat3B = 38, + Hat4U = 39, + Hat4R = 40, + Hat4D = 41, + Hat4L = 42, + Hat4B = 43, + NoEventIndicated = 44, +} +#[warn(dead_code)] +// Button index map: +// -------------------------------------------------------------- +// | 0 | 1 | | 3 | 4 | (2) +// -------------------------------------------------------------- +// | | 5 | 6 | 7 | | 12 | 11 | 10 | | +// | | +// | | 8 | | 13 | | +// | | 9 | | 14 | | +// | X1/Y1 X2/Y2 | +// | | 16 | | 21 | | +// | | 17 | 15 | 18 || 22 | 20 | 23 | | +// | | 19 | | 24 | | +// -------------------------------------------------------------- +// +/// Button map to HID key (four function layers) +/// Please make sure to set FnL, FnR, ModeL and ModeR at the same position for all layers +/// alt. only set these at function layer 0 and set NoEventIndicated in layer 1-3. +/// Hat button 1-4 = HID B21-B24. +pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ + [ + // Function layer 0 + // HID Key // Button Index + // ----------------------------------------- + HidButton::FnL, // 0 + HidButton::B1, // 1 + HidButton::NoEventIndicated, // 2 Not connected to any button + HidButton::FnR, // 3 + HidButton::B6, // 4 + HidButton::B2, // 5 + HidButton::B3, // 6 + HidButton::ModeL, // 7 + HidButton::B4, // 8 + HidButton::B5, // 9 + HidButton::B7, // 10 + HidButton::B8, // 11 + HidButton::ModeR, // 12 + HidButton::B9, // 13 + HidButton::B10, // 14 + HidButton::Hat1B, // 15 button 21 + HidButton::Hat1U, // 16 + HidButton::Hat1R, // 17 + HidButton::Hat1D, // 18 + HidButton::Hat1L, // 19 + HidButton::Hat2B, // 20 button 22 + HidButton::Hat2U, // 21 + HidButton::Hat2R, // 22 + HidButton::Hat2D, // 23 + HidButton::Hat2L, // 24 + ], + [ + // Function layer 1 (left Fn button pressed) + // HID Key // Button Index + // ----------------------------------------- + HidButton::FnL, // 0 + HidButton::B11, // 1 + HidButton::NoEventIndicated, // 2 Not connected to any button + HidButton::FnR, // 3 + HidButton::B6, // 4 + HidButton::B12, // 5 + HidButton::B13, // 6 + HidButton::ModeL, // 7 + HidButton::B14, // 8 + HidButton::B15, // 9 + HidButton::B7, // 10 + HidButton::B8, // 11 + HidButton::ModeR, // 12 + HidButton::B9, // 13 + HidButton::B10, // 14 + HidButton::Hat3B, // 15 button 23 + HidButton::Hat3U, // 16 + HidButton::Hat3R, // 17 + HidButton::Hat3D, // 18 + HidButton::Hat3L, // 19 + HidButton::Hat2B, // 20 button 22 + HidButton::Hat2U, // 21 + HidButton::Hat2R, // 22 + HidButton::Hat2D, // 23 + HidButton::Hat2L, // 24 + ], + [ + // Function layer 2 (right Fn button pressed) + // HID Key // Button Index + // ----------------------------------------- + HidButton::FnL, // 0 + HidButton::B1, // 1 + HidButton::NoEventIndicated, // 2 Not connected to any button + HidButton::FnR, // 3 + HidButton::B16, // 4 + HidButton::B2, // 5 + HidButton::B3, // 6 + HidButton::ModeL, // 7 + HidButton::B4, // 8 + HidButton::B5, // 9 + HidButton::B17, // 10 + HidButton::B18, // 11 + HidButton::ModeR, // 12 + HidButton::B19, // 13 + HidButton::B20, // 14 + HidButton::Hat1B, // 15 button 21 + HidButton::Hat1U, // 16 + HidButton::Hat1R, // 17 + HidButton::Hat1D, // 18 + HidButton::Hat1L, // 19 + HidButton::Hat4B, // 20 button 24 + HidButton::Hat4U, // 21 + HidButton::Hat4R, // 22 + HidButton::Hat4D, // 23 + HidButton::Hat4L, // 24 + ], + [ + // Function layer 3 (left + right Fn button pressed) + // HID Key // Button Index + // ----------------------------------------- + HidButton::FnL, // 0 + HidButton::B11, // 1 + HidButton::NoEventIndicated, // 2 Not connected to any button + HidButton::FnR, // 3 + HidButton::B16, // 4 + HidButton::B12, // 5 + HidButton::B13, // 6 + HidButton::ModeL, // 7 + HidButton::B14, // 8 + HidButton::B15, // 9 + HidButton::B17, // 10 + HidButton::B18, // 11 + HidButton::ModeR, // 12 + HidButton::B19, // 13 + HidButton::B20, // 14 + HidButton::Hat3B, // 15 button 23 + HidButton::Hat3U, // 16 + HidButton::Hat3R, // 17 + HidButton::Hat3D, // 18 + HidButton::Hat3L, // 19 + HidButton::Hat4B, // 20 button 24 + HidButton::Hat4U, // 21 + HidButton::Hat4R, // 22 + HidButton::Hat4D, // 23 + HidButton::Hat4L, // 24 + ], +]; + +pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ + // Function layer 0 + // HID Key // Button Index + // ----------------------------------------- + ElrsButton::CH7OFF, // 0 + ElrsButton::CH7ON, // 1 + ElrsButton::NoEventIndicated, // 2 Not connected to any button + ElrsButton::CH8OFF, // 3 + ElrsButton::CH8ON, // 4 + ElrsButton::CH9ON, // 5 + ElrsButton::CH9OFF, // 6 + ElrsButton::CH5, // 7 + ElrsButton::CH10ON, // 8 + ElrsButton::CH10OFF, // 9 + ElrsButton::CH11ON, // 10 + ElrsButton::CH11OFF, // 11 + ElrsButton::CH6, // 12 + ElrsButton::CH12ON, // 13 + ElrsButton::CH12OFF, // 14 + ElrsButton::NoEventIndicated, // 15 + ElrsButton::NoEventIndicated, // 16 + ElrsButton::NoEventIndicated, // 17 + ElrsButton::NoEventIndicated, // 18 + ElrsButton::NoEventIndicated, // 19 + ElrsButton::NoEventIndicated, // 20 + ElrsButton::NoEventIndicated, // 21 + ElrsButton::NoEventIndicated, // 22 + ElrsButton::NoEventIndicated, // 23 + ElrsButton::NoEventIndicated, // 24 +]; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs new file mode 100644 index 0000000..057eb15 --- /dev/null +++ b/rp2040/src/main.rs @@ -0,0 +1,751 @@ +//! Project: CMtec CMDR joystick 24 +//! Date: 2023-08-01 +//! Author: Christoffer Martinsson +//! Email: cm@cmtec.se +//! License: Please refer to LICENSE in root directory + +#![no_std] +#![no_main] + +mod button_matrix; +mod layout; +mod status_led; +mod usb_joystick_device; + +use button_matrix::ButtonMatrix; +use core::convert::Infallible; +use cortex_m::delay::Delay; +use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS}; +use embedded_hal::adc::OneShot; +use embedded_hal::digital::v2::*; +use embedded_hal::timer::CountDown; +use fugit::ExtU32; +use libm::powf; +use panic_halt as _; +use rp2040_hal::{ + adc::Adc, + gpio::{Function, FunctionConfig, PinId, ValidPinMode}, + pio::StateMachineIndex, +}; +use status_led::{StatusMode, Ws2812StatusLed}; +use usb_device::class_prelude::*; +use usb_device::prelude::*; +use usb_joystick_device::{JoystickConfig, JoystickReport}; +use usbd_human_interface_device::prelude::*; +use waveshare_rp2040_zero::entry; +use waveshare_rp2040_zero::{ + hal::{ + clocks::{init_clocks_and_plls, Clock}, + pac, + pio::PIOExt, + timer::Timer, + watchdog::Watchdog, + Sio, + }, + Pins, XOSC_CRYSTAL_FREQ, +}; + +// Public constants +pub const BUTTON_ROWS: usize = 5; +pub const BUTTON_COLS: usize = 5; +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 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; + +// Analog smoothing settings. +pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS; +pub const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS; +pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32; + +// Public types +#[derive(Copy, Clone, Default)] +pub struct Button { + pub pressed: bool, + pub previous_pressed: bool, + pub fn_mode: u8, +} + +#[derive(Copy, Clone)] +pub struct GimbalAxis { + pub value: u16, + pub previous_value: u16, + pub idle_value: u16, + pub max: u16, + pub min: u16, + pub center: u16, + pub fn_mode: u8, + pub deadzone: (u16, u16, u16), + pub expo: f32, +} + +impl Default for GimbalAxis { + fn default() -> Self { + GimbalAxis { + value: AXIS_CENTER, + previous_value: AXIS_CENTER, + idle_value: AXIS_CENTER, + max: AXIS_MAX, + min: AXIS_MIN, + center: AXIS_CENTER, + fn_mode: 0, + deadzone: (50, 50, 50), + expo: 0.2, + } + } +} + +#[entry] +fn main() -> ! { + // Grab our singleton objects + let mut pac = pac::Peripherals::take().unwrap(); + + // Set up the watchdog driver - needed by the clock setup code + let mut watchdog = Watchdog::new(pac.WATCHDOG); + + // Configure clocks and PLLs + let clocks = init_clocks_and_plls( + XOSC_CRYSTAL_FREQ, + pac.XOSC, + pac.CLOCKS, + pac.PLL_SYS, + pac.PLL_USB, + &mut pac.RESETS, + &mut watchdog, + ) + .ok() + .unwrap(); + + let core = pac::CorePeripherals::take().unwrap(); + + // The single-cycle I/O block controls our GPIO pins + let sio = Sio::new(pac.SIO); + + // Set the pins to their default state + let pins = Pins::new( + pac.IO_BANK0, + pac.PADS_BANK0, + sio.gpio_bank0, + &mut pac.RESETS, + ); + + // Enable adc + let mut adc = Adc::new(pac.ADC, &mut pac.RESETS); + + // Configure ADC input pins + // Have not figured out hov to store the adc pins in an array yet + // TODO: Find a way to store adc pins in an array + let mut adc_pin_left_x = pins.gp29.into_floating_input(); + let mut adc_pin_left_y = pins.gp28.into_floating_input(); + let mut adc_pin_right_x = pins.gp27.into_floating_input(); + let mut adc_pin_right_y = pins.gp26.into_floating_input(); + + // 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(), + ]; + + // 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(), + ]; + + // Create button matrix object that scans all buttons + let mut button_matrix: ButtonMatrix = + ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, 5); + + // Initialize button matrix + button_matrix.init_pins(); + + // Create status LED + let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS); + let mut status_led = Ws2812StatusLed::new( + pins.neopixel.into_mode(), + &mut pio, + sm0, + clocks.peripheral_clock.freq(), + ); + + // 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); + } + if button_matrix.buttons_pressed()[0] { + status_led.update(StatusMode::Bootloader); + let gpio_activity_pin_mask: u32 = 0; + let disable_interface_mask: u32 = 0; + rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask); + } + + // Create timers + let timer = Timer::new(pac.TIMER, &mut pac.RESETS); + + let mut usb_hid_report_count_down = timer.count_down(); + usb_hid_report_count_down.start(10.millis()); + + let mut scan_count_down = timer.count_down(); + scan_count_down.start(1.millis()); + + 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 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 axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS]; + let mut buttons: [Button; NUMBER_OF_BUTTONS] = [Button::default(); NUMBER_OF_BUTTONS]; + + // 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; + + // 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), + DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), + DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), + ]; + + // Configure USB + let usb_bus = UsbBusAllocator::new(waveshare_rp2040_zero::hal::usb::UsbBus::new( + pac.USBCTRL_REGS, + pac.USBCTRL_DPRAM, + clocks.usb_clock, + true, + &mut pac.RESETS, + )); + + let mut usb_hid_joystick = UsbHidClassBuilder::new() + .add_device(JoystickConfig::default()) + .build(&usb_bus); + + let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x1209, 0x0002)) + .manufacturer("CMtec") + .product("CMDR Joystick") + .serial_number("0001") + .build(); + + 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); + } + // ----------------------------------------------------------- + + 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()); + + 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, + ); + } + + 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; + } + + if status_led_count_down.wait().is_ok() { + update_status_led( + &mut status_led, + &mut activity, + &usb_active, + &idle, + &safety_check, + ); + } + + if usb_hid_report_count_down.wait().is_ok() && activity { + // Dont send USB HID joystick report if there is no activity + // This is to avoid preventing the computer from going to sleep + match usb_hid_joystick.device().write_report(&get_joystick_report( + &mut buttons, + &mut axis, + &mode, + )) { + Err(UsbHidError::WouldBlock) => {} + Ok(_) => {} + Err(e) => { + status_led.update(StatusMode::Error); + core::panic!("Failed to write joystick report: {:?}", e) + } + }; + } + + // Check if all axis are in idle position and no buttons are pressed + if idle && !safety_check && !usb_active { + safety_check = true; + } + + // TODO: Implement ELRS + if elrs_count_down.wait().is_ok() && !usb_active && safety_check {} + } +} + +/// Update status LED colour based on function layer and capslock +/// +/// USB mode = green +/// ELRS mode = orange +/// Activity = flashing blue +/// Error = steady red (ERROR) +/// +/// # Arguments +/// * `status_led` - Reference to status LED +/// * `activity` - Reference to bool that indicates if there is activity +/// * `usb_active` - Reference to bool that indicates if USB is active +/// * `axis_idle` - Reference to bool that indicates if all axis are in idle position +/// * `safety_check` - Reference to bool that indicates if safety check has passed +fn update_status_led( + status_led: &mut Ws2812StatusLed, + activity: &mut bool, + usb_active: &bool, + axis_idle: &bool, + safety_check: &bool, +) where + P: PIOExt + FunctionConfig, + I: PinId, + Function

: ValidPinMode, + SM: StateMachineIndex, +{ + if !usb_active && !*safety_check { + status_led.update(StatusMode::Warning); + } 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; + } 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); + } else if status_led.get_mode() != StatusMode::Other { + status_led.update(StatusMode::Other); + } +} + +/// Get current Fn mode (0, 1, 2 or 3 and alt l/r mode) +/// layout::MAP contains the button types +/// +/// # Arguments +/// +/// * `pressed_keys` - Array of pressed keys +fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { + // Check how many Fn keys are pressed + let mut mode: u8 = 0; + let mut fn_l_active: bool = false; + let mut fn_r_active: bool = false; + let mut alt_l_active: bool = false; + let mut alt_r_active: bool = false; + + for (index, key) in pressed_keys.iter().enumerate() { + if *key && layout::HID_MAP[0][index] == layout::HidButton::FnL { + fn_l_active = true; + } + if *key && layout::HID_MAP[0][index] == layout::HidButton::FnR { + fn_r_active = true; + } + if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeL { + alt_l_active = true; + } + if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeR { + alt_r_active = true; + } + } + + if fn_l_active && fn_r_active { + mode = 3; + } else if fn_r_active { + mode = 2; + } else if fn_l_active { + mode = 1; + } + + // Set bit 4 and 5 if alt l/r is active + if alt_l_active { + mode |= 0x10; + } + if alt_r_active { + mode |= 0x20; + } + + mode +} + +/// Generate keyboard report based on pressed keys and Fn mode (0, 1 or 2) +/// layout::MAP contains the keycodes for each key in each Fn mode +/// +/// # Arguments +/// +/// * `matrix_keys` - Array of pressed keys +/// * `axis` - Array of joystick axis values +/// * `fn_mode` - Fn mode (0, 1, 2 or 3) +/// * `alt_l_mode` - Is left alt mode active +/// * `alt_r_mode` - Is right alt mode active +fn get_joystick_report( + matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], + axis: &mut [GimbalAxis; 4], + mode: &u8, +) -> JoystickReport { + let mut x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; + let mut y: u16 = axis[GIMBAL_AXIS_RIGHT_Y].value; + let 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, + ); + } + + // 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) + { + x = AXIS_CENTER; + rx = axis[GIMBAL_AXIS_RIGHT_X].value; + } + if mode & 0x20 == 0x20 + && (axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 2 || axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 3) + { + y = AXIS_CENTER; + ry = axis[GIMBAL_AXIS_RIGHT_Y].value; + } + + // 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]; + for (index, key) in matrix_keys.iter_mut().enumerate() { + if key.pressed + && layout::HID_MAP[key.fn_mode as usize][index] >= layout::HidButton::Hat1U + && layout::HID_MAP[key.fn_mode as usize][index] <= layout::HidButton::Hat4B + { + hats[(layout::HID_MAP[key.fn_mode as usize][index] as usize + - layout::HidButton::Hat1U as usize) + / 5] |= 1 + << ((layout::HID_MAP[key.fn_mode as usize][index] as usize + - layout::HidButton::Hat1U as usize) + - (5 * ((layout::HID_MAP[key.fn_mode as usize][index] as usize + - layout::HidButton::Hat1U as usize) + / 5))); + } + } + + // 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]); + + // 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); + + // Update button state for joystick button 1-20 + for (index, key) in matrix_keys.iter_mut().enumerate() { + if key.pressed + && layout::HID_MAP[key.fn_mode as usize][index] as usize + >= layout::HidButton::B1 as usize + && layout::HID_MAP[key.fn_mode as usize][index] as usize + <= layout::HidButton::B20 as usize + { + buttons |= 1 << layout::HID_MAP[key.fn_mode as usize][index] as usize; + } + } + + JoystickReport { + x, + y, + z, + rx, + ry, + rz, + hat1, + hat2, + hat3, + hat4, + buttons, + } +} + +/// Format hat value from 5 switches to USB HID coded value and button state +/// +/// # Arguments +/// * `input` - Hat value coded as +/// bit 1-4: direction (U R D L) +/// bit 5: button state +/// 0 = not pressed +/// 1 = pressed +fn format_hat_value(input: u8) -> (u8, u8) { + const HAT_CENTER: u8 = 0xf; + const HAT_UP: u8 = 0; + const HAT_UP_RIGHT: u8 = 1; + const HAT_RIGHT: u8 = 2; + const HAT_DOWN_RIGHT: u8 = 3; + const HAT_DOWN: u8 = 4; + const HAT_DOWN_LEFT: u8 = 5; + 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, + _ => HAT_CENTER, + }; + + // Alpine hat switch button filter + let mut button_state: u8 = 0; + if input & 0x10 == 0x10 && direction == HAT_CENTER { + button_state = 1; + } + + (direction, button_state) +} + +/// Calculate value for joystick axis +/// +/// # Arguments +/// * `value` - Value to calibrate +/// * `min` - Lower bound of the value's current range +/// * `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 +fn calculate_axis_value( + value: u16, + min: u16, + max: u16, + center: u16, + deadzone: (u16, u16, u16), + expo: f32, +) -> u16 { + let mut calibrated_value = AXIS_CENTER; + + if value > (center + deadzone.1) { + calibrated_value = remap( + value, + center + deadzone.1, + max - deadzone.2, + AXIS_CENTER, + AXIS_MAX, + ); + } else if value < (center - deadzone.1) { + calibrated_value = remap( + value, + min + deadzone.0, + center - deadzone.1, + AXIS_MIN, + AXIS_CENTER, + ); + } + + 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, + ); + } + + calibrated_value +} + +/// Remapping values from one range to another +/// +/// # Arguments +/// * `value` - Value to remap +/// * `in_min` - Lower bound of the value's current range +/// * `in_max` - Upper bound of the value's current range +/// * `out_min` - Lower bound of the value's target range +/// * `out_max` - Upper bound of the value's target range +fn remap(value: u16, in_min: u16, in_max: u16, out_min: u16, out_max: u16) -> u16 { + constrain( + (value as i64 - in_min as i64) * (out_max as i64 - out_min as i64) + / (in_max as i64 - in_min as i64) + + out_min as i64, + out_min as i64, + out_max as i64, + ) as u16 +} + +/// Constrain a value to a given range +/// +/// # Arguments +/// * `value` - Value to constrain +/// * `out_min` - Lower bound of the value's target range +/// * `out_max` - Upper bound of the value's target range +fn constrain(value: T, out_min: T, out_max: T) -> T { + if value < out_min { + out_min + } else if value > out_max { + out_max + } else { + value + } +} diff --git a/rp2040/src/status_led.rs b/rp2040/src/status_led.rs new file mode 100644 index 0000000..65f163f --- /dev/null +++ b/rp2040/src/status_led.rs @@ -0,0 +1,140 @@ +//! 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::{ + gpio::{Function, FunctionConfig, Pin, PinId, ValidPinMode}, + pio::{PIOExt, StateMachineIndex, UninitStateMachine, PIO}, +}; +use smart_leds::{SmartLedsWrite, RGB8}; +use ws2812_pio::Ws2812Direct; + +/// Status LED modes +/// +/// * OFF = Syatem offline +/// * NORMAL = All system Ok +/// * ACTIVITY = System activity +/// * OTHER = Other activity +/// * WARNING = Warning +/// * ERROR = Error +/// * BOOTLOADER = Bootloader active +#[allow(dead_code)] +#[derive(PartialEq, Eq, Copy, Clone)] +pub enum StatusMode { + Off = 0, + Normal = 1, + Activity = 2, + Other = 3, + Warning = 4, + Error = 5, + Bootloader = 6, +} +#[warn(dead_code)] + +/// Status LED driver +/// This driver uses the PIO state machine to drive a WS2812 LED +/// +/// # Example +/// +/// ``` +/// let mut status_led = Ws2812StatusLed::new( +/// pins.neopixel.into_mode(), +/// &mut pio, +/// sm0, +/// clocks.peripheral_clock.freq(), +/// ); +/// ``` +pub struct Ws2812StatusLed +where + I: PinId, + P: PIOExt + FunctionConfig, + Function

: ValidPinMode, + SM: StateMachineIndex, +{ + ws2812_direct: Ws2812Direct, + state: bool, + mode: StatusMode, +} + +impl Ws2812StatusLed +where + I: PinId, + P: PIOExt + FunctionConfig, + Function

: ValidPinMode, + SM: StateMachineIndex, +{ + /// Creates a new instance of this driver. + /// + /// # Arguments + /// + /// * `pin` - PIO pin + /// * `pio` - PIO instance + /// * `sm` - PIO state machine + /// * `clock_freq` - PIO clock frequency + pub fn new( + pin: Pin>, + pio: &mut PIO

, + sm: UninitStateMachine<(P, SM)>, + clock_freq: fugit::HertzU32, + ) -> Self { + // prepare the PIO program + let ws2812_direct = Ws2812Direct::new(pin, pio, sm, clock_freq); + let state = false; + let mode = StatusMode::Off; + Self { + ws2812_direct, + state, + mode, + } + } + + /// Get current status mode + pub fn get_mode(&self) -> StatusMode { + self.mode + } + + /// Update status LED + /// Depending on the mode, the LED will be set to a different colour + /// + /// * OFF = off + /// * NORMAL = green + /// * ACTIVITY = blue + /// * OTHER = orange + /// * WARNING = red (flashing) + /// * ERROR = red + /// * BOOTLOADER = purple + /// + /// Make sure to call this function regularly to keep the LED flashing + pub fn update(&mut self, mode: StatusMode) { + let colors: [RGB8; 7] = [ + (0, 0, 0).into(), // Off + (10, 7, 0).into(), // Green + (10, 4, 10).into(), // Blue + (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 && !self.state { + self.ws2812_direct + .write([colors[mode as usize]].iter().copied()) + .unwrap(); + self.state = true; + } else if mode == StatusMode::Warning || mode == StatusMode::Off { + self.ws2812_direct + .write([colors[0]].iter().copied()) + .unwrap(); + self.state = false; + } else { + self.ws2812_direct + .write([colors[mode as usize]].iter().copied()) + .unwrap(); + self.state = true; + } + } +} diff --git a/rp2040/src/usb_joystick_device.rs b/rp2040/src/usb_joystick_device.rs new file mode 100644 index 0000000..1d81a58 --- /dev/null +++ b/rp2040/src/usb_joystick_device.rs @@ -0,0 +1,209 @@ +//! 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 core::default::Default; +use fugit::ExtU32; +use usb_device::bus::UsbBus; +use usb_device::class_prelude::UsbBusAllocator; +use usbd_human_interface_device::usb_class::prelude::*; + +// Fetched from https://github.com/embassy-rs/embassy/blob/e3efda2249640e0b4881289aa609c96a26a7479a/embassy-hal-common/src/fmt.rs +#[cfg(feature = "defmt")] +macro_rules! unwrap { + ($($x:tt)*) => { + ::defmt::unwrap!($($x)*) + }; +} + +#[cfg(not(feature = "defmt"))] +macro_rules! unwrap { + ($arg:expr) => { + match $crate::usb_joystick_device::Try::into_result($arg) { + ::core::result::Result::Ok(t) => t, + ::core::result::Result::Err(e) => { + ::core::panic!("unwrap of `{}` failed: {:?}", ::core::stringify!($arg), e); + } + } + }; + ($arg:expr, $($msg:expr),+ $(,)? ) => { + match $crate::usb_joystick_device::Try::into_result($arg) { + ::core::result::Result::Ok(t) => t, + ::core::result::Result::Err(e) => { + ::core::panic!("unwrap of `{}` failed: {}: {:?}", ::core::stringify!($arg), ::core::format_args!($($msg,)*), e); + } + } + } +} + +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +pub struct NoneError; + +pub trait Try { + type Ok; + type Error; + fn into_result(self) -> Result; +} + +impl Try for Option { + type Ok = T; + type Error = NoneError; + + #[inline] + fn into_result(self) -> Result { + self.ok_or(NoneError) + } +} + +impl Try for Result { + type Ok = T; + type Error = E; + + #[inline] + fn into_result(self) -> Self { + self + } +} + +// 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 +#[rustfmt::skip] +pub const JOYSTICK_DESCRIPTOR: &[u8] = &[ + 0x05, 0x01, // Usage Page (Generic Desktop) + 0x09, 0x04, // Usage (Joystick) + 0xa1, 0x01, // Collection (Application) + 0x09, 0x01, // Usage Page (Pointer) + 0xa1, 0x00, // Collection (Physical) + 0x09, 0x30, // Usage (X) + 0x09, 0x31, // Usage (Y) + 0x09, 0x32, // Usage (Z) + 0x09, 0x33, // Usage (RX) + 0x09, 0x34, // Usage (RY) + 0x09, 0x35, // Usage (RZ) + 0x15, 0x00, // Logical Minimum (0) + 0x26, 0xFF, 0x0F, // Logical Maximum (4095) + 0x75, 0x0C, // Report Size (12) + 0x95, 0x06, // Report count (6) + 0x81, 0x02, // Input (Data, Variable, Absolute) + 0xc0, // End Collection + 0x05, 0x09, // Usage Page (Button) + 0x19, 0x01, // Usage Minimum (1) + 0x29, 0x18, // Usage Maximum (24) + 0x15, 0x00, // Logical Minimum (0) + 0x25, 0x01, // Logical Maximum (1) + 0x75, 0x01, // Report Size (1) + 0x95, 0x18, // Report Count (24) + 0x81, 0x02, // Input (Data, Variable, Absolute) + 0x15, 0x00, // Logical Minimum (0) + 0x25, 0x07, // Logical Maximum (7) + 0x35, 0x00, // Physical Minimum (0) + 0x46, 0x3B, 0x01, // Physical Maximum (315) + 0x75, 0x04, // Report Size (4) + 0x95, 0x04, // Report Count (4) + 0x65, 0x14, // Unit (20) + 0x05, 0x01, // Usage Page (Generic Desktop) + 0x09, 0x39, // Usage (Hat switch) + 0x09, 0x39, // Usage (Hat switch) + 0x09, 0x39, // Usage (Hat switch) + 0x09, 0x39, // Usage (Hat switch) + 0x81, 0x42, // Input (variable,absolute,null_state) + 0xc0, // End Collection +]; + +#[derive(Clone, Copy, Debug, Eq, PartialEq, Default)] +pub struct JoystickReport { + pub x: u16, // 12bit + pub y: u16, // 12bit + pub z: u16, // 12bit + pub rx: u16, // 12bit + pub ry: u16, // 12bit + pub rz: u16, // 12bit + pub buttons: u32, // 24bit + pub hat1: u8, // 4bit + pub hat2: u8, // 4bit + pub hat3: u8, // 4bit + pub hat4: u8, // 4bit +} + +pub struct Joystick<'a, B: UsbBus> { + interface: Interface<'a, B, InBytes16, OutNone, ReportSingle>, +} + +impl<'a, B: UsbBus> Joystick<'a, B> { + pub fn write_report(&mut self, report: &JoystickReport) -> Result<(), UsbHidError> { + let mut data: [u8; 14] = [0; 14]; + + // Did not make the packed struct work, so doing it manually + // TODO: make this work with packed struct + data[0] = report.x as u8; + data[1] = ((report.x >> 8) as u8) | ((report.y << 4) as u8); + data[2] = (report.y >> 4) as u8; + data[3] = report.z as u8; + data[4] = ((report.z >> 8) as u8) | ((report.rx << 4) as u8); + data[5] = (report.rx >> 4) as u8; + data[6] = report.ry as u8; + data[7] = ((report.ry >> 8) as u8) | ((report.rz << 4) as u8); + data[8] = (report.rz >> 4) as u8; + 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); + + self.interface + .write_report(&data) + .map(|_| ()) + .map_err(UsbHidError::from) + } +} + +impl<'a, B: UsbBus> DeviceClass<'a> for Joystick<'a, B> { + type I = Interface<'a, B, InBytes16, OutNone, ReportSingle>; + + fn interface(&mut self) -> &mut Self::I { + &mut self.interface + } + + fn reset(&mut self) {} + + fn tick(&mut self) -> Result<(), UsbHidError> { + Ok(()) + } +} + +pub struct JoystickConfig<'a> { + interface: InterfaceConfig<'a, InBytes16, OutNone, ReportSingle>, +} + +impl<'a> Default for JoystickConfig<'a> { + #[must_use] + fn default() -> Self { + Self::new( + unwrap!(unwrap!(InterfaceBuilder::new(JOYSTICK_DESCRIPTOR)) + .boot_device(InterfaceProtocol::None) + .description("Joystick") + .in_endpoint(10.millis())) + .without_out_endpoint() + .build(), + ) + } +} + +impl<'a> JoystickConfig<'a> { + #[must_use] + pub fn new(interface: InterfaceConfig<'a, InBytes16, OutNone, ReportSingle>) -> Self { + Self { interface } + } +} + +impl<'a, B: UsbBus + 'a> UsbAllocatable<'a, B> for JoystickConfig<'a> { + type Allocated = Joystick<'a, B>; + + fn allocate(self, usb_alloc: &'a UsbBusAllocator) -> Self::Allocated { + Self::Allocated { + interface: Interface::new(usb_alloc, self.interface), + } + } +} diff --git a/firmware/.gitignore b/teensylc/.gitignore similarity index 100% rename from firmware/.gitignore rename to teensylc/.gitignore diff --git a/firmware/.gitkeep b/teensylc/.gitkeep similarity index 100% rename from firmware/.gitkeep rename to teensylc/.gitkeep diff --git a/firmware/Makefile b/teensylc/Makefile similarity index 100% rename from firmware/Makefile rename to teensylc/Makefile diff --git a/firmware/extra_script.py b/teensylc/extra_script.py similarity index 100% rename from firmware/extra_script.py rename to teensylc/extra_script.py diff --git a/firmware/include/.gitkeep b/teensylc/include/.gitkeep similarity index 100% rename from firmware/include/.gitkeep rename to teensylc/include/.gitkeep diff --git a/firmware/lib/.gitkeep b/teensylc/lib/.gitkeep similarity index 100% rename from firmware/lib/.gitkeep rename to teensylc/lib/.gitkeep diff --git a/firmware/lib/teensy-libc b/teensylc/lib/teensy-libc similarity index 100% rename from firmware/lib/teensy-libc rename to teensylc/lib/teensy-libc diff --git a/firmware/platformio.ini b/teensylc/platformio.ini similarity index 100% rename from firmware/platformio.ini rename to teensylc/platformio.ini diff --git a/firmware/src/cmdr_joystick.cpp b/teensylc/src/cmdr_joystick.cpp similarity index 87% rename from firmware/src/cmdr_joystick.cpp rename to teensylc/src/cmdr_joystick.cpp index 20ca156..16de500 100644 --- a/firmware/src/cmdr_joystick.cpp +++ b/teensylc/src/cmdr_joystick.cpp @@ -279,23 +279,10 @@ void send_usb_data() { Joystick.Zrotate(joystick_x1_10bit); Joystick.Z(joystick_y1_10bit); - - if (fn_mode == 2) { - Joystick.X(AXIS_10BIT_CENTER); - Joystick.Y(AXIS_10BIT_CENTER); - Joystick.sliderRight(joystick_x2_10bit); - Joystick.sliderLeft(joystick_y2_10bit); - } else if (fn_mode == 1) { - Joystick.X(AXIS_10BIT_CENTER); - Joystick.Y(joystick_y2_10bit); - Joystick.sliderRight(joystick_x2_10bit); - Joystick.sliderLeft(AXIS_10BIT_CENTER); - } else { - Joystick.X(joystick_x2_10bit); - Joystick.Y(joystick_y2_10bit); - Joystick.sliderRight(AXIS_10BIT_CENTER); - Joystick.sliderLeft(AXIS_10BIT_CENTER); - } + Joystick.X(joystick_x2_10bit); + Joystick.Y(joystick_y2_10bit); + Joystick.sliderRight(AXIS_10BIT_CENTER); + Joystick.sliderLeft(AXIS_10BIT_CENTER); // Set USB digital channels for (int i = 1; i < 32; i++) { @@ -304,29 +291,14 @@ void send_usb_data() { Joystick.hat(JOYSTICK_HAT_CENTER); - if (fn_mode == 2) { - if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(12, 1); - if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(13, 1); - if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(14, 1); - if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(15, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(16, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(17, 1); - } else if (fn_mode == 1) { - if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(8, 1); - if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(9, 1); - if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(10, 1); - if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(11, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1); - } else { - if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(3, 1); - if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(4, 1); - if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(5, 1); - if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(6, 1); - if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(7, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1); - if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1); - } + if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(3, 1); + if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(8, 1); + if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(4, 1); + if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(5, 1); + if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(6, 1); + if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(7, 1); + if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1); + if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1); Joystick.send_now(); } @@ -397,15 +369,6 @@ void process_input_data() { joystick_y2_12bit = apply_calibration_12bit(analog_y2_gimbal_value, joystick_y2_12bit_min, joystick_y2_12bit_max, joystick_y2_12bit_center, DEADZONE_Y, exp_constant); - // Check fn mode - fn_mode = 0; - if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) { - fn_mode = 1; - if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) { - fn_mode = 2; - } - } - // Check toggle mode buttons if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) { toggle_button_mode = true;