improved telemetry link

reduced send speed and improved receive logic to recover faster

a next step is to try forgoing tokio-serial and the codec trait
for (hopefully) better real-time performance

common/src/lib.rs

@@ -41,7 +41,7 @@ impl RobotState {
     }
 
     /// Safe command to send in uncertian states
-    pub fn stopped() -> Self {
+    pub const fn stopped() -> Self {
         Self { drive: (0.0, 0.0), underglow: Vector3::new(100, 0, 0) }
     }
 }

northbridge/src/framed_codec.rs

@@ -3,7 +3,7 @@ use std::result;
 use anyhow::{anyhow, Ok};
 use common::{Response, RobotState};
 use framed::{BoxPayload, FRAME_END_SYMBOL};
-use tokio_util::{bytes::BytesMut, codec::{Decoder, Encoder}};
+use tokio_util::{bytes::{Buf, BytesMut}, codec::{Decoder, Encoder}};
 
 pub struct FramedCodec {
     codec: framed::bytes::Codec,
@@ -25,11 +25,27 @@ impl Decoder for FramedCodec {
             return Ok(None);
         }
 
+        let terminator_idx = src.iter().position(|b| *b == 0).unwrap();
+
         let (message, remainder) = match postcard::take_from_bytes_cobs(src) {
             result::Result::Ok(v) => v,
             result::Result::Err(e) => {
-                src.clear();
-                Err(e)?
+                println!("telem err: {e}");
+                // skip to the next terminator and try one more time
+                src.advance(terminator_idx+1);
+                if !src.contains(&0) {
+                    println!("incomplete frame");
+                    return Ok(None);
+                }
+
+                match postcard::take_from_bytes_cobs(src) {
+                    result::Result::Ok(v) => v,
+                    result::Result::Err(e) => {
+                        src.clear();
+                        println!("telem err: {e}, cleared");
+                        return Ok(None);
+                    }
+                }
             }
         };
         

northbridge/src/main.rs

@@ -8,7 +8,7 @@ use crossbeam::queue::ArrayQueue;
 use framed_codec::FramedCodec;
 use futures::{FutureExt, SinkExt, StreamExt, TryStreamExt};
 use nalgebra::{SimdPartialOrd, SimdValue, Vector3};
-use tokio::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, error::RecvError, Receiver, Sender}, watch::{self, channel }, RwLock}, task::JoinHandle, time::{error::Elapsed, timeout}};
+use tokio::{io::{AsyncReadExt, AsyncWriteExt}, net::{TcpListener, TcpSocket}, sync::{broadcast::{self, error::RecvError, Receiver, Sender}, watch::{self, channel }, RwLock}, task::JoinHandle, time::{error::Elapsed, sleep, timeout}};
 use tokio_serial::SerialPortBuilderExt;
 use tokio_util::codec::Framed;
 
@@ -29,10 +29,6 @@ async fn main() -> Result<()> {
 
     let (mut write, mut read) = Framed::new(serial, FramedCodec::new()).split();
 
-    loop {
-        write.send(RobotState::stopped()).await?;
-    }
-
     let (sender, mut command_receiver) = channel(RobotState::stopped());
 
     let state = Arc::new(RwLock::new(RobotInfo {

southbridge/src/main.rs

@@ -86,7 +86,7 @@ async fn main(spawner: Spawner) {
 
     Timer::after_secs(13).await; // pi 0 serial ports act strange during the boot process
 
-    //spawner.spawn(bus_voltage_monitor(p.ADC, p.PIN_28, p.ADC_TEMP_SENSOR)).unwrap();
+    spawner.spawn(bus_voltage_monitor(p.ADC, p.PIN_28, p.ADC_TEMP_SENSOR)).unwrap();
 
     let config = embassy_rp::i2c::Config::default();
     let bus = embassy_rp::i2c::I2c::new_async(p.I2C1, p.PIN_19, p.PIN_18, Irqs, config);
@@ -173,7 +173,7 @@ async fn decoder(mut rx: BufferedUartRx<'static, UART1>) {
 #[embassy_executor::task]
 async fn telemetry(mut tx: BufferedUartTx<'static, UART1>) {
     info!("Telemetry waiting...");
-    //COMMANDS.receiver().unwrap().get().await;
+    COMMANDS.receiver().unwrap().get().await;
     info!("Telemetry started...");
     loop {
         let data = with_timeout(Duration::from_millis(20), SENSOR_DATA.receive()).await.ok();
@@ -221,13 +221,20 @@ async fn bus_voltage_monitor(adc: ADC, bus: PIN_28, temp: ADC_TEMP_SENSOR) {
         (rounded_temp_x10 as f32) / 10.0
     }
 
+    let mut i = 0;
+
     loop {
+        i += 1;
         let level = adc.read(&mut bus_voltage).await.unwrap();
         BUS_VOLTAGE.store(level, Ordering::Release);
         // empirically calculated against $20 microcenter voltmeter (10k & 33k divider circuit)
-        SENSOR_DATA.send(SensorData::BusVoltage(level as f32 / BUS_ADC_TO_VOLTS)).await;
         let temp = adc.read(&mut ts).await.unwrap();
-        SENSOR_DATA.send(SensorData::AmbientTemperature(convert_to_celsius(temp))).await;
+
+        if i % 10 == 0 { // only send telemetry every 30 milliseconds
+            SENSOR_DATA.send(SensorData::BusVoltage(level as f32 / BUS_ADC_TO_VOLTS)).await;
+            SENSOR_DATA.send(SensorData::AmbientTemperature(convert_to_celsius(temp))).await;
+        }
+
         Timer::after_millis(3).await;
     }
 }