IMU rotator

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,8 @@ 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"
 
 # Other crates
 static_cell = "1"

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");
 

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<Self, InvalidBitPattern> {
+		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<Self, InvalidNum> {
+		// 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<Self, InvalidBitPattern> {
+		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<Self, InvalidNum> {
+		// 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;

firmware/src/imu/bmi160/mod.rs → 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<I: I2c> Imu for Bmi160<I> {
 
 	const IMU_TYPE: ImuType = ImuType::Bmi160;
 
-	fn quat(&mut self) -> nb::Result<Quat, Self::Error> {
-		let data = self.driver.data(SensorSelector::new().gyro())?;
-		let gyro_vel_euler = data.gyro.unwrap();
+	async fn data(&mut self) -> Result<ImuData, Self::Error> {
+		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<u32>,
-) -> impl crate::imu::Imu {
-	Bmi160::new(i2c, delay).expect("Failed to initialize BMI160")
-}