send telemetry over UART

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,
+    pub enabled: bool,
-    sensor_data: Option<SensorData>,
+    pub sensor_data: Option<SensorData>,
-    uptime_micros: u64,
+    pub uptime_micros: u64,
 }
 
-#[derive(Serialize, Deserialize)]
+#[derive(Serialize, Deserialize, Debug)]
 pub enum SensorData {
     /// degrees per second
     Gyro(Vector3<f32>),

southbridge/Cargo.lock

@@ -1073,6 +1073,9 @@ name = "portable-atomic"
 version = "1.10.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "280dc24453071f1b63954171985a0b0d30058d287960968b9b2aca264c8d4ee6"
+dependencies = [
+ "critical-section",
+]
 
 [[package]]
 name = "postcard"
@@ -1365,11 +1368,14 @@ 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",
+ "portable-atomic",
  "postcard",
  "serde",
+ "static_cell",
 ]
 
 [[package]]
@@ -1397,6 +1403,15 @@ version = "1.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3"
 
+[[package]]
+name = "static_cell"
+version = "2.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d89b0684884a883431282db1e4343f34afc2ff6996fe1f4a1664519b66e14c1e"
+dependencies = [
+ "portable-atomic",
+]
+
 [[package]]
 name = "string_cache"
 version = "0.8.7"

southbridge/Cargo.toml

@@ -28,3 +28,6 @@ mpu6050 = { git = "https://git.ank.dev/ank/mpu6050" }
 nalgebra = { version = "0.31.2", default-features=false, features = ["serde-serialize-no-std"] }
 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"

southbridge/src/main.rs

@@ -1,21 +1,33 @@
 #![no_std]
 #![no_main]
 
-use core::panic::PanicInfo;
+use core::{panic::PanicInfo, sync::atomic::Ordering};
 
+use common::{Command, Response, SensorData};
 use embassy_executor::Spawner;
-use embassy_rp::{bind_interrupts, peripherals::USB, pwm::{self, Pwm}, usb::Driver};
+use embassy_rp::{bind_interrupts, peripherals::{UART0, UART1, USB}, pwm::{self, Pwm}, uart::{BufferedInterruptHandler, BufferedUart, BufferedUartRx, BufferedUartTx, Config}, usb::Driver};
-use embassy_time::{Duration, Timer};
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel};
-use log::{error, info};
+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};
+use nalgebra::clamp;
+use portable_atomic::AtomicBool;
+use static_cell::{ConstStaticCell, StaticCell};
 
 bind_interrupts!(struct Irqs {
-    I2C0_IRQ => embassy_rp::i2c::InterruptHandler<embassy_rp::peripherals::I2C0>;
+    I2C1_IRQ => embassy_rp::i2c::InterruptHandler<embassy_rp::peripherals::I2C1>;
     USBCTRL_IRQ => embassy_rp::usb::InterruptHandler<USB>;
+    UART1_IRQ => BufferedInterruptHandler<UART1>;
 });
 
+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>) {
-    embassy_usb_logger::run!(1024, log::LevelFilter::Debug, driver);
+    embassy_usb_logger::run!(1024, log::LevelFilter::Trace, driver);
 }
 
 #[panic_handler]
@@ -36,13 +48,144 @@ async fn main(spawner: Spawner) {
     drive_conf.top = 62500; // 20ms
     drive_conf.compare_b = 4687; // 1.5ms
     drive_conf.compare_a = 4687; // 1.5ms
-    let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, drive_conf.clone());
+    let stopped = drive_conf.clone();
+    let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_16, p.PIN_17, drive_conf.clone());
 
     let config = embassy_rp::i2c::Config::default();
-    let bus = embassy_rp::i2c::I2c::new_async(p.I2C0, p.PIN_21, p.PIN_20, Irqs, config);
+    let bus = embassy_rp::i2c::I2c::new_async(p.I2C1, p.PIN_19, p.PIN_18, Irqs, config);
+
+    static TX_BUF: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0u8;1024]);
+    let tx_buf = TX_BUF.take();
+    static RX_BUF: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0u8;1024]);
+    let rx_buf = RX_BUF.take();
+
+    let uart = BufferedUart::new(p.UART1, Irqs, p.PIN_20, p.PIN_21, tx_buf, rx_buf, Config::default());
+
+    let (tx,rx) = uart.split();
+
+    spawner.spawn(decoder(rx)).unwrap();
+
+    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
+            drive_pwm.set_config(&stopped);
+        }
+
+        let command = if enabled.load(Ordering::Acquire) { 
+            let Ok(command) = with_deadline(enable_watchdog + enable_watchdog_time, COMMANDS.receive()).await else {
+                warn!("no message received");
+                enabled.store(false, Ordering::Release);
+                continue;
+            };
+            command
+        } else {
+            COMMANDS.receive().await
+        };
+
+        match command {
+            Command::Twist(forward, right) => {
+                drive_conf.compare_a = calc_speed(forward - right);
+                drive_conf.compare_b = calc_speed(forward + right);
+                drive_pwm.set_config(&drive_conf);
+            },
+            Command::Stop => { 
+                drive_pwm.set_config(&stopped);
+            },
+            Command::Enable => {
+                enabled.store(true, Ordering::Release);
+                enable_watchdog = Instant::now();
+            },
+            Command::Disable => {
+                enabled.store(false, Ordering::Release);
+                drive_pwm.set_config(&stopped);
+            },
+            Command::FeedWatchdog => {
+                enable_watchdog = Instant::now();
+            }
+            c => { error!("{c:?} unimplemented") }
+        };
+
         info!("Blink");
         Timer::after(Duration::from_millis(100)).await;
     }
 }
+
+/// Receive data from the pi 0 over UART and deserialize it
+#[embassy_executor::task]
+async fn decoder(mut rx: BufferedUartRx<'static, UART1>) {
+    info!("Reading...");
+    let mut codec = framed::bytes::Config::default().to_codec();
+
+    loop {
+        let data = rx.fill_buf().await.unwrap();
+        let Some(end) =  data.iter().position(|e| *e == FRAME_END_SYMBOL) else { continue; };
+        let data = &data[..end];
+
+        let mut buf = [0u8; 1024];
+        let Ok(len) = codec.decode_to_slice(data, &mut buf) else {
+            error!("frame decode fail");
+            continue;
+        };
+
+        let Ok(data) = postcard::from_bytes::<Command>(&buf[..len]) else {
+            error!("message decode fail");
+            continue;
+        };
+
+        trace!("received {data:?}");
+
+        COMMANDS.send(data).await;
+
+        rx.consume(end);
+    }
+}
+
+/// 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.;
+    let speed = speed.clamp(-1., 1.);
+
+    let ms = (speed/2.0)+1.5;
+
+    (COUNTS_PER_MS * ms) as u16
+}