Update sbg-rs crate.

boards/nav/src/main.rs

@@ -2,6 +2,7 @@
 #![no_main]
 
 mod data_manager;
+mod types;
 
 use common_arm::*;
 use core::num::{NonZeroU16, NonZeroU8};
@@ -23,6 +24,7 @@ use stm32h7xx_hal::prelude::*;
 use stm32h7xx_hal::spi;
 use stm32h7xx_hal::{rcc, rcc::rec};
 use systick_monotonic::*;
+use types::SBGSerial;
 
 /// Custom panic handler.
 /// Reset the system if a panic occurs.

boards/nav/src/sbg_manager.rs

@@ -0,0 +1,207 @@
+use crate::types::{ConfigSBG, SBGBuffer, SBGTransfer};
+use atsamd_hal::clock::v2::gclk::Gclk0Id;
+use atsamd_hal::clock::v2::pclk::Pclk;
+use atsamd_hal::dmac;
+use atsamd_hal::dmac::Transfer;
+use atsamd_hal::gpio::{Pin, Reset, PB02, PB03};
+use atsamd_hal::pac::{MCLK, RTC};
+use atsamd_hal::sercom::IoSet6;
+// use atsamd_hal::prelude::_atsamd21_hal_time_U32Ext;
+use atsamd_hal::rtc::Rtc;
+use core::alloc::{GlobalAlloc, Layout};
+use core::ffi::c_void;
+use core::mem::size_of;
+use core::ptr;
+// use atsamd_hal::time::*;
+use crate::app::sbg_handle_data;
+use crate::app::sbg_sd_task as sbg_sd;
+use atsamd_hal::prelude::*;
+use atsamd_hal::sercom::{uart, Sercom, Sercom5};
+use common_arm::spawn;
+use defmt::info;
+use embedded_alloc::Heap;
+use rtic::Mutex;
+use sbg_rs::sbg;
+use sbg_rs::sbg::{CallbackData, SBG, SBG_BUFFER_SIZE};
+
+pub static mut BUF_DST: SBGBuffer = &mut [0; SBG_BUFFER_SIZE];
+
+// Simple heap required by the SBG library
+static HEAP: Heap = Heap::empty();
+
+pub struct SBGManager {
+    sbg_device: SBG,
+    xfer: Option<SBGTransfer>,
+}
+
+impl SBGManager {
+    pub fn new(
+        rx: Pin<PB03, Reset>,
+        tx: Pin<PB02, Reset>,
+        pclk_sercom5: Pclk<Sercom5, Gclk0Id>,
+        mclk: &mut MCLK,
+        sercom5: Sercom5,
+        rtc: RTC,
+        mut dma_channel: dmac::Channel<dmac::Ch0, dmac::Ready>,
+    ) -> Self {
+        /* Initialize the Heap */
+        {
+            use core::mem::MaybeUninit;
+            const HEAP_SIZE: usize = 1024;
+            static mut HEAP_MEM: [MaybeUninit<u8>; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE];
+            unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) }
+        }
+
+        let pads_sbg = uart::Pads::<Sercom5, IoSet6>::default().rx(rx).tx(tx);
+        let uart_sbg = ConfigSBG::new(mclk, sercom5, pads_sbg, pclk_sercom5.freq())
+            .baud(
+                115200.Hz(),
+                uart::BaudMode::Fractional(uart::Oversampling::Bits8),
+            )
+            .enable();
+
+        let (sbg_rx, sbg_tx) = uart_sbg.split();
+
+        /* DMAC config */
+        dma_channel
+            .as_mut()
+            .enable_interrupts(dmac::InterruptFlags::new().with_tcmpl(true));
+        let xfer = Transfer::new(dma_channel, sbg_rx, unsafe { &mut *BUF_DST }, false)
+            .expect("DMA err")
+            .begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST);
+
+        // There is a bug within the HAL that improperly configures the RTC
+        // in count32 mode. This is circumvented by first using clock mode then
+        // converting to count32 mode.
+        let rtc_temp = Rtc::clock_mode(rtc, 1024.Hz(), mclk);
+        let mut rtc = rtc_temp.into_count32_mode();
+        rtc.set_count32(0);
+
+        let sbg: sbg::SBG = sbg::SBG::new(sbg_tx, rtc, |data| {
+            sbg_handle_data::spawn(data).ok();
+        });
+
+        SBGManager {
+            sbg_device: sbg,
+            xfer: Some(xfer),
+        }
+    }
+}
+
+pub fn sbg_handle_data(mut cx: sbg_handle_data::Context, data: CallbackData) {
+    cx.shared.data_manager.lock(|manager| match data {
+        CallbackData::UtcTime(x) => manager.utc_time = Some(x),
+        CallbackData::Air(x) => manager.air = Some(x),
+        CallbackData::EkfQuat(x) => manager.ekf_quat = Some(x),
+        CallbackData::EkfNav(x) => manager.ekf_nav = Some(x),
+        CallbackData::Imu(x) => manager.imu = Some(x),
+        CallbackData::GpsVel(x) => manager.gps_vel = Some(x),
+        CallbackData::GpsPos(x) => manager.gps_pos = Some(x),
+    });
+}
+
+pub fn sbg_sd_task(mut cx: crate::app::sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]) {
+    cx.shared.sd_manager.lock(|manager| {
+        if let Some(mut file) = manager.file.take() {
+            cx.shared.em.run(|| {
+                manager.write(&mut file, &data)?;
+                Ok(())
+            });
+            manager.file = Some(file); // give the file back after use
+        } else if let Ok(mut file) = manager.open_file("sbg.txt") {
+            cx.shared.em.run(|| {
+                manager.write(&mut file, &data)?;
+                Ok(())
+            });
+            manager.file = Some(file);
+        }
+    });
+}
+/**
+ * Handles the DMA interrupt.
+ * Handles the SBG data.
+ */
+pub fn sbg_dma(cx: crate::app::sbg_dma::Context) {
+    let sbg = cx.local.sbg_manager;
+
+    match &mut sbg.xfer {
+        Some(xfer) => {
+            if xfer.complete() {
+                let (chan0, source, buf) = sbg.xfer.take().unwrap().stop();
+                let mut xfer = dmac::Transfer::new(chan0, source, unsafe { &mut *BUF_DST }, false)
+                    .unwrap()
+                    .begin(Sercom5::DMA_RX_TRIGGER, dmac::TriggerAction::BURST);
+                let buf_clone = buf.clone();
+                sbg.sbg_device.read_data(buf);
+                unsafe { BUF_DST.copy_from_slice(&[0; SBG_BUFFER_SIZE]) };
+                xfer.block_transfer_interrupt();
+                sbg.xfer = Some(xfer);
+                cx.shared.em.run(|| {
+                    spawn!(sbg_sd, buf_clone)?; // this warning isn't right but it's fine
+                    Ok(())
+                });
+            }
+        }
+        None => {
+            // it should be impossible to reach here.
+            info!("None");
+        }
+    }
+}
+
+/// Stored right before an allocation. Stores information that is needed to deallocate memory.
+#[derive(Copy, Clone)]
+struct AllocInfo {
+    layout: Layout,
+    ptr: *mut u8,
+}
+
+/// Custom malloc for the SBG library. This uses the HEAP object initialized at the start of the
+/// [`SBGManager`]. The [`Layout`] of the allocation is stored right before the returned pointed,
+/// which makes it possible to implement [`free`] without any other data structures.
+#[no_mangle]
+pub extern "C" fn malloc(size: usize) -> *mut c_void {
+    if size == 0 {
+        return ptr::null_mut();
+    }
+
+    // Get a layout for both the requested size
+    let header_layout = Layout::new::<AllocInfo>();
+    let requested_layout = Layout::from_size_align(size, 8).unwrap();
+    let (layout, offset) = header_layout.extend(requested_layout).unwrap();
+
+    // Ask the allocator for memory
+    let orig_ptr = unsafe { HEAP.alloc(layout) };
+    if orig_ptr.is_null() {
+        return orig_ptr as *mut c_void;
+    }
+
+    // Compute the pointer that we will return
+    let result_ptr = unsafe { orig_ptr.add(offset) };
+
+    // Store the allocation information right before the returned pointer
+    let info_ptr = unsafe { result_ptr.sub(size_of::<AllocInfo>()) as *mut AllocInfo };
+    unsafe {
+        info_ptr.write_unaligned(AllocInfo {
+            layout,
+            ptr: orig_ptr,
+        });
+    }
+
+    result_ptr as *mut c_void
+}
+
+/// Custom free implementation for the SBG library. This uses the stored allocation information
+/// right before the pointer to free up the resources.
+///
+/// SAFETY: The value passed to ptr must have been obtained from a previous call to [`malloc`].
+#[no_mangle]
+pub unsafe extern "C" fn free(ptr: *mut c_void) {
+    assert!(!ptr.is_null());
+
+    let info_ptr = unsafe { ptr.sub(size_of::<AllocInfo>()) as *const AllocInfo };
+    let info = unsafe { info_ptr.read_unaligned() };
+    unsafe {
+        HEAP.dealloc(info.ptr, info.layout);
+    }
+}

