Merge branch 'rp2040' into 'main'
Rp2040 See merge request cm/cmdr-joystick!1
This commit is contained in:
commit
f99a92fa26
2
.gitignore
vendored
2
.gitignore
vendored
@ -1,2 +1,4 @@
|
||||
firmware/.cache/clangd/index
|
||||
firmware/compile_commands.json
|
||||
rp2040/target
|
||||
rp2040/Cargo.lock
|
||||
|
||||
137
README.md
137
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
|
||||
|
||||
| B3 | | B1 |
|
||||
| Fn1 | | B2 |
|
||||
--------------------------------------------
|
||||
| | B4 | | B5 | |
|
||||
--------------------------------------------------------------
|
||||
| FnL | B1 | | B5 | FnR |
|
||||
--------------------------------------------------------------
|
||||
| | B2 | B3 | MoL | | MoR | B7 | B6 | |
|
||||
| |
|
||||
| X1,Y1 X2,Y2 |
|
||||
| | B6 | | B7 | |
|
||||
--------------------------------------------
|
||||
| | B4 | | B8 | |
|
||||
| | B17 | | B18 | |
|
||||
| Z/RZ X/Y |
|
||||
| | H1U | | H2U | |
|
||||
| | H1L | H1B | H1R || H2L | H2B | H2R | |
|
||||
| | H1D | | H2D | |
|
||||
--------------------------------------------------------------
|
||||
|
||||
USB Joystick Layer 1 (Fn1)
|
||||
|
||||
| Fn2 | | B1 |
|
||||
| Fn1 | | B2 |
|
||||
--------------------------------------------
|
||||
| | B8 | | B9 | |
|
||||
USB Joystick Layer 1 (FnL)
|
||||
--------------------------------------------------------------
|
||||
| FnL | B9 | | B5 | FnR |
|
||||
--------------------------------------------------------------
|
||||
| | B10 | B11 | MoL | | MoR | B7 | B6 | |
|
||||
| |
|
||||
| X1,Y1 X3,Y2 |
|
||||
| | B10 | | B11 | |
|
||||
--------------------------------------------
|
||||
| | B12 | | B8 | |
|
||||
| | B19 | | B18 | |
|
||||
| Z/RZ X/Y |
|
||||
| | H3U | | H2U | |
|
||||
| | H3L | H3B | H3R || H2L | H2B | H2R | |
|
||||
| | H3D | | H2D | |
|
||||
--------------------------------------------------------------
|
||||
|
||||
USB Joystick Layer 2 (Fn2)
|
||||
|
||||
| Fn2 | | B16 |
|
||||
| Fn1 | | B17 |
|
||||
--------------------------------------------
|
||||
| | B12 | | B13 | |
|
||||
USB Joystick Layer 2 (FnR)
|
||||
--------------------------------------------------------------
|
||||
| FnL | B1 | | B13 | FnR |
|
||||
--------------------------------------------------------------
|
||||
| | B2 | B3 | MoL | | MoR | B15 | B14 | |
|
||||
| |
|
||||
| X1,Y1 X3,Y3 |
|
||||
| | B14 | | B15 | |
|
||||
--------------------------------------------
|
||||
| | 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
|
||||
|
||||
| CH6 on | | CH5 on |
|
||||
| CH6 off | | CH5 off |
|
||||
--------------------------------------------
|
||||
| | CH7 | | CH8 | |
|
||||
--------------------------------------------------------------
|
||||
| CH7 | CH8 | | CH9 | CH10|
|
||||
--------------------------------------------------------------
|
||||
| | CH11| - | CH5 | | CH6 | - | CH12| |
|
||||
| |
|
||||
| X,Y X,Y |
|
||||
| CH1,CH2 | CH9 | | CH10 | CH3,CH4 |
|
||||
--------------------------------------------
|
||||
| | - | | - | |
|
||||
| | - | | - | |
|
||||
| X(CH1)/Y(CH2) X(CH3)/Y(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
|
||||
|
||||
42
eCAD/experiment_board.md
Normal file
42
eCAD/experiment_board.md
Normal file
@ -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
|
||||
42
rp2040/.cargo/config
Normal file
42
rp2040/.cargo/config
Normal file
@ -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"
|
||||
59
rp2040/Cargo.toml
Normal file
59
rp2040/Cargo.toml
Normal file
@ -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
|
||||
|
||||
15
rp2040/memory.x
Normal file
15
rp2040/memory.x
Normal file
@ -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;
|
||||
12
rp2040/pico-load
Executable file
12
rp2040/pico-load
Executable file
@ -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
|
||||
98
rp2040/src/button_matrix.rs
Normal file
98
rp2040/src/button_matrix.rs
Normal file
@ -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<Error = Infallible>; R],
|
||||
cols: &'a mut [&'a mut dyn OutputPin<Error = Infallible>; 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<Error = Infallible>; R],
|
||||
cols: &'a mut [&'a mut dyn OutputPin<Error = Infallible>; 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
|
||||
}
|
||||
}
|
||||
261
rp2040/src/layout.rs
Normal file
261
rp2040/src/layout.rs
Normal file
@ -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
|
||||
];
|
||||
751
rp2040/src/main.rs
Normal file
751
rp2040/src/main.rs
Normal file
@ -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<Error = Infallible>; 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<Error = Infallible>; 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<BUTTON_ROWS, BUTTON_COLS, NUMBER_OF_BUTTONS> =
|
||||
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<P, SM, I>(
|
||||
status_led: &mut Ws2812StatusLed<P, SM, I>,
|
||||
activity: &mut bool,
|
||||
usb_active: &bool,
|
||||
axis_idle: &bool,
|
||||
safety_check: &bool,
|
||||
) where
|
||||
P: PIOExt + FunctionConfig,
|
||||
I: PinId,
|
||||
Function<P>: ValidPinMode<I>,
|
||||
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<T: PartialOrd>(value: T, out_min: T, out_max: T) -> T {
|
||||
if value < out_min {
|
||||
out_min
|
||||
} else if value > out_max {
|
||||
out_max
|
||||
} else {
|
||||
value
|
||||
}
|
||||
}
|
||||
140
rp2040/src/status_led.rs
Normal file
140
rp2040/src/status_led.rs
Normal file
@ -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<P, SM, I>
|
||||
where
|
||||
I: PinId,
|
||||
P: PIOExt + FunctionConfig,
|
||||
Function<P>: ValidPinMode<I>,
|
||||
SM: StateMachineIndex,
|
||||
{
|
||||
ws2812_direct: Ws2812Direct<P, SM, I>,
|
||||
state: bool,
|
||||
mode: StatusMode,
|
||||
}
|
||||
|
||||
impl<P, SM, I> Ws2812StatusLed<P, SM, I>
|
||||
where
|
||||
I: PinId,
|
||||
P: PIOExt + FunctionConfig,
|
||||
Function<P>: ValidPinMode<I>,
|
||||
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<I, Function<P>>,
|
||||
pio: &mut PIO<P>,
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
209
rp2040/src/usb_joystick_device.rs
Normal file
209
rp2040/src/usb_joystick_device.rs
Normal file
@ -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<Self::Ok, Self::Error>;
|
||||
}
|
||||
|
||||
impl<T> Try for Option<T> {
|
||||
type Ok = T;
|
||||
type Error = NoneError;
|
||||
|
||||
#[inline]
|
||||
fn into_result(self) -> Result<T, NoneError> {
|
||||
self.ok_or(NoneError)
|
||||
}
|
||||
}
|
||||
|
||||
impl<T, E> Try for Result<T, E> {
|
||||
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<B>) -> Self::Allocated {
|
||||
Self::Allocated {
|
||||
interface: Interface::new(usb_alloc, self.interface),
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user