From 1c8229a949cf6363fcb1da6e861e1bdeea7e2a6e Mon Sep 17 00:00:00 2001
From: Christoffer Martinsson <cm@cmtec.se>
Date: Wed, 30 Aug 2023 17:15:30 +0200
Subject: [PATCH] ELRS read update

---
 .gitignore         |  1 +
 rp2040/src/elrs.rs | 34 +++++++++++++++++++---------------
 2 files changed, 20 insertions(+), 15 deletions(-)

diff --git a/.gitignore b/.gitignore
index ca385b8..bf04439 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@ firmware/.cache/clangd/index
 firmware/compile_commands.json
 rp2040/target
 rp2040/Cargo.lock
+firmware/.pio/build
diff --git a/rp2040/src/elrs.rs b/rp2040/src/elrs.rs
index 8f8e571..f6433bd 100644
--- a/rp2040/src/elrs.rs
+++ b/rp2040/src/elrs.rs
@@ -137,37 +137,41 @@ where
     }
 
     pub fn check_link(&mut self) {
-        let mut buffer: [u8; 640] = [0; 640];
-        // let _ = self.uart.read_raw(&mut buffer);
+        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 byte in buffer.iter() {
-            if *byte == 0xEA && !self.rx_sync {
+        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 = *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 {
-                self.rx_buffer[self.rx_buffer_index] = *byte;
+                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 i in 0..self.rx_package_size {
-                    crc = CRSF_CRC8TAB[(crc ^ self.rx_buffer[i]) as usize];
+                for j in 0..self.rx_package_size {
+                    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_timeout = 500;
                 }
                 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) {