cm/cmdr-joystick
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ELRS read update

Browse Source
This commit is contained in:
Christoffer Martinsson 2023-08-30 17:15:30 +02:00
parent f2f40a2a7e
commit 1c8229a949
2 changed files with 20 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -2,3 +2,4 @@ firmware/.cache/clangd/index
firmware/compile_commands.json firmware/compile_commands.json
rp2040/target rp2040/target
rp2040/Cargo.lock rp2040/Cargo.lock
firmware/.pio/build

34
rp2040/src/elrs.rs
View File

@ -137,37 +137,41 @@ where
} }
pub fn check_link(&mut self) { pub fn check_link(&mut self) {
let mut buffer: [u8; 640] = [0; 640]; if self.elrs_connected_timeout == 0 {
// let _ = self.uart.read_raw(&mut buffer); 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; let mut crc: u8 = 0;
for byte in buffer.iter() { for i in 0..bytes_to_read {
if *byte == 0xEA && !self.rx_sync { if tmp_buffer[i] == 0xEA && !self.rx_sync {
self.rx_buffer_index = 0; self.rx_buffer_index = 0;
self.rx_package_size = 0; self.rx_package_size = 0;
self.rx_sync = true; self.rx_sync = true;
} else if self.rx_sync && self.rx_package_size == 0 { } else if self.rx_sync && self.rx_package_size == 0 {
self.rx_package_size = *byte as usize; self.rx_package_size = tmp_buffer[i] as usize;
} else if self.rx_sync && self.rx_buffer_index < self.rx_package_size { } else if self.rx_sync && self.rx_buffer_index < self.rx_package_size {
self.rx_buffer[self.rx_buffer_index] = *byte; self.rx_buffer[self.rx_buffer_index] = tmp_buffer[i];
self.rx_buffer_index += 1; self.rx_buffer_index += 1;
} else if self.rx_buffer_index == self.rx_package_size { } else if self.rx_buffer_index == self.rx_package_size {
for i in 0..self.rx_package_size { for j in 0..self.rx_package_size {
crc = CRSF_CRC8TAB[(crc ^ self.rx_buffer[i]) as usize]; crc = CRSF_CRC8TAB[(crc ^ self.rx_buffer[j]) as usize];
} }
if crc == *byte { if crc == tmp_buffer[i] {
self.elrs_connected = true; self.elrs_connected = true;
self.elrs_connected_timeout = 500; self.elrs_connected_timeout = 500;
} }
self.rx_sync = false; self.rx_sync = false;
} }
} }
if self.elrs_connected_timeout == 0 {
self.elrs_connected = false;
} else {
self.elrs_connected_timeout -= 1;
}
} }
pub fn reset(&mut self) { pub fn reset(&mut self) {