boards/nav/src/types.rs

@@ -0,0 +1,5 @@
+use hal::pac;
+use hal::serial::Serial;
+use stm32h7xx_hal as hal;
+
+pub type SBGSerial = Serial<pac::UART4>;

libraries/sbg-rs/src/sbg.rs

@@ -25,10 +25,6 @@ use hal::sercom::{IoSet1, IoSet6, Sercom5};
 use heapless::Deque;
 use messages::sensor::*;
 
-type Pads = uart::PadsFromIds<Sercom5, IoSet6, PB03, PB02>;
-type PadsCDC = uart::PadsFromIds<Sercom5, IoSet1, PB17, PB16>;
-type Config = uart::Config<Pads, EightBit>;
-
 /**
  * Max buffer size for SBG messages.
  */
@@ -45,13 +41,14 @@ static mut BUF: &[u8; SBG_BUFFER_SIZE] = &[0; SBG_BUFFER_SIZE];
 
 static mut DEQ: Deque<u8, 4096> = Deque::new();
 
-/**
- * Holds the RTC instance. This is used to get the current time.
- */
-static mut RTC: Option<hal::rtc::Rtc<hal::rtc::Count32Mode>> = None;
-
 static mut DATA_CALLBACK: Option<fn(CallbackData)> = None;
 
