From 0748cb25175e660af523e1e4c2f614f40c464b53 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Thu, 30 Jan 2025 18:40:37 -0500
Subject: [PATCH] send telemetry over UART

---
 common/src/lib.rs       | 12 +++++------
 southbridge/Cargo.lock  |  1 +
 southbridge/Cargo.toml  |  1 +
 southbridge/src/main.rs | 46 +++++++++++++++++++++++++++++++++++++----
 4 files changed, 50 insertions(+), 10 deletions(-)

diff --git a/common/src/lib.rs b/common/src/lib.rs
index 8c77b73..b5b9a36 100644
--- a/common/src/lib.rs
+++ b/common/src/lib.rs
@@ -4,7 +4,7 @@
 use nalgebra::Vector3;
 use serde::{Deserialize, Serialize};
 
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Debug)]
 pub enum Command {
     /// Forward, clockwise
     Twist(f32,f32),
@@ -18,14 +18,14 @@ pub enum Command {
     Disable,
 }
 
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Debug)]
 pub struct Response {
-    enabled: bool,
-    sensor_data: Option<SensorData>,
-    uptime_micros: u64,
+    pub enabled: bool,
+    pub sensor_data: Option<SensorData>,
+    pub uptime_micros: u64,
 }
 
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Debug)]
 pub enum SensorData {
     /// degrees per second
     Gyro(Vector3<f32>),
diff --git a/southbridge/Cargo.lock b/southbridge/Cargo.lock
index f3d03c3..27b61cc 100644
--- a/southbridge/Cargo.lock
+++ b/southbridge/Cargo.lock
@@ -1368,6 +1368,7 @@ dependencies = [
  "embedded-io-async",
  "embedded-storage",
  "framed 0.4.3 (git+https://git.ank.dev/ank/framed-rs)",
+ "heapless 0.8.0",
  "log",
  "mpu6050",
  "nalgebra",
diff --git a/southbridge/Cargo.toml b/southbridge/Cargo.toml
index 240c3a4..6a74c2b 100644
--- a/southbridge/Cargo.toml
+++ b/southbridge/Cargo.toml
@@ -30,3 +30,4 @@ serde = { version = "1.0.203", default-features = false, features = ["derive"] }
 postcard = "1.0.0"
 static_cell = "2.1"
 portable-atomic = { version = "1.5", features = ["critical-section"] }
+heapless = "0.8"
diff --git a/southbridge/src/main.rs b/southbridge/src/main.rs
index 4ddc9a7..1dcb4d2 100644
--- a/southbridge/src/main.rs
+++ b/southbridge/src/main.rs
@@ -3,11 +3,12 @@
 
 use core::{panic::PanicInfo, sync::atomic::Ordering};
 
-use common::Command;
+use common::{Command, Response, SensorData};
 use embassy_executor::Spawner;
-use embassy_rp::{bind_interrupts, peripherals::{UART0, UART1, USB}, pwm::{self, Pwm}, uart::{BufferedInterruptHandler, BufferedUart, BufferedUartRx, Config}, usb::Driver};
+use embassy_rp::{bind_interrupts, peripherals::{UART0, UART1, USB}, pwm::{self, Pwm}, uart::{BufferedInterruptHandler, BufferedUart, BufferedUartRx, BufferedUartTx, Config}, usb::Driver};
 use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel};
 use embassy_time::{with_deadline, with_timeout, Duration, Instant, Timer};
+use embedded_io::Write;
 use embedded_io_async::{BufRead, Read};
 use framed::{bytes, FRAME_END_SYMBOL};
 use log::{error, info, trace, warn};
@@ -22,6 +23,7 @@ bind_interrupts!(struct Irqs {
 });
 
 pub static COMMANDS: Channel<CriticalSectionRawMutex, Command, 5> = Channel::new();
+pub static SENSOR_DATA: Channel<CriticalSectionRawMutex, SensorData, 5> = Channel::new();
 
 #[embassy_executor::task]
 async fn logger_task(driver: Driver<'static, USB>) {
@@ -59,14 +61,18 @@ async fn main(spawner: Spawner) {
 
     let uart = BufferedUart::new(p.UART1, Irqs, p.PIN_20, p.PIN_21, tx_buf, rx_buf, Config::default());
 
-    let (mut tx,rx) = uart.split();
+    let (tx,rx) = uart.split();
 
     spawner.spawn(decoder(rx)).unwrap();
 
-    let enabled = AtomicBool::new(false);
+    static ENABLED: AtomicBool = AtomicBool::new(false);
+    spawner.spawn(telemetry(tx, &ENABLED)).unwrap();
+
+    let enabled = &ENABLED;
     let mut enable_watchdog = Instant::now();
     let enable_watchdog_time = Duration::from_millis(50);
 
+
     loop {
         if !enabled.load(Ordering::Acquire) {
             // stop all motors
@@ -142,6 +148,38 @@ 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();
+
+        let packet = Response {
+            enabled: enabled.load(Ordering::Acquire),
+            sensor_data: data,
+            uptime_micros: Instant::now().as_micros(),
+        };
+
+        let Ok(serialized) = postcard::to_vec::<_, 1024>(&packet) else {
+            error!("serialization error");
+            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]) {
+            error!("transport error {e:?}");
+        }
+    }
+}
+
 /// -1 to 1
 fn calc_speed(speed: f32) -> u16 {
     const COUNTS_PER_MS: f32 = 3125.;