tasks: implement a task for writing measurement straems to the SD card

src/bin/main.rs

@@ -75,7 +75,7 @@ async fn main(spawner: Spawner) {
     static MOTION_BUS: StaticCell<Channel<CriticalSectionRawMutex,Measurement,5> > = StaticCell::new();
     let motion_bus = MOTION_BUS.init_with(|| { Channel::new() });
 
-    static RECORDING_BUS: StaticCell<PubSubChannel<CriticalSectionRawMutex,Measurement,1, 1, 1> > = StaticCell::new();
+    static RECORDING_BUS: StaticCell<PubSubChannel<CriticalSectionRawMutex,Measurement,1, 2, 1> > = StaticCell::new();
     let recording_bus = RECORDING_BUS.init_with(|| { PubSubChannel::new() });
 
     info!("Setting up rendering pipeline");
@@ -114,6 +114,16 @@ async fn main(spawner: Spawner) {
     let mut io = esp_hal::gpio::Io::new(peripherals.IO_MUX);
     io.set_interrupt_handler(gpio_interrupt_handler);
 
+    spawner.must_spawn(renderbug_bike::tasks::sd_card::sdcard_task(
+        peripherals.SPI3.degrade(),
+        peripherals.GPIO41.degrade(),
+        peripherals.GPIO2.degrade(),
+        peripherals.GPIO1.degrade(),
+        Output::new(peripherals.GPIO40.degrade(), esp_hal::gpio::Level::High, OutputConfig::default()),
+        sd_detect_interrupt,
+        recording_bus.dyn_subscriber().unwrap()
+    ));
+
     #[cfg(feature="i2c")]
     {
         use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;

src/tasks/mod.rs

@@ -14,6 +14,8 @@ pub mod oled_render;
 
 pub mod usb_power;
 
+pub mod sd_card;
+
 // Prediction engines
 pub mod motion;
 

src/tasks/sd_card.rs

@@ -0,0 +1,142 @@
+use core::cell::RefCell;
+
+use embassy_sync::pubsub::DynSubscriber;
+use embedded_hal_bus::spi::RefCellDevice;
+use embedded_sdmmc::{Mode, SdCard, TimeSource, VolumeIdx, VolumeManager};
+use esp_hal::{delay::Delay, gpio::{AnyPin, Level, Output}, spi::{master::{AnySpi, Config, Spi}}, time::Rate};
+use rmp::encode::{RmpWrite, RmpWriteErr};
+
+use crate::{events::Measurement, gpio_interrupt::PinInterrupt, simdata::IMUReading, storage::SimDataRecorder};
+use log::*;
+
+struct SdTimeSource;
+
+// TODSO: We need a whole time keeping stack that can sync from GPS or NTP
+impl TimeSource for SdTimeSource {
+    fn get_timestamp(&self) -> embedded_sdmmc::Timestamp {
+        embedded_sdmmc::Timestamp::from_fat(0, 0)
+    }
+}
+
+#[allow(clippy::single_match)]
+#[embassy_executor::task]
+pub async fn sdcard_task(
+    spi: AnySpi<'static>,
+    sck: AnyPin<'static>,
+    mosi: AnyPin<'static>,
+    miso: AnyPin<'static>,
+    cs: Output<'static>,
+    sd_detect_interrupt: PinInterrupt<'static>,
+    mut recording_stream: DynSubscriber<'static, Measurement>
+) {
+
+    let sd_spi = Spi::new(spi, Config::default().with_frequency(Rate::from_khz(400)).with_mode(esp_hal::spi::Mode::_0))
+        .unwrap()
+        .with_sck(sck)
+        .with_mosi(mosi)
+        .with_miso(miso)
+        .into_async();
+
+    let sd_ref = RefCell::new(sd_spi);
+    let sd_device = RefCellDevice::new(&sd_ref, cs, Delay::new()).unwrap();
+    let mut sd_card = SdCard::new(sd_device, Delay::new());
+    
+    loop {
+        match sd_detect_interrupt.wait_for_interrupt().await {
+            Level::High => {
+                info!("SD card inserted!");
+                sd_detect_interrupt.listen();
+            },
+            Level::Low => {
+                info!("SD card removed!");
+                sd_detect_interrupt.listen();
+                continue;
+            }
+        }
+
+        sd_card.mark_card_uninit();
+
+
+        match sd_card.get_card_type() {
+            Some(card_type) => info!("SD card detected: {:?}", card_type),
+            None => {
+                error!("Failed to determine SD card type!");
+                continue;
+            }
+        }
+
+        let volumes = VolumeManager::new(sd_card, SdTimeSource);
+        let vol = volumes.open_volume(VolumeIdx(0)).unwrap();
+        let rootfs = vol.open_root_dir().unwrap();
+
+        let stream = rootfs.open_file_in_dir("renderbug-sensors.rmp",Mode::ReadWriteCreateOrTruncate).unwrap();
+        let rmp_stream = EmbeddedRmp(stream);
+
+        let mut recorder = SimDataRecorder::new(EmbeddedRmp(rmp_stream));
+
+        loop {
+            // FIXME: THis chunk could really go into an impl From<Measurement> for EventRecord
+            match recording_stream.next_message_pure().await {
+                Measurement::IMU { accel, gyro } => {
+                    let reading = IMUReading {
+                        accel_x: accel.x as f64,
+                        accel_y: accel.y as f64,
+                        accel_z: accel.z as f64,
+                        gyro_x: gyro.x as f64,
+                        gyro_y: gyro.y as f64,
+                        gyro_z: gyro.z as f64
+                    };
+                    if recorder.write_next(reading).is_err() {
+                        error!("Failed to write IMU reading to SD card");
+                        break;
+                    }
+                    info!("Wrote IMU to SD card");
+                },
+                _ => ()
+            }
+        }
+
+        drop(recorder);
+        drop(rootfs);
+        drop(vol);
+
+        sd_card = volumes.free().0;
+    }
+}
+
+struct EmbeddedRmp<W: embedded_io::Write>(W);
+
+#[derive(Debug)]
+struct EmbeddedRmpError<T>(T);
+
+impl<T: core::fmt::Debug + 'static> RmpWriteErr for EmbeddedRmpError<T> {
+}
+
+impl<T> core::fmt::Display for EmbeddedRmpError<T> where T: core::fmt::Debug {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        write!(f, "{:?}", self.0)
+    }
+}
+
+impl<W: embedded_io::Write> RmpWrite for EmbeddedRmp<W> where W::Error: core::fmt::Debug + 'static {
+    type Error = EmbeddedRmpError<W::Error>;
+
+    fn write_bytes(&mut self, buf: &[u8]) -> Result<(), Self::Error> {
+        self.0.write(buf).map_err(|e| { EmbeddedRmpError(e)})?;
+        Ok(())
+    }
+}
+
+impl<W: embedded_io::Write> embedded_io::ErrorType for EmbeddedRmp<W> {
+    type Error = W::Error;
+}
+
+impl<W: embedded_io::Write> embedded_io::Write for EmbeddedRmp<W> {
+    fn write(&mut self, buf: &[u8]) -> Result<usize, Self::Error> {
+        self.0.write(buf)
+    }
+
+    fn flush(&mut self) -> Result<(), Self::Error> {
+        self.0.flush()
+    }
+}