+static mut SERIAL_WRITE_CALLBACK: Option<fn(&[u8])> = None;
+
+static mut RTC_GET_TIME: Option<fn() -> u32> = None;
+
+static mut SERIAL_FLUSH_CALLBACK: Option<fn()> = None;
+
 pub enum CallbackData {
     UtcTime(UtcTime),
     Air(Air),
@@ -68,7 +65,6 @@ struct UARTSBGInterface {
 
 pub struct SBG {
     UARTSBGInterface: UARTSBGInterface,
-    serial_device: Uart<Config, uart::TxDuplex>,
     handle: _SbgEComHandle,
     isInitialized: bool,
 }
@@ -79,23 +75,16 @@ impl SBG {
      * Takes ownership of the serial device and RTC instance.
      */
     pub fn new(
-        mut serial_device: Uart<Config, uart::TxDuplex>,
-        rtc: hal::rtc::Rtc<hal::rtc::Count32Mode>,
         callback: fn(CallbackData),
+        serial_write_callback: fn(&[u8]),
+        rtc_get_time: fn() -> u32,
+        serial_flush_callback: fn(),
     ) -> Self {
-        // SAFETY: We are accessing a static variable.
-        // This is safe because we are the only ones who have access to it.
-        // Panic if the RTC instance is already taken, this
-        // only can happen if the SBG instance is created twice.
-        if unsafe { RTC.is_some() } {
-            panic!("RTC instance is already taken!");
-        }
         // SAFETY: We are assigning the RTC instance to a static variable.
         // This is safe because we are the only ones who have access to it.
-        unsafe { RTC = Some(rtc) };
         let interface = UARTSBGInterface {
             interface: &mut _SbgInterface {
-                handle: &mut serial_device as *mut Uart<Config, uart::TxDuplex> as *mut c_void,
+                handle: null_mut() as *mut c_void, // SAFEY: No idea what I just did.
                 type_: 0,
                 name: [0; 48],
                 pDestroyFunc: Some(SBG::SbgDestroyFunc),
@@ -130,17 +119,22 @@ impl SBG {
             cmdDefaultTimeOut: 500,
         };
 
-        unsafe { DATA_CALLBACK = Some(callback) }
+        unsafe {
+            DATA_CALLBACK = Some(callback);
+            SERIAL_WRITE_CALLBACK = Some(serial_write_callback);
+            RTC_GET_TIME = Some(rtc_get_time);
+            SERIAL_FLUSH_CALLBACK = Some(serial_flush_callback);
+        }
 
         let isInitialized = false;
 
         SBG {
             UARTSBGInterface: interface,
-            serial_device,
             handle: handle,
             isInitialized,
         }
     }
+
     /**
      * Returns true if the SBG is initialized.
      */
@@ -323,11 +317,7 @@ impl SBG {
         if pBuffer.is_null() {
             return _SbgErrorCode_SBG_NULL_POINTER;
         }
-        // SAFETY: We are casting a c_void pointer to a Uart peripheral pointer.
-        // This is safe because we only have one sbg object and we ensure that
-        // the peripheral pointer is not accessed during the lifetime of this function.
-        let serial: *mut Uart<Config, uart::TxDuplex> =
-            unsafe { (*pInterface).handle as *mut Uart<Config, uart::TxDuplex> };
+
         // SAFETY: We are casting a c_void pointer to a u8 pointer and then creating a slice from it.
         // This is safe because we ensure pBuffer is valid, pBuffer is not accessed during the lifetime of this function,
         // and the SBGECom library ensures the buffer given is of the correct size.
@@ -339,10 +329,9 @@ impl SBG {
             }
             // SAFETY: We are accessing a Uart Peripheral pointer.
             // This is safe because we ensure that the pointer is not accessed during the lifetime of this function.
-            let result = unsafe { nb::block!(serial.as_mut().unwrap().write(array[counter])) };
-            match result {
-                Ok(_) => counter += 1,
-                Err(_) => return _SbgErrorCode_SBG_WRITE_ERROR,
+            match unsafe { SERIAL_WRITE_CALLBACK } {
+                Some(callback) => callback(&array[counter..counter + 1]),
+                None => return _SbgErrorCode_SBG_WRITE_ERROR,
             }
         }
         _SbgErrorCode_SBG_NO_ERROR
@@ -413,13 +402,11 @@ impl SBG {
         // SAFETY: We are casting a c_void pointer to a Uart peripheral pointer.
         // This is safe because we only have one sbg object and we ensure that
         // the peripheral pointer is not accessed during the lifetime of this function.
-        let serial: *mut Uart<Config, Duplex> =
-            unsafe { (*pInterface).handle as *mut Uart<Config, Duplex> };
-        let result = unsafe { serial.as_mut().unwrap().flush() };
-        match result {
-            Ok(_) => return _SbgErrorCode_SBG_NO_ERROR,
-            Err(_) => return _SbgErrorCode_SBG_READ_ERROR,
+        match unsafe { SERIAL_FLUSH_CALLBACK } {
+            Some(callback) => callback(),
+            None => return _SbgErrorCode_SBG_WRITE_ERROR,
         }
+        _SbgErrorCode_SBG_NO_ERROR
     }
 
     /**
@@ -494,11 +481,9 @@ pub unsafe extern "C" fn sbgPlatformDebugLogMsg(
 pub extern "C" fn sbgGetTime() -> u32 {
     // SAFETY: We are accessing a static mut variable.
     // This is safe because this is the only place where we access the RTC.
-    unsafe {
-        match &RTC {
-            Some(x) => x.count32(),
-            None => 0, // bad error handling but we can't panic, maybe we should force the timeout to be zero in the event there is no RTC.
-        }
+    match unsafe { RTC_GET_TIME } {
+        Some(get_time) => get_time(),
+        None => 0,
     }
 }