diff --git a/src/bin/main.rs b/src/bin/main.rs index d237723..8768364 100644 --- a/src/bin/main.rs +++ b/src/bin/main.rs @@ -75,7 +75,7 @@ async fn main(spawner: Spawner) { static MOTION_BUS: StaticCell > = StaticCell::new(); let motion_bus = MOTION_BUS.init_with(|| { Channel::new() }); - static RECORDING_BUS: StaticCell > = StaticCell::new(); + static RECORDING_BUS: StaticCell > = 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; diff --git a/src/tasks/mod.rs b/src/tasks/mod.rs index 47d82c9..14924a8 100644 --- a/src/tasks/mod.rs +++ b/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; diff --git a/src/tasks/sd_card.rs b/src/tasks/sd_card.rs new file mode 100644 index 0000000..956ad79 --- /dev/null +++ b/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 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); + +#[derive(Debug)] +struct EmbeddedRmpError(T); + +impl RmpWriteErr for EmbeddedRmpError { +} + +impl core::fmt::Display for EmbeddedRmpError where T: core::fmt::Debug { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + write!(f, "{:?}", self.0) + } +} + +impl RmpWrite for EmbeddedRmp where W::Error: core::fmt::Debug + 'static { + type Error = EmbeddedRmpError; + + fn write_bytes(&mut self, buf: &[u8]) -> Result<(), Self::Error> { + self.0.write(buf).map_err(|e| { EmbeddedRmpError(e)})?; + Ok(()) + } +} + +impl embedded_io::ErrorType for EmbeddedRmp { + type Error = W::Error; +} + +impl embedded_io::Write for EmbeddedRmp { + fn write(&mut self, buf: &[u8]) -> Result { + self.0.write(buf) + } + + fn flush(&mut self) -> Result<(), Self::Error> { + self.0.flush() + } +} \ No newline at end of file