receive telemetry on a separate thread

converter/src/main.rs

@@ -5,35 +5,35 @@ use postcard::to_vec_cobs;
 use heapless::Vec;
 
 fn main() {
-    let mut port = serialport::new("/dev/ttyACM1", 115200)
+    let mut port = serialport::new("/dev/ttyACM0", 115200)
         .timeout(Duration::from_secs(1000))
         .open().expect("port missing");
     println!("connected over serial");
 
-    let command: Arc<Mutex<Command>> = Arc::new(Mutex::new(Command { left: 0.0, right: 0.0, ultrasonic_enable: false}));
+    let command: Arc<Mutex<Command>> = Arc::new(Mutex::new(Command { left: 0.0, right: 0.0, ultrasonic_enable: true}));
     let telemetry: Arc<Mutex<Telemetry>> = Arc::new(Mutex::new(Telemetry { distance: f32::NAN }));
 
+    // LabVIEW communication thread
     {
         let command = command.clone();
         let telemetry = telemetry.clone();
         std::thread::spawn(move || server(command,telemetry));
     }
 
-
+    // Telemetry receive thread
+    {
         let reader = port.try_clone().unwrap();
-        let mut reader = BufReader::new(reader);
+        let reader = BufReader::new(reader);
+        let telemetry = telemetry.clone();
+        std::thread::spawn(move || recv_telem(telemetry, reader));
+    }
+
+    // Command transmit thread (main)
     loop {
         let command = command.lock().unwrap().clone();
         let encoded: Vec<u8, 20> = to_vec_cobs(&command).unwrap();
         port.write(&encoded).expect("port write fail");
-        println!("sending");
 
-        let mut recv_buf = std::vec::Vec::new();
-        let Ok(len) = reader.read_until(0, &mut recv_buf) else {continue;};
-        if let Ok(telem) = postcard::from_bytes_cobs::<Telemetry>(&mut recv_buf[0..len]) {
-            println!("dist: {}", telem.distance);
-            *telemetry.lock().unwrap() = telem;
-        }
         sleep(Duration::from_millis(200));
     }
 }
@@ -43,9 +43,9 @@ fn recv_telem<T: Read>(telemetry: Arc<Mutex<Telemetry>>, mut reader: BufReader<T
     loop {
         let Ok(len) = reader.read_until(0, &mut recv_buf) else {continue;};
         if let Ok(telem) = postcard::from_bytes_cobs::<Telemetry>(&mut recv_buf[0..len]) {
-            println!("dist: {}", telem.distance);
             *telemetry.lock().unwrap() = telem;
         }
+        recv_buf.clear();
     }
 }
 

robot-controller/src/main.rs

@@ -206,20 +206,23 @@ fn main() -> ! {
                 telem.distance = f32::NAN;
             }
 
-            let mut buf = [0;20];
-            let encoded = postcard::to_slice_cobs(&telem, &mut buf).unwrap();
-
-            for word in encoded {
-                let _ = tx.write(*word);
-                let _ = tx.flush();
-                arduino_hal::delay_us(100);
-            }
-
-            arduino_hal::delay_ms(250);
         } else {
-            let _ = tx.write(0);
+            // ultrasonic disabled
+            telem.distance = f32::NAN;
+        }
+
+        let mut buf = [0;20];
+        let encoded = postcard::to_slice_cobs(&telem, &mut buf).unwrap();
+
+        for word in encoded {
+            let _ = tx.write(*word);
             let _ = tx.flush();
-            arduino_hal::delay_us(500);
+            arduino_hal::delay_us(100); // delay to ensure linux can keep up
+        }
+
+        if command.ultrasonic_enable {
+            // wait for the echoes to fade
+            arduino_hal::delay_ms(50);
         }
     }
 }