diff --git a/firmware/src/imu/bmi160/math.rs b/firmware/src/imu/drivers/bmi160/math.rs similarity index 100% rename from firmware/src/imu/bmi160/math.rs rename to firmware/src/imu/drivers/bmi160/math.rs diff --git a/firmware/src/imu/bmi160/mod.rs b/firmware/src/imu/drivers/bmi160/mod.rs similarity index 91% rename from firmware/src/imu/bmi160/mod.rs rename to firmware/src/imu/drivers/bmi160/mod.rs index be21f6f3..6989b0fd 100644 --- a/firmware/src/imu/bmi160/mod.rs +++ b/firmware/src/imu/drivers/bmi160/mod.rs @@ -1,11 +1,11 @@ mod math; -use self::math::discrete_to_radians; -use super::{Imu, Quat}; +use self::math::{discrete_to_radians, GyroFsr}; +use crate::aliases::I2c; +use crate::imu::{FusedImu, Quat}; use crate::utils; -use crate::{aliases::I2c, imu::ඞ::math::GyroFsr}; -use bmi160::{AccelerometerPowerMode, GyroscopePowerMode, SensorSelector}; +use ::bmi160::{AccelerometerPowerMode, GyroscopePowerMode, SensorSelector}; use defmt::{debug, trace}; use embedded_hal::blocking::delay::DelayMs; use firmware_protocol::ImuType; @@ -76,7 +76,7 @@ impl Bmi160 { } } -impl Imu for Bmi160 { +impl FusedImu for Bmi160 { type Error = BmiError; const IMU_TYPE: ImuType = ImuType::Bmi160; @@ -102,6 +102,6 @@ impl Imu for Bmi160 { pub fn new_imu( i2c: impl crate::aliases::I2c, delay: &mut impl DelayMs, -) -> impl crate::imu::Imu { +) -> impl crate::imu::FusedImu { Bmi160::new(i2c, delay).expect("Failed to initialize BMI160") } diff --git a/firmware/src/imu/drivers/mod.rs b/firmware/src/imu/drivers/mod.rs new file mode 100644 index 00000000..8e296a18 --- /dev/null +++ b/firmware/src/imu/drivers/mod.rs @@ -0,0 +1,14 @@ +mod stubbed; + +#[cfg(feature = "imu-stubbed")] +pub mod ඞ { + pub use crate::imu::drivers::stubbed::*; +} + +#[cfg(feature = "imu-mpu6050")] +#[path = "mpu6050.rs"] +pub mod ඞ; + +#[cfg(feature = "imu-bmi160")] +#[path = "bmi160/mod.rs"] +pub mod ඞ; diff --git a/firmware/src/imu/mpu6050.rs b/firmware/src/imu/drivers/mpu6050.rs similarity index 95% rename from firmware/src/imu/mpu6050.rs rename to firmware/src/imu/drivers/mpu6050.rs index 431862b3..32ffd7a3 100644 --- a/firmware/src/imu/mpu6050.rs +++ b/firmware/src/imu/drivers/mpu6050.rs @@ -1,5 +1,5 @@ -use super::{Imu, Quat}; use crate::aliases::I2c; +use crate::imu::{FusedImu, Quat}; use crate::utils; use defmt::{debug, trace, warn}; @@ -49,7 +49,7 @@ impl Mpu6050 { } } -impl Imu for Mpu6050 { +impl FusedImu for Mpu6050 { type Error = mpu6050_dmp::error::Error; const IMU_TYPE: ImuType = ImuType::Mpu6050; @@ -76,6 +76,6 @@ impl Imu for Mpu6050 { pub fn new_imu( i2c: impl crate::aliases::I2c, delay: &mut impl DelayMs, -) -> impl crate::imu::Imu { +) -> impl crate::imu::FusedImu { Mpu6050::new(i2c, delay).expect("Failed to initialize MPU6050") } diff --git a/firmware/src/imu/stubbed.rs b/firmware/src/imu/drivers/stubbed.rs similarity index 82% rename from firmware/src/imu/stubbed.rs rename to firmware/src/imu/drivers/stubbed.rs index 95be420c..47f73e1a 100644 --- a/firmware/src/imu/stubbed.rs +++ b/firmware/src/imu/drivers/stubbed.rs @@ -1,4 +1,4 @@ -use super::{Imu, Quat}; +use crate::imu::{FusedImu, Quat}; use defmt::debug; use embedded_hal::blocking::delay::DelayMs; @@ -7,7 +7,7 @@ use firmware_protocol::ImuType; /// Fakes an IMU for easier testing. struct FakeImu; -impl Imu for FakeImu { +impl FusedImu for FakeImu { type Error = (); const IMU_TYPE: ImuType = ImuType::Unknown(0xFF); @@ -21,7 +21,7 @@ impl Imu for FakeImu { pub fn new_imu( _i2c: impl crate::aliases::I2c, _delay: &mut impl DelayMs, -) -> impl crate::imu::Imu { +) -> impl crate::imu::FusedImu { debug!("Created FakeImu"); FakeImu } diff --git a/firmware/src/imu/fusion/mod.rs b/firmware/src/imu/fusion/mod.rs new file mode 100644 index 00000000..359db1c2 --- /dev/null +++ b/firmware/src/imu/fusion/mod.rs @@ -0,0 +1 @@ +//! TODO: Add fusion diff --git a/firmware/src/imu/mod.rs b/firmware/src/imu/mod.rs index b2185e8f..9be4082f 100644 --- a/firmware/src/imu/mod.rs +++ b/firmware/src/imu/mod.rs @@ -1,17 +1,5 @@ -mod stubbed; - -#[cfg(feature = "imu-stubbed")] -mod ඞ { - pub use crate::imu::stubbed::*; -} - -#[cfg(feature = "imu-mpu6050")] -#[path = "mpu6050.rs"] -mod ඞ; - -#[cfg(feature = "imu-bmi160")] -#[path = "bmi160/mod.rs"] -mod ඞ; +mod drivers; +mod fusion; use defmt::{debug, info, trace, warn}; use embassy_executor::task; @@ -25,7 +13,7 @@ use crate::{ pub type Quat = nalgebra::UnitQuaternion; -pub trait Imu { +pub trait FusedImu { type Error: core::fmt::Debug; const IMU_TYPE: ImuType; @@ -51,7 +39,7 @@ async fn imu_task_inner( mut delay: impl crate::aliases::Delay, ) -> ! { debug!("Imu task"); - let mut imu = ඞ::new_imu(i2c, &mut delay); + let mut imu = self::drivers::ඞ::new_imu(i2c, &mut delay); info!("Initialized IMU!"); loop {