initial work on bno085

flight/src/data/mod.rs

@@ -0,0 +1,2 @@
+pub mod reader;
+pub mod writer;

flight/src/data/reader.rs

@@ -0,0 +1,157 @@
+pub trait ReadableDataType {
+    const BYTES: usize;
+    fn from_be_slice(data: &[u8]) -> Self;
+    fn from_le_slice(data: &[u8]) -> Self;
+}
+
+macro_rules! primitive_readable {
+    ( $primitive:ty, $length:expr ) => {
+        impl ReadableDataType for $primitive {
+            const BYTES: usize = $length;
+            fn from_be_slice(data: &[u8]) -> Self {
+                Self::from_be_bytes(data.try_into().unwrap())
+            }
+            fn from_le_slice(data: &[u8]) -> Self {
+                Self::from_le_bytes(data.try_into().unwrap())
+            }
+        }
+    };
+}
+
+primitive_readable!(u8, 1);
+primitive_readable!(u16, 2);
+primitive_readable!(u32, 4);
+primitive_readable!(u64, 8);
+primitive_readable!(u128, 16);
+primitive_readable!(usize, 8);
+primitive_readable!(i8, 1);
+primitive_readable!(i16, 2);
+primitive_readable!(i32, 4);
+primitive_readable!(i64, 8);
+primitive_readable!(i128, 16);
+primitive_readable!(isize, 8);
+primitive_readable!(f32, 4);
+primitive_readable!(f64, 8);
+
+trait DataReadAccess {
+    fn get_data(&self) -> &[u8];
+    fn get_position(&self) -> usize;
+    fn set_position(&mut self, position: usize);
+}
+
+pub trait DataReader {
+    fn reset(&mut self);
+
+    fn read_be<T>(&mut self) -> Option<T> where T: ReadableDataType;
+    fn read_le<T>(&mut self) -> Option<T> where T: ReadableDataType;
+    fn read(&mut self, bytes: usize) -> Option<&[u8]>;
+}
+
+pub struct DataViewReader<'a> {
+    data: &'a [u8],
+    position: usize,
+}
+
+impl<'a> DataViewReader<'a> {
+    pub fn new(data: &'a [u8]) -> Self {
+        Self {
+            data,
+            position: 0usize
+        }
+    }
+}
+
+impl<'a> DataReadAccess for DataViewReader<'a> {
+    fn get_data(&self) -> &[u8] {
+        self.data
+    }
+
+    fn get_position(&self) -> usize {
+        self.position
+    }
+
+    fn set_position(&mut self, position: usize) {
+        self.position = position;
+    }
+}
+
+pub struct DataBufferReader<const LENGTH: usize> {
+    data: [u8; LENGTH],
+    position: usize,
+}
+
+impl<const LENGTH: usize> DataBufferReader<LENGTH> {
+    pub fn new() -> Self {
+        Self {
+            data: [0u8; LENGTH],
+            position: 0usize,
+        }
+    }
+
+    pub fn get_buffer(&mut self) -> &mut [u8] {
+        &mut self.data
+    }
+    
+    pub fn get_write_buffer(&mut self, length: usize) -> &mut [u8] {
+        self.reset();
+        &mut self.data[..length]
+    }
+}
+
+impl<const LENGTH: usize> DataReadAccess for DataBufferReader<LENGTH> {
+    fn get_data(&self) -> &[u8] {
+        &self.data
+    }
+
+    fn get_position(&self) -> usize {
+        self.position
+    }
+
+    fn set_position(&mut self, position: usize) {
+        self.position = position;
+    }
+}
+
+impl<U: DataReadAccess> DataReader for U {
+    fn reset(&mut self) {
+        self.set_position(0);
+    }
+
+    fn read_be<T>(&mut self) -> Option<T>
+    where
+        T: ReadableDataType {
+        let position = self.get_position();
+        if self.get_data().len() < position + T::BYTES {
+            return None;
+        }
+        let result = T::from_be_slice(
+            &self.get_data()[position..(position + T::BYTES)]
+        );
+        self.set_position(position + T::BYTES);
+        Some(result)
+    }
+
+    fn read_le<T>(&mut self) -> Option<T>
+    where
+        T: ReadableDataType {
+        let position = self.get_position();
+        if self.get_data().len() < position + T::BYTES {
+            return None;
+        }
+        let result = T::from_le_slice(
+            &self.get_data()[position..(position + T::BYTES)]
+        );
+        self.set_position(position + T::BYTES);
+        Some(result)
+    }
+
+    fn read(&mut self, bytes: usize) -> Option<&[u8]> {
+        let position = self.get_position();
+        if self.get_data().len() < position + bytes {
+            return None;
+        }
+        self.set_position(position + bytes);
+        let result = &self.get_data()[position..(position + bytes)];
+        Some(result)
+    }
+}

