From f9ff1c0b41be26bb615954403c0f7dd1b13d758d Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Mon, 31 Jul 2023 09:20:22 +0200 Subject: [PATCH 01/16] First rp2040 code port --- .gitignore | 2 + rp2040/.cargo/config | 42 ++++ rp2040/Cargo.toml | 58 +++++ rp2040/memory.x | 15 ++ rp2040/pico-load | 12 + rp2040/src/button_matrix.rs | 98 ++++++++ rp2040/src/layout.rs | 189 ++++++++++++++++ rp2040/src/main.rs | 364 ++++++++++++++++++++++++++++++ rp2040/src/status_led.rs | 130 +++++++++++ rp2040/src/usb_joystick_device.rs | 206 +++++++++++++++++ 10 files changed, 1116 insertions(+) create mode 100644 rp2040/.cargo/config create mode 100644 rp2040/Cargo.toml create mode 100644 rp2040/memory.x create mode 100755 rp2040/pico-load create mode 100644 rp2040/src/button_matrix.rs create mode 100644 rp2040/src/layout.rs create mode 100644 rp2040/src/main.rs create mode 100644 rp2040/src/status_led.rs create mode 100644 rp2040/src/usb_joystick_device.rs 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/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..0270364 --- /dev/null +++ b/rp2040/Cargo.toml @@ -0,0 +1,58 @@ +[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" +packed_struct = { version = "0.10", default-features = false } +pio = "0.2.0" +defmt = { version = "0.3", optional = true } + +[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..2c7e0da --- /dev/null +++ b/rp2040/src/button_matrix.rs @@ -0,0 +1,98 @@ +//! Project: CMtec CMDR Keyboard 42 +//! Date: 2023-07-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..dcd1bd4 --- /dev/null +++ b/rp2040/src/layout.rs @@ -0,0 +1,189 @@ +//! Project: CMtec CMDR Keyboard 42 +//! Date: 2023-07-01 +//! Author: Christoffer Martinsson +//! Email: cm@cmtec.se +//! License: Please refer to LICENSE in root directory + +use crate::NUMBER_OF_KEYS; + +#[derive(Debug, PartialEq, Copy, Clone)] +pub enum ButtonType { + 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, + Hat1L = 25, + Hat1R = 26, + Hat1D = 27, + Hat2U = 28, + Hat2L = 29, + Hat2R = 30, + Hat2D = 31, + Hat3U = 32, + Hat3L = 33, + Hat3R = 34, + Hat3D = 35, + Hat4U = 36, + Hat4L = 37, + Hat4R = 38, + Hat4D = 39, + NotConnected = 40, +} + +// Button index map: +// -------------------------------------------------------------- +// | 0 | 1 | | 2 | 3 | (4) +// -------------------------------------------------------------- +// | | 5 | 6 | 7 | | 12 | 11 | 10 | | +// | | (9) (14) +// | | 8 | | 13 | | +// | X1/Y1 X2/Y2 | +// | | 16 | | 21 | | +// | | 17 | 15 | 18 || 22 | 20 | 23 | | +// | | 19 | | 24 | | +// -------------------------------------------------------------- +// +/// Button map to HID key (three Function layers) +pub const MAP: [[ButtonType; NUMBER_OF_KEYS]; 4] = [ + [ + // Function layer 0 + // HID Key // Button Index + // ----------------------------------------- + ButtonType::FnL, // 0 + ButtonType::B1, // 1 + ButtonType::B6, // 2 + ButtonType::FnR, // 3 + ButtonType::NotConnected, // 4 + ButtonType::B2, // 5 + ButtonType::B3, // 6 + ButtonType::ModeL, // 7 + ButtonType::B4, // 8 + ButtonType::NotConnected, // 9 + ButtonType::B7, // 10 + ButtonType::B8, // 11 + ButtonType::ModeR, // 12 + ButtonType::B9, // 13 + ButtonType::NotConnected, // 14 + ButtonType::B5, // 15 + ButtonType::Hat1U, // 16 + ButtonType::Hat1L, // 17 + ButtonType::Hat1R, // 18 + ButtonType::Hat1D, // 19 + ButtonType::B10, // 20 + ButtonType::Hat2U, // 21 + ButtonType::Hat2L, // 22 + ButtonType::Hat2R, // 23 + ButtonType::Hat2D, // 24 + ], + [ + // Function layer left + // HID Key // Button Index + // ----------------------------------------- + ButtonType::FnL, // 0 + ButtonType::B11, // 1 + ButtonType::B6, // 2 + ButtonType::FnR, // 3 + ButtonType::NotConnected, // 4 + ButtonType::B12, // 5 + ButtonType::B13, // 6 + ButtonType::ModeL, // 7 + ButtonType::B14, // 8 + ButtonType::NotConnected, // 9 + ButtonType::B7, // 10 + ButtonType::B8, // 11 + ButtonType::ModeR, // 12 + ButtonType::B9, // 13 + ButtonType::NotConnected, // 14 + ButtonType::B15, // 15 + ButtonType::Hat1U, // 16 + ButtonType::Hat1L, // 17 + ButtonType::Hat1R, // 18 + ButtonType::Hat1D, // 19 + ButtonType::B10, // 20 + ButtonType::Hat2U, // 21 + ButtonType::Hat2L, // 22 + ButtonType::Hat2R, // 23 + ButtonType::Hat2D, // 24 + ], + [ + // Function layer right + // HID Key // Button Index + // ----------------------------------------- + ButtonType::FnL, // 0 + ButtonType::B1, // 1 + ButtonType::B16, // 2 + ButtonType::FnR, // 3 + ButtonType::NotConnected, // 4 + ButtonType::B2, // 5 + ButtonType::B3, // 6 + ButtonType::ModeL, // 7 + ButtonType::B4, // 8 + ButtonType::NotConnected, // 9 + ButtonType::B17, // 10 + ButtonType::B18, // 11 + ButtonType::ModeR, // 12 + ButtonType::B19, // 13 + ButtonType::NotConnected, // 14 + ButtonType::B5, // 15 + ButtonType::Hat3U, // 16 + ButtonType::Hat3L, // 17 + ButtonType::Hat3R, // 18 + ButtonType::Hat3D, // 19 + ButtonType::B20, // 20 + ButtonType::Hat2U, // 21 + ButtonType::Hat2L, // 22 + ButtonType::Hat2R, // 23 + ButtonType::Hat2D, // 24 + ], + [ + // Function layer left + right + // HID Key // Button Index + // ----------------------------------------- + ButtonType::FnL, // 0 + ButtonType::B11, // 1 + ButtonType::B16, // 2 + ButtonType::FnR, // 3 + ButtonType::NotConnected, // 4 + ButtonType::B12, // 5 + ButtonType::B13, // 6 + ButtonType::ModeL, // 7 + ButtonType::B14, // 8 + ButtonType::NotConnected, // 9 + ButtonType::B17, // 10 + ButtonType::B18, // 11 + ButtonType::ModeR, // 12 + ButtonType::B19, // 13 + ButtonType::NotConnected, // 14 + ButtonType::B15, // 15 + ButtonType::Hat3U, // 16 + ButtonType::Hat3L, // 17 + ButtonType::Hat3R, // 18 + ButtonType::Hat3D, // 19 + ButtonType::B20, // 20 + ButtonType::Hat4U, // 21 + ButtonType::Hat4L, // 22 + ButtonType::Hat4R, // 23 + ButtonType::Hat4D, // 24 + ], +]; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs new file mode 100644 index 0000000..12bc3f1 --- /dev/null +++ b/rp2040/src/main.rs @@ -0,0 +1,364 @@ +//! Project: CMtec CMDR Keyboard 42 +//! Date: 2023-07-01 +//! Author: Christoffer Martinsson +//! Email: cm@cmtec.se +//! License: Please refer to LICENSE in root directory + +#![no_std] +#![no_main] + +mod button_matrix; +// mod fmt; +mod layout; +mod status_led; +mod usb_joystick_device; + +use button_matrix::ButtonMatrix; +use core::convert::Infallible; +use cortex_m::delay::Delay; +use embedded_hal::digital::v2::*; +use embedded_hal::timer::CountDown; +use fugit::ExtU32; +use panic_halt as _; +use rp2040_hal::{ + 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 KEY_ROWS: usize = 5; +pub const KEY_COLS: usize = 5; +pub const NUMBER_OF_KEYS: usize = KEY_ROWS * KEY_COLS; + +// Public types +#[derive(Copy, Clone, Default)] +pub struct KeyboardButton { + pub pressed: bool, + pub previous_pressed: bool, + pub fn_mode: u8, +} + +#[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, + ); + + // Setting up array with pins connected to button rows + let button_matrix_row_pins: &[&dyn InputPin; KEY_ROWS] = &[ + &pins.gp9.into_pull_up_input(), + &pins.gp10.into_pull_up_input(), + &pins.gp11.into_pull_up_input(), + &pins.gp12.into_pull_up_input(), + &pins.gp13.into_pull_up_input(), + ]; + + // Setting up array with pins connected to button columns + let button_matrix_col_pins: &mut [&mut dyn OutputPin; KEY_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 the PCB buttons + let mut button_matrix: ButtonMatrix = + ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, 5); + + // 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 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(); + + // 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(), + ); + + // Create keyboard button array + let mut buttons: [KeyboardButton; NUMBER_OF_KEYS] = [KeyboardButton::default(); NUMBER_OF_KEYS]; + + // Create timers/delays + let timer = Timer::new(pac.TIMER, &mut pac.RESETS); + let mut delay = Delay::new(core.SYST, clocks.system_clock.freq().to_Hz()); + + let mut usb_hid_report_count_down = timer.count_down(); + usb_hid_report_count_down.start(10.millis()); + + let mut usb_tick_count_down = timer.count_down(); + usb_tick_count_down.start(1.millis()); + + let mut status_led_count_down = timer.count_down(); + status_led_count_down.start(250.millis()); + + // Create variables to track caps lock and fn mode + let mut fn_mode: u8; + + // Initialize button matrix + button_matrix.init_pins(); + + // Scan matrix to get initial state + for _ in 0..10 { + button_matrix.scan_matrix(&mut delay); + } + + // Check if first key is pressed while power on. If yes then enter bootloader + 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); + } + + status_led.update(StatusMode::Normal); + + loop { + // if status_led_count_down.wait().is_ok() { + // update_status_led(&mut status_led, &caps_lock_active, &gui_lock_state); + // } + + if usb_hid_report_count_down.wait().is_ok() { + let pressed_keys = button_matrix.buttons_pressed(); + + fn_mode = get_fn_mode(pressed_keys); + + for (index, key) in pressed_keys.iter().enumerate() { + buttons[index].pressed = *key; + } + + match joystick + .device() + .write_report(&get_joy_report(&mut buttons, fn_mode)) + { + Err(UsbHidError::WouldBlock) => {} + Err(UsbHidError::SerializationError) => { + status_led.update(StatusMode::Bootloader); + } + Err(UsbHidError::UsbError(UsbError::BufferOverflow)) => { + status_led.update(StatusMode::Bootloader); + } + Ok(_) => {} + Err(e) => { + status_led.update(StatusMode::Error); + core::panic!("Failed to write joystick report: {:?}", e) + } + }; + } + + if usb_tick_count_down.wait().is_ok() { + button_matrix.scan_matrix(&mut delay); + } + + if usb_dev.poll(&mut [&mut joystick]) {} + } +} + +/// Update status LED colour based on function layer and capslock +/// +/// Normal = green (NORMAL) +/// GUI lock = blue (GUI LOCK) +/// Capslock active = flashing red (WARNING) +/// Error = steady red (ERROR) +/// +/// # Arguments +/// * `status_led` - Reference to status LED +/// * `caps_lock_active` - Is capslock active +fn update_status_led( + status_led: &mut Ws2812StatusLed, + caps_lock_active: &bool, + gui_lock_state: &u8, +) where + P: PIOExt + FunctionConfig, + I: PinId, + Function

: ValidPinMode, + SM: StateMachineIndex, +{ + if *caps_lock_active { + status_led.update(StatusMode::Warning); + } else if *gui_lock_state != 0 { + status_led.update(StatusMode::Activity); + } else { + status_led.update(StatusMode::Normal); + } +} + +/// Get current Fn mode (0, 1, 2 or 3) +/// layout::FN_BUTTONS contains the button types +/// +/// # Arguments +/// +/// * `pressed_keys` - Array of pressed keys +fn get_fn_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { + // Check how many Fn keys are pressed + let mut fn_mode: u8 = 0; + let mut fn_l_active: bool = false; + let mut fn_r_active: bool = false; + + for (index, key) in pressed_keys.iter().enumerate() { + if *key && layout::MAP[0][index] == layout::ButtonType::FnL { + fn_l_active = true; + } + if *key && layout::MAP[0][index] == layout::ButtonType::FnR { + fn_r_active = true; + } + } + + if fn_l_active && fn_r_active { + fn_mode = 3; + } else if fn_l_active { + fn_mode = 2; + } else if fn_r_active { + fn_mode = 1; + } + + fn_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 +/// * `fn_mode` - Current function layer +fn get_joy_report( + matrix_keys: &mut [KeyboardButton; NUMBER_OF_KEYS], + fn_mode: u8, +) -> JoystickReport { + let mut x: u16 = 0x03ff; + let mut y: u16 = 0x03ff; + let mut z: u16 = 0x03ff; + let mut rx: u16 = 0x03ff; + let mut ry: u16 = 0x03ff; + let mut rz: u16 = 0x03ff; + let mut buttons: u32 = 0; + let mut hat1: u8 = 0xf; + let mut hat2: u8 = 0xf; + let mut hat3: u8 = 0xf; + let mut hat4: u8 = 0xf; + + // Filter report based on Fn mode and pressed keys + for (index, key) in matrix_keys.iter_mut().enumerate() { + // Set fn mode for the pressed button + if key.pressed != key.previous_pressed && key.pressed { + key.fn_mode = fn_mode; + } + key.previous_pressed = key.pressed; + + // Skip key if defined as NoEventIndicated + if layout::MAP[key.fn_mode as usize][index] == layout::ButtonType::NotConnected { + continue; + } + + // Update button state + if key.pressed + && layout::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::B20 as usize + { + buttons |= 1 << layout::MAP[fn_mode as usize][index] as usize; + } + + // Update hat state + if key.pressed + && layout::MAP[fn_mode as usize][index] as usize >= layout::ButtonType::Hat1U as usize + && layout::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::Hat4D as usize + { + match layout::MAP[fn_mode as usize][index] { + layout::ButtonType::Hat1U => hat1 = 0, + layout::ButtonType::Hat1R => hat1 = 2, + layout::ButtonType::Hat1D => hat1 = 4, + layout::ButtonType::Hat1L => hat1 = 6, + layout::ButtonType::Hat2U => hat2 = 0, + layout::ButtonType::Hat2R => hat2 = 2, + layout::ButtonType::Hat2D => hat2 = 4, + layout::ButtonType::Hat2L => hat2 = 6, + layout::ButtonType::Hat3U => hat3 = 0, + layout::ButtonType::Hat3R => hat3 = 2, + layout::ButtonType::Hat3D => hat3 = 4, + layout::ButtonType::Hat3L => hat3 = 6, + layout::ButtonType::Hat4U => hat4 = 0, + layout::ButtonType::Hat4R => hat4 = 2, + layout::ButtonType::Hat4D => hat4 = 4, + layout::ButtonType::Hat4L => hat4 = 6, + _ => {} + } + } + } + + JoystickReport { + x, + y, + z, + rx, + ry, + rz, + hat1, + hat2, + hat3, + hat4, + buttons, + } +} diff --git a/rp2040/src/status_led.rs b/rp2040/src/status_led.rs new file mode 100644 index 0000000..d56af7b --- /dev/null +++ b/rp2040/src/status_led.rs @@ -0,0 +1,130 @@ +//! Project: CMtec CMDR Keyboard 42 +//! Date: 2023-07-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, +} + +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; + Self { + ws2812_direct, + state, + } + } + + /// 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 + ]; + + 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..65faf67 --- /dev/null +++ b/rp2040/src/usb_joystick_device.rs @@ -0,0 +1,206 @@ +//!HID joystick +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, 20pc 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, 0x10, // Usage Maximum (16) + 0x15, 0x00, // Logical Minimum (0) + 0x25, 0x01, // Logical Maximum (1) + 0x75, 0x01, // Report Size (1) + 0x95, 0x14, // Report Count (20) + 0x81, 0x02, // Input (Data, Variable, Absolute) + 0x75, 0x01, // Report Size (1) PADDING + 0x95, 0x04, // Report Count (4) PADDING + 0x81, 0x03, // Input (Const, Variable, Absolute) PADDING + 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, // 20bit + 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 + 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), + } + } +} From 9a87974c7112d01c664e40a3d642d3d95b5ec97e Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Tue, 1 Aug 2023 07:56:11 +0200 Subject: [PATCH 02/16] Changed to 20 buttons. More code implementation --- rp2040/Cargo.toml | 2 +- rp2040/src/main.rs | 293 ++++++++++++++++++++++++------ rp2040/src/usb_joystick_device.rs | 2 +- 3 files changed, 242 insertions(+), 55 deletions(-) diff --git a/rp2040/Cargo.toml b/rp2040/Cargo.toml index 0270364..4aebbcd 100644 --- a/rp2040/Cargo.toml +++ b/rp2040/Cargo.toml @@ -19,9 +19,9 @@ smart-leds-trait = "0.2.1" ws2812-pio = "0.6.0" usbd-human-interface-device = "0.4.2" usb-device = "0.2" -packed_struct = { version = "0.10", default-features = false } pio = "0.2.0" defmt = { version = "0.3", optional = true } +libm = "0.2.7" [features] # This is the set of features we enable by default diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 12bc3f1..998a84c 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -8,7 +8,6 @@ #![no_main] mod button_matrix; -// mod fmt; mod layout; mod status_led; mod usb_joystick_device; @@ -16,11 +15,14 @@ mod usb_joystick_device; use button_matrix::ButtonMatrix; use core::convert::Infallible; use cortex_m::delay::Delay; +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, }; @@ -47,14 +49,34 @@ pub const KEY_ROWS: usize = 5; pub const KEY_COLS: usize = 5; pub const NUMBER_OF_KEYS: usize = KEY_ROWS * KEY_COLS; +const HID_AXIS_MIN: u16 = 0; +const HID_AXIS_MAX: u16 = 4095; +const HID_AXIS_CENTER: u16 = HID_AXIS_MAX / 2; + +const HAT_CENTER: u8 = 0xf; +const HAT_UP: u8 = 0; +const HAT_RIGHT: u8 = 2; +const HAT_DOWN: u8 = 4; +const HAT_LEFT: u8 = 6; + // Public types #[derive(Copy, Clone, Default)] -pub struct KeyboardButton { +pub struct JoystickButton { pub pressed: bool, pub previous_pressed: bool, pub fn_mode: u8, } +#[derive(Copy, Clone, Default)] +pub struct JoystickAxis { + pub value: u16, + pub previous_value: u16, + pub max: u16, + pub min: u16, + pub center: u16, + pub fn_mode: u8, +} + #[entry] fn main() -> ! { // Grab our singleton objects @@ -89,6 +111,15 @@ fn main() -> ! { &mut pac.RESETS, ); + // Enable adc + let mut adc = Adc::new(pac.ADC, &mut pac.RESETS); + + // Configure ADC input pins + let mut x1_pin = pins.gp26.into_floating_input(); + let mut y1_pin = pins.gp27.into_floating_input(); + let mut x2_pin = pins.gp28.into_floating_input(); + let mut y2_pin = pins.gp29.into_floating_input(); + // Setting up array with pins connected to button rows let button_matrix_row_pins: &[&dyn InputPin; KEY_ROWS] = &[ &pins.gp9.into_pull_up_input(), @@ -139,8 +170,8 @@ fn main() -> ! { clocks.peripheral_clock.freq(), ); - // Create keyboard button array - let mut buttons: [KeyboardButton; NUMBER_OF_KEYS] = [KeyboardButton::default(); NUMBER_OF_KEYS]; + // Create joystick button array + let mut buttons: [JoystickButton; NUMBER_OF_KEYS] = [JoystickButton::default(); NUMBER_OF_KEYS]; // Create timers/delays let timer = Timer::new(pac.TIMER, &mut pac.RESETS); @@ -155,8 +186,13 @@ fn main() -> ! { let mut status_led_count_down = timer.count_down(); status_led_count_down.start(250.millis()); - // Create variables to track caps lock and fn mode + // Create variables to track all modes let mut fn_mode: u8; + let mut alt_l_mode: bool; + let mut alt_r_mode: bool; + + // Create joystick axis array + let mut axis: [JoystickAxis; 4]; // Initialize button matrix button_matrix.init_pins(); @@ -174,8 +210,6 @@ fn main() -> ! { rp2040_hal::rom_data::reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask); } - status_led.update(StatusMode::Normal); - loop { // if status_led_count_down.wait().is_ok() { // update_status_led(&mut status_led, &caps_lock_active, &gui_lock_state); @@ -184,23 +218,20 @@ fn main() -> ! { if usb_hid_report_count_down.wait().is_ok() { let pressed_keys = button_matrix.buttons_pressed(); - fn_mode = get_fn_mode(pressed_keys); + (fn_mode, alt_l_mode, alt_r_mode) = get_mode(pressed_keys); for (index, key) in pressed_keys.iter().enumerate() { buttons[index].pressed = *key; } - match joystick - .device() - .write_report(&get_joy_report(&mut buttons, fn_mode)) - { + match joystick.device().write_report(&get_joystick_report( + &mut buttons, + axis, + fn_mode, + alt_l_mode, + alt_r_mode, + )) { Err(UsbHidError::WouldBlock) => {} - Err(UsbHidError::SerializationError) => { - status_led.update(StatusMode::Bootloader); - } - Err(UsbHidError::UsbError(UsbError::BufferOverflow)) => { - status_led.update(StatusMode::Bootloader); - } Ok(_) => {} Err(e) => { status_led.update(StatusMode::Error); @@ -211,6 +242,39 @@ fn main() -> ! { if usb_tick_count_down.wait().is_ok() { button_matrix.scan_matrix(&mut delay); + + axis[0].value = apply_calibration( + adc.read(&mut x1_pin).unwrap(), + HID_AXIS_MIN, + HID_AXIS_MAX, + HID_AXIS_CENTER, + 50, + 0.2, + ); + axis[1].value = apply_calibration( + adc.read(&mut y1_pin).unwrap(), + HID_AXIS_MIN, + HID_AXIS_MAX, + HID_AXIS_CENTER, + 50, + 0.2, + ); + axis[2].value = apply_calibration( + adc.read(&mut x2_pin).unwrap(), + HID_AXIS_MIN, + HID_AXIS_MAX, + HID_AXIS_CENTER, + 50, + 0.2, + ); + axis[3].value = apply_calibration( + adc.read(&mut y2_pin).unwrap(), + HID_AXIS_MIN, + HID_AXIS_MAX, + HID_AXIS_CENTER, + 0, + 0.0, + ); } if usb_dev.poll(&mut [&mut joystick]) {} @@ -246,17 +310,19 @@ fn update_status_led( } } -/// Get current Fn mode (0, 1, 2 or 3) -/// layout::FN_BUTTONS contains the button types +/// 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_fn_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { +fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> (u8, bool, bool) { // Check how many Fn keys are pressed let mut fn_mode: u8 = 0; let mut fn_l_active: bool = false; let mut fn_r_active: bool = false; + let mut alt_l_mode: bool = false; + let mut alt_r_mode: bool = false; for (index, key) in pressed_keys.iter().enumerate() { if *key && layout::MAP[0][index] == layout::ButtonType::FnL { @@ -265,6 +331,12 @@ fn get_fn_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { if *key && layout::MAP[0][index] == layout::ButtonType::FnR { fn_r_active = true; } + if *key && layout::MAP[0][index] == layout::ButtonType::ModeL { + alt_l_mode = true; + } + if *key && layout::MAP[0][index] == layout::ButtonType::ModeR { + alt_r_mode = true; + } } if fn_l_active && fn_r_active { @@ -275,7 +347,7 @@ fn get_fn_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { fn_mode = 1; } - fn_mode + (fn_mode, alt_l_mode, alt_r_mode) } /// Generate keyboard report based on pressed keys and Fn mode (0, 1 or 2) @@ -284,22 +356,56 @@ fn get_fn_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { /// # Arguments /// /// * `matrix_keys` - Array of pressed keys -/// * `fn_mode` - Current function layer -fn get_joy_report( - matrix_keys: &mut [KeyboardButton; NUMBER_OF_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 [JoystickButton; NUMBER_OF_KEYS], + axis: [JoystickAxis; 4], fn_mode: u8, + alt_l_mode: bool, + alt_r_mode: bool, ) -> JoystickReport { - let mut x: u16 = 0x03ff; - let mut y: u16 = 0x03ff; - let mut z: u16 = 0x03ff; - let mut rx: u16 = 0x03ff; - let mut ry: u16 = 0x03ff; - let mut rz: u16 = 0x03ff; + let mut x: u16 = axis[2].value; // right gimbal normal mode + let mut y: u16 = axis[3].value; // right gimbal normal mode + let z: u16 = axis[0].value; // left gimbal normal mode + let mut rx: u16 = HID_AXIS_CENTER; // right gimbal alt mode + let mut ry: u16 = HID_AXIS_CENTER; // right gimbal alt mode + let mut rz: u16 = axis[1].value; // left gimbal normal and alt mode let mut buttons: u32 = 0; - let mut hat1: u8 = 0xf; - let mut hat2: u8 = 0xf; - let mut hat3: u8 = 0xf; - let mut hat4: u8 = 0xf; + let mut hat1: u8 = HAT_CENTER; + let mut hat2: u8 = HAT_CENTER; + let mut hat3: u8 = HAT_CENTER; + let mut hat4: u8 = HAT_CENTER; + + if alt_l_mode && (axis[1].fn_mode == 0 || axis[1].fn_mode == 2) { + rz = remap( + axis[1].value, + HID_AXIS_MIN, + HID_AXIS_MAX, + HID_AXIS_CENTER, + HID_AXIS_MAX, + ); + } else if alt_l_mode && (axis[1].fn_mode == 1 || axis[1].fn_mode == 3) { + rz = remap( + axis[1].value, + HID_AXIS_MIN, + HID_AXIS_MAX, + HID_AXIS_CENTER, + HID_AXIS_MIN, + ); + } + + if alt_r_mode && (axis[2].fn_mode == 2 || axis[2].fn_mode == 3) { + x = HID_AXIS_CENTER; + rx = axis[2].value; + } + + if alt_r_mode && (axis[3].fn_mode == 2 || axis[3].fn_mode == 3) { + y = HID_AXIS_CENTER; + ry = axis[3].value; + } // Filter report based on Fn mode and pressed keys for (index, key) in matrix_keys.iter_mut().enumerate() { @@ -320,29 +426,28 @@ fn get_joy_report( { buttons |= 1 << layout::MAP[fn_mode as usize][index] as usize; } - // Update hat state - if key.pressed + else if key.pressed && layout::MAP[fn_mode as usize][index] as usize >= layout::ButtonType::Hat1U as usize && layout::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::Hat4D as usize { match layout::MAP[fn_mode as usize][index] { - layout::ButtonType::Hat1U => hat1 = 0, - layout::ButtonType::Hat1R => hat1 = 2, - layout::ButtonType::Hat1D => hat1 = 4, - layout::ButtonType::Hat1L => hat1 = 6, - layout::ButtonType::Hat2U => hat2 = 0, - layout::ButtonType::Hat2R => hat2 = 2, - layout::ButtonType::Hat2D => hat2 = 4, - layout::ButtonType::Hat2L => hat2 = 6, - layout::ButtonType::Hat3U => hat3 = 0, - layout::ButtonType::Hat3R => hat3 = 2, - layout::ButtonType::Hat3D => hat3 = 4, - layout::ButtonType::Hat3L => hat3 = 6, - layout::ButtonType::Hat4U => hat4 = 0, - layout::ButtonType::Hat4R => hat4 = 2, - layout::ButtonType::Hat4D => hat4 = 4, - layout::ButtonType::Hat4L => hat4 = 6, + layout::ButtonType::Hat1U => hat1 = HAT_UP, + layout::ButtonType::Hat1R => hat1 = HAT_RIGHT, + layout::ButtonType::Hat1D => hat1 = HAT_DOWN, + layout::ButtonType::Hat1L => hat1 = HAT_LEFT, + layout::ButtonType::Hat2U => hat2 = HAT_UP, + layout::ButtonType::Hat2R => hat2 = HAT_RIGHT, + layout::ButtonType::Hat2D => hat2 = HAT_DOWN, + layout::ButtonType::Hat2L => hat2 = HAT_LEFT, + layout::ButtonType::Hat3U => hat3 = HAT_UP, + layout::ButtonType::Hat3R => hat3 = HAT_RIGHT, + layout::ButtonType::Hat3D => hat3 = HAT_DOWN, + layout::ButtonType::Hat3L => hat3 = HAT_RIGHT, + layout::ButtonType::Hat4U => hat4 = HAT_UP, + layout::ButtonType::Hat4R => hat4 = HAT_RIGHT, + layout::ButtonType::Hat4D => hat4 = HAT_DOWN, + layout::ButtonType::Hat4L => hat4 = HAT_LEFT, _ => {} } } @@ -362,3 +467,85 @@ fn get_joy_report( buttons, } } + +/// 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 { + (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min +} + +/// 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 + } +} + +fn calibrate_axis(axis: [JoystickAxis; 4]) {} + +fn apply_calibration( + gimbal_value: u16, + min_value: u16, + max_value: u16, + center_value: u16, + deadband_value: u16, + expo_value: f32, +) -> u16 { + let mut calibrated_value = HID_AXIS_CENTER; + + if gimbal_value > (center_value + deadband_value) { + calibrated_value = constrain( + remap( + gimbal_value, + center_value + deadband_value, + max_value, + HID_AXIS_CENTER, + HID_AXIS_MAX, + ), + HID_AXIS_CENTER, + HID_AXIS_MAX, + ); + } else if gimbal_value < (center_value - deadband_value) { + calibrated_value = constrain( + remap( + gimbal_value, + min_value, + center_value - deadband_value, + HID_AXIS_MIN, + HID_AXIS_CENTER, + ), + HID_AXIS_MIN, + HID_AXIS_CENTER, + ); + } + + if expo_value != 0.0 { + let joystick_x_float = calibrated_value as f32 / HID_AXIS_MAX as f32; + /* Calculate expo using 9th order polynomial function with 0.5 as center point */ + let joystick_x_exp: f32 = expo_value * (0.5 + 256.0 * powf(joystick_x_float - 0.5, 9.0)) + + (1.0 - expo_value) * joystick_x_float; + + calibrated_value = constrain( + (joystick_x_exp * HID_AXIS_MAX as f32) as u16, + HID_AXIS_MIN, + HID_AXIS_MAX, + ); + } + + calibrated_value +} diff --git a/rp2040/src/usb_joystick_device.rs b/rp2040/src/usb_joystick_device.rs index 65faf67..2fd20e0 100644 --- a/rp2040/src/usb_joystick_device.rs +++ b/rp2040/src/usb_joystick_device.rs @@ -85,7 +85,7 @@ pub const JOYSTICK_DESCRIPTOR: &[u8] = &[ 0xc0, // End Collection 0x05, 0x09, // Usage Page (Button) 0x19, 0x01, // Usage Minimum (1) - 0x29, 0x10, // Usage Maximum (16) + 0x29, 0x14, // Usage Maximum (20) 0x15, 0x00, // Logical Minimum (0) 0x25, 0x01, // Logical Maximum (1) 0x75, 0x01, // Report Size (1) From 3c073cd9fdf908d428f0879fb478e28cd676062c Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Wed, 2 Aug 2023 20:26:30 +0200 Subject: [PATCH 03/16] Reordered layout buttons. Added smoother functionality. Code cleanup --- rp2040/Cargo.toml | 1 + rp2040/src/layout.rs | 248 +++++++++++----------- rp2040/src/main.rs | 483 +++++++++++++++++++++++++------------------ 3 files changed, 407 insertions(+), 325 deletions(-) diff --git a/rp2040/Cargo.toml b/rp2040/Cargo.toml index 4aebbcd..dfc0c83 100644 --- a/rp2040/Cargo.toml +++ b/rp2040/Cargo.toml @@ -22,6 +22,7 @@ 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 diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index dcd1bd4..2d5dbd1 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -24,31 +24,31 @@ pub enum ButtonType { B14 = 13, B15 = 14, B16 = 15, - B17 = 16, - B18 = 17, - B19 = 18, - B20 = 19, - FnL = 20, - FnR = 21, - ModeL = 22, - ModeR = 23, - Hat1U = 24, - Hat1L = 25, - Hat1R = 26, - Hat1D = 27, - Hat2U = 28, - Hat2L = 29, - Hat2R = 30, - Hat2D = 31, - Hat3U = 32, + FnL = 16, + FnR = 17, + ModeL = 18, + ModeR = 19, + Hat1U = 20, + Hat1R = 21, + Hat1D = 22, + Hat1L = 23, + Hat1B = 24, + Hat2U = 25, + Hat2R = 26, + Hat2D = 27, + Hat2L = 28, + Hat2B = 29, + Hat3U = 30, + Hat3R = 31, + Hat3D = 32, Hat3L = 33, - Hat3R = 34, - Hat3D = 35, - Hat4U = 36, - Hat4L = 37, - Hat4R = 38, - Hat4D = 39, - NotConnected = 40, + Hat3B = 34, + Hat4U = 35, + Hat4R = 36, + Hat4D = 37, + Hat4L = 38, + Hat4B = 39, + NoEventIndicated = 40, } // Button index map: @@ -70,120 +70,120 @@ pub const MAP: [[ButtonType; NUMBER_OF_KEYS]; 4] = [ // Function layer 0 // HID Key // Button Index // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B1, // 1 - ButtonType::B6, // 2 - ButtonType::FnR, // 3 - ButtonType::NotConnected, // 4 - ButtonType::B2, // 5 - ButtonType::B3, // 6 - ButtonType::ModeL, // 7 - ButtonType::B4, // 8 - ButtonType::NotConnected, // 9 - ButtonType::B7, // 10 - ButtonType::B8, // 11 - ButtonType::ModeR, // 12 - ButtonType::B9, // 13 - ButtonType::NotConnected, // 14 - ButtonType::B5, // 15 - ButtonType::Hat1U, // 16 - ButtonType::Hat1L, // 17 - ButtonType::Hat1R, // 18 - ButtonType::Hat1D, // 19 - ButtonType::B10, // 20 - ButtonType::Hat2U, // 21 - ButtonType::Hat2L, // 22 - ButtonType::Hat2R, // 23 - ButtonType::Hat2D, // 24 + ButtonType::FnL, // 0 + ButtonType::B1, // 1 + ButtonType::B5, // 2 + ButtonType::FnR, // 3 + ButtonType::NoEventIndicated, // 4 + ButtonType::B2, // 5 + ButtonType::B3, // 6 + ButtonType::ModeL, // 7 + ButtonType::B4, // 8 + ButtonType::NoEventIndicated, // 9 + ButtonType::B6, // 10 + ButtonType::B7, // 11 + ButtonType::ModeR, // 12 + ButtonType::B8, // 13 + ButtonType::NoEventIndicated, // 14 + ButtonType::Hat1B, // 15 + ButtonType::Hat1U, // 16 + ButtonType::Hat1L, // 17 + ButtonType::Hat1R, // 18 + ButtonType::Hat1D, // 19 + ButtonType::Hat2B, // 20 + ButtonType::Hat2U, // 21 + ButtonType::Hat2L, // 22 + ButtonType::Hat2R, // 23 + ButtonType::Hat2D, // 24 ], [ // Function layer left // HID Key // Button Index // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B11, // 1 - ButtonType::B6, // 2 - ButtonType::FnR, // 3 - ButtonType::NotConnected, // 4 - ButtonType::B12, // 5 - ButtonType::B13, // 6 - ButtonType::ModeL, // 7 - ButtonType::B14, // 8 - ButtonType::NotConnected, // 9 - ButtonType::B7, // 10 - ButtonType::B8, // 11 - ButtonType::ModeR, // 12 - ButtonType::B9, // 13 - ButtonType::NotConnected, // 14 - ButtonType::B15, // 15 - ButtonType::Hat1U, // 16 - ButtonType::Hat1L, // 17 - ButtonType::Hat1R, // 18 - ButtonType::Hat1D, // 19 - ButtonType::B10, // 20 - ButtonType::Hat2U, // 21 - ButtonType::Hat2L, // 22 - ButtonType::Hat2R, // 23 - ButtonType::Hat2D, // 24 + ButtonType::FnL, // 0 + ButtonType::B9, // 1 + ButtonType::B5, // 2 + ButtonType::FnR, // 3 + ButtonType::NoEventIndicated, // 4 + ButtonType::B10, // 5 + ButtonType::B11, // 6 + ButtonType::ModeL, // 7 + ButtonType::B12, // 8 + ButtonType::NoEventIndicated, // 9 + ButtonType::B6, // 10 + ButtonType::B7, // 11 + ButtonType::ModeR, // 12 + ButtonType::B8, // 13 + ButtonType::NoEventIndicated, // 14 + ButtonType::Hat3B, // 15 + ButtonType::Hat3U, // 16 + ButtonType::Hat3L, // 17 + ButtonType::Hat3R, // 18 + ButtonType::Hat3D, // 19 + ButtonType::Hat2B, // 20 + ButtonType::Hat2U, // 21 + ButtonType::Hat2L, // 22 + ButtonType::Hat2R, // 23 + ButtonType::Hat2D, // 24 ], [ // Function layer right // HID Key // Button Index // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B1, // 1 - ButtonType::B16, // 2 - ButtonType::FnR, // 3 - ButtonType::NotConnected, // 4 - ButtonType::B2, // 5 - ButtonType::B3, // 6 - ButtonType::ModeL, // 7 - ButtonType::B4, // 8 - ButtonType::NotConnected, // 9 - ButtonType::B17, // 10 - ButtonType::B18, // 11 - ButtonType::ModeR, // 12 - ButtonType::B19, // 13 - ButtonType::NotConnected, // 14 - ButtonType::B5, // 15 - ButtonType::Hat3U, // 16 - ButtonType::Hat3L, // 17 - ButtonType::Hat3R, // 18 - ButtonType::Hat3D, // 19 - ButtonType::B20, // 20 - ButtonType::Hat2U, // 21 - ButtonType::Hat2L, // 22 - ButtonType::Hat2R, // 23 - ButtonType::Hat2D, // 24 + ButtonType::FnL, // 0 + ButtonType::B1, // 1 + ButtonType::B13, // 2 + ButtonType::FnR, // 3 + ButtonType::NoEventIndicated, // 4 + ButtonType::B2, // 5 + ButtonType::B3, // 6 + ButtonType::ModeL, // 7 + ButtonType::B4, // 8 + ButtonType::NoEventIndicated, // 9 + ButtonType::B14, // 10 + ButtonType::B15, // 11 + ButtonType::ModeR, // 12 + ButtonType::B16, // 13 + ButtonType::NoEventIndicated, // 14 + ButtonType::Hat1B, // 15 + ButtonType::Hat1U, // 16 + ButtonType::Hat1L, // 17 + ButtonType::Hat1R, // 18 + ButtonType::Hat1D, // 19 + ButtonType::Hat4B, // 20 + ButtonType::Hat4U, // 21 + ButtonType::Hat4L, // 22 + ButtonType::Hat4R, // 23 + ButtonType::Hat4D, // 24 ], [ // Function layer left + right // HID Key // Button Index // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B11, // 1 - ButtonType::B16, // 2 - ButtonType::FnR, // 3 - ButtonType::NotConnected, // 4 - ButtonType::B12, // 5 - ButtonType::B13, // 6 - ButtonType::ModeL, // 7 - ButtonType::B14, // 8 - ButtonType::NotConnected, // 9 - ButtonType::B17, // 10 - ButtonType::B18, // 11 - ButtonType::ModeR, // 12 - ButtonType::B19, // 13 - ButtonType::NotConnected, // 14 - ButtonType::B15, // 15 - ButtonType::Hat3U, // 16 - ButtonType::Hat3L, // 17 - ButtonType::Hat3R, // 18 - ButtonType::Hat3D, // 19 - ButtonType::B20, // 20 - ButtonType::Hat4U, // 21 - ButtonType::Hat4L, // 22 - ButtonType::Hat4R, // 23 - ButtonType::Hat4D, // 24 + ButtonType::FnL, // 0 + ButtonType::B9, // 1 + ButtonType::B13, // 2 + ButtonType::FnR, // 3 + ButtonType::NoEventIndicated, // 4 + ButtonType::B10, // 5 + ButtonType::B11, // 6 + ButtonType::ModeL, // 7 + ButtonType::B12, // 8 + ButtonType::NoEventIndicated, // 9 + ButtonType::B14, // 10 + ButtonType::B15, // 11 + ButtonType::ModeR, // 12 + ButtonType::B16, // 13 + ButtonType::NoEventIndicated, // 14 + ButtonType::Hat3B, // 15 + ButtonType::Hat3U, // 16 + ButtonType::Hat3L, // 17 + ButtonType::Hat3R, // 18 + ButtonType::Hat3D, // 19 + ButtonType::Hat4B, // 20 + ButtonType::Hat4U, // 21 + ButtonType::Hat4L, // 22 + ButtonType::Hat4R, // 23 + ButtonType::Hat4D, // 24 ], ]; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 998a84c..dc83e7e 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -15,6 +15,7 @@ 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; @@ -49,15 +50,20 @@ pub const KEY_ROWS: usize = 5; pub const KEY_COLS: usize = 5; pub const NUMBER_OF_KEYS: usize = KEY_ROWS * KEY_COLS; -const HID_AXIS_MIN: u16 = 0; -const HID_AXIS_MAX: u16 = 4095; -const HID_AXIS_CENTER: u16 = HID_AXIS_MAX / 2; +pub const AXIS_MIN: u16 = 0; +pub const AXIS_MAX: u16 = 4095; +pub const AXIS_CENTER: u16 = AXIS_MAX / 2; -const HAT_CENTER: u8 = 0xf; -const HAT_UP: u8 = 0; -const HAT_RIGHT: u8 = 2; -const HAT_DOWN: u8 = 4; -const HAT_LEFT: u8 = 6; +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; + +// Create an instance with suitable 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)] @@ -67,14 +73,31 @@ pub struct JoystickButton { pub fn_mode: u8, } -#[derive(Copy, Clone, Default)] -pub struct JoystickAxis { +#[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, + idle_value: AXIS_CENTER, + max: AXIS_MAX, + min: AXIS_MIN, + center: AXIS_CENTER, + fn_mode: 0, + deadzone: (500, 50, 500), + expo: 0.2, + } + } } #[entry] @@ -115,10 +138,10 @@ fn main() -> ! { let mut adc = Adc::new(pac.ADC, &mut pac.RESETS); // Configure ADC input pins - let mut x1_pin = pins.gp26.into_floating_input(); - let mut y1_pin = pins.gp27.into_floating_input(); - let mut x2_pin = pins.gp28.into_floating_input(); - let mut y2_pin = pins.gp29.into_floating_input(); + let mut adc_pin_left_x = pins.gp26.into_floating_input(); + let mut adc_pin_left_y = pins.gp27.into_floating_input(); + let mut adc_pin_right_x = pins.gp28.into_floating_input(); + let mut adc_pin_right_y = pins.gp29.into_floating_input(); // Setting up array with pins connected to button rows let button_matrix_row_pins: &[&dyn InputPin; KEY_ROWS] = &[ @@ -151,7 +174,7 @@ fn main() -> ! { &mut pac.RESETS, )); - let mut joystick = UsbHidClassBuilder::new() + let mut usb_hid_joystick = UsbHidClassBuilder::new() .add_device(JoystickConfig::default()) .build(&usb_bus); @@ -186,13 +209,24 @@ fn main() -> ! { let mut status_led_count_down = timer.count_down(); status_led_count_down.start(250.millis()); - // Create variables to track all modes - let mut fn_mode: u8; - let mut alt_l_mode: bool; - let mut alt_r_mode: bool; + // Create variable to track modes + let mut fn_mode: u8 = 0; // Create joystick axis array - let mut axis: [JoystickAxis; 4]; + let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS]; + + // 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 = (500, 0, 500); + axis[GIMBAL_AXIS_LEFT_Y].expo = 0.0; + + // Create dynamic smoother array for gimbal axis + let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [ + DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), + DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), + DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), + DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), + ]; // Initialize button matrix button_matrix.init_pins(); @@ -211,25 +245,23 @@ fn main() -> ! { } loop { - // if status_led_count_down.wait().is_ok() { - // update_status_led(&mut status_led, &caps_lock_active, &gui_lock_state); - // } + if status_led_count_down.wait().is_ok() { + update_status_led(&mut status_led, &fn_mode); + } if usb_hid_report_count_down.wait().is_ok() { let pressed_keys = button_matrix.buttons_pressed(); - (fn_mode, alt_l_mode, alt_r_mode) = get_mode(pressed_keys); + fn_mode = get_mode(pressed_keys); for (index, key) in pressed_keys.iter().enumerate() { buttons[index].pressed = *key; } - match joystick.device().write_report(&get_joystick_report( + match usb_hid_joystick.device().write_report(&get_joystick_report( &mut buttons, - axis, + &mut axis, fn_mode, - alt_l_mode, - alt_r_mode, )) { Err(UsbHidError::WouldBlock) => {} Ok(_) => {} @@ -243,41 +275,26 @@ fn main() -> ! { if usb_tick_count_down.wait().is_ok() { button_matrix.scan_matrix(&mut delay); - axis[0].value = apply_calibration( - adc.read(&mut x1_pin).unwrap(), - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - 50, - 0.2, - ); - axis[1].value = apply_calibration( - adc.read(&mut y1_pin).unwrap(), - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - 50, - 0.2, - ); - axis[2].value = apply_calibration( - adc.read(&mut x2_pin).unwrap(), - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - 50, - 0.2, - ); - axis[3].value = apply_calibration( - adc.read(&mut y2_pin).unwrap(), - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - 0, - 0.0, - ); + // Read ADC values and smooth them + 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()); + + // Update axis values + for (index, item) in axis.iter_mut().enumerate() { + item.value = calculate_axis_value( + smoother[index].value() as u16, + item.min, + item.max, + item.center, + item.deadzone, + item.expo, + ); + } } - if usb_dev.poll(&mut [&mut joystick]) {} + if usb_dev.poll(&mut [&mut usb_hid_joystick]) {} } } @@ -291,19 +308,14 @@ fn main() -> ! { /// # Arguments /// * `status_led` - Reference to status LED /// * `caps_lock_active` - Is capslock active -fn update_status_led( - status_led: &mut Ws2812StatusLed, - caps_lock_active: &bool, - gui_lock_state: &u8, -) where +fn update_status_led(status_led: &mut Ws2812StatusLed, fn_mode: &u8) +where P: PIOExt + FunctionConfig, I: PinId, Function

: ValidPinMode, SM: StateMachineIndex, { - if *caps_lock_active { - status_led.update(StatusMode::Warning); - } else if *gui_lock_state != 0 { + if *fn_mode & 0x10 == 0x10 { status_led.update(StatusMode::Activity); } else { status_led.update(StatusMode::Normal); @@ -316,13 +328,13 @@ fn update_status_led( /// # Arguments /// /// * `pressed_keys` - Array of pressed keys -fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> (u8, bool, bool) { +fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { // Check how many Fn keys are pressed let mut fn_mode: u8 = 0; let mut fn_l_active: bool = false; let mut fn_r_active: bool = false; - let mut alt_l_mode: bool = false; - let mut alt_r_mode: 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::MAP[0][index] == layout::ButtonType::FnL { @@ -332,10 +344,10 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> (u8, bool, bool) { fn_r_active = true; } if *key && layout::MAP[0][index] == layout::ButtonType::ModeL { - alt_l_mode = true; + alt_l_active = true; } if *key && layout::MAP[0][index] == layout::ButtonType::ModeR { - alt_r_mode = true; + alt_r_active = true; } } @@ -347,7 +359,15 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> (u8, bool, bool) { fn_mode = 1; } - (fn_mode, alt_l_mode, alt_r_mode) + // Set bit 4 and 5 if alt l/r is active + if alt_l_active { + fn_mode |= 0x10; + } + if alt_r_active { + fn_mode |= 0x20; + } + + fn_mode } /// Generate keyboard report based on pressed keys and Fn mode (0, 1 or 2) @@ -362,95 +382,111 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> (u8, bool, bool) { /// * `alt_r_mode` - Is right alt mode active fn get_joystick_report( matrix_keys: &mut [JoystickButton; NUMBER_OF_KEYS], - axis: [JoystickAxis; 4], + axis: &mut [GimbalAxis; 4], fn_mode: u8, - alt_l_mode: bool, - alt_r_mode: bool, ) -> JoystickReport { - let mut x: u16 = axis[2].value; // right gimbal normal mode - let mut y: u16 = axis[3].value; // right gimbal normal mode - let z: u16 = axis[0].value; // left gimbal normal mode - let mut rx: u16 = HID_AXIS_CENTER; // right gimbal alt mode - let mut ry: u16 = HID_AXIS_CENTER; // right gimbal alt mode - let mut rz: u16 = axis[1].value; // left gimbal normal and alt mode - let mut buttons: u32 = 0; - let mut hat1: u8 = HAT_CENTER; - let mut hat2: u8 = HAT_CENTER; - let mut hat3: u8 = HAT_CENTER; - let mut hat4: u8 = HAT_CENTER; + 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; - if alt_l_mode && (axis[1].fn_mode == 0 || axis[1].fn_mode == 2) { + // Update Fn mode for all axis + for item in axis.iter_mut() { + if item.value == item.idle_value { + item.fn_mode = fn_mode & 0x0F; + } + } + + // Left Alt mode active + // Full range of left gimbal gives half range of joystick axis (center to max) + // Left Fn mode = reversed range (center to min) + if fn_mode & 0x10 == 0x10 + && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 0 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 2) + { rz = remap( - axis[1].value, - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - HID_AXIS_MAX, + axis[GIMBAL_AXIS_LEFT_Y].value, + AXIS_MIN, + AXIS_MAX, + AXIS_CENTER, + AXIS_MAX, ); - } else if alt_l_mode && (axis[1].fn_mode == 1 || axis[1].fn_mode == 3) { + } else if fn_mode & 0x10 == 0x10 + && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 1 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 3) + { rz = remap( - axis[1].value, - HID_AXIS_MIN, - HID_AXIS_MAX, - HID_AXIS_CENTER, - HID_AXIS_MIN, + axis[GIMBAL_AXIS_LEFT_Y].value, + AXIS_MIN, + AXIS_MAX, + AXIS_CENTER, + AXIS_MIN, ); } - if alt_r_mode && (axis[2].fn_mode == 2 || axis[2].fn_mode == 3) { - x = HID_AXIS_CENTER; - rx = axis[2].value; + // Right Alt mode active + // Right gimbal control third joystick axis when right Fn mode is active + if fn_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 fn_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; } - if alt_r_mode && (axis[3].fn_mode == 2 || axis[3].fn_mode == 3) { - y = HID_AXIS_CENTER; - ry = axis[3].value; - } - - // Filter report based on Fn mode and pressed keys - for (index, key) in matrix_keys.iter_mut().enumerate() { - // Set fn mode for the pressed button + // Set fn mode for pressed button + for key in matrix_keys.iter_mut() { if key.pressed != key.previous_pressed && key.pressed { - key.fn_mode = fn_mode; + key.fn_mode = fn_mode & 0x0F; } key.previous_pressed = key.pressed; + } - // Skip key if defined as NoEventIndicated - if layout::MAP[key.fn_mode as usize][index] == layout::ButtonType::NotConnected { - continue; - } - - // Update button state + // 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::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::B20 as usize + && layout::MAP[fn_mode as usize][index] as usize >= layout::ButtonType::Hat1U as usize + && layout::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::Hat4B as usize + { + hats[(index - layout::ButtonType::Hat1U as usize) / 4] |= 1 + << ((index - layout::ButtonType::Hat1U as usize) + + (5 * ((index - layout::ButtonType::Hat1U as usize) / 4))); + } + } + + // 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 17-20 according to hat button 1-4 + let mut buttons: u32 = (hat_button1 as u32) << 16 + | ((hat_button2 as u32) << 17) + | ((hat_button3 as u32) << 18) + | ((hat_button4 as u32) << 19); + + // Update button state for joystick button 1-16 + for (index, key) in matrix_keys.iter_mut().enumerate() { + if key.pressed + && layout::MAP[fn_mode as usize][index] as usize >= layout::ButtonType::B1 as usize + && layout::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::B16 as usize { buttons |= 1 << layout::MAP[fn_mode as usize][index] as usize; } - // Update hat state - else if key.pressed - && layout::MAP[fn_mode as usize][index] as usize >= layout::ButtonType::Hat1U as usize - && layout::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::Hat4D as usize - { - match layout::MAP[fn_mode as usize][index] { - layout::ButtonType::Hat1U => hat1 = HAT_UP, - layout::ButtonType::Hat1R => hat1 = HAT_RIGHT, - layout::ButtonType::Hat1D => hat1 = HAT_DOWN, - layout::ButtonType::Hat1L => hat1 = HAT_LEFT, - layout::ButtonType::Hat2U => hat2 = HAT_UP, - layout::ButtonType::Hat2R => hat2 = HAT_RIGHT, - layout::ButtonType::Hat2D => hat2 = HAT_DOWN, - layout::ButtonType::Hat2L => hat2 = HAT_LEFT, - layout::ButtonType::Hat3U => hat3 = HAT_UP, - layout::ButtonType::Hat3R => hat3 = HAT_RIGHT, - layout::ButtonType::Hat3D => hat3 = HAT_DOWN, - layout::ButtonType::Hat3L => hat3 = HAT_RIGHT, - layout::ButtonType::Hat4U => hat4 = HAT_UP, - layout::ButtonType::Hat4R => hat4 = HAT_RIGHT, - layout::ButtonType::Hat4D => hat4 = HAT_DOWN, - layout::ButtonType::Hat4L => hat4 = HAT_LEFT, - _ => {} - } - } } JoystickReport { @@ -468,6 +504,99 @@ fn get_joystick_report( } } +/// 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 @@ -477,7 +606,13 @@ fn get_joystick_report( /// * `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 { - (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min + 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 @@ -495,57 +630,3 @@ fn constrain(value: T, out_min: T, out_max: T) -> T { value } } - -fn calibrate_axis(axis: [JoystickAxis; 4]) {} - -fn apply_calibration( - gimbal_value: u16, - min_value: u16, - max_value: u16, - center_value: u16, - deadband_value: u16, - expo_value: f32, -) -> u16 { - let mut calibrated_value = HID_AXIS_CENTER; - - if gimbal_value > (center_value + deadband_value) { - calibrated_value = constrain( - remap( - gimbal_value, - center_value + deadband_value, - max_value, - HID_AXIS_CENTER, - HID_AXIS_MAX, - ), - HID_AXIS_CENTER, - HID_AXIS_MAX, - ); - } else if gimbal_value < (center_value - deadband_value) { - calibrated_value = constrain( - remap( - gimbal_value, - min_value, - center_value - deadband_value, - HID_AXIS_MIN, - HID_AXIS_CENTER, - ), - HID_AXIS_MIN, - HID_AXIS_CENTER, - ); - } - - if expo_value != 0.0 { - let joystick_x_float = calibrated_value as f32 / HID_AXIS_MAX as f32; - /* Calculate expo using 9th order polynomial function with 0.5 as center point */ - let joystick_x_exp: f32 = expo_value * (0.5 + 256.0 * powf(joystick_x_float - 0.5, 9.0)) - + (1.0 - expo_value) * joystick_x_float; - - calibrated_value = constrain( - (joystick_x_exp * HID_AXIS_MAX as f32) as u16, - HID_AXIS_MIN, - HID_AXIS_MAX, - ); - } - - calibrated_value -} From dce996484c72bf636534a03b2474d5fd89f2353e Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sat, 5 Aug 2023 13:00:00 +0200 Subject: [PATCH 04/16] Added prototype board --- eCAD/experiment_board.md | 42 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 eCAD/experiment_board.md 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 From 30e640625a9a8b877b052126dfa521eec91d86e3 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sat, 5 Aug 2023 13:00:36 +0200 Subject: [PATCH 05/16] Updated header --- rp2040/src/button_matrix.rs | 4 ++-- rp2040/src/status_led.rs | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rp2040/src/button_matrix.rs b/rp2040/src/button_matrix.rs index 2c7e0da..2d20098 100644 --- a/rp2040/src/button_matrix.rs +++ b/rp2040/src/button_matrix.rs @@ -1,5 +1,5 @@ -//! Project: CMtec CMDR Keyboard 42 -//! Date: 2023-07-01 +//! Project: CMtec CMDR joystick 24 +//! Date: 2023-08-01 //! Author: Christoffer Martinsson //! Email: cm@cmtec.se //! License: Please refer to LICENSE in root directory diff --git a/rp2040/src/status_led.rs b/rp2040/src/status_led.rs index d56af7b..1f5e03a 100644 --- a/rp2040/src/status_led.rs +++ b/rp2040/src/status_led.rs @@ -1,5 +1,5 @@ -//! Project: CMtec CMDR Keyboard 42 -//! Date: 2023-07-01 +//! Project: CMtec CMDR joystick 24 +//! Date: 2023-08-01 //! Author: Christoffer Martinsson //! Email: cm@cmtec.se //! License: Please refer to LICENSE in root directory @@ -16,7 +16,7 @@ use ws2812_pio::Ws2812Direct; /// * OFF = Syatem offline /// * NORMAL = All system Ok /// * ACTIVITY = System activity -/// * OTHER = Other activity +/// * OTHER = Other activity /// * WARNING = Warning /// * ERROR = Error /// * BOOTLOADER = Bootloader active From 8c952f26eda6c5ae57a9632a7bdbd5ec8e20f7a3 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sat, 5 Aug 2023 13:01:01 +0200 Subject: [PATCH 06/16] Chaged to 24 HID buttons --- rp2040/src/layout.rs | 99 +++++++++++++++++-------------- rp2040/src/main.rs | 96 ++++++++++++++++-------------- rp2040/src/usb_joystick_device.rs | 19 +++--- 3 files changed, 115 insertions(+), 99 deletions(-) diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index 2d5dbd1..ffd23db 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -1,10 +1,10 @@ -//! Project: CMtec CMDR Keyboard 42 -//! Date: 2023-07-01 +//! 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_KEYS; +use crate::NUMBER_OF_BUTTONS; #[derive(Debug, PartialEq, Copy, Clone)] pub enum ButtonType { @@ -24,31 +24,35 @@ pub enum ButtonType { B14 = 13, B15 = 14, B16 = 15, - FnL = 16, - FnR = 17, - ModeL = 18, - ModeR = 19, - Hat1U = 20, - Hat1R = 21, - Hat1D = 22, - Hat1L = 23, - Hat1B = 24, - Hat2U = 25, - Hat2R = 26, - Hat2D = 27, - Hat2L = 28, - Hat2B = 29, - Hat3U = 30, - Hat3R = 31, - Hat3D = 32, - Hat3L = 33, - Hat3B = 34, - Hat4U = 35, - Hat4R = 36, - Hat4D = 37, - Hat4L = 38, - Hat4B = 39, - NoEventIndicated = 40, + 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, } // Button index map: @@ -56,16 +60,19 @@ pub enum ButtonType { // | 0 | 1 | | 2 | 3 | (4) // -------------------------------------------------------------- // | | 5 | 6 | 7 | | 12 | 11 | 10 | | -// | | (9) (14) +// | | // | | 8 | | 13 | | +// | | 9 | | 14 | | // | X1/Y1 X2/Y2 | // | | 16 | | 21 | | // | | 17 | 15 | 18 || 22 | 20 | 23 | | // | | 19 | | 24 | | // -------------------------------------------------------------- // -/// Button map to HID key (three Function layers) -pub const MAP: [[ButtonType; NUMBER_OF_KEYS]; 4] = [ +/// 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. +pub const MAP: [[ButtonType; NUMBER_OF_BUTTONS]; 4] = [ [ // Function layer 0 // HID Key // Button Index @@ -74,17 +81,17 @@ pub const MAP: [[ButtonType; NUMBER_OF_KEYS]; 4] = [ ButtonType::B1, // 1 ButtonType::B5, // 2 ButtonType::FnR, // 3 - ButtonType::NoEventIndicated, // 4 + ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::B2, // 5 ButtonType::B3, // 6 ButtonType::ModeL, // 7 ButtonType::B4, // 8 - ButtonType::NoEventIndicated, // 9 + ButtonType::B17, // 9 ButtonType::B6, // 10 ButtonType::B7, // 11 ButtonType::ModeR, // 12 ButtonType::B8, // 13 - ButtonType::NoEventIndicated, // 14 + ButtonType::B18, // 14 ButtonType::Hat1B, // 15 ButtonType::Hat1U, // 16 ButtonType::Hat1L, // 17 @@ -97,24 +104,24 @@ pub const MAP: [[ButtonType; NUMBER_OF_KEYS]; 4] = [ ButtonType::Hat2D, // 24 ], [ - // Function layer left + // Function layer 1 (left Fn button pressed) // HID Key // Button Index // ----------------------------------------- ButtonType::FnL, // 0 ButtonType::B9, // 1 ButtonType::B5, // 2 ButtonType::FnR, // 3 - ButtonType::NoEventIndicated, // 4 + ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::B10, // 5 ButtonType::B11, // 6 ButtonType::ModeL, // 7 ButtonType::B12, // 8 - ButtonType::NoEventIndicated, // 9 + ButtonType::B19, // 9 ButtonType::B6, // 10 ButtonType::B7, // 11 ButtonType::ModeR, // 12 ButtonType::B8, // 13 - ButtonType::NoEventIndicated, // 14 + ButtonType::B18, // 14 ButtonType::Hat3B, // 15 ButtonType::Hat3U, // 16 ButtonType::Hat3L, // 17 @@ -127,24 +134,24 @@ pub const MAP: [[ButtonType; NUMBER_OF_KEYS]; 4] = [ ButtonType::Hat2D, // 24 ], [ - // Function layer right + // Function layer 2 (right Fn button pressed) // HID Key // Button Index // ----------------------------------------- ButtonType::FnL, // 0 ButtonType::B1, // 1 ButtonType::B13, // 2 ButtonType::FnR, // 3 - ButtonType::NoEventIndicated, // 4 + ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::B2, // 5 ButtonType::B3, // 6 ButtonType::ModeL, // 7 ButtonType::B4, // 8 - ButtonType::NoEventIndicated, // 9 + ButtonType::B17, // 9 ButtonType::B14, // 10 ButtonType::B15, // 11 ButtonType::ModeR, // 12 ButtonType::B16, // 13 - ButtonType::NoEventIndicated, // 14 + ButtonType::B20, // 14 ButtonType::Hat1B, // 15 ButtonType::Hat1U, // 16 ButtonType::Hat1L, // 17 @@ -157,24 +164,24 @@ pub const MAP: [[ButtonType; NUMBER_OF_KEYS]; 4] = [ ButtonType::Hat4D, // 24 ], [ - // Function layer left + right + // Function layer 3 (left + right Fn button pressed) // HID Key // Button Index // ----------------------------------------- ButtonType::FnL, // 0 ButtonType::B9, // 1 ButtonType::B13, // 2 ButtonType::FnR, // 3 - ButtonType::NoEventIndicated, // 4 + ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::B10, // 5 ButtonType::B11, // 6 ButtonType::ModeL, // 7 ButtonType::B12, // 8 - ButtonType::NoEventIndicated, // 9 + ButtonType::B19, // 9 ButtonType::B14, // 10 ButtonType::B15, // 11 ButtonType::ModeR, // 12 ButtonType::B16, // 13 - ButtonType::NoEventIndicated, // 14 + ButtonType::B20, // 14 ButtonType::Hat3B, // 15 ButtonType::Hat3U, // 16 ButtonType::Hat3L, // 17 diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index dc83e7e..527a75f 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -1,5 +1,5 @@ -//! Project: CMtec CMDR Keyboard 42 -//! Date: 2023-07-01 +//! Project: CMtec CMDR joystick 24 +//! Date: 2023-08-01 //! Author: Christoffer Martinsson //! Email: cm@cmtec.se //! License: Please refer to LICENSE in root directory @@ -46,9 +46,9 @@ use waveshare_rp2040_zero::{ }; // Public constants -pub const KEY_ROWS: usize = 5; -pub const KEY_COLS: usize = 5; -pub const NUMBER_OF_KEYS: usize = KEY_ROWS * KEY_COLS; +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; @@ -67,7 +67,7 @@ pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32; // Public types #[derive(Copy, Clone, Default)] -pub struct JoystickButton { +pub struct Button { pub pressed: bool, pub previous_pressed: bool, pub fn_mode: u8, @@ -138,13 +138,15 @@ fn main() -> ! { 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.gp26.into_floating_input(); let mut adc_pin_left_y = pins.gp27.into_floating_input(); let mut adc_pin_right_x = pins.gp28.into_floating_input(); let mut adc_pin_right_y = pins.gp29.into_floating_input(); // Setting up array with pins connected to button rows - let button_matrix_row_pins: &[&dyn InputPin; KEY_ROWS] = &[ + let button_matrix_row_pins: &[&dyn InputPin; BUTTON_ROWS] = &[ &pins.gp9.into_pull_up_input(), &pins.gp10.into_pull_up_input(), &pins.gp11.into_pull_up_input(), @@ -153,7 +155,7 @@ fn main() -> ! { ]; // Setting up array with pins connected to button columns - let button_matrix_col_pins: &mut [&mut dyn OutputPin; KEY_COLS] = &mut [ + 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(), @@ -161,10 +163,13 @@ fn main() -> ! { &mut pins.gp8.into_push_pull_output(), ]; - // Create button matrix object that scans all the PCB buttons - let mut button_matrix: ButtonMatrix = + // 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(); + // Configure USB let usb_bus = UsbBusAllocator::new(waveshare_rp2040_zero::hal::usb::UsbBus::new( pac.USBCTRL_REGS, @@ -193,9 +198,6 @@ fn main() -> ! { clocks.peripheral_clock.freq(), ); - // Create joystick button array - let mut buttons: [JoystickButton; NUMBER_OF_KEYS] = [JoystickButton::default(); NUMBER_OF_KEYS]; - // Create timers/delays let timer = Timer::new(pac.TIMER, &mut pac.RESETS); let mut delay = Delay::new(core.SYST, clocks.system_clock.freq().to_Hz()); @@ -203,8 +205,8 @@ fn main() -> ! { let mut usb_hid_report_count_down = timer.count_down(); usb_hid_report_count_down.start(10.millis()); - let mut usb_tick_count_down = timer.count_down(); - usb_tick_count_down.start(1.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(250.millis()); @@ -212,8 +214,9 @@ fn main() -> ! { // Create variable to track modes let mut fn_mode: u8 = 0; - // Create joystick axis array + // Create joystick button/axis array 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; @@ -221,6 +224,7 @@ fn main() -> ! { axis[GIMBAL_AXIS_LEFT_Y].expo = 0.0; // 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), @@ -228,9 +232,6 @@ fn main() -> ! { DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), ]; - // Initialize button matrix - button_matrix.init_pins(); - // Scan matrix to get initial state for _ in 0..10 { button_matrix.scan_matrix(&mut delay); @@ -261,7 +262,7 @@ fn main() -> ! { match usb_hid_joystick.device().write_report(&get_joystick_report( &mut buttons, &mut axis, - fn_mode, + &fn_mode, )) { Err(UsbHidError::WouldBlock) => {} Ok(_) => {} @@ -272,16 +273,17 @@ fn main() -> ! { }; } - if usb_tick_count_down.wait().is_ok() { + if scan_count_down.wait().is_ok() { button_matrix.scan_matrix(&mut delay); - // Read ADC values and smooth them + // 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()); - // Update axis values for (index, item) in axis.iter_mut().enumerate() { item.value = calculate_axis_value( smoother[index].value() as u16, @@ -301,8 +303,7 @@ fn main() -> ! { /// Update status LED colour based on function layer and capslock /// /// Normal = green (NORMAL) -/// GUI lock = blue (GUI LOCK) -/// Capslock active = flashing red (WARNING) +/// Left Alt mode = blue (GUI LOCK) /// Error = steady red (ERROR) /// /// # Arguments @@ -328,7 +329,7 @@ where /// # Arguments /// /// * `pressed_keys` - Array of pressed keys -fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { +fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { // Check how many Fn keys are pressed let mut fn_mode: u8 = 0; let mut fn_l_active: bool = false; @@ -381,9 +382,9 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_KEYS]) -> u8 { /// * `alt_l_mode` - Is left alt mode active /// * `alt_r_mode` - Is right alt mode active fn get_joystick_report( - matrix_keys: &mut [JoystickButton; NUMBER_OF_KEYS], + matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], axis: &mut [GimbalAxis; 4], - fn_mode: u8, + fn_mode: &u8, ) -> JoystickReport { let mut x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; let mut y: u16 = axis[GIMBAL_AXIS_RIGHT_Y].value; @@ -392,14 +393,15 @@ fn get_joystick_report( let mut ry: u16 = AXIS_CENTER; let mut rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value; - // Update Fn mode for all axis + // Update Fn mode for all axis that are in idle position + // This is to avoid the Fn mode switching when moving the gimbal for item in axis.iter_mut() { if item.value == item.idle_value { item.fn_mode = fn_mode & 0x0F; } } - // Left Alt mode active + // 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 fn_mode & 0x10 == 0x10 @@ -424,7 +426,7 @@ fn get_joystick_report( ); } - // Right Alt mode active + // Right Alt mode active (bit 5) // Right gimbal control third joystick axis when right Fn mode is active if fn_mode & 0x20 == 0x20 && (axis[GIMBAL_AXIS_RIGHT_X].fn_mode == 2 || axis[GIMBAL_AXIS_RIGHT_X].fn_mode == 3) @@ -439,12 +441,12 @@ fn get_joystick_report( ry = axis[GIMBAL_AXIS_RIGHT_Y].value; } - // Set fn mode for pressed button + // Set fn mode for all keys taht are in idle position + // This is to avoid the Fn mode switching when using a button for key in matrix_keys.iter_mut() { - if key.pressed != key.previous_pressed && key.pressed { + if !key.pressed { key.fn_mode = fn_mode & 0x0F; } - key.previous_pressed = key.pressed; } // Generate array for all four hat switches with following structure: @@ -458,8 +460,10 @@ fn get_joystick_report( let mut hats: [u8; 4] = [0; 4]; for (index, key) in matrix_keys.iter_mut().enumerate() { if key.pressed - && layout::MAP[fn_mode as usize][index] as usize >= layout::ButtonType::Hat1U as usize - && layout::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::Hat4B as usize + && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize + >= layout::ButtonType::Hat1U as usize + && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize + <= layout::ButtonType::Hat4B as usize { hats[(index - layout::ButtonType::Hat1U as usize) / 4] |= 1 << ((index - layout::ButtonType::Hat1U as usize) @@ -473,19 +477,21 @@ fn get_joystick_report( let (hat3, hat_button3) = format_hat_value(hats[2]); let (hat4, hat_button4) = format_hat_value(hats[3]); - // Update button state for joystick button 17-20 according to hat button 1-4 - let mut buttons: u32 = (hat_button1 as u32) << 16 - | ((hat_button2 as u32) << 17) - | ((hat_button3 as u32) << 18) - | ((hat_button4 as u32) << 19); + // 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-16 + // Update button state for joystick button 1-20 for (index, key) in matrix_keys.iter_mut().enumerate() { if key.pressed - && layout::MAP[fn_mode as usize][index] as usize >= layout::ButtonType::B1 as usize - && layout::MAP[fn_mode as usize][index] as usize <= layout::ButtonType::B16 as usize + && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize + >= layout::ButtonType::B1 as usize + && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize + <= layout::ButtonType::B20 as usize { - buttons |= 1 << layout::MAP[fn_mode as usize][index] as usize; + buttons |= 1 << layout::MAP[(fn_mode & 0x0F) as usize][index] as usize; } } diff --git a/rp2040/src/usb_joystick_device.rs b/rp2040/src/usb_joystick_device.rs index 2fd20e0..1d81a58 100644 --- a/rp2040/src/usb_joystick_device.rs +++ b/rp2040/src/usb_joystick_device.rs @@ -1,4 +1,9 @@ -//!HID joystick +//! 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; @@ -63,7 +68,7 @@ impl Try for Result { } // Based on example device from https://github.com/dlkj/usbd-human-interface-device/blob/main/src/device/joystick.rs -// Updated to 6pc 12bit axis, 20pc buttons and 4pc hat switches +// Updated to 6pc 12bit axis, 24pc buttons and 4pc hat switches #[rustfmt::skip] pub const JOYSTICK_DESCRIPTOR: &[u8] = &[ 0x05, 0x01, // Usage Page (Generic Desktop) @@ -85,15 +90,12 @@ pub const JOYSTICK_DESCRIPTOR: &[u8] = &[ 0xc0, // End Collection 0x05, 0x09, // Usage Page (Button) 0x19, 0x01, // Usage Minimum (1) - 0x29, 0x14, // Usage Maximum (20) + 0x29, 0x18, // Usage Maximum (24) 0x15, 0x00, // Logical Minimum (0) 0x25, 0x01, // Logical Maximum (1) 0x75, 0x01, // Report Size (1) - 0x95, 0x14, // Report Count (20) + 0x95, 0x18, // Report Count (24) 0x81, 0x02, // Input (Data, Variable, Absolute) - 0x75, 0x01, // Report Size (1) PADDING - 0x95, 0x04, // Report Count (4) PADDING - 0x81, 0x03, // Input (Const, Variable, Absolute) PADDING 0x15, 0x00, // Logical Minimum (0) 0x25, 0x07, // Logical Maximum (7) 0x35, 0x00, // Physical Minimum (0) @@ -118,7 +120,7 @@ pub struct JoystickReport { pub rx: u16, // 12bit pub ry: u16, // 12bit pub rz: u16, // 12bit - pub buttons: u32, // 20bit + pub buttons: u32, // 24bit pub hat1: u8, // 4bit pub hat2: u8, // 4bit pub hat3: u8, // 4bit @@ -134,6 +136,7 @@ impl<'a, B: UsbBus> Joystick<'a, B> { 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; From 9440c1d566c6559f1342a54855b8ed9a6caf34ae Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sat, 5 Aug 2023 13:47:14 +0200 Subject: [PATCH 07/16] Updated readme --- README.md | 136 ++++++++++++++++++++++++++++++------------------------ 1 file changed, 76 insertions(+), 60 deletions(-) diff --git a/README.md b/README.md index db5dedc..1f4499b 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,29 @@ 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 +- 6x Cherry MX switches +- 2x Alpine hat switches +- 1x PCB - 1x Bottom case - 1x Top plate -- 2x Gimbal spacers +- 2x Hat swith top ## 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 From 6e1e450095a71701ef1685087e4052dabd903fc4 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sat, 5 Aug 2023 23:43:54 +0200 Subject: [PATCH 08/16] Changed layout due to prototype board --- rp2040/src/layout.rs | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index ffd23db..1ff4a4c 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -57,7 +57,7 @@ pub enum ButtonType { // Button index map: // -------------------------------------------------------------- -// | 0 | 1 | | 2 | 3 | (4) +// | 3 | 4 | | 0 | 1 | (2) // -------------------------------------------------------------- // | | 5 | 6 | 7 | | 12 | 11 | 10 | | // | | @@ -77,11 +77,11 @@ pub const MAP: [[ButtonType; NUMBER_OF_BUTTONS]; 4] = [ // Function layer 0 // HID Key // Button Index // ----------------------------------------- + ButtonType::FnR, // 3 + ButtonType::B5, // 2 + ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::FnL, // 0 ButtonType::B1, // 1 - ButtonType::B5, // 2 - ButtonType::FnR, // 3 - ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::B2, // 5 ButtonType::B3, // 6 ButtonType::ModeL, // 7 @@ -107,11 +107,11 @@ pub const MAP: [[ButtonType; NUMBER_OF_BUTTONS]; 4] = [ // Function layer 1 (left Fn button pressed) // HID Key // Button Index // ----------------------------------------- + ButtonType::FnR, // 3 + ButtonType::B5, // 2 + ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::FnL, // 0 ButtonType::B9, // 1 - ButtonType::B5, // 2 - ButtonType::FnR, // 3 - ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::B10, // 5 ButtonType::B11, // 6 ButtonType::ModeL, // 7 @@ -137,11 +137,11 @@ pub const MAP: [[ButtonType; NUMBER_OF_BUTTONS]; 4] = [ // Function layer 2 (right Fn button pressed) // HID Key // Button Index // ----------------------------------------- + ButtonType::FnR, // 3 + ButtonType::B13, // 2 + ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::FnL, // 0 ButtonType::B1, // 1 - ButtonType::B13, // 2 - ButtonType::FnR, // 3 - ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::B2, // 5 ButtonType::B3, // 6 ButtonType::ModeL, // 7 @@ -167,11 +167,11 @@ pub const MAP: [[ButtonType; NUMBER_OF_BUTTONS]; 4] = [ // Function layer 3 (left + right Fn button pressed) // HID Key // Button Index // ----------------------------------------- + ButtonType::FnR, // 3 + ButtonType::B13, // 2 + ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::FnL, // 0 ButtonType::B9, // 1 - ButtonType::B13, // 2 - ButtonType::FnR, // 3 - ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::B10, // 5 ButtonType::B11, // 6 ButtonType::ModeL, // 7 From d3c1a271227ad0306dd0599c9619edc924882852 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sat, 5 Aug 2023 23:44:12 +0200 Subject: [PATCH 09/16] Fixed major bugs --- rp2040/src/main.rs | 65 ++++++++++++++++++++++++---------------------- 1 file changed, 34 insertions(+), 31 deletions(-) diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 527a75f..df9d5dd 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -147,11 +147,11 @@ fn main() -> ! { // Setting up array with pins connected to button rows let button_matrix_row_pins: &[&dyn InputPin; BUTTON_ROWS] = &[ - &pins.gp9.into_pull_up_input(), - &pins.gp10.into_pull_up_input(), &pins.gp11.into_pull_up_input(), - &pins.gp12.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 @@ -212,7 +212,7 @@ fn main() -> ! { status_led_count_down.start(250.millis()); // Create variable to track modes - let mut fn_mode: u8 = 0; + let mut mode: u8 = 0; // Create joystick button/axis array let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS]; @@ -247,13 +247,13 @@ fn main() -> ! { loop { if status_led_count_down.wait().is_ok() { - update_status_led(&mut status_led, &fn_mode); + update_status_led(&mut status_led, &mode); } if usb_hid_report_count_down.wait().is_ok() { let pressed_keys = button_matrix.buttons_pressed(); - fn_mode = get_mode(pressed_keys); + mode = get_mode(pressed_keys); for (index, key) in pressed_keys.iter().enumerate() { buttons[index].pressed = *key; @@ -262,7 +262,7 @@ fn main() -> ! { match usb_hid_joystick.device().write_report(&get_joystick_report( &mut buttons, &mut axis, - &fn_mode, + &mode, )) { Err(UsbHidError::WouldBlock) => {} Ok(_) => {} @@ -331,7 +331,7 @@ where /// * `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 fn_mode: u8 = 0; + 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; @@ -353,22 +353,22 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { } if fn_l_active && fn_r_active { - fn_mode = 3; + mode = 3; } else if fn_l_active { - fn_mode = 2; + mode = 2; } else if fn_r_active { - fn_mode = 1; + mode = 1; } // Set bit 4 and 5 if alt l/r is active if alt_l_active { - fn_mode |= 0x10; + mode |= 0x10; } if alt_r_active { - fn_mode |= 0x20; + mode |= 0x20; } - fn_mode + mode } /// Generate keyboard report based on pressed keys and Fn mode (0, 1 or 2) @@ -384,7 +384,7 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { fn get_joystick_report( matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], axis: &mut [GimbalAxis; 4], - fn_mode: &u8, + mode: &u8, ) -> JoystickReport { let mut x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value; let mut y: u16 = axis[GIMBAL_AXIS_RIGHT_Y].value; @@ -397,14 +397,14 @@ fn get_joystick_report( // This is to avoid the Fn mode switching when moving the gimbal for item in axis.iter_mut() { if item.value == item.idle_value { - item.fn_mode = fn_mode & 0x0F; + item.fn_mode = mode & 0x0F; } } // 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 fn_mode & 0x10 == 0x10 + if mode & 0x10 == 0x10 && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 0 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 2) { rz = remap( @@ -414,7 +414,7 @@ fn get_joystick_report( AXIS_CENTER, AXIS_MAX, ); - } else if fn_mode & 0x10 == 0x10 + } else if mode & 0x10 == 0x10 && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 1 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 3) { rz = remap( @@ -428,13 +428,13 @@ fn get_joystick_report( // Right Alt mode active (bit 5) // Right gimbal control third joystick axis when right Fn mode is active - if fn_mode & 0x20 == 0x20 + 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 fn_mode & 0x20 == 0x20 + if mode & 0x20 == 0x20 && (axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 2 || axis[GIMBAL_AXIS_RIGHT_Y].fn_mode == 3) { y = AXIS_CENTER; @@ -445,7 +445,7 @@ fn get_joystick_report( // This is to avoid the Fn mode switching when using a button for key in matrix_keys.iter_mut() { if !key.pressed { - key.fn_mode = fn_mode & 0x0F; + key.fn_mode = mode & 0x0F; } } @@ -460,14 +460,19 @@ fn get_joystick_report( let mut hats: [u8; 4] = [0; 4]; for (index, key) in matrix_keys.iter_mut().enumerate() { if key.pressed - && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize + && layout::MAP[key.fn_mode as usize][index] as usize >= layout::ButtonType::Hat1U as usize - && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize + && layout::MAP[key.fn_mode as usize][index] as usize <= layout::ButtonType::Hat4B as usize { - hats[(index - layout::ButtonType::Hat1U as usize) / 4] |= 1 - << ((index - layout::ButtonType::Hat1U as usize) - + (5 * ((index - layout::ButtonType::Hat1U as usize) / 4))); + hats[(layout::MAP[key.fn_mode as usize][index] as usize + - layout::ButtonType::Hat1U as usize) + / 5] |= 1 + << ((layout::MAP[key.fn_mode as usize][index] as usize + - layout::ButtonType::Hat1U as usize) + - (5 * ((layout::MAP[key.fn_mode as usize][index] as usize + - layout::ButtonType::Hat1U as usize) + / 5))); } } @@ -486,12 +491,10 @@ fn get_joystick_report( // Update button state for joystick button 1-20 for (index, key) in matrix_keys.iter_mut().enumerate() { if key.pressed - && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize - >= layout::ButtonType::B1 as usize - && layout::MAP[(fn_mode & 0x0F) as usize][index] as usize - <= layout::ButtonType::B20 as usize + && layout::MAP[key.fn_mode as usize][index] as usize >= layout::ButtonType::B1 as usize + && layout::MAP[key.fn_mode as usize][index] as usize <= layout::ButtonType::B20 as usize { - buttons |= 1 << layout::MAP[(fn_mode & 0x0F) as usize][index] as usize; + buttons |= 1 << layout::MAP[key.fn_mode as usize][index] as usize; } } From 164453946782e8dc21b323f299a74ddc91aabfff Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sun, 6 Aug 2023 16:38:11 +0200 Subject: [PATCH 10/16] Updated readme --- README.md | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 1f4499b..5d9c0b2 100644 --- a/README.md +++ b/README.md @@ -99,11 +99,12 @@ ELRS Layer - 2x FrSky M7 or M10 gimbals - 6x Kailh choc low profile switches - 6x Cherry MX switches -- 2x Alpine hat switches -- 1x PCB -- 1x Bottom case -- 1x Top plate -- 2x Hat swith top +- 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 From a8f5b605d3eae3f5f1382a92c8bf40a1346365ff Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sun, 6 Aug 2023 16:38:46 +0200 Subject: [PATCH 11/16] Changed layout once more dua to prototype board --- rp2040/src/layout.rs | 143 ++++++++++++++++++++++--------------------- 1 file changed, 72 insertions(+), 71 deletions(-) diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index 1ff4a4c..3a5c061 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -57,7 +57,7 @@ pub enum ButtonType { // Button index map: // -------------------------------------------------------------- -// | 3 | 4 | | 0 | 1 | (2) +// | 0 | 1 | | 3 | 4 | (2) // -------------------------------------------------------------- // | | 5 | 6 | 7 | | 12 | 11 | 10 | | // | | @@ -72,125 +72,126 @@ pub enum ButtonType { /// 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 MAP: [[ButtonType; NUMBER_OF_BUTTONS]; 4] = [ [ // Function layer 0 // HID Key // Button Index // ----------------------------------------- - ButtonType::FnR, // 3 - ButtonType::B5, // 2 - ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::FnL, // 0 ButtonType::B1, // 1 + ButtonType::NoEventIndicated, // 2 Not connected to any button + ButtonType::FnR, // 3 + ButtonType::B6, // 4 ButtonType::B2, // 5 ButtonType::B3, // 6 ButtonType::ModeL, // 7 ButtonType::B4, // 8 - ButtonType::B17, // 9 - ButtonType::B6, // 10 - ButtonType::B7, // 11 + ButtonType::B5, // 9 + ButtonType::B7, // 10 + ButtonType::B8, // 11 ButtonType::ModeR, // 12 - ButtonType::B8, // 13 - ButtonType::B18, // 14 - ButtonType::Hat1B, // 15 + ButtonType::B9, // 13 + ButtonType::B10, // 14 + ButtonType::Hat1B, // 15 button 21 ButtonType::Hat1U, // 16 - ButtonType::Hat1L, // 17 - ButtonType::Hat1R, // 18 - ButtonType::Hat1D, // 19 - ButtonType::Hat2B, // 20 + ButtonType::Hat1R, // 17 + ButtonType::Hat1D, // 18 + ButtonType::Hat1L, // 19 + ButtonType::Hat2B, // 20 button 22 ButtonType::Hat2U, // 21 - ButtonType::Hat2L, // 22 - ButtonType::Hat2R, // 23 - ButtonType::Hat2D, // 24 + ButtonType::Hat2R, // 22 + ButtonType::Hat2D, // 23 + ButtonType::Hat2L, // 24 ], [ // Function layer 1 (left Fn button pressed) // HID Key // Button Index // ----------------------------------------- - ButtonType::FnR, // 3 - ButtonType::B5, // 2 - ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::FnL, // 0 - ButtonType::B9, // 1 - ButtonType::B10, // 5 - ButtonType::B11, // 6 + ButtonType::B11, // 1 + ButtonType::NoEventIndicated, // 2 Not connected to any button + ButtonType::FnR, // 3 + ButtonType::B6, // 4 + ButtonType::B12, // 5 + ButtonType::B13, // 6 ButtonType::ModeL, // 7 - ButtonType::B12, // 8 - ButtonType::B19, // 9 - ButtonType::B6, // 10 - ButtonType::B7, // 11 + ButtonType::B14, // 8 + ButtonType::B15, // 9 + ButtonType::B7, // 10 + ButtonType::B8, // 11 ButtonType::ModeR, // 12 - ButtonType::B8, // 13 - ButtonType::B18, // 14 - ButtonType::Hat3B, // 15 + ButtonType::B9, // 13 + ButtonType::B10, // 14 + ButtonType::Hat3B, // 15 button 23 ButtonType::Hat3U, // 16 - ButtonType::Hat3L, // 17 - ButtonType::Hat3R, // 18 - ButtonType::Hat3D, // 19 - ButtonType::Hat2B, // 20 + ButtonType::Hat3R, // 17 + ButtonType::Hat3D, // 18 + ButtonType::Hat3L, // 19 + ButtonType::Hat2B, // 20 button 22 ButtonType::Hat2U, // 21 - ButtonType::Hat2L, // 22 - ButtonType::Hat2R, // 23 - ButtonType::Hat2D, // 24 + ButtonType::Hat2R, // 22 + ButtonType::Hat2D, // 23 + ButtonType::Hat2L, // 24 ], [ // Function layer 2 (right Fn button pressed) // HID Key // Button Index // ----------------------------------------- - ButtonType::FnR, // 3 - ButtonType::B13, // 2 - ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::FnL, // 0 ButtonType::B1, // 1 + ButtonType::NoEventIndicated, // 2 Not connected to any button + ButtonType::FnR, // 3 + ButtonType::B16, // 4 ButtonType::B2, // 5 ButtonType::B3, // 6 ButtonType::ModeL, // 7 ButtonType::B4, // 8 - ButtonType::B17, // 9 - ButtonType::B14, // 10 - ButtonType::B15, // 11 + ButtonType::B5, // 9 + ButtonType::B17, // 10 + ButtonType::B18, // 11 ButtonType::ModeR, // 12 - ButtonType::B16, // 13 + ButtonType::B19, // 13 ButtonType::B20, // 14 - ButtonType::Hat1B, // 15 + ButtonType::Hat1B, // 15 button 21 ButtonType::Hat1U, // 16 - ButtonType::Hat1L, // 17 - ButtonType::Hat1R, // 18 - ButtonType::Hat1D, // 19 - ButtonType::Hat4B, // 20 + ButtonType::Hat1R, // 17 + ButtonType::Hat1D, // 18 + ButtonType::Hat1L, // 19 + ButtonType::Hat4B, // 20 button 24 ButtonType::Hat4U, // 21 - ButtonType::Hat4L, // 22 - ButtonType::Hat4R, // 23 - ButtonType::Hat4D, // 24 + ButtonType::Hat4R, // 22 + ButtonType::Hat4D, // 23 + ButtonType::Hat4L, // 24 ], [ // Function layer 3 (left + right Fn button pressed) // HID Key // Button Index // ----------------------------------------- - ButtonType::FnR, // 3 - ButtonType::B13, // 2 - ButtonType::NoEventIndicated, // 4 Not connected to any button ButtonType::FnL, // 0 - ButtonType::B9, // 1 - ButtonType::B10, // 5 - ButtonType::B11, // 6 + ButtonType::B11, // 1 + ButtonType::NoEventIndicated, // 2 Not connected to any button + ButtonType::FnR, // 3 + ButtonType::B16, // 4 + ButtonType::B12, // 5 + ButtonType::B13, // 6 ButtonType::ModeL, // 7 - ButtonType::B12, // 8 - ButtonType::B19, // 9 - ButtonType::B14, // 10 - ButtonType::B15, // 11 + ButtonType::B14, // 8 + ButtonType::B15, // 9 + ButtonType::B17, // 10 + ButtonType::B18, // 11 ButtonType::ModeR, // 12 - ButtonType::B16, // 13 + ButtonType::B19, // 13 ButtonType::B20, // 14 - ButtonType::Hat3B, // 15 + ButtonType::Hat3B, // 15 button 23 ButtonType::Hat3U, // 16 - ButtonType::Hat3L, // 17 - ButtonType::Hat3R, // 18 - ButtonType::Hat3D, // 19 - ButtonType::Hat4B, // 20 + ButtonType::Hat3R, // 17 + ButtonType::Hat3D, // 18 + ButtonType::Hat3L, // 19 + ButtonType::Hat4B, // 20 button 24 ButtonType::Hat4U, // 21 - ButtonType::Hat4L, // 22 - ButtonType::Hat4R, // 23 - ButtonType::Hat4D, // 24 + ButtonType::Hat4R, // 22 + ButtonType::Hat4D, // 23 + ButtonType::Hat4L, // 24 ], ]; From 64ddd03ad08476fa4a04d5315197abc4e3b67b46 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sun, 6 Aug 2023 16:39:05 +0200 Subject: [PATCH 12/16] Fixed some bugs --- rp2040/src/main.rs | 43 ++++++++++++++++++++++++++++++------------- 1 file changed, 30 insertions(+), 13 deletions(-) diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index df9d5dd..c131ffe 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -140,10 +140,10 @@ fn main() -> ! { // 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.gp26.into_floating_input(); - let mut adc_pin_left_y = pins.gp27.into_floating_input(); - let mut adc_pin_right_x = pins.gp28.into_floating_input(); - let mut adc_pin_right_y = pins.gp29.into_floating_input(); + 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] = &[ @@ -259,6 +259,22 @@ fn main() -> ! { buttons[index].pressed = *key; } + 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, + ); + } + match usb_hid_joystick.device().write_report(&get_joystick_report( &mut buttons, &mut axis, @@ -354,9 +370,9 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { if fn_l_active && fn_r_active { mode = 3; - } else if fn_l_active { - mode = 2; } else if fn_r_active { + mode = 2; + } else if fn_l_active { mode = 1; } @@ -417,13 +433,14 @@ fn get_joystick_report( } else if mode & 0x10 == 0x10 && (axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 1 || axis[GIMBAL_AXIS_LEFT_Y].fn_mode == 3) { - rz = remap( - axis[GIMBAL_AXIS_LEFT_Y].value, - AXIS_MIN, - AXIS_MAX, - AXIS_CENTER, - AXIS_MIN, - ); + rz = AXIS_MAX + - remap( + axis[GIMBAL_AXIS_LEFT_Y].value, + AXIS_MIN, + AXIS_MAX, + AXIS_CENTER, + AXIS_MAX, + ); } // Right Alt mode active (bit 5) From c31efd0f15e6fc15d08b659f6f8619867735e8e5 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sun, 6 Aug 2023 19:17:33 +0200 Subject: [PATCH 13/16] Added manually calibration values. !!! Needs to be romoved and inplemented with EEPROM !!! --- rp2040/src/main.rs | 48 ++++++++++++++++++++++++++++++---------------- 1 file changed, 31 insertions(+), 17 deletions(-) diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index c131ffe..49187d6 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -60,7 +60,7 @@ pub const GIMBAL_AXIS_LEFT_Y: usize = 1; pub const GIMBAL_AXIS_RIGHT_X: usize = 2; pub const GIMBAL_AXIS_RIGHT_Y: usize = 3; -// Create an instance with suitable settings. +// 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; @@ -69,7 +69,6 @@ pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32; #[derive(Copy, Clone, Default)] pub struct Button { pub pressed: bool, - pub previous_pressed: bool, pub fn_mode: u8, } @@ -94,7 +93,7 @@ impl Default for GimbalAxis { min: AXIS_MIN, center: AXIS_CENTER, fn_mode: 0, - deadzone: (500, 50, 500), + deadzone: (50, 50, 50), expo: 0.2, } } @@ -170,6 +169,19 @@ fn main() -> ! { // Initialize button matrix button_matrix.init_pins(); + // Scan matrix to get initial state + for _ in 0..10 { + button_matrix.scan_matrix(&mut delay); + } + + // Fallback way to enter bootloader + 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); + } + // Configure USB let usb_bus = UsbBusAllocator::new(waveshare_rp2040_zero::hal::usb::UsbBus::new( pac.USBCTRL_REGS, @@ -220,9 +232,24 @@ fn main() -> ! { // 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 = (500, 0, 500); + 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] = [ @@ -232,19 +259,6 @@ fn main() -> ! { DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), ]; - // Scan matrix to get initial state - for _ in 0..10 { - button_matrix.scan_matrix(&mut delay); - } - - // Check if first key is pressed while power on. If yes then enter bootloader - 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); - } - loop { if status_led_count_down.wait().is_ok() { update_status_led(&mut status_led, &mode); From 86d2f8ea95c91a725bbf3e87ef668611cc011548 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sun, 6 Aug 2023 19:18:48 +0200 Subject: [PATCH 14/16] Fixed error --- rp2040/src/main.rs | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 49187d6..8cbafa3 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -169,19 +169,6 @@ fn main() -> ! { // Initialize button matrix button_matrix.init_pins(); - // Scan matrix to get initial state - for _ in 0..10 { - button_matrix.scan_matrix(&mut delay); - } - - // Fallback way to enter bootloader - 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); - } - // Configure USB let usb_bus = UsbBusAllocator::new(waveshare_rp2040_zero::hal::usb::UsbBus::new( pac.USBCTRL_REGS, @@ -259,6 +246,19 @@ fn main() -> ! { DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), ]; + // Scan matrix to get initial state + for _ in 0..10 { + button_matrix.scan_matrix(&mut delay); + } + + // Fallback way to enter bootloader + 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); + } + loop { if status_led_count_down.wait().is_ok() { update_status_led(&mut status_led, &mode); From 2bb2c8c010453f6b0f448b2dcae4daa229949568 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Wed, 9 Aug 2023 18:07:05 +0200 Subject: [PATCH 15/16] Added get_mode to status_led class --- rp2040/src/status_led.rs | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/rp2040/src/status_led.rs b/rp2040/src/status_led.rs index 1f5e03a..65f163f 100644 --- a/rp2040/src/status_led.rs +++ b/rp2040/src/status_led.rs @@ -55,6 +55,7 @@ where { ws2812_direct: Ws2812Direct, state: bool, + mode: StatusMode, } impl Ws2812StatusLed @@ -81,12 +82,19 @@ where // 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 /// @@ -110,6 +118,8 @@ where (0, 10, 10).into(), // Purple ]; + self.mode = mode; + if mode == StatusMode::Warning && !self.state { self.ws2812_direct .write([colors[mode as usize]].iter().copied()) From b907f7d7919f6c8f65c848dc97b2e776b2feee48 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Wed, 9 Aug 2023 18:07:32 +0200 Subject: [PATCH 16/16] Started to add ELRS support. Code cleanup --- rp2040/src/layout.rs | 272 +++++++++++++++++++++++-------------- rp2040/src/main.rs | 315 +++++++++++++++++++++++++++---------------- 2 files changed, 365 insertions(+), 222 deletions(-) diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index 3a5c061..ef43076 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -6,8 +6,41 @@ use crate::NUMBER_OF_BUTTONS; -#[derive(Debug, PartialEq, Copy, Clone)] -pub enum ButtonType { +#[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, @@ -54,7 +87,7 @@ pub enum ButtonType { Hat4B = 43, NoEventIndicated = 44, } - +#[warn(dead_code)] // Button index map: // -------------------------------------------------------------- // | 0 | 1 | | 3 | 4 | (2) @@ -73,125 +106,156 @@ pub enum ButtonType { /// 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 MAP: [[ButtonType; NUMBER_OF_BUTTONS]; 4] = [ +pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ [ // Function layer 0 // HID Key // Button Index // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B1, // 1 - ButtonType::NoEventIndicated, // 2 Not connected to any button - ButtonType::FnR, // 3 - ButtonType::B6, // 4 - ButtonType::B2, // 5 - ButtonType::B3, // 6 - ButtonType::ModeL, // 7 - ButtonType::B4, // 8 - ButtonType::B5, // 9 - ButtonType::B7, // 10 - ButtonType::B8, // 11 - ButtonType::ModeR, // 12 - ButtonType::B9, // 13 - ButtonType::B10, // 14 - ButtonType::Hat1B, // 15 button 21 - ButtonType::Hat1U, // 16 - ButtonType::Hat1R, // 17 - ButtonType::Hat1D, // 18 - ButtonType::Hat1L, // 19 - ButtonType::Hat2B, // 20 button 22 - ButtonType::Hat2U, // 21 - ButtonType::Hat2R, // 22 - ButtonType::Hat2D, // 23 - ButtonType::Hat2L, // 24 + 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 // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B11, // 1 - ButtonType::NoEventIndicated, // 2 Not connected to any button - ButtonType::FnR, // 3 - ButtonType::B6, // 4 - ButtonType::B12, // 5 - ButtonType::B13, // 6 - ButtonType::ModeL, // 7 - ButtonType::B14, // 8 - ButtonType::B15, // 9 - ButtonType::B7, // 10 - ButtonType::B8, // 11 - ButtonType::ModeR, // 12 - ButtonType::B9, // 13 - ButtonType::B10, // 14 - ButtonType::Hat3B, // 15 button 23 - ButtonType::Hat3U, // 16 - ButtonType::Hat3R, // 17 - ButtonType::Hat3D, // 18 - ButtonType::Hat3L, // 19 - ButtonType::Hat2B, // 20 button 22 - ButtonType::Hat2U, // 21 - ButtonType::Hat2R, // 22 - ButtonType::Hat2D, // 23 - ButtonType::Hat2L, // 24 + 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 // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B1, // 1 - ButtonType::NoEventIndicated, // 2 Not connected to any button - ButtonType::FnR, // 3 - ButtonType::B16, // 4 - ButtonType::B2, // 5 - ButtonType::B3, // 6 - ButtonType::ModeL, // 7 - ButtonType::B4, // 8 - ButtonType::B5, // 9 - ButtonType::B17, // 10 - ButtonType::B18, // 11 - ButtonType::ModeR, // 12 - ButtonType::B19, // 13 - ButtonType::B20, // 14 - ButtonType::Hat1B, // 15 button 21 - ButtonType::Hat1U, // 16 - ButtonType::Hat1R, // 17 - ButtonType::Hat1D, // 18 - ButtonType::Hat1L, // 19 - ButtonType::Hat4B, // 20 button 24 - ButtonType::Hat4U, // 21 - ButtonType::Hat4R, // 22 - ButtonType::Hat4D, // 23 - ButtonType::Hat4L, // 24 + 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 // ----------------------------------------- - ButtonType::FnL, // 0 - ButtonType::B11, // 1 - ButtonType::NoEventIndicated, // 2 Not connected to any button - ButtonType::FnR, // 3 - ButtonType::B16, // 4 - ButtonType::B12, // 5 - ButtonType::B13, // 6 - ButtonType::ModeL, // 7 - ButtonType::B14, // 8 - ButtonType::B15, // 9 - ButtonType::B17, // 10 - ButtonType::B18, // 11 - ButtonType::ModeR, // 12 - ButtonType::B19, // 13 - ButtonType::B20, // 14 - ButtonType::Hat3B, // 15 button 23 - ButtonType::Hat3U, // 16 - ButtonType::Hat3R, // 17 - ButtonType::Hat3D, // 18 - ButtonType::Hat3L, // 19 - ButtonType::Hat4B, // 20 button 24 - ButtonType::Hat4U, // 21 - ButtonType::Hat4R, // 22 - ButtonType::Hat4D, // 23 - ButtonType::Hat4L, // 24 + HidButton::FnL, // 0 + HidButton::B11, // 1 + HidButton::NoEventIndicated, // 2 Not connected to any button + HidButton::FnR, // 3 + HidButton::B16, // 4 + HidButton::B12, // 5 + HidButton::B13, // 6 + HidButton::ModeL, // 7 + HidButton::B14, // 8 + HidButton::B15, // 9 + HidButton::B17, // 10 + HidButton::B18, // 11 + HidButton::ModeR, // 12 + HidButton::B19, // 13 + HidButton::B20, // 14 + HidButton::Hat3B, // 15 button 23 + HidButton::Hat3U, // 16 + HidButton::Hat3R, // 17 + HidButton::Hat3D, // 18 + HidButton::Hat3L, // 19 + HidButton::Hat4B, // 20 button 24 + HidButton::Hat4U, // 21 + HidButton::Hat4R, // 22 + HidButton::Hat4D, // 23 + HidButton::Hat4L, // 24 ], ]; + +pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ + // Function layer 0 + // HID Key // Button Index + // ----------------------------------------- + ElrsButton::CH7OFF, // 0 + ElrsButton::CH7ON, // 1 + ElrsButton::NoEventIndicated, // 2 Not connected to any button + ElrsButton::CH8OFF, // 3 + ElrsButton::CH8ON, // 4 + ElrsButton::CH9ON, // 5 + ElrsButton::CH9OFF, // 6 + ElrsButton::CH5, // 7 + ElrsButton::CH10ON, // 8 + ElrsButton::CH10OFF, // 9 + ElrsButton::CH11ON, // 10 + ElrsButton::CH11OFF, // 11 + ElrsButton::CH6, // 12 + ElrsButton::CH12ON, // 13 + ElrsButton::CH12OFF, // 14 + ElrsButton::NoEventIndicated, // 15 + ElrsButton::NoEventIndicated, // 16 + ElrsButton::NoEventIndicated, // 17 + ElrsButton::NoEventIndicated, // 18 + ElrsButton::NoEventIndicated, // 19 + ElrsButton::NoEventIndicated, // 20 + ElrsButton::NoEventIndicated, // 21 + ElrsButton::NoEventIndicated, // 22 + ElrsButton::NoEventIndicated, // 23 + ElrsButton::NoEventIndicated, // 24 +]; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 8cbafa3..057eb15 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -69,12 +69,14 @@ pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32; #[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, @@ -88,6 +90,7 @@ 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, @@ -169,25 +172,6 @@ fn main() -> ! { // Initialize button matrix button_matrix.init_pins(); - // 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(); - // Create status LED let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS); let mut status_led = Ws2812StatusLed::new( @@ -197,9 +181,21 @@ fn main() -> ! { clocks.peripheral_clock.freq(), ); - // Create timers/delays - let timer = Timer::new(pac.TIMER, &mut pac.RESETS); + // 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()); @@ -208,12 +204,16 @@ fn main() -> ! { scan_count_down.start(1.millis()); let mut status_led_count_down = timer.count_down(); - status_led_count_down.start(250.millis()); + status_led_count_down.start(50.millis()); + + let mut elrs_count_down = timer.count_down(); + elrs_count_down.start(1660u32.micros()); - // Create variable to track modes let mut mode: u8 = 0; - - // Create joystick button/axis array + 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]; @@ -246,62 +246,41 @@ fn main() -> ! { DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY), ]; - // Scan matrix to get initial state - for _ in 0..10 { - button_matrix.scan_matrix(&mut delay); - } + // 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, + )); - // Fallback way to enter bootloader - 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); - } + 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 { - if status_led_count_down.wait().is_ok() { - update_status_led(&mut status_led, &mode); - } - - if usb_hid_report_count_down.wait().is_ok() { - let pressed_keys = button_matrix.buttons_pressed(); - - mode = get_mode(pressed_keys); - - for (index, key) in pressed_keys.iter().enumerate() { - buttons[index].pressed = *key; - } - - 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, - ); - } - - 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) - } - }; + // 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); @@ -324,32 +303,148 @@ fn main() -> ! { 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]) {} + 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 /// -/// Normal = green (NORMAL) -/// Left Alt mode = blue (GUI LOCK) +/// USB mode = green +/// ELRS mode = orange +/// Activity = flashing blue /// Error = steady red (ERROR) /// /// # Arguments /// * `status_led` - Reference to status LED -/// * `caps_lock_active` - Is capslock active -fn update_status_led(status_led: &mut Ws2812StatusLed, fn_mode: &u8) -where +/// * `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 *fn_mode & 0x10 == 0x10 { + 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 { + } 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); } } @@ -368,16 +463,16 @@ fn get_mode(pressed_keys: [bool; NUMBER_OF_BUTTONS]) -> u8 { let mut alt_r_active: bool = false; for (index, key) in pressed_keys.iter().enumerate() { - if *key && layout::MAP[0][index] == layout::ButtonType::FnL { + if *key && layout::HID_MAP[0][index] == layout::HidButton::FnL { fn_l_active = true; } - if *key && layout::MAP[0][index] == layout::ButtonType::FnR { + if *key && layout::HID_MAP[0][index] == layout::HidButton::FnR { fn_r_active = true; } - if *key && layout::MAP[0][index] == layout::ButtonType::ModeL { + if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeL { alt_l_active = true; } - if *key && layout::MAP[0][index] == layout::ButtonType::ModeR { + if *key && layout::HID_MAP[0][index] == layout::HidButton::ModeR { alt_r_active = true; } } @@ -423,14 +518,6 @@ fn get_joystick_report( let mut ry: u16 = AXIS_CENTER; let mut rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value; - // Update Fn mode for all axis that are in idle position - // This is to avoid the Fn mode switching when moving the gimbal - for item in axis.iter_mut() { - if item.value == item.idle_value { - item.fn_mode = mode & 0x0F; - } - } - // 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) @@ -472,14 +559,6 @@ fn get_joystick_report( ry = axis[GIMBAL_AXIS_RIGHT_Y].value; } - // Set fn mode for all keys taht are in idle position - // This is to avoid the Fn mode switching when using a button - for key in matrix_keys.iter_mut() { - if !key.pressed { - key.fn_mode = mode & 0x0F; - } - } - // Generate array for all four hat switches with following structure: // * bit 1: Up // * bit 2: Right @@ -491,18 +570,16 @@ fn get_joystick_report( let mut hats: [u8; 4] = [0; 4]; for (index, key) in matrix_keys.iter_mut().enumerate() { if key.pressed - && layout::MAP[key.fn_mode as usize][index] as usize - >= layout::ButtonType::Hat1U as usize - && layout::MAP[key.fn_mode as usize][index] as usize - <= layout::ButtonType::Hat4B as usize + && 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::MAP[key.fn_mode as usize][index] as usize - - layout::ButtonType::Hat1U as usize) + hats[(layout::HID_MAP[key.fn_mode as usize][index] as usize + - layout::HidButton::Hat1U as usize) / 5] |= 1 - << ((layout::MAP[key.fn_mode as usize][index] as usize - - layout::ButtonType::Hat1U as usize) - - (5 * ((layout::MAP[key.fn_mode as usize][index] as usize - - layout::ButtonType::Hat1U as usize) + << ((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))); } } @@ -522,10 +599,12 @@ fn get_joystick_report( // Update button state for joystick button 1-20 for (index, key) in matrix_keys.iter_mut().enumerate() { if key.pressed - && layout::MAP[key.fn_mode as usize][index] as usize >= layout::ButtonType::B1 as usize - && layout::MAP[key.fn_mode as usize][index] as usize <= layout::ButtonType::B20 as usize + && 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::MAP[key.fn_mode as usize][index] as usize; + buttons |= 1 << layout::HID_MAP[key.fn_mode as usize][index] as usize; } }