Added support for 2 extra buttons (on TX/RX). Removed ELRS support. Extended HID to 32 buttons.
This commit is contained in:
parent
bb9e160e08
commit
00a2e9cbdf
@ -1,194 +0,0 @@
|
||||
//! Project: CMtec CMDR joystick 24
|
||||
//! Date: 2023-08-01
|
||||
//! Author: Christoffer Martinsson
|
||||
//! Email: cm@cmtec.se
|
||||
//! License: Please refer to LICENSE in root directory
|
||||
|
||||
use embedded_hal::serial::Read;
|
||||
use rp2040_hal::uart::{Enabled, State, UartDevice, UartPeripheral, ValidUartPinout};
|
||||
|
||||
const CRSF_CRC8TAB: [u8; 256] = [
|
||||
0x00, 0xD5, 0x7F, 0xAA, 0xFE, 0x2B, 0x81, 0x54, 0x29, 0xFC, 0x56, 0x83, 0xD7, 0x02, 0xA8, 0x7D,
|
||||
0x52, 0x87, 0x2D, 0xF8, 0xAC, 0x79, 0xD3, 0x06, 0x7B, 0xAE, 0x04, 0xD1, 0x85, 0x50, 0xFA, 0x2F,
|
||||
0xA4, 0x71, 0xDB, 0x0E, 0x5A, 0x8F, 0x25, 0xF0, 0x8D, 0x58, 0xF2, 0x27, 0x73, 0xA6, 0x0C, 0xD9,
|
||||
0xF6, 0x23, 0x89, 0x5C, 0x08, 0xDD, 0x77, 0xA2, 0xDF, 0x0A, 0xA0, 0x75, 0x21, 0xF4, 0x5E, 0x8B,
|
||||
0x9D, 0x48, 0xE2, 0x37, 0x63, 0xB6, 0x1C, 0xC9, 0xB4, 0x61, 0xCB, 0x1E, 0x4A, 0x9F, 0x35, 0xE0,
|
||||
0xCF, 0x1A, 0xB0, 0x65, 0x31, 0xE4, 0x4E, 0x9B, 0xE6, 0x33, 0x99, 0x4C, 0x18, 0xCD, 0x67, 0xB2,
|
||||
0x39, 0xEC, 0x46, 0x93, 0xC7, 0x12, 0xB8, 0x6D, 0x10, 0xC5, 0x6F, 0xBA, 0xEE, 0x3B, 0x91, 0x44,
|
||||
0x6B, 0xBE, 0x14, 0xC1, 0x95, 0x40, 0xEA, 0x3F, 0x42, 0x97, 0x3D, 0xE8, 0xBC, 0x69, 0xC3, 0x16,
|
||||
0xEF, 0x3A, 0x90, 0x45, 0x11, 0xC4, 0x6E, 0xBB, 0xC6, 0x13, 0xB9, 0x6C, 0x38, 0xED, 0x47, 0x92,
|
||||
0xBD, 0x68, 0xC2, 0x17, 0x43, 0x96, 0x3C, 0xE9, 0x94, 0x41, 0xEB, 0x3E, 0x6A, 0xBF, 0x15, 0xC0,
|
||||
0x4B, 0x9E, 0x34, 0xE1, 0xB5, 0x60, 0xCA, 0x1F, 0x62, 0xB7, 0x1D, 0xC8, 0x9C, 0x49, 0xE3, 0x36,
|
||||
0x19, 0xCC, 0x66, 0xB3, 0xE7, 0x32, 0x98, 0x4D, 0x30, 0xE5, 0x4F, 0x9A, 0xCE, 0x1B, 0xB1, 0x64,
|
||||
0x72, 0xA7, 0x0D, 0xD8, 0x8C, 0x59, 0xF3, 0x26, 0x5B, 0x8E, 0x24, 0xF1, 0xA5, 0x70, 0xDA, 0x0F,
|
||||
0x20, 0xF5, 0x5F, 0x8A, 0xDE, 0x0B, 0xA1, 0x74, 0x09, 0xDC, 0x76, 0xA3, 0xF7, 0x22, 0x88, 0x5D,
|
||||
0xD6, 0x03, 0xA9, 0x7C, 0x28, 0xFD, 0x57, 0x82, 0xFF, 0x2A, 0x80, 0x55, 0x01, 0xD4, 0x7E, 0xAB,
|
||||
0x84, 0x51, 0xFB, 0x2E, 0x7A, 0xAF, 0x05, 0xD0, 0xAD, 0x78, 0xD2, 0x07, 0x53, 0x86, 0x2C, 0xF9,
|
||||
];
|
||||
|
||||
const CONNECTION_TIMEOUT: u16 = 500;
|
||||
const INIT_TIMEOUT: u16 = 1000;
|
||||
|
||||
pub struct Elrs<S: State, D: UartDevice, P>
|
||||
where
|
||||
S: State,
|
||||
D: UartDevice,
|
||||
P: ValidUartPinout<D>,
|
||||
{
|
||||
uart: UartPeripheral<S, D, P>,
|
||||
elsr_init_done: bool,
|
||||
elrs_init_counter: u16,
|
||||
elrs_connected: bool,
|
||||
elrs_connected_timeout: u16,
|
||||
rx_buffer_index: usize,
|
||||
}
|
||||
|
||||
impl<S, D, P> Elrs<S, D, P>
|
||||
where
|
||||
S: State,
|
||||
D: UartDevice,
|
||||
P: ValidUartPinout<D>,
|
||||
{
|
||||
pub fn new(uart: UartPeripheral<S, D, P>) -> Self {
|
||||
let elsr_init_done = false;
|
||||
let elrs_init_counter = 0;
|
||||
let elrs_connected = false;
|
||||
let elrs_connected_timeout = CONNECTION_TIMEOUT;
|
||||
let rx_buffer_index = 0;
|
||||
Self {
|
||||
uart,
|
||||
elsr_init_done,
|
||||
elrs_init_counter,
|
||||
elrs_connected,
|
||||
elrs_connected_timeout,
|
||||
rx_buffer_index,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<D, P> Elrs<Enabled, D, P>
|
||||
where
|
||||
D: UartDevice,
|
||||
P: ValidUartPinout<D>,
|
||||
{
|
||||
pub fn send(&mut self, data: [u16; 12]) {
|
||||
if self.elsr_init_done {
|
||||
self.uart
|
||||
.write_full_blocking(&self.prepare_crsf_data_packet(data));
|
||||
return;
|
||||
}
|
||||
if self.elrs_init_counter < INIT_TIMEOUT {
|
||||
self.uart
|
||||
.write_full_blocking(&self.prepare_crsf_data_packet(data));
|
||||
self.elrs_init_counter += 1;
|
||||
} else if self.elrs_init_counter < INIT_TIMEOUT + 5 {
|
||||
self.uart
|
||||
// Setting Packet Rate to 150Hz
|
||||
// 0 = 50Hz LoRa, 1 = 100Hz Full, 2 = 150Hz LoRa, 3 = 250Hz LoRa,
|
||||
// 4 = 333H Full, 5 = 500Hz LoRa, 6 = 250Hz DejaVu, 7 = 500Hz DejaVu,
|
||||
// 8 = 500Hz FLRC, 9 = 1000Hz FLRC
|
||||
.write_full_blocking(&self.prepare_crsf_cmd_packet(0x01, 0x00));
|
||||
self.elrs_init_counter += 1;
|
||||
} else if self.elrs_init_counter < INIT_TIMEOUT + 10 {
|
||||
self.uart
|
||||
// Setting Power to 10mW
|
||||
// 0 = 10mW, 1 = 25mW, 2 = 50mW, 3 = 100mW,
|
||||
// 4 = 200mW, 5 = 500mW, 6 = 1000mW, 7 = 2000mW
|
||||
.write_full_blocking(&self.prepare_crsf_cmd_packet(0x06, 0x00));
|
||||
self.elrs_init_counter += 1;
|
||||
} else {
|
||||
self.elsr_init_done = true;
|
||||
}
|
||||
|
||||
self.check_link_frame();
|
||||
}
|
||||
|
||||
pub fn connected(&self) -> bool {
|
||||
self.elrs_connected
|
||||
}
|
||||
|
||||
pub fn reset(&mut self) {
|
||||
self.elsr_init_done = false;
|
||||
self.elrs_init_counter = 0;
|
||||
}
|
||||
|
||||
fn check_link_frame(&mut self) {
|
||||
if self.elrs_connected_timeout == 0 {
|
||||
self.elrs_connected = false;
|
||||
} else {
|
||||
self.elrs_connected_timeout -= 1;
|
||||
}
|
||||
|
||||
let mut rx_byte: u8;
|
||||
|
||||
while self.uart.uart_is_readable() {
|
||||
match self.uart.read() {
|
||||
Ok(byte) => {
|
||||
rx_byte = byte;
|
||||
}
|
||||
Err(_) => {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// Simple RX telemetry link frame detection: 0xEA, 0xXX, 0x14
|
||||
if (rx_byte == 0xEA && self.rx_buffer_index == 0) || self.rx_buffer_index == 1 {
|
||||
self.rx_buffer_index += 1;
|
||||
} else if rx_byte == 0x14 && self.rx_buffer_index == 2 {
|
||||
self.elrs_connected = true;
|
||||
self.elrs_connected_timeout = CONNECTION_TIMEOUT;
|
||||
self.rx_buffer_index = 0;
|
||||
} else {
|
||||
self.rx_buffer_index = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn prepare_crsf_data_packet(&self, data: [u16; 12]) -> [u8; 26] {
|
||||
let mut packet: [u8; 26] = [0; 26];
|
||||
let mut crc: u8 = 0;
|
||||
packet[0] = 0xEE;
|
||||
packet[1] = 0x18;
|
||||
packet[2] = 0x16;
|
||||
packet[3] = data[0] as u8;
|
||||
packet[4] = ((data[0] >> 8) | (data[1] << 3)) as u8;
|
||||
packet[5] = ((data[1] >> 5) | (data[2] << 6)) as u8;
|
||||
packet[6] = (data[2] >> 2) as u8;
|
||||
packet[7] = ((data[2] >> 10) | (data[3] << 1)) as u8;
|
||||
packet[8] = ((data[3] >> 7) | (data[4] << 4)) as u8;
|
||||
packet[9] = ((data[4] >> 4) | (data[5] << 7)) as u8;
|
||||
packet[10] = (data[5] >> 1) as u8;
|
||||
packet[11] = ((data[5] >> 9) | (data[6] << 2)) as u8;
|
||||
packet[12] = ((data[6] >> 6) | (data[7] << 5)) as u8;
|
||||
packet[13] = (data[7] >> 3) as u8;
|
||||
packet[14] = data[8] as u8;
|
||||
packet[15] = ((data[8] >> 8) | (data[9] << 3)) as u8;
|
||||
packet[16] = ((data[9] >> 5) | (data[10] << 6)) as u8;
|
||||
packet[17] = (data[10] >> 2) as u8;
|
||||
packet[18] = ((data[10] >> 10) | (data[11] << 1)) as u8;
|
||||
packet[19] = (data[11] >> 7) as u8;
|
||||
// Channel 13-16 are not used in ELRS
|
||||
for i in 2..25 {
|
||||
crc = CRSF_CRC8TAB[(crc ^ packet[i]) as usize];
|
||||
}
|
||||
packet[25] = crc;
|
||||
packet
|
||||
}
|
||||
|
||||
fn prepare_crsf_cmd_packet(&self, command: u8, value: u8) -> [u8; 8] {
|
||||
let mut packet: [u8; 8] = [0; 8];
|
||||
let mut crc: u8 = 0;
|
||||
packet[0] = 0xEE;
|
||||
packet[1] = 0x06;
|
||||
packet[2] = 0x2D;
|
||||
packet[3] = 0xEE;
|
||||
packet[4] = 0xEA;
|
||||
packet[5] = command;
|
||||
packet[6] = value;
|
||||
for i in 2..7 {
|
||||
crc = CRSF_CRC8TAB[(crc ^ packet[i]) as usize];
|
||||
}
|
||||
packet[7] = crc;
|
||||
packet
|
||||
}
|
||||
}
|
||||
@ -6,7 +6,7 @@
|
||||
//
|
||||
// HW Button index map:
|
||||
// ---------------------------------------------------------------
|
||||
// | 0 L| 1 U| | 2 | | 3 L| 4 U|
|
||||
// | 0 L| 1 U| 25 U | | 2 | | 26 U | 4 U| 3 L|
|
||||
// ---------------------------------------------------------------
|
||||
// | | 5 | 6 | 7 | | 12 | 11 | 10 | |
|
||||
// | |
|
||||
@ -18,11 +18,11 @@
|
||||
// | | 18 | | 23 | |
|
||||
// ---------------------------------------------------------------
|
||||
//
|
||||
// USB HID joystick map (Enabled by pressing HW button 2+4):
|
||||
// USB HID joystick map :
|
||||
// ---------------------------------------------------------------
|
||||
// | B1 L| B2 U| | B3 | | B4 L| B5 U|
|
||||
// | Fn L| B2 U| B26 U| | B3 | | B28 U| B5 U| B4 L|
|
||||
// ---------------------------------------------------------------
|
||||
// | | B6 | B7 | B8/16| |B13/17| B12 | B11 | |
|
||||
// | | TH | B6 | B8/16| |B13/17| B12 | B11 | |
|
||||
// | |
|
||||
// | | B9 | | B14 | |
|
||||
// | | B10 | | B15 | |
|
||||
@ -31,34 +31,26 @@
|
||||
// | | H1L | B18 | H1R | | H2L | B19 | H2R | |
|
||||
// | | H1D | | H2D | |
|
||||
// ---------------------------------------------------------------
|
||||
// Press and hold HW button 0 (B1) changes following:
|
||||
// * B2 => B21
|
||||
// * B4 => B22
|
||||
// * B11 => B23
|
||||
// * B12 => B24
|
||||
// * hat1 => hat3 (button press B20).
|
||||
// * hat2 => hat4 (button bpress B25)
|
||||
//
|
||||
// ELRS channel map (Enabled by pressing HW button 2+3):
|
||||
// (+ = ON, - = OFF, CHxP/M/Z = trim)
|
||||
// USB HID joystick map (Fn):
|
||||
// ---------------------------------------------------------------
|
||||
// |CH7-L|CH7+U| | - | |CH8-L|CH8+U|
|
||||
// | Fn L| B21 U| B27 U| | B3 | | B28 U| B5 U| B22 L|
|
||||
// ---------------------------------------------------------------
|
||||
// | | THL | CH9 | CH5 | | CH6 |CH11-|CH11+| |
|
||||
// | | TH | B7 | B8/16| |B13/17| B24 | B23 | |
|
||||
// | |
|
||||
// | |CH10+| |CH12+| |
|
||||
// | |CH10-| |CH12-| |
|
||||
// | CH1/CH2 CH3/CH4 |
|
||||
// | | - | |CH4P | |
|
||||
// | |CH1M |CH12Z|CH1P | |CH3M |CH34Z|CH3P | |
|
||||
// | | - | |CH4M | |
|
||||
// | | B29 | | B31 | |
|
||||
// | | B30 | | B32 | |
|
||||
// | X1/Y1 X2/Y2 |
|
||||
// | | H3U | | H4U | |
|
||||
// | | H3L | B20 | H3R | | H4L | B25 | H4R | |
|
||||
// | | H3D | | H4D | |
|
||||
// ---------------------------------------------------------------
|
||||
//
|
||||
// Config Layer (holding CONFIG button)
|
||||
// ---------------------------------------------------------------
|
||||
// |BOOT L| CAL U| | CONFIG | | USB L|ELRS U|
|
||||
// |BOOT L| CAL U| | CONFIG | | - | - |
|
||||
// ---------------------------------------------------------------
|
||||
// | | THL-| THL+| - | | - | - | - | |
|
||||
// | | - | - | - | | - | - | - | |
|
||||
// | |
|
||||
// | | - | | - | |
|
||||
// | | - | | - | |
|
||||
@ -72,7 +64,6 @@
|
||||
#![no_main]
|
||||
|
||||
mod button_matrix;
|
||||
mod elrs;
|
||||
mod status_led;
|
||||
mod usb_joystick_device;
|
||||
|
||||
@ -81,7 +72,6 @@ use core::convert::Infallible;
|
||||
use cortex_m::delay::Delay;
|
||||
use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS};
|
||||
use eeprom24x::{Eeprom24x, SlaveAddr};
|
||||
use elrs::Elrs;
|
||||
use embedded_hal::adc::OneShot;
|
||||
use embedded_hal::digital::v2::*;
|
||||
use embedded_hal::timer::CountDown;
|
||||
@ -90,10 +80,9 @@ use libm::powf;
|
||||
use panic_halt as _;
|
||||
use rp2040_hal::{
|
||||
adc::Adc,
|
||||
gpio::{Function, FunctionConfig, FunctionUart, PinId, ValidPinMode},
|
||||
gpio::{Function, FunctionConfig, PinId, ValidPinMode},
|
||||
i2c::I2C,
|
||||
pio::StateMachineIndex,
|
||||
uart::{DataBits, StopBits, UartConfig, UartPeripheral},
|
||||
};
|
||||
use status_led::{StatusMode, Ws2812StatusLed};
|
||||
use usb_device::class_prelude::*;
|
||||
@ -122,10 +111,6 @@ pub const AXIS_MIN: u16 = 0;
|
||||
pub const AXIS_MAX: u16 = 4095;
|
||||
pub const AXIS_CENTER: u16 = (AXIS_MIN + AXIS_MAX) / 2;
|
||||
|
||||
pub const ELRS_MIN: u16 = 172;
|
||||
pub const ELRS_MAX: u16 = 1811;
|
||||
pub const ELRS_CENTER: u16 = (ELRS_MIN + ELRS_MAX) / 2;
|
||||
|
||||
pub const NBR_OF_GIMBAL_AXIS: usize = 4;
|
||||
pub const GIMBAL_AXIS_LEFT_X: usize = 0;
|
||||
pub const GIMBAL_AXIS_LEFT_Y: usize = 1;
|
||||
@ -157,12 +142,6 @@ pub struct Button {
|
||||
pub usb_button_sec_pressed: bool,
|
||||
pub usb_button_toggle_enable: bool,
|
||||
pub usb_release_timeout: u16,
|
||||
pub elrs_changed: bool,
|
||||
pub elrs_changed_to_pressed: bool,
|
||||
pub elrs_channel: usize,
|
||||
pub elrs_lock_enable: bool,
|
||||
pub elrs_lock_state_on: bool,
|
||||
pub elrs_lock_state: bool,
|
||||
}
|
||||
|
||||
#[derive(Copy, Clone)]
|
||||
@ -192,7 +171,7 @@ impl Default for GimbalAxis {
|
||||
deadzone: (100, 50, 100),
|
||||
expo: true,
|
||||
trim: 0,
|
||||
hold: 0,
|
||||
hold: AXIS_CENTER,
|
||||
hold_pending: false,
|
||||
}
|
||||
}
|
||||
@ -232,19 +211,6 @@ fn main() -> ! {
|
||||
&mut pac.RESETS,
|
||||
);
|
||||
|
||||
// Set up UART on GP0 and GP1 (Pico pins 1 and 2)
|
||||
let uart_pins = (
|
||||
pins.gp0.into_mode::<FunctionUart>(),
|
||||
pins.gp1.into_mode::<FunctionUart>(),
|
||||
);
|
||||
|
||||
let elrs_uart = UartPeripheral::new(pac.UART0, uart_pins, &mut pac.RESETS)
|
||||
.enable(
|
||||
UartConfig::new(400000.Hz(), DataBits::Eight, None, StopBits::One),
|
||||
clocks.peripheral_clock.freq(),
|
||||
)
|
||||
.unwrap();
|
||||
|
||||
let i2c = I2C::i2c1(
|
||||
pac.I2C1,
|
||||
pins.gp14.into_mode(), // sda
|
||||
@ -286,9 +252,6 @@ fn main() -> ! {
|
||||
&mut pins.gp13.into_push_pull_output(),
|
||||
];
|
||||
|
||||
let mut elrs_en_pin = pins.gp2.into_push_pull_output();
|
||||
let mut elrs = Elrs::new(elrs_uart);
|
||||
|
||||
// Create button matrix object that scans all buttons
|
||||
let mut button_matrix: ButtonMatrix<BUTTON_ROWS, BUTTON_COLS, NUMBER_OF_BUTTONS> =
|
||||
ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, DEBOUNCE);
|
||||
@ -296,6 +259,10 @@ fn main() -> ! {
|
||||
// Initialize button matrix
|
||||
button_matrix.init_pins();
|
||||
|
||||
// Setup extra buttons (connected to TX/RX pins)
|
||||
let left_extra_button = pins.gp1.into_pull_up_input();
|
||||
let right_extra_button = pins.gp0.into_pull_up_input();
|
||||
|
||||
// Create status LED
|
||||
let (mut pio, sm0, _, _, _) = pac.PIO0.split(&mut pac.RESETS);
|
||||
let mut status_led = Ws2812StatusLed::new(
|
||||
@ -331,26 +298,16 @@ fn main() -> ! {
|
||||
let mut data_process_count_down = timer.count_down();
|
||||
data_process_count_down.start(1200u32.micros());
|
||||
|
||||
let mut elrs_start_count_down = timer.count_down();
|
||||
elrs_start_count_down.start(2000.millis());
|
||||
|
||||
let mut elrs_update_count_down = timer.count_down();
|
||||
elrs_update_count_down.start(1666u32.micros());
|
||||
|
||||
let mut usb_update_count_down = timer.count_down();
|
||||
usb_update_count_down.start(10.millis());
|
||||
|
||||
let mut safety_check: bool = false;
|
||||
let mut usb_activity: bool = false;
|
||||
let mut idle: bool = false;
|
||||
let mut usb_active: bool = false;
|
||||
let mut elrs_active: bool = false;
|
||||
let _elrs_connected: bool = false;
|
||||
let mut calibration_active: bool = false;
|
||||
let mut throttle_hold_enable: bool = false;
|
||||
|
||||
let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS];
|
||||
let mut buttons: [Button; NUMBER_OF_BUTTONS] = [Button::default(); NUMBER_OF_BUTTONS];
|
||||
let mut buttons: [Button; NUMBER_OF_BUTTONS + 2] = [Button::default(); NUMBER_OF_BUTTONS + 2];
|
||||
let mut gimbal_mode: u8;
|
||||
|
||||
// Set up usb button layout
|
||||
@ -365,7 +322,7 @@ fn main() -> ! {
|
||||
buttons[3].usb_button_sec = 22;
|
||||
buttons[3].usb_button_sec_trigger_index = 0;
|
||||
buttons[4].usb_button = 5;
|
||||
buttons[5].usb_button = 6;
|
||||
buttons[5].usb_button = 0;
|
||||
buttons[6].usb_button = 7;
|
||||
buttons[7].usb_button = 8;
|
||||
buttons[7].usb_button_sec = 16;
|
||||
@ -393,36 +350,11 @@ fn main() -> ! {
|
||||
buttons[20].usb_button_sec_enable = true;
|
||||
buttons[20].usb_button_sec = 25;
|
||||
buttons[20].usb_button_sec_trigger_index = 0;
|
||||
|
||||
// Set up elrs button layout
|
||||
buttons[0].elrs_channel = 7;
|
||||
buttons[0].elrs_lock_enable = true;
|
||||
buttons[1].elrs_channel = 7;
|
||||
buttons[1].elrs_lock_enable = true;
|
||||
buttons[1].elrs_lock_state_on = true;
|
||||
buttons[3].elrs_channel = 8;
|
||||
buttons[3].elrs_lock_enable = true;
|
||||
buttons[4].elrs_channel = 8;
|
||||
buttons[4].elrs_lock_enable = true;
|
||||
buttons[4].elrs_lock_state_on = true;
|
||||
buttons[6].elrs_channel = 9;
|
||||
buttons[7].elrs_channel = 5;
|
||||
buttons[8].elrs_channel = 10;
|
||||
buttons[8].elrs_lock_enable = true;
|
||||
buttons[9].elrs_channel = 10;
|
||||
buttons[9].elrs_lock_enable = true;
|
||||
buttons[9].elrs_lock_state_on = true;
|
||||
buttons[10].elrs_channel = 11;
|
||||
buttons[10].elrs_lock_enable = true;
|
||||
buttons[11].elrs_channel = 11;
|
||||
buttons[11].elrs_lock_enable = true;
|
||||
buttons[11].elrs_lock_state_on = true;
|
||||
buttons[12].elrs_channel = 6;
|
||||
buttons[13].elrs_channel = 12;
|
||||
buttons[13].elrs_lock_enable = true;
|
||||
buttons[14].elrs_channel = 12;
|
||||
buttons[14].elrs_lock_enable = true;
|
||||
buttons[14].elrs_lock_state_on = true;
|
||||
buttons[25].usb_button = 26;
|
||||
buttons[25].usb_button_sec_enable = true;
|
||||
buttons[25].usb_button_sec = 27;
|
||||
buttons[25].usb_button_sec_trigger_index = 0;
|
||||
buttons[26].usb_button = 28;
|
||||
|
||||
// Table for gimbal expo curve lookup insded of doing floating point math for every analog read
|
||||
let expo_lut: [u16; AXIS_MAX as usize + 1] = generate_expo_lut(0.3);
|
||||
@ -502,25 +434,21 @@ fn main() -> ! {
|
||||
update_status_led(
|
||||
&mut status_led,
|
||||
&usb_active,
|
||||
&elrs_active,
|
||||
&elrs.connected(),
|
||||
&safety_check,
|
||||
&calibration_active,
|
||||
&throttle_hold_enable,
|
||||
);
|
||||
}
|
||||
|
||||
// Check if all axis are in idle position and no buttons are pressed
|
||||
if idle && !safety_check && elrs_active {
|
||||
safety_check = true;
|
||||
}
|
||||
|
||||
if data_process_count_down.wait().is_ok() {
|
||||
// Update pressed keys status
|
||||
for (index, key) in button_matrix.buttons_pressed().iter().enumerate() {
|
||||
buttons[index].pressed = *key;
|
||||
}
|
||||
|
||||
// Updated extra buttons
|
||||
buttons[25].pressed = left_extra_button.is_low().unwrap();
|
||||
buttons[26].pressed = right_extra_button.is_low().unwrap();
|
||||
|
||||
// Secondary way to enter bootloader (pressing all left hands buttons except the hat
|
||||
if buttons[0].pressed && buttons[2].pressed {
|
||||
status_led.update(StatusMode::Bootloader);
|
||||
@ -532,21 +460,8 @@ fn main() -> ! {
|
||||
);
|
||||
}
|
||||
|
||||
// ON/OFF switch for ELRS radio
|
||||
if buttons[4].pressed && buttons[2].pressed && !elrs_active {
|
||||
safety_check = false;
|
||||
elrs_active = true;
|
||||
} else if buttons[3].pressed && buttons[2].pressed && elrs_active {
|
||||
elrs_active = false;
|
||||
}
|
||||
|
||||
// ON/OFF switch for Throttle hold mode
|
||||
if buttons[6].pressed && buttons[2].pressed && !throttle_hold_enable {
|
||||
throttle_hold_enable = true;
|
||||
axis[GIMBAL_AXIS_LEFT_Y].hold = 0;
|
||||
} else if buttons[5].pressed && buttons[2].pressed && throttle_hold_enable {
|
||||
throttle_hold_enable = false;
|
||||
}
|
||||
// // ON/OFF switch for Throttle hold mode
|
||||
throttle_hold_enable = axis[GIMBAL_AXIS_LEFT_Y].hold != AXIS_CENTER;
|
||||
|
||||
// Calibration of center position (pressing all right hands buttons except
|
||||
// the hat switch)
|
||||
@ -615,6 +530,7 @@ fn main() -> ! {
|
||||
}
|
||||
|
||||
// Process throttle hold value
|
||||
let unprocessed_value = axis[GIMBAL_AXIS_LEFT_Y].value;
|
||||
if throttle_hold_enable
|
||||
&& axis[GIMBAL_AXIS_LEFT_Y].value < AXIS_CENTER
|
||||
&& !axis[GIMBAL_AXIS_LEFT_Y].hold_pending
|
||||
@ -644,13 +560,6 @@ fn main() -> ! {
|
||||
axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold;
|
||||
}
|
||||
|
||||
// Set idle state
|
||||
for key in buttons.iter_mut() {
|
||||
if key.pressed {
|
||||
idle = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Generate led activity when gimbal is moved from idle position
|
||||
for item in axis.iter_mut() {
|
||||
if item.value != item.previous_value {
|
||||
@ -671,18 +580,23 @@ fn main() -> ! {
|
||||
if key.pressed != key.previous_pressed {
|
||||
key.usb_changed = true;
|
||||
key.usb_changed_to_pressed = key.pressed;
|
||||
key.elrs_changed = true;
|
||||
key.elrs_changed_to_pressed = key.pressed;
|
||||
usb_activity = true;
|
||||
}
|
||||
// Set throttle_hold_value
|
||||
if key.pressed != key.previous_pressed
|
||||
&& key.pressed
|
||||
&& throttle_hold_enable
|
||||
&& index == 5
|
||||
&& unprocessed_value != AXIS_CENTER
|
||||
{
|
||||
axis[GIMBAL_AXIS_LEFT_Y].hold = axis[GIMBAL_AXIS_LEFT_Y].value;
|
||||
axis[GIMBAL_AXIS_LEFT_Y].hold_pending = true;
|
||||
} else if key.pressed != key.previous_pressed
|
||||
&& key.pressed
|
||||
&& index == 5
|
||||
&& unprocessed_value == AXIS_CENTER
|
||||
{
|
||||
axis[GIMBAL_AXIS_LEFT_Y].hold = AXIS_CENTER;
|
||||
axis[GIMBAL_AXIS_LEFT_Y].hold_pending = true;
|
||||
}
|
||||
key.previous_pressed = key.pressed;
|
||||
}
|
||||
@ -692,20 +606,6 @@ fn main() -> ! {
|
||||
for axis in axis.iter_mut() {
|
||||
axis.hold = 0;
|
||||
}
|
||||
for button in buttons.iter_mut() {
|
||||
button.elrs_lock_state = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if elrs_update_count_down.wait().is_ok() {
|
||||
// Send ELRS data
|
||||
if elrs_active {
|
||||
elrs_en_pin.set_high().unwrap();
|
||||
elrs.send(get_elrs_channels(&mut buttons, &mut axis));
|
||||
} else {
|
||||
elrs_en_pin.set_low().unwrap();
|
||||
elrs.reset();
|
||||
}
|
||||
}
|
||||
|
||||
@ -732,7 +632,6 @@ fn main() -> ! {
|
||||
/// Update status LED colour based on function layer and capslock
|
||||
///
|
||||
/// USB mode = green
|
||||
/// ELRS mode = orange
|
||||
/// Activity = flashing blue
|
||||
/// Error = steady red (ERROR)
|
||||
///
|
||||
@ -745,9 +644,6 @@ fn main() -> ! {
|
||||
fn update_status_led<P, SM, I>(
|
||||
status_led: &mut Ws2812StatusLed<P, SM, I>,
|
||||
usb_active: &bool,
|
||||
elrs_active: &bool,
|
||||
elrs_connected: &bool,
|
||||
safety_check: &bool,
|
||||
calibration_active: &bool,
|
||||
throttle_hold_enable: &bool,
|
||||
) where
|
||||
@ -757,19 +653,13 @@ fn update_status_led<P, SM, I>(
|
||||
SM: StateMachineIndex,
|
||||
{
|
||||
if *calibration_active {
|
||||
status_led.update(StatusMode::ActivityFlash);
|
||||
} else if *elrs_active && !*safety_check {
|
||||
status_led.update(StatusMode::Warning);
|
||||
} else if !*usb_active && !*elrs_active {
|
||||
} else if !*usb_active {
|
||||
status_led.update(StatusMode::NormalFlash);
|
||||
} else if *usb_active && !*elrs_active && *throttle_hold_enable {
|
||||
status_led.update(StatusMode::Activity);
|
||||
} else if *usb_active && !*elrs_active && !*throttle_hold_enable {
|
||||
status_led.update(StatusMode::Normal);
|
||||
} else if *elrs_active && *elrs_connected {
|
||||
} else if *usb_active && *throttle_hold_enable {
|
||||
status_led.update(StatusMode::Other);
|
||||
} else if *elrs_active && !*elrs_connected {
|
||||
status_led.update(StatusMode::OtherFlash);
|
||||
} else if *usb_active && !*throttle_hold_enable {
|
||||
status_led.update(StatusMode::Normal);
|
||||
}
|
||||
}
|
||||
|
||||
@ -781,16 +671,16 @@ fn update_status_led<P, SM, I>(
|
||||
/// * `matrix_keys` - Array of pressed keys
|
||||
/// * `axis` - Array of joystick axis values
|
||||
fn get_joystick_report(
|
||||
matrix_keys: &mut [Button; NUMBER_OF_BUTTONS],
|
||||
matrix_keys: &mut [Button; NUMBER_OF_BUTTONS + 2],
|
||||
axis: &mut [GimbalAxis; 4],
|
||||
throttle_hold_enable: &bool,
|
||||
) -> JoystickReport {
|
||||
let x: u16 = axis[GIMBAL_AXIS_RIGHT_X].value;
|
||||
let y: u16 = AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value;
|
||||
let z: u16 = axis[GIMBAL_AXIS_LEFT_X].value;
|
||||
let rx: u16 = AXIS_CENTER;
|
||||
let x: u16 = axis[GIMBAL_AXIS_LEFT_X].value;
|
||||
let y: u16 = AXIS_MAX - axis[GIMBAL_AXIS_LEFT_Y].value;
|
||||
let z: u16 = axis[GIMBAL_AXIS_RIGHT_X].value;
|
||||
let rx: u16 = AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value;
|
||||
let ry: u16 = AXIS_CENTER;
|
||||
let rz: u16 = axis[GIMBAL_AXIS_LEFT_Y].value;
|
||||
let rz: u16 = AXIS_CENTER;
|
||||
let mut hat1: u8 = 0xf;
|
||||
let mut hat2: u8 = 0xf;
|
||||
let mut hat3: u8 = 0xf;
|
||||
@ -858,7 +748,7 @@ fn get_joystick_report(
|
||||
|
||||
// Update button array with Sec button trigger status
|
||||
// Using indexing instead of iterating to be able to iterate inside loop
|
||||
for index in 0..NUMBER_OF_BUTTONS {
|
||||
for index in 0..NUMBER_OF_BUTTONS + 2 {
|
||||
let mut sec_button_pressed: bool = false;
|
||||
for (sec_index, sec_key) in matrix_keys.iter().enumerate() {
|
||||
if matrix_keys[index].usb_button_sec_enable
|
||||
@ -1073,147 +963,3 @@ fn generate_expo_lut(expo: f32) -> [u16; AXIS_MAX as usize + 1] {
|
||||
}
|
||||
lut
|
||||
}
|
||||
|
||||
/// Get ELRS channel values
|
||||
///
|
||||
/// # Arguments
|
||||
/// * `matrix_keys` - Array of buttons
|
||||
/// * `axis` - Array of axis
|
||||
fn get_elrs_channels(
|
||||
matrix_keys: &mut [Button; NUMBER_OF_BUTTONS],
|
||||
axis: &mut [GimbalAxis; 4],
|
||||
) -> [u16; 12] {
|
||||
let mut channels: [u16; 12] = [ELRS_MIN; 12];
|
||||
|
||||
// Check and store trim values
|
||||
for (index, key) in matrix_keys.iter_mut().enumerate() {
|
||||
// Left gimbal X
|
||||
if key.pressed && index == 17 && axis[GIMBAL_AXIS_LEFT_X].trim < ELRS_CENTER as i16 {
|
||||
axis[GIMBAL_AXIS_LEFT_X].trim += 1;
|
||||
} else if key.pressed
|
||||
&& index == 19
|
||||
&& axis[GIMBAL_AXIS_LEFT_X].trim > (0 - ELRS_CENTER as i16)
|
||||
{
|
||||
axis[GIMBAL_AXIS_LEFT_X].trim -= 1;
|
||||
// Left gimbal Y
|
||||
} else if key.pressed && index == 16 && axis[GIMBAL_AXIS_LEFT_Y].trim < ELRS_CENTER as i16 {
|
||||
axis[GIMBAL_AXIS_LEFT_Y].trim += 1;
|
||||
} else if key.pressed
|
||||
&& index == 18
|
||||
&& axis[GIMBAL_AXIS_LEFT_Y].trim > (0 - ELRS_CENTER as i16)
|
||||
{
|
||||
axis[GIMBAL_AXIS_LEFT_Y].trim -= 1;
|
||||
// Right gimbal X
|
||||
} else if key.pressed && index == 22 && axis[GIMBAL_AXIS_RIGHT_X].trim < ELRS_CENTER as i16
|
||||
{
|
||||
axis[GIMBAL_AXIS_RIGHT_X].trim += 1;
|
||||
} else if key.pressed
|
||||
&& index == 24
|
||||
&& axis[GIMBAL_AXIS_RIGHT_X].trim > (0 - ELRS_CENTER as i16)
|
||||
{
|
||||
axis[GIMBAL_AXIS_RIGHT_X].trim -= 1;
|
||||
// Right gimbal Y
|
||||
} else if key.pressed && index == 21 && axis[GIMBAL_AXIS_RIGHT_Y].trim < ELRS_CENTER as i16
|
||||
{
|
||||
axis[GIMBAL_AXIS_RIGHT_Y].trim += 1;
|
||||
} else if key.pressed
|
||||
&& index == 23
|
||||
&& axis[GIMBAL_AXIS_RIGHT_Y].trim > (0 - ELRS_CENTER as i16)
|
||||
{
|
||||
axis[GIMBAL_AXIS_RIGHT_Y].trim -= 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Alpine hat switch filter
|
||||
let mut hat_left_button_only: bool = true;
|
||||
let mut hat_right_button_only: bool = true;
|
||||
for (index, key) in matrix_keys.iter_mut().enumerate() {
|
||||
if key.pressed && (16..=19).contains(&index) {
|
||||
hat_left_button_only = false;
|
||||
}
|
||||
if key.pressed && (21..=24).contains(&index) {
|
||||
hat_right_button_only = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Check and reset trim values
|
||||
for (index, key) in matrix_keys.iter_mut().enumerate() {
|
||||
if key.elrs_changed && key.elrs_changed_to_pressed && index == 15 && hat_left_button_only {
|
||||
axis[GIMBAL_AXIS_LEFT_X].trim = 0;
|
||||
axis[GIMBAL_AXIS_LEFT_Y].trim = 0;
|
||||
} else if key.elrs_changed
|
||||
&& key.elrs_changed_to_pressed
|
||||
&& index == 20
|
||||
&& hat_right_button_only
|
||||
{
|
||||
axis[GIMBAL_AXIS_RIGHT_X].trim = 0;
|
||||
axis[GIMBAL_AXIS_RIGHT_Y].trim = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Match ELRS channel 1-4 to new min/max values
|
||||
for (index, item) in axis.iter_mut().enumerate() {
|
||||
channels[index] = remap(item.value, AXIS_MIN, AXIS_MAX, ELRS_MIN, ELRS_MAX);
|
||||
}
|
||||
|
||||
// Apply trim to ELRS channel 1,3,4
|
||||
for (index, item) in axis.iter().enumerate() {
|
||||
if index != GIMBAL_AXIS_LEFT_Y && channels[index] > ELRS_CENTER {
|
||||
channels[index] = remap(
|
||||
channels[index],
|
||||
ELRS_CENTER,
|
||||
ELRS_MAX,
|
||||
(ELRS_CENTER as i16 + item.trim) as u16,
|
||||
ELRS_MAX,
|
||||
);
|
||||
} else if index != GIMBAL_AXIS_LEFT_Y && channels[index] < ELRS_CENTER {
|
||||
channels[index] = remap(
|
||||
channels[index],
|
||||
ELRS_MIN,
|
||||
ELRS_CENTER,
|
||||
ELRS_MIN,
|
||||
(ELRS_CENTER as i16 + item.trim) as u16,
|
||||
);
|
||||
} else if index != GIMBAL_AXIS_LEFT_Y {
|
||||
channels[index] = (ELRS_CENTER as i16 + item.trim) as u16;
|
||||
}
|
||||
}
|
||||
|
||||
// Update button state for ELRS channels
|
||||
for key in matrix_keys.iter_mut() {
|
||||
if key.elrs_changed
|
||||
&& key.elrs_changed_to_pressed
|
||||
&& key.elrs_lock_enable
|
||||
&& key.elrs_lock_state_on
|
||||
&& key.elrs_channel != 0
|
||||
{
|
||||
key.elrs_lock_state = true;
|
||||
} else if key.elrs_changed
|
||||
&& !key.elrs_changed_to_pressed
|
||||
&& key.elrs_lock_enable
|
||||
&& !key.elrs_lock_state_on
|
||||
&& key.elrs_channel != 0
|
||||
{
|
||||
key.elrs_lock_state = false;
|
||||
} else if key.pressed && !key.elrs_lock_enable && key.elrs_channel != 0 {
|
||||
channels[key.elrs_channel] = ELRS_MAX;
|
||||
}
|
||||
}
|
||||
|
||||
// Apply locking to ELRS channels
|
||||
for key in matrix_keys.iter_mut() {
|
||||
if key.elrs_lock_enable
|
||||
&& key.elrs_lock_state
|
||||
&& key.elrs_lock_state_on
|
||||
&& key.elrs_channel != 0
|
||||
{
|
||||
channels[key.elrs_channel] = ELRS_MAX;
|
||||
}
|
||||
}
|
||||
|
||||
// Reset changed flags
|
||||
for key in matrix_keys.iter_mut() {
|
||||
key.elrs_changed = false;
|
||||
}
|
||||
channels
|
||||
}
|
||||
|
||||
@ -90,15 +90,12 @@ pub const JOYSTICK_DESCRIPTOR: &[u8] = &[
|
||||
0xc0, // End Collection
|
||||
0x05, 0x09, // Usage Page (Button)
|
||||
0x19, 0x01, // Usage Minimum (1)
|
||||
0x29, 0x19, // Usage Maximum (25)
|
||||
0x29, 0x20, // Usage Maximum (32)
|
||||
0x15, 0x00, // Logical Minimum (0)
|
||||
0x25, 0x01, // Logical Maximum (1)
|
||||
0x75, 0x01, // Report Size (1)
|
||||
0x95, 0x19, // Report Count (25)
|
||||
0x95, 0x20, // Report Count (32)
|
||||
0x81, 0x02, // Input (Data, Variable, Absolute)
|
||||
0x75, 0x01, // Report Size (1) PADDING
|
||||
0x95, 0x07, // Report Count (7) PADDING
|
||||
0x81, 0x03, // Input (Const, Variable, Absolute) PADDING
|
||||
0x15, 0x00, // Logical Minimum (0)
|
||||
0x25, 0x07, // Logical Maximum (7)
|
||||
0x35, 0x00, // Physical Minimum (0)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user