tmp debugging

Author: TheButlah

firmware/Cargo.lock

@@ -223,6 +223,8 @@ dotenvy = "0.15"
 # 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/tstellanova/bno080", rev = "f95e860" }
 
 # Emable use of `atomic-polyfill`
 defmt = { git = "https://github.com/TheButlah/defmt", rev = "b5d5e316ea56250dcb25b3549bcf4bfdb1687796" }

firmware/Cargo.toml

@@ -2,13 +2,13 @@ use crate::aliases::{Delay, I2c};
 use crate::imu::{Imu, Quat};
 use crate::utils;
 
-use ::bno080::interface::I2cInterface;
+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::DEFAULT_ADDRESS;
+pub const I2C_ADDR: u8 = ::bno080::interface::i2c::ALTERNATE_ADDRESS;
 
 pub type DriverError<I2c> = ::bno080::wrapper::WrapperError<
 	<I2cInterface<I2c> as SensorInterface>::SensorError,
@@ -24,28 +24,85 @@ impl<I: crate::aliases::I2c> Bno080<I> {
 	) -> Result<Self, DriverError<I>> {
 		let interface = ::bno080::interface::I2cInterface::new(i2c, I2C_ADDR);
 		let mut driver = ::bno080::wrapper::BNO080::new_with_interface(interface);
-		utils::retry(
-			4,
-			(),
-			|_| -> Result<(), ((), DriverError<I>)> {
-				delay.delay_ms(100u32);
-				trace!("Flushing comms");
-				let _ = driver.eat_all_messages(delay);
-				delay.delay_ms(100u32);
-				trace!("Constructing IMU");
 
-				driver.init(delay).map_err(|e| ((), e))?;
-				debug!("Initialized bno080 driver");
-				delay.delay_ms(100u32);
-				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))?;
+		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 })
 	}
 }

firmware/src/imu/bno080.rs

@@ -69,7 +69,7 @@ pub fn get_peripherals() -> Peripherals<I2cConcrete<'static>, DelayConcrete> {
 		p.I2C0,
 		sda,
 		scl,
-		400u32.kHz(),
+		100u32.kHz(),
 		&mut system.peripheral_clock_control,
 		&clocks,
 	);