From ad82a25329fd9bca3173a09180fc8aa71b63b46f Mon Sep 17 00:00:00 2001 From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com> Date: Wed, 5 Mar 2025 22:30:18 -0500 Subject: [PATCH] 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 | 2 +- northbridge/src/framed_codec.rs | 22 +++++++++++++++++++--- northbridge/src/main.rs | 6 +----- southbridge/src/main.rs | 15 +++++++++++---- 4 files changed, 32 insertions(+), 13 deletions(-) diff --git a/common/src/lib.rs b/common/src/lib.rs index 41635ab..027c3a7 100644 --- a/common/src/lib.rs +++ b/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) } } } diff --git a/northbridge/src/framed_codec.rs b/northbridge/src/framed_codec.rs index fcc7f34..a0692d5 100644 --- a/northbridge/src/framed_codec.rs +++ b/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); + } + } } }; diff --git a/northbridge/src/main.rs b/northbridge/src/main.rs index f0d4125..7bd84e8 100644 --- a/northbridge/src/main.rs +++ b/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 { diff --git a/southbridge/src/main.rs b/southbridge/src/main.rs index 4dbd9ad..1d7bb5d 100644 --- a/southbridge/src/main.rs +++ b/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; } }