From f16ecb30dfee34fc13af537a910907e3c7b7b5d7 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Mon, 18 Sep 2023 07:22:11 +0200 Subject: [PATCH] More updates to ELRS implementation --- rp2040/src/elrs.rs | 125 ++++++++++++++------------------------- rp2040/src/layout.rs | 90 ++++++++++++++++++++++++---- rp2040/src/main.rs | 1 - rp2040/src/status_led.rs | 3 + 4 files changed, 125 insertions(+), 94 deletions(-) diff --git a/rp2040/src/elrs.rs b/rp2040/src/elrs.rs index f6433bd..4d08f81 100644 --- a/rp2040/src/elrs.rs +++ b/rp2040/src/elrs.rs @@ -4,6 +4,7 @@ //! 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] = [ @@ -25,6 +26,9 @@ const CRSF_CRC8TAB: [u8; 256] = [ 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 where S: State, @@ -36,20 +40,7 @@ where elrs_init_counter: u16, elrs_connected: bool, elrs_connected_timeout: u16, - uplink_rssi_1: u8, - uplink_rssi_2: u8, - uplink_package_success: u8, - uplink_snr: i8, - diversity: u8, - rf_mode: u8, - uplink_tx_power: u8, - downlink_rssi: u8, - downlink_package_success: u8, - downlink_snr: i8, - rx_buffer: [u8; 64], rx_buffer_index: usize, - rx_sync: bool, - rx_package_size: usize, } impl Elrs @@ -62,41 +53,15 @@ where let elsr_init_done = false; let elrs_init_counter = 0; let elrs_connected = false; - let elrs_connected_timeout = 500; - let uplink_rssi_1 = 0; - let uplink_rssi_2 = 0; - let uplink_package_success = 0; - let uplink_snr = 0; - let diversity = 0; - let rf_mode = 0; - let uplink_tx_power = 0; - let downlink_rssi = 0; - let downlink_package_success = 0; - let downlink_snr = 0; - let rx_buffer = [0; 64]; + let elrs_connected_timeout = CONNECTION_TIMEOUT; let rx_buffer_index = 0; - let rx_sync = false; - let rx_package_size = 0; Self { uart, elsr_init_done, elrs_init_counter, elrs_connected, elrs_connected_timeout, - uplink_rssi_1, - uplink_rssi_2, - uplink_package_success, - uplink_snr, - diversity, - rf_mode, - uplink_tx_power, - downlink_rssi, - downlink_package_success, - downlink_snr, - rx_buffer, rx_buffer_index, - rx_sync, - rx_package_size, } } } @@ -112,19 +77,19 @@ where .write_full_blocking(&self.prepare_crsf_data_packet(data)); return; } - if self.elrs_init_counter < 5000 { + 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 < 5005 { + } 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, 0x02)); + .write_full_blocking(&self.prepare_crsf_cmd_packet(0x01, 0x00)); self.elrs_init_counter += 1; - } else if self.elrs_init_counter < 5010 { + } else if self.elrs_init_counter < INIT_TIMEOUT + 10 { self.uart // Setting Power to 10mW // 0 = 10mW, 1 = 25mW, 2 = 50mW, 3 = 100mW, @@ -134,44 +99,12 @@ where } else { self.elsr_init_done = true; } + + self.check_link_frame(); } - pub fn check_link(&mut self) { - if self.elrs_connected_timeout == 0 { - self.elrs_connected = false; - } else { - self.elrs_connected_timeout -= 1; - } - - if !self.uart.uart_is_readable() { - return; - } - - let mut tmp_buffer: [u8; 256] = [0; 256]; - let bytes_to_read = self.uart.read_raw(&mut tmp_buffer).unwrap(); - let mut crc: u8 = 0; - - for i in 0..bytes_to_read { - if tmp_buffer[i] == 0xEA && !self.rx_sync { - self.rx_buffer_index = 0; - self.rx_package_size = 0; - self.rx_sync = true; - } else if self.rx_sync && self.rx_package_size == 0 { - self.rx_package_size = tmp_buffer[i] as usize; - } else if self.rx_sync && self.rx_buffer_index < self.rx_package_size { - self.rx_buffer[self.rx_buffer_index] = tmp_buffer[i]; - self.rx_buffer_index += 1; - } else if self.rx_buffer_index == self.rx_package_size { - for j in 0..self.rx_package_size { - crc = CRSF_CRC8TAB[(crc ^ self.rx_buffer[j]) as usize]; - } - if crc == tmp_buffer[i] { - self.elrs_connected = true; - self.elrs_connected_timeout = 500; - } - self.rx_sync = false; - } - } + pub fn connected(&self) -> bool { + self.elrs_connected } pub fn reset(&mut self) { @@ -179,8 +112,36 @@ where self.elrs_init_counter = 0; } - pub fn connected(&self) -> bool { - self.elrs_connected + 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] { diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index 4749c28..bbb62fb 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -6,6 +6,17 @@ use crate::NUMBER_OF_BUTTONS; +#[allow(dead_code)] +#[derive(Debug, PartialEq, PartialOrd, Copy, Clone)] +pub enum ConfigButton { + CONF = 0, + BOOT = 1, + CAL = 2, + ELRS = 3, + USB = 4, + NoEventIndicated = 5, +} + #[allow(dead_code)] #[derive(Debug, PartialEq, PartialOrd, Copy, Clone)] pub enum ElrsButton { @@ -102,7 +113,7 @@ pub enum HidButton { #[warn(dead_code)] // Button index map: // -------------------------------------------------------------- -// | 0 | 1 | | 2 | | 3 | 4 | +// | 0 L| 1 U| | 2 | | 3 L| 4 U| // --------------------------------------------------------------- // | | 5 | 6 | 7 | | 12 | 11 | 10 | | // | | @@ -138,12 +149,12 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B9, // 13 HidButton::B10, // 14 - HidButton::H1B, // 15 button 22 + HidButton::H1B, // 15 HidButton::H1U, // 16 HidButton::H1R, // 17 HidButton::H1D, // 18 HidButton::H1L, // 19 - HidButton::H2B, // 20 button 23 + HidButton::H2B, // 20 HidButton::H2U, // 21 HidButton::H2R, // 22 HidButton::H2D, // 23 @@ -168,12 +179,12 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B9, // 13 HidButton::B10, // 14 - HidButton::H3B, // 15 button 24 + HidButton::H3B, // 15 HidButton::H3U, // 16 HidButton::H3R, // 17 HidButton::H3D, // 18 HidButton::H3L, // 19 - HidButton::H2B, // 20 button 23 + HidButton::H2B, // 20 HidButton::H2U, // 21 HidButton::H2R, // 22 HidButton::H2D, // 23 @@ -198,12 +209,12 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B19, // 13 HidButton::B20, // 14 - HidButton::H1B, // 15 button 22 + HidButton::H1B, // 15 HidButton::H1U, // 16 HidButton::H1R, // 17 HidButton::H1D, // 18 HidButton::H1L, // 19 - HidButton::H4B, // 20 button 25 + HidButton::H4B, // 20 HidButton::H4U, // 21 HidButton::H4R, // 22 HidButton::H4D, // 23 @@ -228,12 +239,12 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B19, // 13 HidButton::B20, // 14 - HidButton::H3B, // 15 button 24 + HidButton::H3B, // 15 HidButton::H3U, // 16 HidButton::H3R, // 17 HidButton::H3D, // 18 HidButton::H3L, // 19 - HidButton::H4B, // 20 button 25 + HidButton::H4B, // 20 HidButton::H4U, // 21 HidButton::H4R, // 22 HidButton::H4D, // 23 @@ -241,9 +252,22 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ ], ]; +// Button index map: +// -------------------------------------------------------------- +// | 0 L| 1 U| | 2 | | 3 L| 4 U| +// --------------------------------------------------------------- +// | | 5 | 6 | 7 | | 12 | 11 | 10 | | +// | | +// | | 8 | | 13 | | +// | | 9 | | 14 | | +// | X1/Y1 X2/Y2 | +// | | 16 | | 21 | | +// | | 17 | 15 | 18 | | 22 | 20 | 23 | | +// | | 19 | | 24 | | +// --------------------------------------------------------------- +// pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ - // Function layer 0 - // HID Key // Button Index + // ELRS Key // Button Index // ----------------------------------------- ElrsButton::CH7OFF, // 0 ElrsButton::CH7ON, // 1 @@ -271,3 +295,47 @@ pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ ElrsButton::CH4M, // 23 ElrsButton::CH3M, // 24 ]; + +// Button index map: +// -------------------------------------------------------------- +// | 0 L| 1 U| | 2 | | 3 L| 4 U| +// --------------------------------------------------------------- +// | | 5 | 6 | 7 | | 12 | 11 | 10 | | +// | | +// | | 8 | | 13 | | +// | | 9 | | 14 | | +// | X1/Y1 X2/Y2 | +// | | 16 | | 21 | | +// | | 17 | 15 | 18 | | 22 | 20 | 23 | | +// | | 19 | | 24 | | +// --------------------------------------------------------------- +// +pub const CONFIG_MAP: [ConfigButton; NUMBER_OF_BUTTONS] = [ + // Config Key // Button Index + // ----------------------------------------- + ConfigButton::BOOT, // 0 + ConfigButton::CAL, // 1 + ConfigButton::CONF, // 2 + ConfigButton::USB, // 3 + ConfigButton::ELRS, // 4 + ConfigButton::NoEventIndicated, // 5 + ConfigButton::NoEventIndicated, // 6 + ConfigButton::NoEventIndicated, // 7 + ConfigButton::NoEventIndicated, // 8 + ConfigButton::NoEventIndicated, // 9 + ConfigButton::NoEventIndicated, // 10 + ConfigButton::NoEventIndicated, // 11 + ConfigButton::NoEventIndicated, // 12 + ConfigButton::NoEventIndicated, // 13 + ConfigButton::NoEventIndicated, // 14 + ConfigButton::NoEventIndicated, // 15 + ConfigButton::NoEventIndicated, // 16 + ConfigButton::NoEventIndicated, // 17 + ConfigButton::NoEventIndicated, // 18 + ConfigButton::NoEventIndicated, // 19 + ConfigButton::NoEventIndicated, // 20 + ConfigButton::NoEventIndicated, // 21 + ConfigButton::NoEventIndicated, // 22 + ConfigButton::NoEventIndicated, // 23 + ConfigButton::NoEventIndicated, // 24 +]; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index a6b1450..431c15e 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -546,7 +546,6 @@ fn main() -> ! { &mut axis, &mut channel_locks, )); - // elrs.check_link(); } else { elrs_en_pin.set_low().unwrap(); elrs.reset(); diff --git a/rp2040/src/status_led.rs b/rp2040/src/status_led.rs index 5108008..40abfc7 100644 --- a/rp2040/src/status_led.rs +++ b/rp2040/src/status_led.rs @@ -103,8 +103,11 @@ where /// /// * OFF = off /// * NORMAL = green + /// * NORMALFLASH = green (flashing) /// * ACTIVITY = blue + /// * ACTIVITYFLASH = blue (flashing) /// * OTHER = orange + /// * OTHERFLASH = orange (flashing) /// * WARNING = red (flashing) /// * ERROR = red /// * BOOTLOADER = purple