More updates to ELRS implementation
This commit is contained in:
parent
d33cc6a324
commit
f16ecb30df
@ -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<S: State, D: UartDevice, P>
|
||||
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<S, D, P> Elrs<S, D, P>
|
||||
@ -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] {
|
||||
|
||||
@ -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
|
||||
];
|
||||
|
||||
@ -546,7 +546,6 @@ fn main() -> ! {
|
||||
&mut axis,
|
||||
&mut channel_locks,
|
||||
));
|
||||
// elrs.check_link();
|
||||
} else {
|
||||
elrs_en_pin.set_low().unwrap();
|
||||
elrs.reset();
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user