Add ISM330DHCX

.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

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"
+ism330dhcx = "0.4"
 
 # 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;