flight/src/data/writer.rs

@@ -0,0 +1,222 @@
+use std::cell::RefCell;
+use std::marker::PhantomData;
+use thiserror::Error;
+
+#[derive(Error, Debug, Copy, Clone)]
+pub enum DataWriterError {
+    #[error("Buffer Full")]
+    BufferFull
+}
+
+pub trait WriteableDataType {
+    const BYTES: usize;
+    fn to_be_slice(&self, data: &mut [u8]);
+    fn to_le_slice(&self, data: &mut [u8]);
+}
+
+macro_rules! primitive_writeable {
+    ( $primitive:ty, $length:expr ) => {
+        impl WriteableDataType for $primitive {
+            const BYTES: usize = $length;
+            fn to_be_slice(&self, data: &mut [u8]) {
+                data.copy_from_slice(&self.to_be_bytes());
+            }
+            fn to_le_slice(&self, data: &mut [u8]) {
+                data.copy_from_slice(&self.to_le_bytes());
+            }
+        }
+    };
+}
+
+primitive_writeable!(u8, 1);
+primitive_writeable!(u16, 2);
+primitive_writeable!(u32, 4);
+primitive_writeable!(u64, 8);
+primitive_writeable!(u128, 16);
+primitive_writeable!(usize, 8);
+primitive_writeable!(i8, 1);
+primitive_writeable!(i16, 2);
+primitive_writeable!(i32, 4);
+primitive_writeable!(i64, 8);
+primitive_writeable!(i128, 16);
+primitive_writeable!(isize, 8);
+primitive_writeable!(f32, 4);
+primitive_writeable!(f64, 8);
+
+trait DataWriteAccess {
+    fn get_data(&mut self) -> &mut [u8];
+    fn get_position(&self) -> usize;
+    fn set_position(&mut self, position: usize);
+}
+
+pub struct DeferredDataWrite<'a, T: WriteableDataType, U> 
+where 
+    RefCell<U>: DataWriter,
+    <RefCell<U> as DataWriter>::WriterType: DataWriteAccess {
+    writer: &'a RefCell<U>,
+    position: usize,
+    _phantom_data: PhantomData<T>,
+}
+
+pub trait DataWriter {
+    type WriterType: DataWriteAccess;
+
+    fn reset(&mut self);
+    fn get_position(&self) -> usize;
+    fn get_buffer(&mut self) -> &[u8];
+    
+    fn push_be<T: WriteableDataType>(&self, data: T) -> Result<(), DataWriterError>;
+    fn push_le<T: WriteableDataType>(&self, data: T) -> Result<(), DataWriterError>;
+    fn push(&self, data: &[u8]) -> Result<(), DataWriterError>;
+    fn defer<T: WriteableDataType>(&self) -> Result<DeferredDataWrite<T, Self::WriterType>, DataWriterError>;
+}
+
+pub struct DataViewWriter<'a> {
+    data: &'a mut [u8],
+    position: usize,
+}
+
+impl<'a> DataViewWriter<'a> {
+    pub fn new(data: &'a mut [u8]) -> RefCell<Self> {
+        RefCell::new(Self {
+            data,
+            position: 0usize
+        })
+    }
+}
+
+impl<'a> DataWriteAccess for DataViewWriter<'a> {
+    fn get_data(&mut self) -> &mut [u8] {
+        self.data
+    }
+
+    fn get_position(&self) -> usize {
+        self.position
+    }
+
+    fn set_position(&mut self, position: usize) {
+        self.position = position;
+    }
+}
+
+pub struct DataBufferWriter<const LENGTH: usize> {
+    data: [u8; LENGTH],
+    position: usize,
+}
+
+impl<const LENGTH: usize> DataBufferWriter<LENGTH> {
+    pub fn new() -> RefCell<Self> {
+        RefCell::new(Self {
+            data: [0u8; LENGTH],
+            position: 0usize
+        })
+    }
+}
+
+impl<const LENGTH: usize> DataWriteAccess for DataBufferWriter<LENGTH> {
+    fn get_data(&mut self) -> &mut [u8] {
+        &mut self.data
+    }
+
+    fn get_position(&self) -> usize {
+        self.position
+    }
+
+    fn set_position(&mut self, position: usize) {
+        self.position = position;
+    }
+}
+
+impl<U: DataWriteAccess> DataWriter for RefCell<U> {
+    type WriterType = U;
+    
+    fn reset(&mut self) {
+        self.get_mut().set_position(0);
+    }
+
+    fn get_position(&self) -> usize {
+        self.borrow().get_position()
+    }
+
+    fn get_buffer(&mut self) -> &[u8] {
+        let position = self.get_position();
+        &self.get_mut().get_data()[..position]
+    }
+
+    fn push_be<T: WriteableDataType>(&self, data: T) -> Result<(), DataWriterError> {
+        let position = self.borrow().get_position();
+        if self.borrow_mut().get_data().len() < position + T::BYTES {
+            return Err(DataWriterError::BufferFull);
+        }
+        data.to_be_slice(&mut self.borrow_mut().get_data()[position..(position + T::BYTES)]);
+        self.borrow_mut().set_position(position + T::BYTES);
+        Ok(())
+    }
+
+    fn push_le<T: WriteableDataType>(&self, data: T) -> Result<(), DataWriterError> {
+        let position = self.borrow().get_position();
+        if self.borrow_mut().get_data().len() < position + T::BYTES {
+            return Err(DataWriterError::BufferFull);
+        }
+        data.to_le_slice(&mut self.borrow_mut().get_data()[position..(position + T::BYTES)]);
+        self.borrow_mut().set_position(position + T::BYTES);
+        Ok(())
+    }
+
+    fn push(&self, data: &[u8]) -> Result<(), DataWriterError> {
+        let position = self.borrow().get_position();
+        if self.borrow_mut().get_data().len() < position + data.len() {
+            return Err(DataWriterError::BufferFull);
+        }
+        self.borrow_mut().get_data()[position..(position + data.len())].copy_from_slice(data);
+        self.borrow_mut().set_position(position + data.len());
+        Ok(())
+    }
+
+    fn defer<T: WriteableDataType>(&self) -> Result<DeferredDataWrite<T, Self::WriterType>, DataWriterError> {
+        let position = self.borrow().get_position();
+        if self.borrow_mut().get_data().len() < position + T::BYTES {
+            return Err(DataWriterError::BufferFull);
+        }
+        self.borrow_mut().set_position(position + T::BYTES);
+        Ok(DeferredDataWrite {
+            writer: self,
+            position,
+            _phantom_data: PhantomData,
+        })
+    }
+}
+
+impl<'a, T: WriteableDataType, U: DataWriteAccess> DeferredDataWrite<'a, T, U> where RefCell<U>: DataWriter {
+    pub fn set_be(self, data: T) {
+        data.to_be_slice(&mut self.writer.borrow_mut().get_data()[self.position..(self.position + T::BYTES)]);
+    }
+    pub fn set_le(self, data: T) {
+        data.to_le_slice(&mut self.writer.borrow_mut().get_data()[self.position..(self.position + T::BYTES)]);
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use anyhow::Result;
+
+    #[test]
+    fn test_buffer() -> Result<()> {
+        let mut writer = DataBufferWriter::<128>::new();
+
+        let deferred = writer.defer::<u16>()?;
+
+        writer.push_be(1u8)?;
+        assert_eq!(1, writer.borrow().data[2]);
+
+        assert_eq!(0, writer.borrow().data[0]);
+        assert_eq!(0, writer.borrow().data[1]);
+        deferred.set_be(0x1234);
+        assert_eq!(0x12, writer.borrow().data[0]);
+        assert_eq!(0x34, writer.borrow().data[1]);
+
+        Ok(())
+    }
+}
+

flight/src/hal/emulated/mod.rs

@@ -1,14 +0,0 @@
-use crate::hal::Hal;
-use log::info;
-
-pub struct EmulatedHal {}
-
-impl EmulatedHal {
-    pub fn new() -> Self {
-        info!("Running in Emulated Hardware Mode");
-
-        Self {}
-    }
-}
-
-impl Hal for EmulatedHal {}

flight/src/hal/mod.rs

@@ -1,19 +0,0 @@
-use anyhow::Result;
-
-pub trait Hal {}
-
-#[cfg(feature = "raspi")]
-mod raspi;
-
-#[cfg(feature = "raspi")]
-pub fn initialize() -> Result<impl Hal> {
-    raspi::RaspiHal::new()
-}
-
-#[cfg(not(feature = "raspi"))]
-mod emulated;
-
-#[cfg(not(feature = "raspi"))]
-pub fn initialize() -> Result<impl Hal> {
-    Ok(emulated::EmulatedHal::new())
-}

flight/src/hal/raspi/mod.rs

@@ -1,23 +0,0 @@
-use crate::hal::Hal;
-use anyhow::Result;
-use log::info;
-use rppal::gpio::Gpio;
-
-pub struct RaspiHal {
-    gpio: Gpio,
-}
-
-impl RaspiHal {
-    pub fn new() -> Result<Self> {
-        let device = rppal::system::DeviceInfo::new()?;
-        info!(
-            "Running on Raspberry Pi Emulated Hardware Model {} with {}",
-            device.model(),
-            device.soc()
-        );
-
-        Self { gpio: Gpio::new()? }
-    }
-}
-
-impl Hal for RaspiHal {}

flight/src/hardware/bno085/mod.rs

@@ -0,0 +1,264 @@
+mod shtp;
+
+use crate::data::reader::{DataBufferReader, DataReader};
+use crate::hardware::error::{NotAvailableError, SpiError};
+use crate::hardware::imu::{Imu, ImuMeasurement};
+use anyhow::{anyhow, bail, ensure, Result};
+use chrono::TimeDelta;
+use embedded_hal::spi::SpiDevice;
+use log::info;
+use num_traits::ToPrimitive;
+use crate::data::writer::{DataBufferWriter, DataWriter};
+
+const RX_BUFFER_LENGTH: usize = 1024;
+
+const SHTP_VERSION_CHANNEL: u8 = 0;
+const SHTP_VERSION_TAG: u8 = 0x80;
+
+const EXECUTABLE_CHANNEL: u8 = 1;
+const EXECUTABLE_RESET: u8 = 1;
+
+const CONTROL_CHANNEL: u8 = 2;
+const INITIALIZE_RESPONSE_ID: u8 = 0xF1;
+const UNSOLICITED_INITIALIZE_COMMAND: u8 = 0x84;
+const SET_FEATURE_REPORT_ID: u8 = 0xFD;
+
+pub struct Bno085<SPI> {
+    spi: SPI,
+    sequence_number: u8,
+}
+
+impl<SPI> Bno085<SPI>
+where
+    SPI : SpiDevice<u8>,
+    SPI::Error : Send,
+    SPI::Error : Sync,
+    SPI::Error : 'static,
+{
+    pub fn new(spi: SPI) -> Self {
+        Self {
+            spi,
+            sequence_number: 0u8
+        }
+    }
+
+    pub fn init(&mut self) -> Result<()> {
+        let mut rx = DataBufferReader::<RX_BUFFER_LENGTH>::new();
+
+        self.spi.read(rx.get_write_buffer(shtp::Header::SIZE)).map_err(SpiError)?;
+        let header = shtp::Header::parse(&mut rx)?;
+        ensure!(header.channel == SHTP_VERSION_CHANNEL, "SHTP Advertisement Expected on Channel 0");
+        ensure!(header.sequence_number == 0, "First Message Sequence Number");
+        self.spi.read(rx.get_write_buffer(header.length as usize)).map_err(SpiError)?;
+        let header = shtp::Header::parse(&mut rx)?;
+        ensure!(header.channel == SHTP_VERSION_CHANNEL, "SHTP Advertisement Expected on Channel 0");
+        ensure!(header.sequence_number == 1, "Second Message Sequence Number");
+        println!("advertisement: {}", hex::encode(&rx.get_buffer()[..(header.length as usize)]));
+        self.handle_generic_message(&mut rx)?;
+
+        // Executable Reset Message
+        self.spi.read(rx.get_write_buffer(shtp::Header::SIZE)).map_err(SpiError)?;
+        let header = shtp::Header::parse(&mut rx)?;
+        ensure!(header.channel == EXECUTABLE_CHANNEL, "SHTP Advertisement Expected on Channel 1");
+        ensure!(header.sequence_number == 2, "Third Message Sequence Number");
+        self.spi.read(rx.get_write_buffer(header.length as usize)).map_err(SpiError)?;
+        let header = shtp::Header::parse(&mut rx)?;
+        ensure!(header.channel == EXECUTABLE_CHANNEL, "SHTP Advertisement Expected on Channel 1");
+        ensure!(header.sequence_number == 3, "Fourth Message Sequence Number");
+        ensure!(rx.read_be::<u8>().ok_or(NotAvailableError)? == EXECUTABLE_RESET, "Executable Reset Complete");
+        println!("reset_message: {}", hex::encode(&rx.get_buffer()[..(header.length as usize)]));
+
+        // Initialization Message
+        self.spi.read(rx.get_write_buffer(shtp::Header::SIZE)).map_err(SpiError)?;
+        let header = shtp::Header::parse(&mut rx)?;
+        ensure!(header.channel == CONTROL_CHANNEL, "SHTP Advertisement Expected on Channel 0");
+        ensure!(header.sequence_number == 4, "Fifth Message Sequence Number");
+        self.spi.read(rx.get_write_buffer(header.length as usize)).map_err(SpiError)?;
+        let header = shtp::Header::parse(&mut rx)?;
+        ensure!(header.channel == CONTROL_CHANNEL, "SHTP Advertisement Expected on Channel 0");
+        ensure!(header.sequence_number == 5, "Sixth Message Sequence Number");
+        println!("init_message : {}", hex::encode(&rx.get_buffer()[..(header.length as usize)]));
+
+        let mut tx = DataBufferWriter::<128>::new();
+        self.produce_sensor_enable(&tx)?;
+        self.spi.write(        tx.get_buffer()).map_err(SpiError)?;
+
+        Ok(())
+    }
+
+    fn handle_generic_message(&self, rx: &mut impl DataReader) -> Result<()> {
+        let tag = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let length = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let value = rx.read(length as usize).ok_or(NotAvailableError)?;
+
+        if tag == SHTP_VERSION_TAG {
+            let shtp_version = String::from_utf8_lossy(&value[..(length as usize - 1)]);
+            info!("BNO085 SHTP Version: {shtp_version}");
+            return Ok(());
+        }
+
+        Err(anyhow!("Unknown Tag {tag}"))
+    }
+
+    fn handle_initialize_response(&self, rx: &mut impl DataReader) -> Result<()> {
+        let report_id = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+
+        if report_id != INITIALIZE_RESPONSE_ID {
+            bail!("Unknown Report ID {report_id}");
+        }
+
+        let _sequence_number = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let command = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        ensure!(command == UNSOLICITED_INITIALIZE_COMMAND, "");
+        let _command_sequence_number = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let _response_sequence_number = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let status = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        ensure!(status == 0, "Status (0 – successful. 1 – Operation failed)");
+        let subsystem = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        info!("BNO085 Subsystem: {subsystem}");
+        let _r2 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let _r3 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let _r4 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let _r5 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let _r6 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let _r7 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let _r8 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let _r9 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+        let _r10 = rx.read_le::<u8>().ok_or(NotAvailableError)?;
+
+        Ok(())
+    }
+
+    fn produce_sensor_enable(
+        &mut self,
+        tx: &impl DataWriter
+    ) -> Result<()> {
+        let starting_position = tx.get_position();
+        let length = tx.defer::<u16>()?;
+        tx.push_be(CONTROL_CHANNEL)?;
+        tx.push_be(self.sequence_number)?;
+        self.sequence_number = self.sequence_number.wrapping_add(1);
+
+        self.produce_set_feature(
+            tx,
+            0x01, // Accelerometer,
+            0,
+            0,
+            TimeDelta::seconds(1) / 100, // 100Hz
+            TimeDelta::zero(),
+            0
+        )?;
+        self.produce_set_feature(
+            tx,
+            0x02, // Gyroscope (Calibrated)
+            0,
+            0,
+            TimeDelta::seconds(1) / 100, // 100Hz
+            TimeDelta::zero(),
+            0
+        )?;
+        self.produce_set_feature(
+            tx,
+            0x03, // Magnetic Field (Calibrated)
+            0,
+            0,
+            TimeDelta::seconds(1) / 100, // 100Hz
+            TimeDelta::zero(),
+            0
+        )?;
+
+        length.set_le((tx.get_position() - starting_position) as u16); // Fill in the length as we finish
+        Ok(())
+    }
+
+    fn produce_set_feature(
+        &self,
+        tx: &impl DataWriter,
+        feature_report_id: u8,
+        feature_flags: u8,
+        change_sensitivity: u16,
+        report_interval: TimeDelta,
+        batch_interval: TimeDelta,
+        configuration_word: u32
+    ) -> Result<()> {
+        tx.push_le(SET_FEATURE_REPORT_ID)?;
+        tx.push_le(feature_report_id)?;
+        tx.push_le(feature_flags)?;
+        tx.push_le(change_sensitivity)?;
+        let report_interval = report_interval.num_microseconds()
+            .and_then(|us| us.to_u32())
+            .ok_or(anyhow!("Could Not Convert Report Interval to Microseconds"))?;
+        tx.push_le(report_interval)?;
+        let batch_interval = batch_interval.num_microseconds()
+            .and_then(|us| us.to_u32())
+            .ok_or(anyhow!("Could Not Convert Batch Interval to Microseconds"))?;
+        tx.push_le(batch_interval)?;
+        tx.push_le(configuration_word)?;
+        Ok(())
+    }
+}
+
+impl<SPI : SpiDevice> Imu for Bno085<SPI> {
+    fn step(&mut self) -> Result<ImuMeasurement> {
+        todo!("asd")
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::test_utils::SilentDrop;
+    use embedded_hal_mock::eh1::spi::Mock as SpiMock;
+    use embedded_hal_mock::eh1::spi::Transaction as SpiTransaction;
+
+    #[test]
+    fn test_init() -> Result<()> {
+        let expectations = vec![
+            SpiTransaction::transaction_start(),
+            SpiTransaction::read_vec(vec![
+                0x0C, 0x00, 0x00, 0x00
+            ]),
+            SpiTransaction::transaction_end(),
+            SpiTransaction::transaction_start(),
+            SpiTransaction::read_vec(vec![
+                0x0C, 0x00, 0x00, 0x01, 0x80, 0x06, 0x30, 0x2E, 0x30, 0x2E, 0x30, 0x00,
+            ]),
+            SpiTransaction::transaction_end(),
+            SpiTransaction::transaction_start(),
+            SpiTransaction::read_vec(vec![
+                0x05, 0x00, 0x01, 0x02
+            ]),
+            SpiTransaction::transaction_end(),
+            SpiTransaction::transaction_start(),
+            SpiTransaction::read_vec(vec![
+                0x05, 0x00, 0x01, 0x03, 1
+            ]),
+            SpiTransaction::transaction_end(),
+            SpiTransaction::transaction_start(),
+            SpiTransaction::read_vec(vec![
+                20, 0x00, 0x02, 0x04
+            ]),
+            SpiTransaction::transaction_end(),
+            SpiTransaction::transaction_start(),
+            SpiTransaction::read_vec(vec![
+                20, 0x00, 0x02, 0x05, 0xF1, 0x00, 0x84, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+            ]),
+            SpiTransaction::transaction_end(),
+            SpiTransaction::transaction_start(),
+            SpiTransaction::write_vec(vec![55, 0, 2, 0, 253, 1, 0, 0, 0, 16, 39, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 253, 2, 0, 0, 0, 16, 39, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 253, 3, 0, 0, 0, 16, 39, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]),
+            SpiTransaction::transaction_end(),
+        ];
+
+        let spi = SpiMock::new(&expectations);
+
+        let mut bno = SilentDrop::new(Bno085::new(spi));
+
+        bno.init()?;
+
+        bno.spi.done();
+
+        Ok(())
+
+    }
+}
+

flight/src/hardware/bno085/shtp.rs

@@ -0,0 +1,26 @@
+use crate::data::reader::DataReader;
+use crate::hardware::error::NotAvailableError;
+use anyhow::Result;
+
+pub struct Header {
+    pub length: u16,
+    pub channel: u8,
+    pub sequence_number: u8
+}
+
+impl Header {
+    pub const SIZE: usize = size_of::<u16>() + size_of::<u8>() + size_of::<u8>();
+    
+    pub fn parse(bytes: &mut impl DataReader) -> Result<Self> {
+        let length = bytes.read_le::<u16>().ok_or(NotAvailableError)?;
+        let channel = bytes.read_le::<u8>().ok_or(NotAvailableError)?;
+        let sequence_number = bytes.read_le::<u8>().ok_or(NotAvailableError)?;
+        
+        Ok(Self {
+            length,
+            channel,
+            sequence_number,
+        })
+    }
+}
+

flight/src/hardware/error.rs

@@ -0,0 +1,24 @@
+use std::error::Error;
+use std::fmt::{Debug, Display, Formatter};
+
+#[derive(Copy, Clone, Debug)]
+pub struct SpiError<ERR: Debug + embedded_hal::spi::Error>(pub ERR);
+
+impl<ERR: Debug + embedded_hal::spi::Error> Display for SpiError<ERR> {
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        writeln!(f, "SpiError({})", self.0.kind())
+    }
+}
+
+impl<ERR: Debug + embedded_hal::spi::Error> Error for SpiError<ERR> { }
+
+#[derive(Copy, Clone, Debug)]
+pub struct NotAvailableError;
+
+impl Display for NotAvailableError {
+    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
+        writeln!(f, "NotAvailableError")
+    }
+}
+
+impl Error for NotAvailableError { }

