initial work on bno085

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 {
+    
+}