From 1c268bba12d226aee7eeaf28486eb03a3a394dc2 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Tue, 4 Feb 2025 13:54:01 -0500
Subject: [PATCH] converted southbridge -> northbridge  to length-data

also divided the pwm clock by ten to align with microseconds
---
 northbridge/src/framed_codec.rs | 33 +++++++++++++++++++--------------
 southbridge/src/main.rs         | 26 ++++++++++----------------
 2 files changed, 29 insertions(+), 30 deletions(-)

diff --git a/northbridge/src/framed_codec.rs b/northbridge/src/framed_codec.rs
index ddd0563..505b8a6 100644
--- a/northbridge/src/framed_codec.rs
+++ b/northbridge/src/framed_codec.rs
@@ -1,4 +1,4 @@
-use anyhow::anyhow;
+use anyhow::{anyhow, Ok};
 use common::Command;
 use framed::{BoxPayload, FRAME_END_SYMBOL};
 use tokio_util::{bytes::BytesMut, codec::{Decoder, Encoder}};
@@ -15,26 +15,31 @@ impl FramedCodec {
 }
 
 impl Decoder for FramedCodec {
-    type Item = BoxPayload;
-    type Error = framed::Error;
+    type Item = Vec<u8>;
+    type Error = anyhow::Error;
 
     fn decode(&mut self, src: &mut BytesMut) -> Result<Option<Self::Item>, Self::Error> {
-
-        match src.iter().position(|e| *e==FRAME_END_SYMBOL) {
-            Some(len) => {
-                let data = src.split_to(len);
-
-                Ok(Some(self.codec.decode_to_box(&data)?))
-            }
-            None => {
-                Ok(None)
-            }
+        if src.len() < 4 {
+            return Ok(None);
         }
+
+        let len = u32::from_be_bytes(src[..4].try_into()?) as usize;
+
+        if src.len() < len + 4 {
+            return Ok(None);
+        }
+
+        let data = src.split_to(len + 4);
+
+        let mut out = Vec::new();
+        out.extend_from_slice(&data[4..]);
+
+        Ok(Some(out))
     }
 }
 
 impl Encoder<Vec<u8>> for FramedCodec {
-    type Error = framed::Error;
+    type Error = anyhow::Error;
 
     fn encode(&mut self, item: Vec<u8>, dst: &mut BytesMut) -> Result<(), Self::Error> {
         dst.extend_from_slice(&item.len().to_be_bytes());
diff --git a/southbridge/src/main.rs b/southbridge/src/main.rs
index 5237ff4..bd24a09 100644
--- a/southbridge/src/main.rs
+++ b/southbridge/src/main.rs
@@ -9,7 +9,6 @@ use embassy_rp::{bind_interrupts, block::ImageDef, gpio::{Level, Output}, periph
 use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel};
 use embassy_time::{with_deadline, with_timeout, Duration, Instant, Timer};
 use embedded_io_async::{BufRead, Read, Write};
-use framed::{bytes, FRAME_END_SYMBOL};
 use log::{error, info, trace, warn};
 use nalgebra::clamp;
 use portable_atomic::AtomicBool;
@@ -51,10 +50,10 @@ async fn main(spawner: Spawner) {
     spawner.spawn(logger_task(driver)).unwrap();
 
     let mut drive_conf: pwm::Config = Default::default();
-    drive_conf.divider = 15.into();
-    drive_conf.top = 50000; // 5ms
-    drive_conf.compare_b = 15000; // 1.5ms
-    drive_conf.compare_a = 15000; // 1.5ms
+    drive_conf.divider = 150.into();
+    drive_conf.top = 20000; // 5ms
+    drive_conf.compare_b = 1000; // 1.5ms
+    drive_conf.compare_a = 1500; // 1.5ms
     let stopped = drive_conf.clone();
     let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_16, p.PIN_17, stopped.clone());
 
@@ -152,9 +151,6 @@ async fn decoder(mut rx: BufferedUartRx<'static, UART1>) {
 /// Receive data from channel and send it over UART
 #[embassy_executor::task]
 async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static AtomicBool) {
-    info!("Reading...");
-    let mut codec = framed::bytes::Config::default().to_codec();
-
     loop {
         let data = with_timeout(Duration::from_millis(20), SENSOR_DATA.receive()).await.ok();
 
@@ -169,13 +165,11 @@ async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static Ato
             continue;
         };
 
-        let mut buf = [0u8; 1024];
-        let Ok(len) = codec.encode_to_slice(&serialized, &mut buf) else {
-            error!("encoding error");
-            continue;
-        };
-
-        if let Err(e) = tx.write_all(&buf[..len]).await {
+        let len = u32::to_be_bytes(serialized.len() as u32);
+        if let Err(e) = tx.write_all(&len).await {
+            error!("transport error {e:?}");
+        }
+        if let Err(e) = tx.write_all(&serialized).await {
             error!("transport error {e:?}");
         }
         tx.flush().await.unwrap();
@@ -184,7 +178,7 @@ async fn telemetry(mut tx: BufferedUartTx<'static, UART1>, enabled: &'static Ato
 
 /// -1 to 1
 fn calc_speed(speed: f32) -> u16 {
-    const COUNTS_PER_MS: f32 = 10000.;
+    const COUNTS_PER_MS: f32 = 1000.;
     let speed = speed.clamp(-1., 1.);
 
     let ms = (speed/2.0)+1.5;