flight/src/hardware/imu/mod.rs

@@ -0,0 +1,12 @@
+use nalgebra::Vector3;
+use anyhow::Result;
+
+pub struct ImuMeasurement {
+    pub acceleration: Vector3<f64>,
+    pub angular_velocity: Vector3<f64>,
+    pub magnetic_field: Vector3<f64>,
+}
+
+pub trait Imu {
+    fn step(&mut self) -> Result<ImuMeasurement>;
+}

flight/src/hardware/mod.rs

@@ -0,0 +1,28 @@
+use anyhow::Result;
+
+pub trait Hardware {
+}
+
+impl Hardware for () {
+    
+}
+
+#[cfg(feature = "raspi")]
+mod raspi;
+
+#[cfg(feature = "raspi")]
+pub fn initialize() -> Result<impl Hardware> {
+    raspi::RaspiHardware::new()
+}
+
+#[cfg(not(feature = "raspi"))]
+#[allow(unreachable_code)]
+pub fn initialize() -> Result<impl Hardware> {
+    panic!("Can not Initialize");
+
+    Ok(())
+}
+
+mod bno085;
+mod imu;
+mod error;

flight/src/hardware/raspi/mod.rs

@@ -0,0 +1,36 @@
+use crate::hardware::Hardware;
+use anyhow::Result;
+use log::info;
+use rppal::gpio::Gpio;
+use rppal::spi::SimpleHalSpiDevice;
+
+const CLOCK_3MHZ: u32 = 3_000_000;
+
+pub struct RaspiHardware {
+    gpio: Gpio,
+}
+
+impl RaspiHardware {
+    pub fn new() -> Result<Self> {
+        let device = rppal::system::DeviceInfo::new()?;
+        info!(
+            "Running on Raspberry Pi Emulated Hardware Model {} with {}",
+            device.model(),
+            device.soc()
+        );
+        
+        Ok(Self {
+            gpio: Gpio::new()?,
+            // spi_bno085: Spi::new(
+            //     Bus::Spi1,
+            //     SlaveSelect::Ss0,
+            //     CLOCK_3MHZ,
+            //     Mode::Mode3,
+            // )?
+        })
+    }
+}
+
+impl Hardware for RaspiHardware {
+    
+}

