Add bno080 support

firmware/.env.template

@@ -12,5 +12,4 @@ DEFMT_LOG="info"
 
 # Provide this if you have a particular board you want to use. Note that relative paths
 # are resolved from the same folder as `build.rs`.
-BOARD="boards/ferrous_slime.toml"
-
+BOARD="boards/dev.toml"

firmware/.gitignore

@@ -3,3 +3,4 @@
 *.bin
 *.uf2
 \.env
+/boards/dev.toml # convenient for testing out settings locally

firmware/.vscode/launch.json

@@ -13,15 +13,16 @@
                 "resetAfterFlashing": true,
                 "haltAfterReset": true,
             },
-            "runtimeExecutable": "/Users/ryan.butler/.cargo/bin/probe-rs-debugger",
-            "consoleLogLevel": "Debug",
+            "connectUnderReset": true,
+            "runtimeExecutable": "/home/ryan/.cargo/bin/probe-rs-debugger",
+            // "consoleLogLevel": "Trace",
             "name": "probe_rs Executable Test",
-            "chip": "nrf52840", //!MODIFY
-            "wireProtocol": "Swd",
+            "chip": "esp32c3", //!MODIFY
+            "wireProtocol": "Jtag",
             "cwd": "${workspaceFolder}",
             "coreConfigs": [
                 {
-                    "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/debug/firmware", //!MODIFY
+                    "programBinary": "${workspaceFolder}/target/riscv32imc-unknown-none-elf/debug/firmware", //!MODIFY
                     "rttEnabled": true,
                 }
             ]

firmware/Cargo.toml

@@ -14,7 +14,7 @@ rust-version.workspace = true
 
 
 [features]
-default = ["mcu-esp32c3", "imu-mpu6050", "log-rtt", "net-wifi"]
+default = ["mcu-esp32c3", "imu-bno080", "log-rtt", "net-wifi"]
 # default = [
 #   "mcu-nrf52840",
 #   "imu-stubbed",
@@ -67,6 +67,7 @@ net-stubbed = []                            # Stubs out network
 # Supported IMUs
 imu-bmi160 = ["dep:bmi160"]
 imu-mpu6050 = ["dep:mpu6050-dmp"]
+imu-bno080 = ["dep:bno080"]
 imu-stubbed = []                  # Stubs out the IMU
 
 # Supported defmt loggers
@@ -193,6 +194,7 @@ defmt-bbq = { version = "0.1", optional = true }
 # Peripheral drivers
 mpu6050-dmp = { version = "0.2", optional = true }
 bmi160 = { version = "0.1", optional = true }
+bno080 = { version = "0.1", optional = true }
 
 # Other crates
 static_cell = "1"
@@ -205,8 +207,8 @@ fugit = "0.3"
 firmware_protocol = { path = "../networking/firmware_protocol", features = [
   "nalgebra031",
 ] }
-load-dotenv = "0.1"
 paste = "1.0"
+load-dotenv = "0.1"
 
 [build-dependencies]
 feature_utils = "0.0.0"
@@ -215,12 +217,14 @@ serde = { version = "1", features = ["derive"] }
 toml = "0.5"
 eyre = "0.6"
 color-eyre = "0.6"
-
+dotenvy = "0.15"
 
 [patch.crates-io]
 # Enable non-default I2C addresses and InitError
 mpu6050-dmp = { git = "https://github.com/TheButlah/mpu6050-dmp-rs", rev = "77128a75a51547baadc0f0ef99aba4432e848f10" }
 bmi160 = { git = "https://github.com/TheButlah/bmi160-rs", rev = "e99802b" }
+# has a way to release i2c that isn't published
+bno080 = { git = "https://github.com/TheButlah/bno080", rev = "f95e860" }
 
 # Emable use of `atomic-polyfill`
 defmt = { git = "https://github.com/TheButlah/defmt", rev = "b5d5e316ea56250dcb25b3549bcf4bfdb1687796" }

firmware/boards/dev.toml

@@ -0,0 +1,7 @@
+[pins]
+scl = "2"
+sda = "4"
+int0 = "6"
+int1 = "7"
+tx = "8"
+rx = "9"

firmware/build.rs

@@ -8,7 +8,7 @@ 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!("imu-stubbed", "imu-bno080", "imu-mpu6050", "imu-bmi160");
 mandatory_and_unique!("log-rtt", "log-usb-serial", "log-uart");
 mandatory_and_unique!("net-wifi", "net-ble", "net-stubbed");
 
