sergeysav/ProjectNautilus
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

initial work on bno085

Browse Source
This commit is contained in:
Sergey Savelyev
2025-09-14 09:05:04 -07:00
parent e59b3f3a5f
commit 23e8fdb575
16 changed files with 1143 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
flight/Cargo.toml
View File

@@ -7,8 +7,18 @@ edition = "2024"
anyhow = "1.0.97"
fern = "0.7.1"
log = "0.4.27"
rppal = { version = "0.22.1", optional = true }
chrono = "0.4.40"
embedded-hal = "1.0.0"
embedded-hal-mock = { version = "0.11.1", optional = true }
rppal = { version = "0.22.1", features = ["hal"], optional = true }
nalgebra = "0.33.2"
hex = "0.4.3"
thiserror = "2.0.12"
num-traits = "0.2.19"
[dev-dependencies]
embedded-hal-mock = { version = "0.11.1" }
[features]
raspi = ["dep:rppal"]
sim = ["dep:embedded-hal-mock"]

2
flight/src/data/mod.rs Normal file
View File

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

157
flight/src/data/reader.rs Normal file
View File

@@ -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)
}
}

222
flight/src/data/writer.rs Normal file
View File

@@ -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(())
}
}

14
flight/src/hal/emulated/mod.rs
View File

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

19
flight/src/hal/mod.rs
View File

@@ -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())
}

23
flight/src/hal/raspi/mod.rs
View File

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

264
flight/src/hardware/bno085/mod.rs Normal file
View File

@@ -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(())
}
}

26
flight/src/hardware/bno085/shtp.rs Normal file
View File

@@ -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,
})
}
}

24
flight/src/hardware/error.rs Normal file
View File

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

12
flight/src/hardware/imu/mod.rs Normal file
View File

@@ -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>;
}

28
flight/src/hardware/mod.rs Normal file
View File

@@ -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;

36
flight/src/hardware/raspi/mod.rs Normal file
View File

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

8
flight/src/lib.rs
View File

@@ -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;

44
flight/src/test_utils.rs Normal file
View File

@@ -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);
}
}