flight/src/lib.rs

@@ -1,9 +1,9 @@
-use crate::hal::initialize;
+use crate::hardware::initialize;
 use crate::logger::setup_logger;
 use anyhow::Result;
 use log::info;
 
-mod hal;
+mod hardware;
 mod logger;
 
 pub fn run() -> Result<()> {
@@ -19,3 +19,7 @@ pub fn run() -> Result<()> {
 
     Ok(())
 }
+
+#[cfg(test)]
+mod test_utils;
+mod data;

flight/src/test_utils.rs

@@ -0,0 +1,44 @@
+use std::mem::ManuallyDrop;
+use std::ops::{Deref, DerefMut};
+use std::panic;
+
+pub struct SilentDrop<T: panic::UnwindSafe> {
+    inner: ManuallyDrop<T>
+}
+
+impl<T: panic::UnwindSafe> SilentDrop<T> {
+    pub fn new(inner: T) -> Self {
+        Self {
+            inner: ManuallyDrop::new(inner)
+        }
+    }
+}
+
+impl<T: panic::UnwindSafe> Deref for SilentDrop<T> {
+    type Target = T;
+
+    fn deref(&self) -> &Self::Target {
+        self.inner.deref()
+    }
+}
+
+impl<T: panic::UnwindSafe> DerefMut for SilentDrop<T> {
+    fn deref_mut(&mut self) -> &mut Self::Target {
+        self.inner.deref_mut()
+    }
+}
+
+impl<T: panic::UnwindSafe> Drop for SilentDrop<T> {
+    fn drop(&mut self) {
+        let prev_hook = panic::take_hook();
+        panic::set_hook(Box::new(|_| {}));
+        let inner = unsafe {
+            ManuallyDrop::take(&mut self.inner)
+        };
+        let destroy_closure = || {
+            drop(inner);
+        };
+        if panic::catch_unwind(destroy_closure).is_err() {};
+        panic::set_hook(prev_hook);
+    }
+}