@@ -33,6 +33,7 @@ macro_rules! memory_x {
 }
 
 fn main() -> Result<()> {
+	let _ = dotenvy::dotenv();
 	#[cfg(all(feature = "mcu-nrf52832", feature = "log-usb-serial"))]
 	compile_error!("the nrf52832 doesn't support USB!");
 
@@ -146,7 +147,8 @@ struct Pins {
 impl BoardConfig {
 	/// Loads a board config from a file
 	fn from_file(p: &Path) -> Result<Self> {
-		let s = std::fs::read_to_string(p).wrap_err("Failed to read board toml")?;
+		let s = std::fs::read_to_string(p.clone())
+			.wrap_err(format!("Failed to read board toml: {}", p.display()))?;
 		toml::from_str(&s).wrap_err("Failed to deserialize board toml")
 	}
 	/// Gets the path to the board config, or errors if we can't pick one.

firmware/rustfmt.toml

@@ -0,0 +1 @@
+../rustfmt.toml

firmware/src/aliases.rs

@@ -51,14 +51,27 @@ pub mod ඞ {
 pub trait I2c:
 	embedded_hal::blocking::i2c::Write<Error = <Self as I2c>::Error>
 	+ embedded_hal::blocking::i2c::WriteRead<Error = <Self as I2c>::Error>
+	+ embedded_hal::blocking::i2c::Read<Error = <Self as I2c>::Error>
 {
 	type Error: core::fmt::Debug;
 }
 impl<
 		T: embedded_hal::blocking::i2c::Write<Error = E>
-			+ embedded_hal::blocking::i2c::WriteRead<Error = E>,
+			+ embedded_hal::blocking::i2c::WriteRead<Error = E>
+			+ embedded_hal::blocking::i2c::Read<Error = E>,
 		E: core::fmt::Debug,
 	> I2c for T
 {
 	type Error = E;
 }
+
+pub trait Delay:
+	embedded_hal::blocking::delay::DelayMs<u8> + embedded_hal::blocking::delay::DelayMs<u32>
+{
+}
+impl<
+		T: embedded_hal::blocking::delay::DelayMs<u8>
+			+ embedded_hal::blocking::delay::DelayMs<u32>,
+	> Delay for T
+{
+}

firmware/src/imu/bno080.rs

@@ -0,0 +1,127 @@
+use crate::aliases::{Delay, I2c};
+use crate::imu::{Imu, Quat};
+use crate::utils;
+
+use bno080::interface::I2cInterface;
+use bno080::interface::SensorInterface;
+use defmt::{debug, trace, warn};
+use firmware_protocol::ImuType;
+
+pub const IMU_REPORT_INTERVAL_MS: u16 = 10;
+pub const I2C_ADDR: u8 = ::bno080::interface::i2c::ALTERNATE_ADDRESS;
+
+pub type DriverError<I2c> = ::bno080::wrapper::WrapperError<
+	<I2cInterface<I2c> as SensorInterface>::SensorError,
+>;
+
+struct Bno080<I: I2c> {
+	driver: ::bno080::wrapper::BNO080<I2cInterface<I>>,
+}
+impl<I: crate::aliases::I2c> Bno080<I> {
+	pub fn new(
+		i2c: I,
+		delay: &mut impl crate::aliases::Delay,
+	) -> Result<Self, DriverError<I>> {
+		let interface = ::bno080::interface::I2cInterface::new(i2c, I2C_ADDR);
+		let mut driver = ::bno080::wrapper::BNO080::new_with_interface(interface);
+
+		let mut last_err = Ok(());
+		for i in 0..4 {
+			delay.delay_ms(100u32);
+			if let Err(e) = driver.soft_reset() {
+				warn!(
+					"Error resetting bno080 (atmpt {}): {:?}",
+					i + 1,
+					defmt::Debug2Format(&e)
+				);
+				last_err = Err(e);
+			} else {
+				last_err = Ok(());
+				break;
+			}
+		}
+		last_err?;
+
+		let mut last_err = Ok(());
+		for i in 0..4 {
+			delay.delay_ms(500u32);
+			if let Err(e) = driver.init(delay) {
+				warn!(
+					"Error initializing bno080 (atmpt {}): {:?}",
+					i + 1,
+					defmt::Debug2Format(&e)
+				);
+				last_err = Err(e);
+			} else {
+				last_err = Ok(());
+				break;
+			}
+		}
+		last_err?;
+
+		let mut last_err = Ok(());
+		for i in 0..4 {
+			if let Err(e) = driver.enable_rotation_vector(IMU_REPORT_INTERVAL_MS) {
+				warn!(
+					"Error enabling rotation (atmpt {}): {:?}",
+					i + 1,
+					defmt::Debug2Format(&e)
+				);
+				last_err = Err(e);
+			} else {
+				last_err = Ok(());
+				break;
+			}
+		}
+		last_err?;
+
+		// utils::retry(
+		// 	4,
+		// 	(),
+		// 	|_| -> Result<(), ((), DriverError<I>)> {
+		// 		// delay.delay_ms(400u32);
+		// 		// trace!("Flushing comms");
+		// 		// let _ = i2c.write(I2C_ADDR, &[0]);
+		//
+		// 		// delay.delay_ms(400u32);
+		// 		trace!("Constructing IMU");
+		//
+		// 		// if let Err(e) = driver.init(delay) {
+		// 		// 	return Err((driver.free().free(), e));
+		// 		// }
+		// 		debug!("Initialized bno080 driver");
+		// 		// delay.delay_ms(100u32);
+		// 		// if let Err(e) = driver.enable_rotation_vector(IMU_REPORT_INTERVAL_MS) {
+		// 		// 	return Err((driver.free().free(), e));
+		// 		// }
+		// 		driver
+		// 			.enable_rotation_vector(IMU_REPORT_INTERVAL_MS)
+		// 			.map_err(|e| ((), e))?;
+		// 		debug!("Enabled rotation vector");
+		// 		Ok(())
+		// 	},
+		// 	|i| warn!("Retrying IMU connection (attempts so far: {})", i + 1),
+		// )
+		// .map_err(|(_, e)| panic!("{:?}", e))?;
+		Ok(Self { driver })
+	}
+}
+
+impl<I: I2c> Imu for Bno080<I> {
+	type Error = DriverError<I>;
+
+	const IMU_TYPE: ImuType = ImuType::Bno080;
+
+	fn quat(&mut self) -> nb::Result<super::Quat, Self::Error> {
+		let [i, j, k, w] = self.driver.rotation_quaternion()?;
+		let q = nalgebra::Quaternion {
+			coords: nalgebra::vector![i, j, k, w],
+		};
+		// TODO: This is already normalized, we can use unsafe for performance
+		Ok(Quat::from_quaternion(q))
+	}
+}
+
+pub fn new_imu(i2c: impl I2c, delay: &mut impl Delay) -> impl Imu {
+	Bno080::new(i2c, delay).expect("Failed to initialize bno080")
+}

firmware/src/imu/mod.rs

@@ -13,10 +13,13 @@ mod ඞ;
 #[path = "bmi160/mod.rs"]
 mod ඞ;
 
+#[cfg(feature = "imu-bno080")]
+#[path = "bno080.rs"]
+mod ඞ;
+
 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::{
@@ -49,7 +52,7 @@ pub async fn imu_task(
 async fn imu_task_inner(
 	quat_signal: &Unreliable<Quat>,
 	i2c: impl crate::aliases::I2c,
-	mut delay: impl DelayMs<u32>,
+	mut delay: impl crate::aliases::Delay,
 ) -> ! {
 	debug!("Imu task");
 	let mut imu = ඞ::new_imu(i2c, &mut delay);

firmware/src/imu/mpu6050.rs

@@ -42,7 +42,7 @@ impl<I: I2c> Mpu6050<I> {
 					fifo_buf: [0; 28],
 				})
 			},
-			|i| debug!("Retrying IMU connection (attempts so far: {})", i + 1),
+			|i| warn!("Retrying IMU connection (attempts so far: {})", i + 1),
 		)
 		// Map converts from tuple -> struct
 		.map_err(|(i2c, error)| InitError { i2c, error })

firmware/src/main.rs

@@ -24,13 +24,8 @@ mod bbq_logger;
 
 use defmt::debug;
 use embassy_executor::Executor;
-use embedded_hal::blocking::delay::DelayMs;
 use static_cell::StaticCell;
 
-use crate::imu::Quat;
-use crate::networking::protocol::Packets;
-use crate::utils::Unreliable;
-
 #[cfg(cortex_m)]
 use cortex_m_rt::entry;
 #[cfg(riscv)]
@@ -40,6 +35,11 @@ use xtensa_lx_rt::entry;
 
 #[entry]
 fn main() -> ! {
+	use crate::imu::Quat;
+	use crate::networking::protocol::Packets;
+	use crate::utils::Unreliable;
+	use embedded_hal::blocking::delay::DelayMs;
+
 	#[cfg(bbq)]
 	let bbq = defmt_bbq::init().unwrap();