diff --git a/.github/workflows/firmware-ci.yml b/.github/workflows/firmware-ci.yml index ecf347da..506d74bd 100644 --- a/.github/workflows/firmware-ci.yml +++ b/.github/workflows/firmware-ci.yml @@ -39,7 +39,6 @@ jobs: strategy: matrix: mcu: [mcu-esp32c3, mcu-esp32, mcu-nrf52840, mcu-nrf52832] - imu: [imu-stubbed] # dont add IMUs here net: [net-stubbed, net-wifi] log: [log-rtt, log-usb-serial, log-uart] include: @@ -59,18 +58,10 @@ jobs: net: net-stubbed log: log-uart target: riscv32imc-unknown-none-elf - imu: imu-mpu6050 - mcu: mcu-esp32c3 net: net-stubbed log: log-uart target: riscv32imc-unknown-none-elf - imu: imu-bmi160 - # add IMUs inside the include so they are only ran once - # - mcu: mcu-esp32c3 - # net: net-stubbed - # log: log-uart - # target: riscv32imc-unknown-none-elf - # imu: imu-bno08x exclude: - mcu: mcu-esp32 log: log-usb-serial @@ -85,7 +76,7 @@ jobs: net: net-wifi env: - FEATURES: ${{ format('{0},{1},{2},{3},{4}', matrix.mcu, matrix.imu, matrix.net, matrix.log, matrix.boot) }} + FEATURES: ${{ format('{0},{1},{2},{3}', matrix.mcu, matrix.net, matrix.log, matrix.boot) }} defaults: run: working-directory: ./firmware diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock index a1ab5cd7..a3e31b3b 100644 --- a/firmware/Cargo.lock +++ b/firmware/Cargo.lock @@ -177,6 +177,18 @@ dependencies = [ "embedded-hal 0.2.7", ] +[[package]] +name = "bno055" +version = "0.3.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b886b6cd2c2ac51e6ce9c9c412c5f925d5f71a2800ba0638ae8d5ef85876420" +dependencies = [ + "bitflags", + "byteorder", + "embedded-hal 0.2.7", + "mint", +] + [[package]] name = "bytemuck" version = "1.12.3" @@ -906,6 +918,7 @@ dependencies = [ "alloc-cortex-m", "bleps", "bmi160", + "bno055", "cfg_aliases", "color-eyre", "cortex-m", @@ -1199,6 +1212,12 @@ dependencies = [ "adler", ] +[[package]] +name = "mint" +version = "0.5.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e53debba6bda7a793e5f99b8dacf19e626084f525f7829104ba9898f367d85ff" + [[package]] name = "mpu6050-dmp" version = "0.2.0" @@ -1221,6 +1240,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "20bd243ab3dbb395b39ee730402d2e5405e448c75133ec49cc977762c4cba3d1" dependencies = [ "approx", + "mint", "nalgebra-macros", "num-complex", "num-rational", diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml index 5917882b..079c1d78 100644 --- a/firmware/Cargo.toml +++ b/firmware/Cargo.toml @@ -14,7 +14,7 @@ rust-version.workspace = true [features] -default = ["mcu-esp32c3", "imu-mpu6050", "log-rtt", "net-wifi"] +default = ["mcu-esp32c3", "log-rtt", "net-wifi"] # default = [ # "mcu-nrf52840", # "imu-stubbed", @@ -64,11 +64,6 @@ net-wifi = ["esp-wifi/wifi", "dep:smoltcp"] # use wifi net-ble = ["esp-wifi/ble", "dep:bleps"] net-stubbed = [] # Stubs out network -# Supported IMUs -imu-bmi160 = ["dep:bmi160"] -imu-mpu6050 = ["dep:mpu6050-dmp"] -imu-stubbed = [] # Stubs out the IMU - # Supported defmt loggers log-rtt = ["dep:defmt-rtt"] log-usb-serial = ["defmt_esp_println?/jtag_serial"] @@ -191,8 +186,9 @@ panic_defmt = { path = "crates/panic_defmt" } defmt-bbq = { version = "0.1", optional = true } # Peripheral drivers -mpu6050-dmp = { version = "0.2", optional = true } -bmi160 = { version = "0.1", optional = true } +mpu6050-dmp = "0.2" +bmi160 = "0.1" +bno055 = "0.3" # Other crates static_cell = "1" @@ -200,6 +196,7 @@ nb = "1" nalgebra = { version = "0.31", default-features = false, features = [ "macros", "libm", + "mint", ] } fugit = "0.3" firmware_protocol = { path = "../networking/firmware_protocol", features = [ diff --git a/firmware/build.rs b/firmware/build.rs index b839cc82..268f178d 100644 --- a/firmware/build.rs +++ b/firmware/build.rs @@ -8,7 +8,6 @@ use std::{ }; mandatory_and_unique!("mcu-esp32", "mcu-esp32c3", "mcu-nrf52832", "mcu-nrf52840"); -mandatory_and_unique!("imu-stubbed", "imu-mpu6050", "imu-bmi160"); mandatory_and_unique!("log-rtt", "log-usb-serial", "log-uart"); mandatory_and_unique!("net-wifi", "net-ble", "net-stubbed"); diff --git a/firmware/src/imu/bmi160/math.rs b/firmware/src/imu/bmi160/math.rs deleted file mode 100644 index 3184fda8..00000000 --- a/firmware/src/imu/bmi160/math.rs +++ /dev/null @@ -1,100 +0,0 @@ -const RAD_PER_DEG: f32 = 2. * core::f32::consts::PI / 360.; -const DEG_PER_RAD: f32 = 1. / RAD_PER_DEG; - -// TODO: This whole module needs unit tests - -/// The Full Scale Range of the gyroscope. For example, D2000 means +/- 2000 degrees/sec -#[derive(Eq, PartialEq, Copy, Clone, Debug)] -#[allow(dead_code)] -pub enum GyroFsr { - D2000, - D1000, - D500, - D250, - D125, -} -#[allow(dead_code)] -impl GyroFsr { - /// The default FSR when the IMU is reset - pub const DEFAULT: Self = Self::D2000; - pub const fn from_reg(v: u8) -> Result { - Ok(match v { - 0b000 => Self::D2000, - 0b001 => Self::D1000, - 0b010 => Self::D500, - 0b011 => Self::D250, - 0b100 => Self::D125, - _ => return Err(InvalidBitPattern), - }) - } - - pub const fn to_reg(self) -> u8 { - match self { - Self::D2000 => 0b000, - Self::D1000 => 0b001, - Self::D500 => 0b010, - Self::D250 => 0b011, - Self::D125 => 0b100, - } - } - - pub const fn as_u16(self) -> u16 { - match self { - Self::D2000 => 2000, - Self::D1000 => 1000, - Self::D500 => 500, - Self::D250 => 250, - Self::D125 => 125, - } - } - - pub const fn from_u16(v: u16) -> Result { - // TODO: I'm not confident this is performant - Ok(match v { - v if v == Self::D2000.as_u16() => Self::D2000, - v if v == Self::D1000.as_u16() => Self::D1000, - v if v == Self::D500.as_u16() => Self::D500, - v if v == Self::D250.as_u16() => Self::D250, - v if v == Self::D125.as_u16() => Self::D125, - _ => return Err(InvalidNum), - }) - } - - /// least signficant bits per deg/s - pub const fn lsb_per_dps(self) -> f32 { - let range: f32 = self.as_u16() as _; - // Add 1 because there is MAX+1 numbers due to `0` - const TMP: f32 = i16::MAX as f32 + 1.; - TMP / range - } - - /// deg/s per least significant bit - pub const fn dps_per_lsb(self) -> f32 { - let range: f32 = self.as_u16() as _; - // Add 1 because there is MAX+1 numbers due to `0` - const TMP: f32 = 1. / (i16::MAX as f32 + 1.); - range * TMP - } - - /// least significant bits per rad/s - pub const fn lsb_per_rad(self) -> f32 { - self.lsb_per_dps() * DEG_PER_RAD - } - - /// rad/s per least significant bit - pub const fn rad_per_lsb(self) -> f32 { - self.dps_per_lsb() * RAD_PER_DEG - } -} - -#[derive(Debug)] -pub struct InvalidBitPattern; - -#[derive(Debug)] -pub struct InvalidNum; - -/// The bmi160 returns the data from the gyro as an `i16`, we must use the Full Scale Range to -/// convert to a float. -pub const fn discrete_to_radians(fsr: GyroFsr, discrete: i16) -> f32 { - discrete as f32 * fsr.rad_per_lsb() -} diff --git a/firmware/src/imu/driver/bmi160/math.rs b/firmware/src/imu/driver/bmi160/math.rs new file mode 100644 index 00000000..1a047a1c --- /dev/null +++ b/firmware/src/imu/driver/bmi160/math.rs @@ -0,0 +1,189 @@ +const RAD_PER_DEG: f32 = 2.0 * core::f32::consts::PI / 360.0; +const DEG_PER_RAD: f32 = 1.0 / RAD_PER_DEG; +const M_PER_G: f32 = 9.81; +const G_PER_M: f32 = 1.0 / M_PER_G; + +// TODO: This whole module needs unit tests + +/// The Full Scale Range of the gyroscope. For example, D2000 means +/- 2000 degrees/sec +#[derive(Eq, PartialEq, Copy, Clone, Debug)] +#[allow(dead_code)] +pub enum GyroFsr { + D2000, + D1000, + D500, + D250, + D125, +} + +#[allow(dead_code)] +impl GyroFsr { + /// The default FSR when the IMU is reset + pub const DEFAULT: Self = Self::D2000; + pub const fn from_reg(v: u8) -> Result { + Ok(match v { + 0b000 => Self::D2000, + 0b001 => Self::D1000, + 0b010 => Self::D500, + 0b011 => Self::D250, + 0b100 => Self::D125, + _ => return Err(InvalidBitPattern), + }) + } + + pub const fn to_reg(self) -> u8 { + match self { + Self::D2000 => 0b000, + Self::D1000 => 0b001, + Self::D500 => 0b010, + Self::D250 => 0b011, + Self::D125 => 0b100, + } + } + + pub const fn as_u16(self) -> u16 { + match self { + Self::D2000 => 2000, + Self::D1000 => 1000, + Self::D500 => 500, + Self::D250 => 250, + Self::D125 => 125, + } + } + + pub const fn from_u16(v: u16) -> Result { + // TODO: I'm not confident this is performant + Ok(match v { + v if v == Self::D2000.as_u16() => Self::D2000, + v if v == Self::D1000.as_u16() => Self::D1000, + v if v == Self::D500.as_u16() => Self::D500, + v if v == Self::D250.as_u16() => Self::D250, + v if v == Self::D125.as_u16() => Self::D125, + _ => return Err(InvalidNum), + }) + } + + /// least signficant bits per deg/s + pub const fn lsb_per_dps(self) -> f32 { + let range: f32 = self.as_u16() as _; + // Add 1 because there is MAX+1 numbers due to `0` + const TMP: f32 = i16::MAX as f32 + 1.; + TMP / range + } + + /// deg/s per least significant bit + pub const fn dps_per_lsb(self) -> f32 { + let range: f32 = self.as_u16() as _; + // Add 1 because there is MAX+1 numbers due to `0` + const TMP: f32 = 1. / (i16::MAX as f32 + 1.); + range * TMP + } + + /// least significant bits per rad/s + pub const fn lsb_per_rad(self) -> f32 { + self.lsb_per_dps() * DEG_PER_RAD + } + + /// rad/s per least significant bit + pub const fn rad_per_lsb(self) -> f32 { + self.dps_per_lsb() * RAD_PER_DEG + } + + /// The bmi160 returns the data from the gyro as an `i16`, we must use the Full Scale Range to + /// convert to a float rad/s + pub const fn convert_rad(self, discrete: i16) -> f32 { + discrete as f32 * self.rad_per_lsb() + } +} + +/// The Full Scale Range of the accelerometer. For example, G2 means +/- 19.6 meters/sec^2 +#[derive(Eq, PartialEq, Copy, Clone, Debug)] +#[allow(dead_code)] +pub enum AccelFsr { + G2, + G4, + G8, + G16, +} + +#[allow(dead_code)] +impl AccelFsr { + /// The default FSR when the IMU is reset + pub const DEFAULT: Self = Self::G2; + pub const fn from_reg(v: u8) -> Result { + Ok(match v { + 0b0011 => Self::G2, + 0b0101 => Self::G4, + 0b1000 => Self::G8, + 0b1100 => Self::G16, + _ => return Err(InvalidBitPattern), + }) + } + + pub const fn to_reg(self) -> u8 { + match self { + Self::G2 => 0b0011, + Self::G4 => 0b0101, + Self::G8 => 0b1000, + Self::G16 => 0b1100, + } + } + + pub const fn as_u16(self) -> u16 { + match self { + Self::G2 => 2, + Self::G4 => 4, + Self::G8 => 8, + Self::G16 => 16, + } + } + + pub const fn from_u16(v: u16) -> Result { + // TODO: I'm not confident this is performant + Ok(match v { + v if v == Self::G2.as_u16() => Self::G2, + v if v == Self::G4.as_u16() => Self::G4, + v if v == Self::G8.as_u16() => Self::G8, + v if v == Self::G16.as_u16() => Self::G16, + _ => return Err(InvalidNum), + }) + } + + /// least signficant bits per g + pub const fn lsb_per_g(self) -> f32 { + let range: f32 = self.as_u16() as _; + // Add 1 because there is MAX+1 numbers due to `0` + const TMP: f32 = i16::MAX as f32 + 1.; + TMP / range + } + + /// g per least significant bit + pub const fn g_per_lsb(self) -> f32 { + let range: f32 = self.as_u16() as _; + // Add 1 because there is MAX+1 numbers due to `0` + const TMP: f32 = 1. / (i16::MAX as f32 + 1.); + range * TMP + } + + /// least significant bits per g + pub const fn lsb_per_m(self) -> f32 { + self.lsb_per_g() * M_PER_G + } + + /// g per least significant bit + pub const fn m_per_lsb(self) -> f32 { + self.g_per_lsb() * G_PER_M + } + + /// The bmi160 returns the data from the accel as an `i16`, we must use the Full Scale Range to + /// convert to a float m/s^2 + pub const fn convert_ms2(self, discrete: i16) -> f32 { + discrete as f32 * self.m_per_lsb() + } +} + +#[derive(Debug)] +pub struct InvalidBitPattern; + +#[derive(Debug)] +pub struct InvalidNum; diff --git a/firmware/src/imu/bmi160/mod.rs b/firmware/src/imu/driver/bmi160/mod.rs similarity index 76% rename from firmware/src/imu/bmi160/mod.rs rename to firmware/src/imu/driver/bmi160/mod.rs index be21f6f3..4ec5b1e3 100644 --- a/firmware/src/imu/bmi160/mod.rs +++ b/firmware/src/imu/driver/bmi160/mod.rs @@ -1,9 +1,10 @@ mod math; -use self::math::discrete_to_radians; -use super::{Imu, Quat}; +use self::math::{AccelFsr, GyroFsr}; +use super::{Imu, ImuData}; +use crate::aliases::I2c; +use crate::imu::Vec3; use crate::utils; -use crate::{aliases::I2c, imu::ඞ::math::GyroFsr}; use bmi160::{AccelerometerPowerMode, GyroscopePowerMode, SensorSelector}; use defmt::{debug, trace}; @@ -81,27 +82,31 @@ impl Imu for Bmi160 { const IMU_TYPE: ImuType = ImuType::Bmi160; - fn quat(&mut self) -> nb::Result { - let data = self.driver.data(SensorSelector::new().gyro())?; - let gyro_vel_euler = data.gyro.unwrap(); + async fn data(&mut self) -> Result { + let data = self.driver.data(SensorSelector::new().gyro().accel())?; + let accel = data.accel.unwrap(); + let gyro = data.gyro.unwrap(); // TODO: We should probably query the IMU for the FSR instead of assuming the default one. - const FSR: GyroFsr = GyroFsr::DEFAULT; + const AFSR: AccelFsr = AccelFsr::DEFAULT; + const GFSR: GyroFsr = GyroFsr::DEFAULT; // TODO: Check that bmi crates conventions for euler angles matches nalgebra. // TODO: Implement sensor fusion and temperature compensation // TODO: This should be integrated to position, lol - Ok(nalgebra::UnitQuaternion::from_euler_angles( - discrete_to_radians(FSR, gyro_vel_euler.x), - discrete_to_radians(FSR, gyro_vel_euler.y), - discrete_to_radians(FSR, gyro_vel_euler.z), - )) + Ok(ImuData { + accel: Vec3::new( + AFSR.convert_ms2(accel.x), + AFSR.convert_ms2(accel.y), + AFSR.convert_ms2(accel.z), + ), + gyro: Vec3::new( + GFSR.convert_rad(gyro.x), + GFSR.convert_rad(gyro.y), + GFSR.convert_rad(gyro.z), + ), + // 6 dof ought to be enough for everyone + mag: None, + }) } } - -pub fn new_imu( - i2c: impl crate::aliases::I2c, - delay: &mut impl DelayMs, -) -> impl crate::imu::Imu { - Bmi160::new(i2c, delay).expect("Failed to initialize BMI160") -} diff --git a/firmware/src/imu/driver/bno055.rs b/firmware/src/imu/driver/bno055.rs new file mode 100644 index 00000000..47b92814 --- /dev/null +++ b/firmware/src/imu/driver/bno055.rs @@ -0,0 +1,74 @@ +use bno055::{BNO055OperationMode as Mode, Bno055 as Driver, Error}; +use embassy_futures::yield_now; +use embedded_hal::blocking::delay::DelayMs; +use firmware_protocol::ImuType; + +use crate::{ + aliases::I2c, + imu::{FusedImu, Quat, Vec3}, +}; + +use super::{Imu, ImuData}; + +pub struct Bno055 { + driver: Driver, +} + +impl Bno055 { + pub fn new( + i2c: I, + delay: &mut impl DelayMs, + ) -> Result::Error>> { + let mut driver = Driver::new(i2c); + + driver.init(delay)?; + + if F { + // Use internal fusion using gyro and accel, no mag + driver.set_mode(Mode::IMU, delay)?; + } else { + // Do not perform fusion on-chip + driver.set_mode(Mode::AMG, delay)?; + } + + Ok(Self { driver }) + } +} + +// Using on-chip fusion +impl FusedImu for Bno055 { + type Error = Error<::Error>; + + const IMU_TYPE: ImuType = ImuType::Bno055; + + async fn quat(&mut self) -> Result { + let quat = self.driver.quaternion()?; + + // why can't we access the timing config... guh + yield_now().await; + + Ok(Quat::from_quaternion(quat.into())) + } +} + +// We like our own fusion code +impl Imu for Bno055 { + type Error = Error<::Error>; + + const IMU_TYPE: ImuType = ImuType::Bno055; + + async fn data(&mut self) -> Result { + let accel: Vec3 = self.driver.accel_data()?.into(); + let gyro: Vec3 = self.driver.gyro_data()?.into(); + let mag: Vec3 = self.driver.mag_data()?.into(); + + // why can't we access the timing config... guh + yield_now().await; + + Ok(ImuData { + accel, + gyro: gyro * 1f32.to_radians(), + mag: Some(mag), + }) + } +} diff --git a/firmware/src/imu/driver/mod.rs b/firmware/src/imu/driver/mod.rs new file mode 100644 index 00000000..64b2d19e --- /dev/null +++ b/firmware/src/imu/driver/mod.rs @@ -0,0 +1,28 @@ +mod bmi160; +mod bno055; +mod mpu6050; + +use firmware_protocol::ImuType; + +use super::Vec3; + +pub use self::bmi160::Bmi160; +pub use self::bno055::Bno055; +pub use self::mpu6050::Mpu6050; + +#[derive(Default, Debug)] +pub struct ImuData { + /// Acceleration vector in m/s^2 + pub accel: Vec3, + /// Gyroscope angular rate in rad/s + pub gyro: Vec3, + /// Vector aligned with the earths magnetic field, in arbitrary units + pub mag: Option, +} + +pub trait Imu { + type Error: core::fmt::Debug; + + const IMU_TYPE: ImuType; + async fn data(&mut self) -> Result; +} diff --git a/firmware/src/imu/mpu6050.rs b/firmware/src/imu/driver/mpu6050.rs similarity index 65% rename from firmware/src/imu/mpu6050.rs rename to firmware/src/imu/driver/mpu6050.rs index 4887587e..f0476c1a 100644 --- a/firmware/src/imu/mpu6050.rs +++ b/firmware/src/imu/driver/mpu6050.rs @@ -1,8 +1,9 @@ -use super::{Imu, Quat}; use crate::aliases::I2c; +use crate::imu::{FusedImu, Quat}; use crate::utils; use defmt::{debug, trace}; +use embassy_futures::yield_now; use embedded_hal::blocking::delay::DelayMs; use firmware_protocol::ImuType; use mpu6050_dmp::address::Address; @@ -49,33 +50,22 @@ impl Mpu6050 { } } -impl Imu for Mpu6050 { +impl FusedImu for Mpu6050 { type Error = mpu6050_dmp::error::Error; const IMU_TYPE: ImuType = ImuType::Mpu6050; - fn quat(&mut self) -> nb::Result { - if self.mpu.get_fifo_count()? >= 28 { - let data = self.mpu.read_fifo(&mut self.fifo_buf)?; - let opt = data.get(..16); - if let Some(data) = opt { - let q = mpu6050_dmp::quaternion::Quaternion::from_bytes(data).unwrap(); - let q = nalgebra::Quaternion { - coords: nalgebra::vector![q.x, q.y, q.z, q.w], - }; - Ok(Quat::from_quaternion(q)) - } else { - Err(nb::Error::WouldBlock) - } - } else { - Err(nb::Error::WouldBlock) + async fn quat(&mut self) -> Result { + while self.mpu.get_fifo_count()? < 28 { + // TODO: mmm probably should guess when the next data is available and properly sleep + yield_now().await } - } -} -pub fn new_imu( - i2c: impl crate::aliases::I2c, - delay: &mut impl DelayMs, -) -> impl crate::imu::Imu { - Mpu6050::new(i2c, delay).expect("Failed to initialize MPU6050") + let data = self.mpu.read_fifo(&mut self.fifo_buf)?; + let q = mpu6050_dmp::quaternion::Quaternion::from_bytes(&data[..16]).unwrap(); + let q = nalgebra::Quaternion { + coords: nalgebra::vector![q.x, q.y, q.z, q.w], + }; + Ok(Quat::from_quaternion(q)) + } } diff --git a/firmware/src/imu/fusion/mod.rs b/firmware/src/imu/fusion/mod.rs new file mode 100644 index 00000000..9b7b0030 --- /dev/null +++ b/firmware/src/imu/fusion/mod.rs @@ -0,0 +1,14 @@ +use firmware_protocol::ImuType; + +mod naive; + +use super::Quat; + +pub use naive::Naive; + +pub trait FusedImu { + type Error: core::fmt::Debug; + + const IMU_TYPE: ImuType; + async fn quat(&mut self) -> Result; +} diff --git a/firmware/src/imu/fusion/naive.rs b/firmware/src/imu/fusion/naive.rs new file mode 100644 index 00000000..ae269b32 --- /dev/null +++ b/firmware/src/imu/fusion/naive.rs @@ -0,0 +1,36 @@ +use firmware_protocol::ImuType; + +use crate::imu::{Imu, Quat}; + +use super::FusedImu; + +/// Minimalist integrating sensor "fusion" +pub struct Naive { + inner: T, + quat: Quat, +} + +impl Naive { + pub fn new(inner: T) -> Naive { + Naive { + inner, + quat: Quat::identity(), + } + } +} + +impl FusedImu for Naive { + type Error = T::Error; + + const IMU_TYPE: ImuType = T::IMU_TYPE; + + async fn quat(&mut self) -> Result { + // Load rotation quaternion from underlying driver + let data = self.inner.data().await?; + let rot = Quat::from_euler_angles(data.gyro.x, data.gyro.y, data.gyro.z); + + // Apply newest movement + self.quat *= rot; + Ok(self.quat) + } +} diff --git a/firmware/src/imu/mod.rs b/firmware/src/imu/mod.rs index f5e6a975..bf70186c 100644 --- a/firmware/src/imu/mod.rs +++ b/firmware/src/imu/mod.rs @@ -1,38 +1,20 @@ -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 driver; +mod fusion; use defmt::{debug, info, trace, warn}; use embassy_executor::task; -use embassy_futures::yield_now; use embedded_hal::blocking::delay::DelayMs; -use firmware_protocol::ImuType; use crate::{ aliases::ඞ::{DelayConcrete, I2cConcrete}, - utils::{nb2a, Unreliable}, + utils::Unreliable, }; -pub type Quat = nalgebra::UnitQuaternion; - -pub trait Imu { - type Error: core::fmt::Debug; +pub use driver::*; +pub use fusion::*; - const IMU_TYPE: ImuType; - // TODO: This should be async - fn quat(&mut self) -> nb::Result; -} +pub type Quat = nalgebra::UnitQuaternion; +pub type Vec3 = nalgebra::Vector3; /// Gets data from the IMU #[task] @@ -52,11 +34,14 @@ async fn imu_task_inner( mut delay: impl DelayMs, ) -> ! { debug!("Imu task"); - let mut imu = ඞ::new_imu(i2c, &mut delay); + // We'll do that later + let mut imu = fusion::Naive::new( + driver::Bmi160::new(i2c, &mut delay).expect("Failed to initialize IMU"), + ); info!("Initialized IMU!"); loop { - let q = match nb2a(|| imu.quat()).await { + let q = match imu.quat().await { Ok(q) => q, Err(err) => { warn!("Error in IMU: {}", defmt::Debug2Format(&err)); @@ -71,6 +56,5 @@ async fn imu_task_inner( q.coords.w ); quat_signal.signal(q); - yield_now().await // Yield to ensure fairness } } diff --git a/firmware/src/imu/stubbed.rs b/firmware/src/imu/stubbed.rs deleted file mode 100644 index 95be420c..00000000 --- a/firmware/src/imu/stubbed.rs +++ /dev/null @@ -1,27 +0,0 @@ -use super::{Imu, Quat}; - -use defmt::debug; -use embedded_hal::blocking::delay::DelayMs; -use firmware_protocol::ImuType; - -/// Fakes an IMU for easier testing. -struct FakeImu; - -impl Imu for FakeImu { - type Error = (); - - const IMU_TYPE: ImuType = ImuType::Unknown(0xFF); - - fn quat(&mut self) -> nb::Result { - Ok(Quat::identity()) - } -} - -#[allow(dead_code)] -pub fn new_imu( - _i2c: impl crate::aliases::I2c, - _delay: &mut impl DelayMs, -) -> impl crate::imu::Imu { - debug!("Created FakeImu"); - FakeImu -} diff --git a/firmware/src/main.rs b/firmware/src/main.rs index f89dc5ec..2f672db1 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -8,6 +8,8 @@ #![feature(alloc_error_handler)] // We want to do some floating point math at compile time #![feature(const_fn_floating_point_arithmetic)] +// Asynchronous IMU sampling +#![feature(async_fn_in_trait)] #![deny(unsafe_op_in_unsafe_fn)] load_dotenv::try_load_dotenv!(); diff --git a/firmware/src/utils.rs b/firmware/src/utils.rs index 14cfe9f0..7e050006 100644 --- a/firmware/src/utils.rs +++ b/firmware/src/utils.rs @@ -1,4 +1,3 @@ -use embassy_futures::yield_now; use embassy_sync::blocking_mutex::raw::NoopRawMutex; /// Signals are used for concurrently updating values, where we only care about @@ -43,16 +42,3 @@ pub fn retry( } last_result } - -/// Converts a nb::Result to an async function by looping and yielding to the async -/// executor. -pub async fn nb2a(mut f: impl FnMut() -> nb::Result) -> Result { - loop { - let v = f(); - match v { - Ok(t) => return Ok(t), - Err(nb::Error::Other(e)) => return Err(e), - Err(nb::Error::WouldBlock) => yield_now().await, - } - } -}