From d5c1f2790cb048c912c095efbdfe3db095f1fd91 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 16:54:35 -0800 Subject: [PATCH 01/24] initial commit --- Cargo.toml | 1 + imu/Cargo.toml | 1 + imu/bindings/build.rs | 7 + imu/bmi088/Cargo.toml | 15 ++ imu/bmi088/src/bin/read_bmi088.rs | 23 ++ imu/bmi088/src/lib.rs | 361 ++++++++++++++++++++++++++++++ imu/bmi088/src/registers.rs | 81 +++++++ imu/src/lib.rs | 4 + 8 files changed, 493 insertions(+) create mode 100644 imu/bindings/build.rs create mode 100644 imu/bmi088/Cargo.toml create mode 100644 imu/bmi088/src/bin/read_bmi088.rs create mode 100644 imu/bmi088/src/lib.rs create mode 100644 imu/bmi088/src/registers.rs diff --git a/Cargo.toml b/Cargo.toml index b460579..5212d83 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -6,6 +6,7 @@ members = [ "imu/hexmove", "imu/hiwonder", "imu/bno055", + "imu/bmi088", ] resolver = "2" diff --git a/imu/Cargo.toml b/imu/Cargo.toml index 6f9f59b..98a186f 100644 --- a/imu/Cargo.toml +++ b/imu/Cargo.toml @@ -18,3 +18,4 @@ hiwonder = "0.2.3" linux_bno055 = "0.0.6" socketcan = "3.3.0" log = "0.4" +linux_bmi088 = { path = "./bmi088" } diff --git a/imu/bindings/build.rs b/imu/bindings/build.rs new file mode 100644 index 0000000..9feb8eb --- /dev/null +++ b/imu/bindings/build.rs @@ -0,0 +1,7 @@ +fn main() { + // Tell cargo to look for Python libraries in the conda env + println!("cargo:rustc-link-search=/home/kuwajerw/anaconda3/envs/zdemos/lib"); + + // Link against Python library (name might be python3.x or python3.x.so) + println!("cargo:rustc-link-lib=python3.11"); +} \ No newline at end of file diff --git a/imu/bmi088/Cargo.toml b/imu/bmi088/Cargo.toml new file mode 100644 index 0000000..1d38e1c --- /dev/null +++ b/imu/bmi088/Cargo.toml @@ -0,0 +1,15 @@ +[package] +name = "linux_bmi088" +version.workspace = true +authors.workspace = true +edition.workspace = true +description = "Interface for BMI088 IMU" +license.workspace = true +repository.workspace = true + +[dependencies] +byteorder = "1.5" +i2cdev = "0.6" +log = "0.4" +num-derive = "0.4" +num-traits = "0.2" \ No newline at end of file diff --git a/imu/bmi088/src/bin/read_bmi088.rs b/imu/bmi088/src/bin/read_bmi088.rs new file mode 100644 index 0000000..403bc35 --- /dev/null +++ b/imu/bmi088/src/bin/read_bmi088.rs @@ -0,0 +1,23 @@ +use linux_bmi088::Bmi088Reader; +use std::thread; +use std::time::Duration; + +fn main() -> Result<(), Box> { + let imu = Bmi088Reader::new("/dev/i2c-1")?; + println!("Reading BMI088 sensor data..."); + + loop { + let data = imu.get_data()?; + println!("Quaternion: w={:.3}, x={:.3}, y={:.3}, z={:.3}", + data.quaternion.w, data.quaternion.x, data.quaternion.y, data.quaternion.z); + println!("Accel: x={:.2} g, y={:.2} g, z={:.2} g", + data.accelerometer.x, data.accelerometer.y, data.accelerometer.z); + println!("Gyro: x={:.2} °/s, y={:.2} °/s, z={:.2} °/s", + data.gyroscope.x, data.gyroscope.y, data.gyroscope.z); + println!("Euler: roll={:.1}°, pitch={:.1}°, yaw={:.1}°", + data.euler.roll, data.euler.pitch, data.euler.yaw); + println!("Temp: {:.1} °C", data.temperature); + println!("----------------------------------------"); + thread::sleep(Duration::from_millis(100)); + } +} \ No newline at end of file diff --git a/imu/bmi088/src/lib.rs b/imu/bmi088/src/lib.rs new file mode 100644 index 0000000..3ee1b27 --- /dev/null +++ b/imu/bmi088/src/lib.rs @@ -0,0 +1,361 @@ +use byteorder::{LittleEndian, ByteOrder}; +use i2cdev::core::I2CDevice; // Add this import +use i2cdev::linux::LinuxI2CDevice; +use log::{error, warn}; +use std::sync::{Arc, RwLock, mpsc}; +use std::thread; +use std::time::Duration; + +mod registers; +use registers::{AccelRegisters, GyroRegisters, Constants, AccelRange, GyroRange}; + +/// 3D vector type. +#[derive(Debug, Clone, Copy, Default)] +pub struct Vector3 { + pub x: f32, + pub y: f32, + pub z: f32, +} + +/// Quaternion type. +#[derive(Debug, Clone, Copy)] +pub struct Quaternion { + pub w: f32, + pub x: f32, + pub y: f32, + pub z: f32, +} + +impl Default for Quaternion { + fn default() -> Self { + Self { + w: 1.0, + x: 0.0, + y: 0.0, + z: 0.0, + } + } +} + +/// Euler angles (in degrees). +#[derive(Debug, Clone, Copy, Default)] +pub struct EulerAngles { + pub roll: f32, + pub pitch: f32, + pub yaw: f32, +} + +/// Sensor data struct. +#[derive(Debug, Clone, Copy)] +pub struct Bmi088Data { + pub quaternion: Quaternion, + pub euler: EulerAngles, + pub accelerometer: Vector3, + pub gyroscope: Vector3, + pub temperature: f32, +} + +impl Default for Bmi088Data { + fn default() -> Self { + Self { + quaternion: Quaternion::default(), + euler: EulerAngles::default(), + accelerometer: Vector3::default(), + gyroscope: Vector3::default(), + temperature: 0.0, + } + } +} + +/// Errors for BMI088 operations. +#[derive(Debug)] +pub enum Error { + I2c(i2cdev::linux::LinuxI2CError), + InvalidChipId, + ReadError, + WriteError, +} + +impl From for Error { + fn from(e: i2cdev::linux::LinuxI2CError) -> Self { + Error::I2c(e) + } +} + +impl std::fmt::Display for Error { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + Error::I2c(e) => write!(f, "I2C error: {}", e), + Error::InvalidChipId => write!(f, "Invalid chip ID"), + Error::ReadError => write!(f, "Read error"), + Error::WriteError => write!(f, "Write error"), + } + } +} + +impl std::error::Error for Error { + fn source(&self) -> Option<&(dyn std::error::Error + 'static)> { + match self { + Error::I2c(e) => Some(e), + _ => None, + } + } +} + +/// Low-level BMI088 driver. +pub struct Bmi088 { + accel_i2c: LinuxI2CDevice, + gyro_i2c: LinuxI2CDevice, + accel_range: AccelRange, + gyro_range: GyroRange, + /// Used for simple Euler integration of gyro data. + simple_euler: EulerAngles, +} + +impl Bmi088 { + /// Initializes the BMI088 sensor on the given I2C bus. + pub fn new(i2c_path: &str) -> Result { + println!("Initializing BMI088..."); + + let mut accel_i2c = LinuxI2CDevice::new(i2c_path, Constants::AccelI2cAddr as u16)?; + let gyro_i2c = LinuxI2CDevice::new(i2c_path, Constants::GyroI2cAddr as u16)?; + + // Verify accelerometer chip ID. + let chip_id = accel_i2c.smbus_read_byte_data(AccelRegisters::ChipId as u8)?; + if chip_id != Constants::AccelChipIdValue as u8 { + return Err(Error::InvalidChipId); + } + println!("Accel chip ID verified: 0x{:02X}", chip_id); + + // Soft reset, power-up, and configure accelerometer. + accel_i2c.smbus_write_byte_data(AccelRegisters::SoftReset as u8, Constants::SoftResetCmd as u8)?; + thread::sleep(Duration::from_millis(50)); + accel_i2c.smbus_write_byte_data(AccelRegisters::PowerCtrl as u8, 0x04)?; + accel_i2c.smbus_write_byte_data(AccelRegisters::AccConf as u8, 0x80)?; + accel_i2c.smbus_write_byte_data(AccelRegisters::AccRange as u8, AccelRange::G3 as u8)?; + + // Configure gyroscope. + let mut gyro_i2c = gyro_i2c; // no mutable needed outside this scope + gyro_i2c.smbus_write_byte_data(GyroRegisters::PowerMode as u8, 0x00)?; + gyro_i2c.smbus_write_byte_data(GyroRegisters::Range as u8, GyroRange::Dps2000 as u8)?; + gyro_i2c.smbus_write_byte_data(GyroRegisters::Bandwidth as u8, 0x07)?; + + Ok(Bmi088 { + accel_i2c, + gyro_i2c, + accel_range: AccelRange::G3, + gyro_range: GyroRange::Dps2000, + simple_euler: EulerAngles::default(), + }) + } + + /// Reads raw accelerometer data and remaps axes so that gravity appears along -Z. + pub fn read_raw_accelerometer(&mut self) -> Result { + let mut buf = [0u8; 6]; + for i in 0..6 { + buf[i] = self.accel_i2c.smbus_read_byte_data(AccelRegisters::AccelXLsb as u8 + i as u8)?; + } + let scale = match self.accel_range { + AccelRange::G3 => 3.0 / 32768.0, + AccelRange::G6 => 6.0 / 32768.0, + AccelRange::G12 => 12.0 / 32768.0, + AccelRange::G24 => 24.0 / 32768.0, + }; + let raw_x = (LittleEndian::read_i16(&buf[0..2]) as f32) * scale; + let raw_y = (LittleEndian::read_i16(&buf[2..4]) as f32) * scale; + let raw_z = (LittleEndian::read_i16(&buf[4..6]) as f32) * scale; + Ok(Vector3 { + x: raw_y, + y: raw_z, + z: raw_x, + }) + } + + /// Reads raw gyroscope data and remaps axes for consistency. + pub fn read_raw_gyroscope(&mut self) -> Result { + let mut buf = [0u8; 6]; + for i in 0..6 { + buf[i] = self.gyro_i2c.smbus_read_byte_data(GyroRegisters::XLsb as u8 + i as u8)?; + } + let scale = match self.gyro_range { + GyroRange::Dps2000 => 2000.0 / 32768.0, + GyroRange::Dps1000 => 1000.0 / 32768.0, + GyroRange::Dps500 => 500.0 / 32768.0, + GyroRange::Dps250 => 250.0 / 32768.0, + GyroRange::Dps125 => 125.0 / 32768.0, + }; + let raw_x = (LittleEndian::read_i16(&buf[0..2]) as f32) * scale; + let raw_y = (LittleEndian::read_i16(&buf[2..4]) as f32) * scale; + let raw_z = (LittleEndian::read_i16(&buf[4..6]) as f32) * scale; + Ok(Vector3 { + x: raw_y, + y: raw_z, + z: raw_x, + }) + } + + /// Reads temperature (in °C) from the accelerometer. + pub fn read_temperature(&mut self) -> Result { + let msb = self.accel_i2c.smbus_read_byte_data(AccelRegisters::TempMsb as u8)? as i16; + let lsb = self.accel_i2c.smbus_read_byte_data(AccelRegisters::TempLsb as u8)? as i16; + let temp_raw = (msb * 8) + (lsb / 32); + Ok((temp_raw as f32) * 0.125 + 23.0) + } + + /// Updates the simple Euler integration state based on gyroscope data. + pub fn update_simple_euler(&mut self, dt: f32) -> Result<(), Error> { + let gyro = self.read_raw_gyroscope()?; + // Standard integration: roll from gyro.x, pitch from gyro.y, yaw from gyro.z. + self.simple_euler.roll += gyro.x * dt; + self.simple_euler.pitch += gyro.y * dt; + self.simple_euler.yaw += gyro.z * dt; + Ok(()) + } + + /// Returns the current integrated Euler angles. + pub fn get_simple_euler_angles(&self) -> EulerAngles { + self.simple_euler + } +} + +/// BMI088Reader runs a background thread to update sensor data continuously. +pub struct Bmi088Reader { + data: Arc>, + command_tx: mpsc::Sender, + running: Arc>, +} + +/// Commands sent to the reading thread. +#[derive(Debug)] +pub enum ImuCommand { + SetAccelRange(AccelRange), + SetGyroRange(GyroRange), + Reset, + Stop, +} + +impl Bmi088Reader { + /// Creates a new BMI088Reader. + pub fn new(i2c_bus: &str) -> Result { + println!("Initializing LOCAL BMI088 implementation..."); + let data = Arc::new(RwLock::new(Bmi088Data::default())); + let running = Arc::new(RwLock::new(true)); + let (command_tx, command_rx) = mpsc::channel(); + let reader = Bmi088Reader { + data: Arc::clone(&data), + command_tx, + running: Arc::clone(&running), + }; + reader.start_reading_thread(i2c_bus, command_rx)?; + Ok(reader) + } + + fn start_reading_thread(&self, i2c_bus: &str, command_rx: mpsc::Receiver) -> Result<(), Error> { + let data = Arc::clone(&self.data); + let running = Arc::clone(&self.running); + let i2c_bus = i2c_bus.to_string(); + let (tx, rx) = mpsc::channel(); + + thread::spawn(move || { + let init_result = Bmi088::new(&i2c_bus); + if let Err(e) = init_result { + error!("Failed to initialize BMI088: {}", e); + let _ = tx.send(Err(e)); + return; + } + let mut imu = init_result.unwrap(); + let _ = tx.send(Ok(())); + let mut simple_euler = EulerAngles::default(); + + while let Ok(guard) = running.read() { + if !*guard { break; } + + // Handle incoming commands. + if let Ok(cmd) = command_rx.try_recv() { + match cmd { + ImuCommand::SetAccelRange(range) => { + imu.accel_range = range; + let _ = imu.accel_i2c.smbus_write_byte_data(AccelRegisters::AccRange as u8, range as u8); + } + ImuCommand::SetGyroRange(range) => { + imu.gyro_range = range; + let _ = imu.gyro_i2c.smbus_write_byte_data(GyroRegisters::Range as u8, range as u8); + } + ImuCommand::Reset => { + // Reinitialize if needed. + let _ = imu = Bmi088::new(&i2c_bus).unwrap(); + } + ImuCommand::Stop => { + if let Ok(mut w) = running.write() { *w = false; } + break; + } + } + } + + let mut sensor_data = Bmi088Data::default(); + if let Ok(accel) = imu.read_raw_accelerometer() { + sensor_data.accelerometer = accel; + } else { + warn!("Failed to read accelerometer"); + } + if let Ok(gyro) = imu.read_raw_gyroscope() { + sensor_data.gyroscope = gyro; + let dt = 0.01; + simple_euler.roll += gyro.x * dt; + simple_euler.pitch += gyro.y * dt; + simple_euler.yaw += gyro.z * dt; + sensor_data.euler = simple_euler; + sensor_data.quaternion = euler_to_quaternion(simple_euler); + } else { + warn!("Failed to read gyroscope"); + } + if let Ok(temp) = imu.read_temperature() { + sensor_data.temperature = temp; + } + + if let Ok(mut shared) = data.write() { + *shared = sensor_data; + } + thread::sleep(Duration::from_millis(10)); + } + }); + + rx.recv().map_err(|_| Error::ReadError)? + } + + /// Returns the most recent sensor data. + pub fn get_data(&self) -> Result { + self.data.read().map(|d| *d).map_err(|_| Error::ReadError) + } + + /// Stops the reading thread. + pub fn stop(&self) -> Result<(), Error> { + self.command_tx.send(ImuCommand::Stop).map_err(|_| Error::WriteError) + } +} + +impl Drop for Bmi088Reader { + fn drop(&mut self) { + let _ = self.stop(); + } +} + +// Add this helper function for quaternion conversion +fn euler_to_quaternion(e: EulerAngles) -> Quaternion { + let (roll, pitch, yaw) = ( + e.roll.to_radians(), + e.pitch.to_radians(), + e.yaw.to_radians() + ); + + let (sr, cr) = (roll * 0.5).sin_cos(); + let (sp, cp) = (pitch * 0.5).sin_cos(); + let (sy, cy) = (yaw * 0.5).sin_cos(); + + Quaternion { + w: cr * cp * cy + sr * sp * sy, + x: sr * cp * cy - cr * sp * sy, + y: cr * sp * cy + sr * cp * sy, + z: cr * cp * sy - sr * sp * cy, + } +} \ No newline at end of file diff --git a/imu/bmi088/src/registers.rs b/imu/bmi088/src/registers.rs new file mode 100644 index 0000000..eb99feb --- /dev/null +++ b/imu/bmi088/src/registers.rs @@ -0,0 +1,81 @@ +use num_derive::{FromPrimitive, ToPrimitive}; + +#[derive(Debug, Clone, Copy, FromPrimitive, ToPrimitive)] +pub enum AccelRegisters { + ChipId = 0x00, + Error = 0x02, + Status = 0x1E, + AccelXLsb = 0x12, + AccelXMsb = 0x13, + AccelYLsb = 0x14, + AccelYMsb = 0x15, + AccelZLsb = 0x16, + AccelZMsb = 0x17, + TempMsb = 0x22, + TempLsb = 0x23, + PowerCtrl = 0x7D, + PowerConf = 0x7C, + AccConf = 0x40, + AccRange = 0x41, + SoftReset = 0x7E, +} + +#[derive(Debug, Clone, Copy, FromPrimitive, ToPrimitive)] +pub enum GyroRegisters { + ChipId = 0x00, + XLsb = 0x02, + XMsb = 0x03, + YLsb = 0x04, + YMsb = 0x05, + ZLsb = 0x06, + ZMsb = 0x07, + Range = 0x0F, + Bandwidth = 0x10, + PowerMode = 0x11, +} + +#[derive(Debug, Clone, Copy)] +pub enum Constants { + AccelChipIdValue = 0x1E, + AccelI2cAddr = 0x19, // SDO pulled high + GyroI2cAddr = 0x69, // SDO pulled high + SoftResetCmd = 0xB6, +} + +#[derive(Debug, Clone, Copy)] +pub enum AccelRange { + G3 = 0x00, + G6 = 0x01, + G12 = 0x02, + G24 = 0x03, +} + +#[derive(Debug, Clone, Copy)] +pub enum GyroRange { + Dps2000 = 0x00, + Dps1000 = 0x01, + Dps500 = 0x02, + Dps250 = 0x03, + Dps125 = 0x04, +} + +#[derive(Debug, Clone, Copy, FromPrimitive, ToPrimitive)] +pub enum AccelOdr { + Hz12_5 = 0x05, + Hz25 = 0x06, + Hz50 = 0x07, + Hz100 = 0x08, + Hz200 = 0x09, + Hz400 = 0x0A, + Hz800 = 0x0B, + Hz1600 = 0x0C, +} + +#[derive(Debug, Clone, Copy, FromPrimitive, ToPrimitive)] +pub enum GyroOdr { + Hz100 = 0x07, + Hz200 = 0x06, + Hz400 = 0x03, + Hz1000 = 0x02, + Hz2000 = 0x01, +} \ No newline at end of file diff --git a/imu/src/lib.rs b/imu/src/lib.rs index 1dfb974..97143a6 100644 --- a/imu/src/lib.rs +++ b/imu/src/lib.rs @@ -9,3 +9,7 @@ pub mod hiwonder { pub mod bno055 { pub use ::linux_bno055::*; } + +pub mod bmi088 { + pub use ::linux_bmi088::*; +} From 57ced3240752a9f6ef8a4bb8557e1e91b0da8e4c Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 16:55:34 -0800 Subject: [PATCH 02/24] remove build file --- imu/bindings/build.rs | 7 ------- 1 file changed, 7 deletions(-) delete mode 100644 imu/bindings/build.rs diff --git a/imu/bindings/build.rs b/imu/bindings/build.rs deleted file mode 100644 index 9feb8eb..0000000 --- a/imu/bindings/build.rs +++ /dev/null @@ -1,7 +0,0 @@ -fn main() { - // Tell cargo to look for Python libraries in the conda env - println!("cargo:rustc-link-search=/home/kuwajerw/anaconda3/envs/zdemos/lib"); - - // Link against Python library (name might be python3.x or python3.x.so) - println!("cargo:rustc-link-lib=python3.11"); -} \ No newline at end of file From a91504b6def2eb11a2d8d07f5d89458cbf25de37 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 17:00:27 -0800 Subject: [PATCH 03/24] bump version --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 5212d83..49cbf32 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.2.3" +version = "0.2.31" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" From f1a955fdbf0a42ef6f001df585446bdb38dd87e6 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 17:11:37 -0800 Subject: [PATCH 04/24] sanity check --- imu/bmi088/src/lib.rs | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/imu/bmi088/src/lib.rs b/imu/bmi088/src/lib.rs index 3ee1b27..000008f 100644 --- a/imu/bmi088/src/lib.rs +++ b/imu/bmi088/src/lib.rs @@ -9,6 +9,10 @@ use std::time::Duration; mod registers; use registers::{AccelRegisters, GyroRegisters, Constants, AccelRange, GyroRange}; +// Add these constants at the top of the file +pub const ACCEL_ADDR: u8 = 0x18; // Default BMI088 accelerometer address +pub const GYRO_ADDR: u8 = 0x68; // Default BMI088 gyroscope address + /// 3D vector type. #[derive(Debug, Clone, Copy, Default)] pub struct Vector3 { @@ -358,4 +362,19 @@ fn euler_to_quaternion(e: EulerAngles) -> Quaternion { y: cr * sp * cy + sr * cp * sy, z: cr * cp * sy - sr * sp * cy, } -} \ No newline at end of file +} + +#[cfg(test)] +mod tests { + #[test] + fn sanity_check_bmi088() { + // This test confirms that we can access the BMI088 code + let accel_addr = 0x18; // Default BMI088 accelerometer address + let gyro_addr = 0x68; // Default BMI088 gyroscope address + + assert_eq!(accel_addr, crate::ACCEL_ADDR); + assert_eq!(gyro_addr, crate::GYRO_ADDR); + + println!("BMI088 crate version: {}", env!("CARGO_PKG_VERSION")); + } +} \ No newline at end of file From 2bff01bc696a8bd85dce2fea32b37d69a343d8fc Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 17:12:18 -0800 Subject: [PATCH 05/24] bump version --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 49cbf32..69d2010 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.2.31" +version = "0.2.32" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" From f7111e4c8fe57801e9616b8cd1ffd973339d6159 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 18:49:34 -0800 Subject: [PATCH 06/24] add functions --- Cargo.toml | 2 +- imu/bmi088/src/lib.rs | 19 ++++++++++++++++++- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 69d2010..7cfd9b4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.2.32" +version = "0.2.34" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" diff --git a/imu/bmi088/src/lib.rs b/imu/bmi088/src/lib.rs index 000008f..66f86c7 100644 --- a/imu/bmi088/src/lib.rs +++ b/imu/bmi088/src/lib.rs @@ -56,7 +56,10 @@ pub struct Bmi088Data { pub euler: EulerAngles, pub accelerometer: Vector3, pub gyroscope: Vector3, + pub linear_acceleration: Vector3, + pub gravity: Vector3, pub temperature: f32, + pub calibration_status: u8, } impl Default for Bmi088Data { @@ -66,7 +69,10 @@ impl Default for Bmi088Data { euler: EulerAngles::default(), accelerometer: Vector3::default(), gyroscope: Vector3::default(), + linear_acceleration: Vector3::default(), + gravity: Vector3::default(), temperature: 0.0, + calibration_status: 0, } } } @@ -329,13 +335,24 @@ impl Bmi088Reader { /// Returns the most recent sensor data. pub fn get_data(&self) -> Result { - self.data.read().map(|d| *d).map_err(|_| Error::ReadError) + self.data.read().map(|data| *data).map_err(|_| Error::ReadError) } /// Stops the reading thread. pub fn stop(&self) -> Result<(), Error> { self.command_tx.send(ImuCommand::Stop).map_err(|_| Error::WriteError) } + + /// Resets the BMI088 sensor. + pub fn reset(&self) -> Result<(), Error> { + self.command_tx.send(ImuCommand::Reset).map_err(|_| Error::WriteError) + } + + /// Sets the mode for BMI088. + /// BMI088 does not support the mode configuration like BNO055, so this is a no-op. + pub fn set_mode(&self, _mode: u8) -> Result<(), Error> { + Ok(()) + } } impl Drop for Bmi088Reader { From ff16433bd95e6233fc977d66c3c8ffc90127209c Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 20:15:22 -0800 Subject: [PATCH 07/24] make init call synchronous --- Cargo.toml | 2 +- imu/bno055/src/lib.rs | 60 +++++++++++++++---------------------------- 2 files changed, 22 insertions(+), 40 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 7cfd9b4..3bf301b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.2.34" +version = "0.2.35" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" diff --git a/imu/bno055/src/lib.rs b/imu/bno055/src/lib.rs index 1a40f69..bf6b627 100644 --- a/imu/bno055/src/lib.rs +++ b/imu/bno055/src/lib.rs @@ -374,54 +374,43 @@ impl Bno055 { pub struct Bno055Reader { data: Arc>, command_tx: mpsc::Sender, - running: Arc>, } impl Bno055Reader { pub fn new(i2c_bus: &str) -> Result { let data = Arc::new(RwLock::new(BnoData::default())); - let running = Arc::new(RwLock::new(true)); let (command_tx, command_rx) = mpsc::channel(); - let reader = Bno055Reader { - data: Arc::clone(&data), - command_tx, - running: Arc::clone(&running), - }; + // Synchronously initialize the BNO055 sensor. + let imu = Bno055::new(i2c_bus)?; + + // Create a local "running" state for the thread. + let running = Arc::new(RwLock::new(true)); - reader.start_reading_thread(i2c_bus, command_rx)?; + // Spawn a thread that continually reads sensor values using the already-initialized sensor. + Self::start_reading_thread_with_imu(imu, Arc::clone(&data), Arc::clone(&running), command_rx); - Ok(reader) + Ok(Bno055Reader { + data, + command_tx, + }) } - fn start_reading_thread( - &self, - i2c_bus: &str, + // Remove the old asynchronous initialization method. + // Instead, create a helper that takes an already-initialized sensor instance. + fn start_reading_thread_with_imu( + mut imu: Bno055, + data: Arc>, + running: Arc>, command_rx: mpsc::Receiver, - ) -> Result<(), Error> { - let data = Arc::clone(&self.data); - let running = Arc::clone(&self.running); - let i2c_bus = i2c_bus.to_string(); - - let (tx, rx) = mpsc::channel(); - + ) { thread::spawn(move || { - // Initialize IMU inside the thread and send result back - let init_result = Bno055::new(&i2c_bus); - if let Err(e) = init_result { - error!("Failed to initialize BNO055: {}", e); - let _ = tx.send(Err(e)); - return; - } - let mut imu = init_result.unwrap(); - let _ = tx.send(Ok(())); - while let Ok(guard) = running.read() { if !*guard { break; } - // Check for any pending commands + // Process any pending commands if let Ok(command) = command_rx.try_recv() { match command { ImuCommand::SetMode(mode) => { @@ -443,10 +432,9 @@ impl Bno055Reader { } } - // Read all sensor data let mut data_holder = BnoData::default(); - // Read all sensor data (same as before) + // Read sensor data if let Ok(quat) = imu.get_quaternion() { data_holder.quaternion = quat; } else { @@ -506,16 +494,10 @@ impl Bno055Reader { *imu_data = data_holder; } - // IMU sends data at 100 Hz + // Poll at roughly 100 Hz thread::sleep(Duration::from_millis(10)); } }); - - // Wait for initialization result before returning - match rx.recv().map_err(|_| Error::ReadError) { - Ok(_) => Ok(()), - Err(e) => Err(e), - } } pub fn set_mode(&self, mode: OperationMode) -> Result<(), Error> { From b6921b28ee4d80ad438594ef18e0a4cf9e4371a3 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 20:39:12 -0800 Subject: [PATCH 08/24] debug logs --- Cargo.toml | 2 +- imu/bno055/src/lib.rs | 128 ++++++++++++++++++++++++++++++------------ 2 files changed, 92 insertions(+), 38 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 3bf301b..870b049 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.2.35" +version = "0.2.36" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" diff --git a/imu/bno055/src/lib.rs b/imu/bno055/src/lib.rs index bf6b627..fd0e156 100644 --- a/imu/bno055/src/lib.rs +++ b/imu/bno055/src/lib.rs @@ -12,6 +12,7 @@ use std::sync::mpsc; use std::sync::{Arc, RwLock}; use std::thread; use std::time::Duration; +use log::debug; #[derive(Debug)] pub enum Error { @@ -139,20 +140,27 @@ impl Bno055 { /// # Arguments /// * `i2c_bus` - The I2C bus path (e.g., "/dev/i2c-1") pub fn new(i2c_bus: &str) -> Result { + debug!("BNO055::new - Opening I2C device on bus: {}", i2c_bus); let i2c = LinuxI2CDevice::new(i2c_bus, Constants::DefaultI2cAddr as u16)?; + debug!("BNO055::new - I2C device opened"); let mut bno = Bno055 { i2c }; // Set page 0 before initialization + debug!("BNO055::new - Setting page to Page0"); bno.set_page(RegisterPage::Page0)?; // Verify we're talking to the right chip + debug!("BNO055::new - Verifying chip ID"); bno.verify_chip_id()?; // Reset the device + debug!("BNO055::new - Resetting sensor"); bno.reset()?; // Configure for NDOF mode (9-axis fusion) + debug!("BNO055::new - Setting sensor mode to Ndof"); bno.set_mode(OperationMode::Ndof)?; + debug!("BNO055::new - Sensor initialization complete"); Ok(bno) } @@ -405,10 +413,12 @@ impl Bno055Reader { command_rx: mpsc::Receiver, ) { thread::spawn(move || { + debug!("BNO055 reading thread started"); while let Ok(guard) = running.read() { if !*guard { break; } + debug!("BNO055 reading thread iteration"); // Process any pending commands if let Ok(command) = command_rx.try_recv() { @@ -416,17 +426,22 @@ impl Bno055Reader { ImuCommand::SetMode(mode) => { if let Err(e) = imu.set_mode(mode) { error!("Failed to set mode: {}", e); + } else { + debug!("Set mode command processed"); } } ImuCommand::Reset => { if let Err(e) = imu.reset() { error!("Failed to reset: {}", e); + } else { + debug!("Reset command processed"); } } ImuCommand::Stop => { if let Ok(mut guard) = running.write() { *guard = false; } + debug!("Stop command processed, exiting thread"); break; } } @@ -434,69 +449,108 @@ impl Bno055Reader { let mut data_holder = BnoData::default(); - // Read sensor data - if let Ok(quat) = imu.get_quaternion() { - data_holder.quaternion = quat; - } else { - warn!("Failed to get quaternion"); + // Read sensor data with debug logging for each measurement. + match imu.get_quaternion() { + Ok(quat) => { + debug!("Quaternion read successfully: {:?}", quat); + data_holder.quaternion = quat; + }, + Err(e) => { + warn!("Failed to get quaternion: {}", e); + }, } - if let Ok(euler) = imu.get_euler_angles() { - data_holder.euler = euler; - } else { - warn!("Failed to get euler angles"); + match imu.get_euler_angles() { + Ok(euler) => { + debug!("Euler angles read successfully: {:?}", euler); + data_holder.euler = euler; + }, + Err(e) => { + warn!("Failed to get euler angles: {}", e); + }, } - if let Ok(accel) = imu.get_accelerometer() { - data_holder.accelerometer = accel; - } else { - warn!("Failed to get accelerometer"); + match imu.get_accelerometer() { + Ok(accel) => { + debug!("Accelerometer read successfully: {:?}", accel); + data_holder.accelerometer = accel; + }, + Err(e) => { + warn!("Failed to get accelerometer: {}", e); + }, } - if let Ok(gyro) = imu.get_gyroscope() { - data_holder.gyroscope = gyro; - } else { - warn!("Failed to get gyroscope"); + match imu.get_gyroscope() { + Ok(gyro) => { + debug!("Gyroscope read successfully: {:?}", gyro); + data_holder.gyroscope = gyro; + }, + Err(e) => { + warn!("Failed to get gyroscope: {}", e); + }, } - if let Ok(mag) = imu.get_magnetometer() { - data_holder.magnetometer = mag; - } else { - warn!("Failed to get magnetometer"); + match imu.get_magnetometer() { + Ok(mag) => { + debug!("Magnetometer read successfully: {:?}", mag); + data_holder.magnetometer = mag; + }, + Err(e) => { + warn!("Failed to get magnetometer: {}", e); + }, } - if let Ok(linear_accel) = imu.get_linear_acceleration() { - data_holder.linear_acceleration = linear_accel; - } else { - warn!("Failed to get linear acceleration"); + match imu.get_linear_acceleration() { + Ok(linear_accel) => { + debug!("Linear acceleration read successfully: {:?}", linear_accel); + data_holder.linear_acceleration = linear_accel; + }, + Err(e) => { + warn!("Failed to get linear acceleration: {}", e); + }, } - if let Ok(gravity) = imu.get_gravity_vector() { - data_holder.gravity = gravity; - } else { - warn!("Failed to get gravity vector"); + match imu.get_gravity_vector() { + Ok(gravity) => { + debug!("Gravity vector read successfully: {:?}", gravity); + data_holder.gravity = gravity; + }, + Err(e) => { + warn!("Failed to get gravity vector: {}", e); + }, } - if let Ok(temp) = imu.get_temperature() { - data_holder.temperature = temp; - } else { - warn!("Failed to get temperature"); + match imu.get_temperature() { + Ok(temp) => { + debug!("Temperature read successfully: {:?}", temp); + data_holder.temperature = temp; + }, + Err(e) => { + warn!("Failed to get temperature: {}", e); + }, } - if let Ok(status) = imu.get_calibration_status() { - data_holder.calibration_status = status; - } else { - warn!("Failed to get calibration status"); + match imu.get_calibration_status() { + Ok(status) => { + debug!("Calibration status read successfully: {:?}", status); + data_holder.calibration_status = status; + }, + Err(e) => { + warn!("Failed to get calibration status: {}", e); + }, } // Update shared data if let Ok(mut imu_data) = data.write() { *imu_data = data_holder; + } else { + warn!("Failed to write sensor data to shared state"); } // Poll at roughly 100 Hz thread::sleep(Duration::from_millis(10)); } + debug!("BNO055 reading thread exiting"); }); } From 37e51712306c7cef9d6db09aba4b39953d45d931 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 21:18:58 -0800 Subject: [PATCH 09/24] just trying to make this work --- Cargo.toml | 2 +- imu/bno055/Cargo.toml | 2 +- imu/bno055/src/lib.rs | 11 +++++++++++ 3 files changed, 13 insertions(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 870b049..c10033b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.2.36" +version = "0.2.37" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" diff --git a/imu/bno055/Cargo.toml b/imu/bno055/Cargo.toml index 7374117..9b59509 100644 --- a/imu/bno055/Cargo.toml +++ b/imu/bno055/Cargo.toml @@ -3,7 +3,7 @@ name = "linux_bno055" readme = "README.md" description = "Interface for interacting with BNO055 IMUs" -version = "0.0.6" +version = "0.0.7" authors.workspace = true edition.workspace = true repository.workspace = true diff --git a/imu/bno055/src/lib.rs b/imu/bno055/src/lib.rs index fd0e156..e42ed0b 100644 --- a/imu/bno055/src/lib.rs +++ b/imu/bno055/src/lib.rs @@ -141,26 +141,33 @@ impl Bno055 { /// * `i2c_bus` - The I2C bus path (e.g., "/dev/i2c-1") pub fn new(i2c_bus: &str) -> Result { debug!("BNO055::new - Opening I2C device on bus: {}", i2c_bus); + println!("BNO055::new - Opening I2C device on bus: {}", i2c_bus); let i2c = LinuxI2CDevice::new(i2c_bus, Constants::DefaultI2cAddr as u16)?; debug!("BNO055::new - I2C device opened"); + println!("BNO055::new - I2C device opened"); let mut bno = Bno055 { i2c }; // Set page 0 before initialization debug!("BNO055::new - Setting page to Page0"); + println!("BNO055::new - Setting page to Page0"); bno.set_page(RegisterPage::Page0)?; // Verify we're talking to the right chip debug!("BNO055::new - Verifying chip ID"); + println!("BNO055::new - Verifying chip ID"); bno.verify_chip_id()?; // Reset the device debug!("BNO055::new - Resetting sensor"); + println!("BNO055::new - Resetting sensor"); bno.reset()?; // Configure for NDOF mode (9-axis fusion) debug!("BNO055::new - Setting sensor mode to Ndof"); + println!("BNO055::new - Setting sensor mode to Ndof"); bno.set_mode(OperationMode::Ndof)?; debug!("BNO055::new - Sensor initialization complete"); + println!("BNO055::new - Sensor initialization complete"); Ok(bno) } @@ -414,11 +421,13 @@ impl Bno055Reader { ) { thread::spawn(move || { debug!("BNO055 reading thread started"); + println!("BNO055 reading thread started"); while let Ok(guard) = running.read() { if !*guard { break; } debug!("BNO055 reading thread iteration"); + println!("BNO055 reading thread iteration"); // Process any pending commands if let Ok(command) = command_rx.try_recv() { @@ -551,6 +560,7 @@ impl Bno055Reader { thread::sleep(Duration::from_millis(10)); } debug!("BNO055 reading thread exiting"); + println!("BNO055 reading thread exiting"); }); } @@ -592,3 +602,4 @@ pub enum ImuCommand { Reset, Stop, } + From d5bed63ba469f769df1b91451a41943676c1ea03 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 22:24:16 -0800 Subject: [PATCH 10/24] change bno055 path to be local --- imu/Cargo.toml | 2 +- imu/bno055/src/lib.rs | 20 ++++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/imu/Cargo.toml b/imu/Cargo.toml index 98a186f..4ff161d 100644 --- a/imu/Cargo.toml +++ b/imu/Cargo.toml @@ -15,7 +15,7 @@ crate-type = ["rlib"] [dependencies] hexmove = "0.1.6" hiwonder = "0.2.3" -linux_bno055 = "0.0.6" +linux_bno055 = { path = "./bno055" } socketcan = "3.3.0" log = "0.4" linux_bmi088 = { path = "./bmi088" } diff --git a/imu/bno055/src/lib.rs b/imu/bno055/src/lib.rs index e42ed0b..9c7bba5 100644 --- a/imu/bno055/src/lib.rs +++ b/imu/bno055/src/lib.rs @@ -141,33 +141,33 @@ impl Bno055 { /// * `i2c_bus` - The I2C bus path (e.g., "/dev/i2c-1") pub fn new(i2c_bus: &str) -> Result { debug!("BNO055::new - Opening I2C device on bus: {}", i2c_bus); - println!("BNO055::new - Opening I2C device on bus: {}", i2c_bus); + info!("BNO055::new - Opening I2C device on bus: {}", i2c_bus); let i2c = LinuxI2CDevice::new(i2c_bus, Constants::DefaultI2cAddr as u16)?; debug!("BNO055::new - I2C device opened"); - println!("BNO055::new - I2C device opened"); + info!("BNO055::new - I2C device opened"); let mut bno = Bno055 { i2c }; // Set page 0 before initialization debug!("BNO055::new - Setting page to Page0"); - println!("BNO055::new - Setting page to Page0"); + info!("BNO055::new - Setting page to Page0"); bno.set_page(RegisterPage::Page0)?; // Verify we're talking to the right chip debug!("BNO055::new - Verifying chip ID"); - println!("BNO055::new - Verifying chip ID"); + info!("BNO055::new - Verifying chip ID"); bno.verify_chip_id()?; // Reset the device debug!("BNO055::new - Resetting sensor"); - println!("BNO055::new - Resetting sensor"); + info!("BNO055::new - Resetting sensor"); bno.reset()?; // Configure for NDOF mode (9-axis fusion) debug!("BNO055::new - Setting sensor mode to Ndof"); - println!("BNO055::new - Setting sensor mode to Ndof"); + info!("BNO055::new - Setting sensor mode to Ndof"); bno.set_mode(OperationMode::Ndof)?; debug!("BNO055::new - Sensor initialization complete"); - println!("BNO055::new - Sensor initialization complete"); + info!("BNO055::new - Sensor initialization complete"); Ok(bno) } @@ -421,13 +421,13 @@ impl Bno055Reader { ) { thread::spawn(move || { debug!("BNO055 reading thread started"); - println!("BNO055 reading thread started"); + info!("BNO055 reading thread started"); while let Ok(guard) = running.read() { if !*guard { break; } debug!("BNO055 reading thread iteration"); - println!("BNO055 reading thread iteration"); + info!("BNO055 reading thread iteration"); // Process any pending commands if let Ok(command) = command_rx.try_recv() { @@ -560,7 +560,7 @@ impl Bno055Reader { thread::sleep(Duration::from_millis(10)); } debug!("BNO055 reading thread exiting"); - println!("BNO055 reading thread exiting"); + info!("BNO055 reading thread exiting"); }); } From d34efb7ef84cd3e88a95c46d96df94e7e87f2f79 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 22:25:33 -0800 Subject: [PATCH 11/24] typo --- imu/bno055/src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/imu/bno055/src/lib.rs b/imu/bno055/src/lib.rs index 9c7bba5..b8e6404 100644 --- a/imu/bno055/src/lib.rs +++ b/imu/bno055/src/lib.rs @@ -12,7 +12,7 @@ use std::sync::mpsc; use std::sync::{Arc, RwLock}; use std::thread; use std::time::Duration; -use log::debug; +use log::{debug, info}; #[derive(Debug)] pub enum Error { From fd2e1ef42f758d8409a75e287fbd681a8568b642 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 22:46:53 -0800 Subject: [PATCH 12/24] bump versions --- Cargo.toml | 2 +- imu/bno055/Cargo.toml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index c10033b..ff3a580 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.2.37" +version = "0.2.38" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" diff --git a/imu/bno055/Cargo.toml b/imu/bno055/Cargo.toml index 9b59509..d8cc9b7 100644 --- a/imu/bno055/Cargo.toml +++ b/imu/bno055/Cargo.toml @@ -3,7 +3,7 @@ name = "linux_bno055" readme = "README.md" description = "Interface for interacting with BNO055 IMUs" -version = "0.0.7" +version = "0.0.8" authors.workspace = true edition.workspace = true repository.workspace = true From b9012d449faa31073b143ac6292e0e04fa8a74f5 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Mon, 10 Feb 2025 23:09:04 -0800 Subject: [PATCH 13/24] reset --- imu/bno055/src/lib.rs | 199 ++++++++++++++++-------------------------- 1 file changed, 76 insertions(+), 123 deletions(-) diff --git a/imu/bno055/src/lib.rs b/imu/bno055/src/lib.rs index b8e6404..33969b5 100644 --- a/imu/bno055/src/lib.rs +++ b/imu/bno055/src/lib.rs @@ -12,7 +12,6 @@ use std::sync::mpsc; use std::sync::{Arc, RwLock}; use std::thread; use std::time::Duration; -use log::{debug, info}; #[derive(Debug)] pub enum Error { @@ -140,34 +139,20 @@ impl Bno055 { /// # Arguments /// * `i2c_bus` - The I2C bus path (e.g., "/dev/i2c-1") pub fn new(i2c_bus: &str) -> Result { - debug!("BNO055::new - Opening I2C device on bus: {}", i2c_bus); - info!("BNO055::new - Opening I2C device on bus: {}", i2c_bus); let i2c = LinuxI2CDevice::new(i2c_bus, Constants::DefaultI2cAddr as u16)?; - debug!("BNO055::new - I2C device opened"); - info!("BNO055::new - I2C device opened"); let mut bno = Bno055 { i2c }; // Set page 0 before initialization - debug!("BNO055::new - Setting page to Page0"); - info!("BNO055::new - Setting page to Page0"); bno.set_page(RegisterPage::Page0)?; // Verify we're talking to the right chip - debug!("BNO055::new - Verifying chip ID"); - info!("BNO055::new - Verifying chip ID"); bno.verify_chip_id()?; // Reset the device - debug!("BNO055::new - Resetting sensor"); - info!("BNO055::new - Resetting sensor"); bno.reset()?; // Configure for NDOF mode (9-axis fusion) - debug!("BNO055::new - Setting sensor mode to Ndof"); - info!("BNO055::new - Setting sensor mode to Ndof"); bno.set_mode(OperationMode::Ndof)?; - debug!("BNO055::new - Sensor initialization complete"); - info!("BNO055::new - Sensor initialization complete"); Ok(bno) } @@ -389,179 +374,148 @@ impl Bno055 { pub struct Bno055Reader { data: Arc>, command_tx: mpsc::Sender, + running: Arc>, } impl Bno055Reader { pub fn new(i2c_bus: &str) -> Result { let data = Arc::new(RwLock::new(BnoData::default())); + let running = Arc::new(RwLock::new(true)); let (command_tx, command_rx) = mpsc::channel(); - // Synchronously initialize the BNO055 sensor. - let imu = Bno055::new(i2c_bus)?; - - // Create a local "running" state for the thread. - let running = Arc::new(RwLock::new(true)); + let reader = Bno055Reader { + data: Arc::clone(&data), + command_tx, + running: Arc::clone(&running), + }; - // Spawn a thread that continually reads sensor values using the already-initialized sensor. - Self::start_reading_thread_with_imu(imu, Arc::clone(&data), Arc::clone(&running), command_rx); + reader.start_reading_thread(i2c_bus, command_rx)?; - Ok(Bno055Reader { - data, - command_tx, - }) + Ok(reader) } - // Remove the old asynchronous initialization method. - // Instead, create a helper that takes an already-initialized sensor instance. - fn start_reading_thread_with_imu( - mut imu: Bno055, - data: Arc>, - running: Arc>, + fn start_reading_thread( + &self, + i2c_bus: &str, command_rx: mpsc::Receiver, - ) { + ) -> Result<(), Error> { + let data = Arc::clone(&self.data); + let running = Arc::clone(&self.running); + let i2c_bus = i2c_bus.to_string(); + + let (tx, rx) = mpsc::channel(); + thread::spawn(move || { - debug!("BNO055 reading thread started"); - info!("BNO055 reading thread started"); + // Initialize IMU inside the thread and send result back + let init_result = Bno055::new(&i2c_bus); + if let Err(e) = init_result { + error!("Failed to initialize BNO055: {}", e); + let _ = tx.send(Err(e)); + return; + } + let mut imu = init_result.unwrap(); + let _ = tx.send(Ok(())); + while let Ok(guard) = running.read() { if !*guard { break; } - debug!("BNO055 reading thread iteration"); - info!("BNO055 reading thread iteration"); - // Process any pending commands + // Check for any pending commands if let Ok(command) = command_rx.try_recv() { match command { ImuCommand::SetMode(mode) => { if let Err(e) = imu.set_mode(mode) { error!("Failed to set mode: {}", e); - } else { - debug!("Set mode command processed"); } } ImuCommand::Reset => { if let Err(e) = imu.reset() { error!("Failed to reset: {}", e); - } else { - debug!("Reset command processed"); } } ImuCommand::Stop => { if let Ok(mut guard) = running.write() { *guard = false; } - debug!("Stop command processed, exiting thread"); break; } } } + // Read all sensor data let mut data_holder = BnoData::default(); - // Read sensor data with debug logging for each measurement. - match imu.get_quaternion() { - Ok(quat) => { - debug!("Quaternion read successfully: {:?}", quat); - data_holder.quaternion = quat; - }, - Err(e) => { - warn!("Failed to get quaternion: {}", e); - }, + // Read all sensor data (same as before) + if let Ok(quat) = imu.get_quaternion() { + data_holder.quaternion = quat; + } else { + warn!("Failed to get quaternion"); } - match imu.get_euler_angles() { - Ok(euler) => { - debug!("Euler angles read successfully: {:?}", euler); - data_holder.euler = euler; - }, - Err(e) => { - warn!("Failed to get euler angles: {}", e); - }, + if let Ok(euler) = imu.get_euler_angles() { + data_holder.euler = euler; + } else { + warn!("Failed to get euler angles"); } - match imu.get_accelerometer() { - Ok(accel) => { - debug!("Accelerometer read successfully: {:?}", accel); - data_holder.accelerometer = accel; - }, - Err(e) => { - warn!("Failed to get accelerometer: {}", e); - }, + if let Ok(accel) = imu.get_accelerometer() { + data_holder.accelerometer = accel; + } else { + warn!("Failed to get accelerometer"); } - match imu.get_gyroscope() { - Ok(gyro) => { - debug!("Gyroscope read successfully: {:?}", gyro); - data_holder.gyroscope = gyro; - }, - Err(e) => { - warn!("Failed to get gyroscope: {}", e); - }, + if let Ok(gyro) = imu.get_gyroscope() { + data_holder.gyroscope = gyro; + } else { + warn!("Failed to get gyroscope"); } - match imu.get_magnetometer() { - Ok(mag) => { - debug!("Magnetometer read successfully: {:?}", mag); - data_holder.magnetometer = mag; - }, - Err(e) => { - warn!("Failed to get magnetometer: {}", e); - }, + if let Ok(mag) = imu.get_magnetometer() { + data_holder.magnetometer = mag; + } else { + warn!("Failed to get magnetometer"); } - match imu.get_linear_acceleration() { - Ok(linear_accel) => { - debug!("Linear acceleration read successfully: {:?}", linear_accel); - data_holder.linear_acceleration = linear_accel; - }, - Err(e) => { - warn!("Failed to get linear acceleration: {}", e); - }, + if let Ok(linear_accel) = imu.get_linear_acceleration() { + data_holder.linear_acceleration = linear_accel; + } else { + warn!("Failed to get linear acceleration"); } - match imu.get_gravity_vector() { - Ok(gravity) => { - debug!("Gravity vector read successfully: {:?}", gravity); - data_holder.gravity = gravity; - }, - Err(e) => { - warn!("Failed to get gravity vector: {}", e); - }, + if let Ok(gravity) = imu.get_gravity_vector() { + data_holder.gravity = gravity; + } else { + warn!("Failed to get gravity vector"); } - match imu.get_temperature() { - Ok(temp) => { - debug!("Temperature read successfully: {:?}", temp); - data_holder.temperature = temp; - }, - Err(e) => { - warn!("Failed to get temperature: {}", e); - }, + if let Ok(temp) = imu.get_temperature() { + data_holder.temperature = temp; + } else { + warn!("Failed to get temperature"); } - match imu.get_calibration_status() { - Ok(status) => { - debug!("Calibration status read successfully: {:?}", status); - data_holder.calibration_status = status; - }, - Err(e) => { - warn!("Failed to get calibration status: {}", e); - }, + if let Ok(status) = imu.get_calibration_status() { + data_holder.calibration_status = status; + } else { + warn!("Failed to get calibration status"); } // Update shared data if let Ok(mut imu_data) = data.write() { *imu_data = data_holder; - } else { - warn!("Failed to write sensor data to shared state"); } - // Poll at roughly 100 Hz + // IMU sends data at 100 Hz thread::sleep(Duration::from_millis(10)); } - debug!("BNO055 reading thread exiting"); - info!("BNO055 reading thread exiting"); }); + + // Wait for initialization result before returning + match rx.recv().map_err(|_| Error::ReadError) { + Ok(_) => Ok(()), + Err(e) => Err(e), + } } pub fn set_mode(&self, mode: OperationMode) -> Result<(), Error> { @@ -601,5 +555,4 @@ pub enum ImuCommand { SetMode(OperationMode), Reset, Stop, -} - +} \ No newline at end of file From f72f4906091351ac1592daedc950531cf150be4c Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Tue, 11 Feb 2025 12:33:14 -0800 Subject: [PATCH 14/24] synchornois init --- Cargo.toml | 2 +- imu/bno055/Cargo.toml | 2 +- imu/bno055/src/lib.rs | 69 +++++++++++-------------------------------- 3 files changed, 20 insertions(+), 53 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index ff3a580..e1fa0fe 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.2.38" +version = "0.2.39" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" diff --git a/imu/bno055/Cargo.toml b/imu/bno055/Cargo.toml index d8cc9b7..d299145 100644 --- a/imu/bno055/Cargo.toml +++ b/imu/bno055/Cargo.toml @@ -3,7 +3,7 @@ name = "linux_bno055" readme = "README.md" description = "Interface for interacting with BNO055 IMUs" -version = "0.0.8" +version = "0.0.81" authors.workspace = true edition.workspace = true repository.workspace = true diff --git a/imu/bno055/src/lib.rs b/imu/bno055/src/lib.rs index 33969b5..d906e57 100644 --- a/imu/bno055/src/lib.rs +++ b/imu/bno055/src/lib.rs @@ -2,7 +2,7 @@ mod registers; use byteorder::{ByteOrder, LittleEndian}; use i2cdev::core::I2CDevice; use i2cdev::linux::LinuxI2CDevice; -use log::{error, warn}; +use log::{debug, error, warn}; pub use registers::OperationMode; use registers::{ AccelRegisters, ChipRegisters, Constants, EulerRegisters, GravityRegisters, GyroRegisters, @@ -374,54 +374,32 @@ impl Bno055 { pub struct Bno055Reader { data: Arc>, command_tx: mpsc::Sender, - running: Arc>, } impl Bno055Reader { pub fn new(i2c_bus: &str) -> Result { let data = Arc::new(RwLock::new(BnoData::default())); - let running = Arc::new(RwLock::new(true)); let (command_tx, command_rx) = mpsc::channel(); - let reader = Bno055Reader { - data: Arc::clone(&data), - command_tx, - running: Arc::clone(&running), - }; + // Synchronously initialize (calibrate) the IMU. + // If this fails, the error is propagated immediately. + let imu = Bno055::new(i2c_bus)?; - reader.start_reading_thread(i2c_bus, command_rx)?; + // Spawn a thread that continuously reads sensor data using the initialized IMU. + Self::start_reading_thread_with_imu(imu, Arc::clone(&data), command_rx); - Ok(reader) + Ok(Bno055Reader { data, command_tx }) } - fn start_reading_thread( - &self, - i2c_bus: &str, - command_rx: mpsc::Receiver, - ) -> Result<(), Error> { - let data = Arc::clone(&self.data); - let running = Arc::clone(&self.running); - let i2c_bus = i2c_bus.to_string(); - - let (tx, rx) = mpsc::channel(); - + fn start_reading_thread_with_imu( + mut imu: Bno055, + data: Arc>, + command_rx: mpsc::Receiver + ) { thread::spawn(move || { - // Initialize IMU inside the thread and send result back - let init_result = Bno055::new(&i2c_bus); - if let Err(e) = init_result { - error!("Failed to initialize BNO055: {}", e); - let _ = tx.send(Err(e)); - return; - } - let mut imu = init_result.unwrap(); - let _ = tx.send(Ok(())); - - while let Ok(guard) = running.read() { - if !*guard { - break; - } - - // Check for any pending commands + debug!("BNO055 reading thread started"); + loop { + // Process any pending commands if let Ok(command) = command_rx.try_recv() { match command { ImuCommand::SetMode(mode) => { @@ -434,19 +412,13 @@ impl Bno055Reader { error!("Failed to reset: {}", e); } } - ImuCommand::Stop => { - if let Ok(mut guard) = running.write() { - *guard = false; - } - break; - } + ImuCommand::Stop => break, } } - // Read all sensor data + // Read sensor data and update shared data let mut data_holder = BnoData::default(); - // Read all sensor data (same as before) if let Ok(quat) = imu.get_quaternion() { data_holder.quaternion = quat; } else { @@ -509,13 +481,8 @@ impl Bno055Reader { // IMU sends data at 100 Hz thread::sleep(Duration::from_millis(10)); } + debug!("BNO055 reading thread exiting"); }); - - // Wait for initialization result before returning - match rx.recv().map_err(|_| Error::ReadError) { - Ok(_) => Ok(()), - Err(e) => Err(e), - } } pub fn set_mode(&self, mode: OperationMode) -> Result<(), Error> { From dce3d2854c556ada54e5f155634a5b10c6211c1e Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Tue, 11 Feb 2025 14:14:55 -0800 Subject: [PATCH 15/24] bmi088 match api better --- Cargo.toml | 2 +- imu/bmi088/src/lib.rs | 12 +++++++----- imu/bno055/Cargo.toml | 2 +- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index e1fa0fe..c9244f7 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.2.39" +version = "0.2.391" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" diff --git a/imu/bmi088/src/lib.rs b/imu/bmi088/src/lib.rs index 66f86c7..071a8bf 100644 --- a/imu/bmi088/src/lib.rs +++ b/imu/bmi088/src/lib.rs @@ -56,9 +56,10 @@ pub struct Bmi088Data { pub euler: EulerAngles, pub accelerometer: Vector3, pub gyroscope: Vector3, + pub magnetometer: Vector3, pub linear_acceleration: Vector3, pub gravity: Vector3, - pub temperature: f32, + pub temperature: i8, pub calibration_status: u8, } @@ -69,9 +70,10 @@ impl Default for Bmi088Data { euler: EulerAngles::default(), accelerometer: Vector3::default(), gyroscope: Vector3::default(), + magnetometer: Vector3::default(), linear_acceleration: Vector3::default(), gravity: Vector3::default(), - temperature: 0.0, + temperature: 0, calibration_status: 0, } } @@ -125,7 +127,7 @@ pub struct Bmi088 { impl Bmi088 { /// Initializes the BMI088 sensor on the given I2C bus. pub fn new(i2c_path: &str) -> Result { - println!("Initializing BMI088..."); + println!("Initializing BMI088... ali latest hello"); let mut accel_i2c = LinuxI2CDevice::new(i2c_path, Constants::AccelI2cAddr as u16)?; let gyro_i2c = LinuxI2CDevice::new(i2c_path, Constants::GyroI2cAddr as u16)?; @@ -205,11 +207,11 @@ impl Bmi088 { } /// Reads temperature (in °C) from the accelerometer. - pub fn read_temperature(&mut self) -> Result { + pub fn read_temperature(&mut self) -> Result { let msb = self.accel_i2c.smbus_read_byte_data(AccelRegisters::TempMsb as u8)? as i16; let lsb = self.accel_i2c.smbus_read_byte_data(AccelRegisters::TempLsb as u8)? as i16; let temp_raw = (msb * 8) + (lsb / 32); - Ok((temp_raw as f32) * 0.125 + 23.0) + Ok(((temp_raw as f32) * 0.125 + 23.0) as i8) } /// Updates the simple Euler integration state based on gyroscope data. diff --git a/imu/bno055/Cargo.toml b/imu/bno055/Cargo.toml index d299145..cd6a8a1 100644 --- a/imu/bno055/Cargo.toml +++ b/imu/bno055/Cargo.toml @@ -3,7 +3,7 @@ name = "linux_bno055" readme = "README.md" description = "Interface for interacting with BNO055 IMUs" -version = "0.0.81" +version = "0.0.82" authors.workspace = true edition.workspace = true repository.workspace = true From 8a92ce65028e4aed0ff282c6b5da4b1afedcc509 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Tue, 11 Feb 2025 17:00:12 -0800 Subject: [PATCH 16/24] formatting --- imu/bmi088/src/bin/read_bmi088.rs | 28 ++++--- imu/bmi088/src/lib.rs | 128 ++++++++++++++++++------------ imu/bmi088/src/registers.rs | 88 ++++++++++---------- imu/bno055/src/lib.rs | 4 +- 4 files changed, 143 insertions(+), 105 deletions(-) diff --git a/imu/bmi088/src/bin/read_bmi088.rs b/imu/bmi088/src/bin/read_bmi088.rs index 403bc35..b39a9d2 100644 --- a/imu/bmi088/src/bin/read_bmi088.rs +++ b/imu/bmi088/src/bin/read_bmi088.rs @@ -5,19 +5,27 @@ use std::time::Duration; fn main() -> Result<(), Box> { let imu = Bmi088Reader::new("/dev/i2c-1")?; println!("Reading BMI088 sensor data..."); - + loop { let data = imu.get_data()?; - println!("Quaternion: w={:.3}, x={:.3}, y={:.3}, z={:.3}", - data.quaternion.w, data.quaternion.x, data.quaternion.y, data.quaternion.z); - println!("Accel: x={:.2} g, y={:.2} g, z={:.2} g", - data.accelerometer.x, data.accelerometer.y, data.accelerometer.z); - println!("Gyro: x={:.2} °/s, y={:.2} °/s, z={:.2} °/s", - data.gyroscope.x, data.gyroscope.y, data.gyroscope.z); - println!("Euler: roll={:.1}°, pitch={:.1}°, yaw={:.1}°", - data.euler.roll, data.euler.pitch, data.euler.yaw); + println!( + "Quaternion: w={:.3}, x={:.3}, y={:.3}, z={:.3}", + data.quaternion.w, data.quaternion.x, data.quaternion.y, data.quaternion.z + ); + println!( + "Accel: x={:.2} g, y={:.2} g, z={:.2} g", + data.accelerometer.x, data.accelerometer.y, data.accelerometer.z + ); + println!( + "Gyro: x={:.2} °/s, y={:.2} °/s, z={:.2} °/s", + data.gyroscope.x, data.gyroscope.y, data.gyroscope.z + ); + println!( + "Euler: roll={:.1}°, pitch={:.1}°, yaw={:.1}°", + data.euler.roll, data.euler.pitch, data.euler.yaw + ); println!("Temp: {:.1} °C", data.temperature); println!("----------------------------------------"); thread::sleep(Duration::from_millis(100)); } -} \ No newline at end of file +} diff --git a/imu/bmi088/src/lib.rs b/imu/bmi088/src/lib.rs index 071a8bf..df1b49b 100644 --- a/imu/bmi088/src/lib.rs +++ b/imu/bmi088/src/lib.rs @@ -1,17 +1,17 @@ -use byteorder::{LittleEndian, ByteOrder}; -use i2cdev::core::I2CDevice; // Add this import +use byteorder::{ByteOrder, LittleEndian}; +use i2cdev::core::I2CDevice; // Add this import use i2cdev::linux::LinuxI2CDevice; -use log::{error, warn}; -use std::sync::{Arc, RwLock, mpsc}; +use log::{debug, error, warn}; +use std::sync::{mpsc, Arc, RwLock}; use std::thread; use std::time::Duration; mod registers; -use registers::{AccelRegisters, GyroRegisters, Constants, AccelRange, GyroRange}; +use registers::{AccelRange, AccelRegisters, Constants, GyroRange, GyroRegisters}; // Add these constants at the top of the file -pub const ACCEL_ADDR: u8 = 0x18; // Default BMI088 accelerometer address -pub const GYRO_ADDR: u8 = 0x68; // Default BMI088 gyroscope address +pub const ACCEL_ADDR: u8 = 0x18; // Default BMI088 accelerometer address +pub const GYRO_ADDR: u8 = 0x68; // Default BMI088 gyroscope address /// 3D vector type. #[derive(Debug, Clone, Copy, Default)] @@ -127,31 +127,34 @@ pub struct Bmi088 { impl Bmi088 { /// Initializes the BMI088 sensor on the given I2C bus. pub fn new(i2c_path: &str) -> Result { - println!("Initializing BMI088... ali latest hello"); + debug!("Initializing Bmi088..."); let mut accel_i2c = LinuxI2CDevice::new(i2c_path, Constants::AccelI2cAddr as u16)?; let gyro_i2c = LinuxI2CDevice::new(i2c_path, Constants::GyroI2cAddr as u16)?; - + // Verify accelerometer chip ID. let chip_id = accel_i2c.smbus_read_byte_data(AccelRegisters::ChipId as u8)?; if chip_id != Constants::AccelChipIdValue as u8 { return Err(Error::InvalidChipId); } - println!("Accel chip ID verified: 0x{:02X}", chip_id); - + debug!("Bmi088 Accel chip ID verified: 0x{:02X}", chip_id); + // Soft reset, power-up, and configure accelerometer. - accel_i2c.smbus_write_byte_data(AccelRegisters::SoftReset as u8, Constants::SoftResetCmd as u8)?; + accel_i2c.smbus_write_byte_data( + AccelRegisters::SoftReset as u8, + Constants::SoftResetCmd as u8, + )?; thread::sleep(Duration::from_millis(50)); accel_i2c.smbus_write_byte_data(AccelRegisters::PowerCtrl as u8, 0x04)?; accel_i2c.smbus_write_byte_data(AccelRegisters::AccConf as u8, 0x80)?; accel_i2c.smbus_write_byte_data(AccelRegisters::AccRange as u8, AccelRange::G3 as u8)?; - + // Configure gyroscope. let mut gyro_i2c = gyro_i2c; // no mutable needed outside this scope gyro_i2c.smbus_write_byte_data(GyroRegisters::PowerMode as u8, 0x00)?; gyro_i2c.smbus_write_byte_data(GyroRegisters::Range as u8, GyroRange::Dps2000 as u8)?; gyro_i2c.smbus_write_byte_data(GyroRegisters::Bandwidth as u8, 0x07)?; - + Ok(Bmi088 { accel_i2c, gyro_i2c, @@ -165,7 +168,9 @@ impl Bmi088 { pub fn read_raw_accelerometer(&mut self) -> Result { let mut buf = [0u8; 6]; for i in 0..6 { - buf[i] = self.accel_i2c.smbus_read_byte_data(AccelRegisters::AccelXLsb as u8 + i as u8)?; + buf[i] = self + .accel_i2c + .smbus_read_byte_data(AccelRegisters::AccelXLsb as u8 + i as u8)?; } let scale = match self.accel_range { AccelRange::G3 => 3.0 / 32768.0, @@ -187,14 +192,16 @@ impl Bmi088 { pub fn read_raw_gyroscope(&mut self) -> Result { let mut buf = [0u8; 6]; for i in 0..6 { - buf[i] = self.gyro_i2c.smbus_read_byte_data(GyroRegisters::XLsb as u8 + i as u8)?; + buf[i] = self + .gyro_i2c + .smbus_read_byte_data(GyroRegisters::XLsb as u8 + i as u8)?; } let scale = match self.gyro_range { GyroRange::Dps2000 => 2000.0 / 32768.0, GyroRange::Dps1000 => 1000.0 / 32768.0, - GyroRange::Dps500 => 500.0 / 32768.0, - GyroRange::Dps250 => 250.0 / 32768.0, - GyroRange::Dps125 => 125.0 / 32768.0, + GyroRange::Dps500 => 500.0 / 32768.0, + GyroRange::Dps250 => 250.0 / 32768.0, + GyroRange::Dps125 => 125.0 / 32768.0, }; let raw_x = (LittleEndian::read_i16(&buf[0..2]) as f32) * scale; let raw_y = (LittleEndian::read_i16(&buf[2..4]) as f32) * scale; @@ -208,8 +215,12 @@ impl Bmi088 { /// Reads temperature (in °C) from the accelerometer. pub fn read_temperature(&mut self) -> Result { - let msb = self.accel_i2c.smbus_read_byte_data(AccelRegisters::TempMsb as u8)? as i16; - let lsb = self.accel_i2c.smbus_read_byte_data(AccelRegisters::TempLsb as u8)? as i16; + let msb = self + .accel_i2c + .smbus_read_byte_data(AccelRegisters::TempMsb as u8)? as i16; + let lsb = self + .accel_i2c + .smbus_read_byte_data(AccelRegisters::TempLsb as u8)? as i16; let temp_raw = (msb * 8) + (lsb / 32); Ok(((temp_raw as f32) * 0.125 + 23.0) as i8) } @@ -218,12 +229,12 @@ impl Bmi088 { pub fn update_simple_euler(&mut self, dt: f32) -> Result<(), Error> { let gyro = self.read_raw_gyroscope()?; // Standard integration: roll from gyro.x, pitch from gyro.y, yaw from gyro.z. - self.simple_euler.roll += gyro.x * dt; + self.simple_euler.roll += gyro.x * dt; self.simple_euler.pitch += gyro.y * dt; - self.simple_euler.yaw += gyro.z * dt; + self.simple_euler.yaw += gyro.z * dt; Ok(()) } - + /// Returns the current integrated Euler angles. pub fn get_simple_euler_angles(&self) -> EulerAngles { self.simple_euler @@ -249,7 +260,7 @@ pub enum ImuCommand { impl Bmi088Reader { /// Creates a new BMI088Reader. pub fn new(i2c_bus: &str) -> Result { - println!("Initializing LOCAL BMI088 implementation..."); + debug!("Initializing Bmi088 Reader..."); let data = Arc::new(RwLock::new(Bmi088Data::default())); let running = Arc::new(RwLock::new(true)); let (command_tx, command_rx) = mpsc::channel(); @@ -261,13 +272,17 @@ impl Bmi088Reader { reader.start_reading_thread(i2c_bus, command_rx)?; Ok(reader) } - - fn start_reading_thread(&self, i2c_bus: &str, command_rx: mpsc::Receiver) -> Result<(), Error> { + + fn start_reading_thread( + &self, + i2c_bus: &str, + command_rx: mpsc::Receiver, + ) -> Result<(), Error> { let data = Arc::clone(&self.data); let running = Arc::clone(&self.running); let i2c_bus = i2c_bus.to_string(); let (tx, rx) = mpsc::channel(); - + thread::spawn(move || { let init_result = Bmi088::new(&i2c_bus); if let Err(e) = init_result { @@ -278,32 +293,40 @@ impl Bmi088Reader { let mut imu = init_result.unwrap(); let _ = tx.send(Ok(())); let mut simple_euler = EulerAngles::default(); - + while let Ok(guard) = running.read() { - if !*guard { break; } - + if !*guard { + break; + } + // Handle incoming commands. if let Ok(cmd) = command_rx.try_recv() { match cmd { ImuCommand::SetAccelRange(range) => { imu.accel_range = range; - let _ = imu.accel_i2c.smbus_write_byte_data(AccelRegisters::AccRange as u8, range as u8); + let _ = imu + .accel_i2c + .smbus_write_byte_data(AccelRegisters::AccRange as u8, range as u8); } ImuCommand::SetGyroRange(range) => { imu.gyro_range = range; - let _ = imu.gyro_i2c.smbus_write_byte_data(GyroRegisters::Range as u8, range as u8); + let _ = imu + .gyro_i2c + .smbus_write_byte_data(GyroRegisters::Range as u8, range as u8); } ImuCommand::Reset => { // Reinitialize if needed. let _ = imu = Bmi088::new(&i2c_bus).unwrap(); } ImuCommand::Stop => { - if let Ok(mut w) = running.write() { *w = false; } + if let Ok(mut w) = running.write() { + *w = false; + } break; } } } - + let mut sensor_data = Bmi088Data::default(); if let Ok(accel) = imu.read_raw_accelerometer() { sensor_data.accelerometer = accel; @@ -324,30 +347,37 @@ impl Bmi088Reader { if let Ok(temp) = imu.read_temperature() { sensor_data.temperature = temp; } - + if let Ok(mut shared) = data.write() { *shared = sensor_data; } thread::sleep(Duration::from_millis(10)); } }); - + rx.recv().map_err(|_| Error::ReadError)? } - + /// Returns the most recent sensor data. pub fn get_data(&self) -> Result { - self.data.read().map(|data| *data).map_err(|_| Error::ReadError) + self.data + .read() + .map(|data| *data) + .map_err(|_| Error::ReadError) } - + /// Stops the reading thread. pub fn stop(&self) -> Result<(), Error> { - self.command_tx.send(ImuCommand::Stop).map_err(|_| Error::WriteError) + self.command_tx + .send(ImuCommand::Stop) + .map_err(|_| Error::WriteError) } /// Resets the BMI088 sensor. pub fn reset(&self) -> Result<(), Error> { - self.command_tx.send(ImuCommand::Reset).map_err(|_| Error::WriteError) + self.command_tx + .send(ImuCommand::Reset) + .map_err(|_| Error::WriteError) } /// Sets the mode for BMI088. @@ -368,9 +398,9 @@ fn euler_to_quaternion(e: EulerAngles) -> Quaternion { let (roll, pitch, yaw) = ( e.roll.to_radians(), e.pitch.to_radians(), - e.yaw.to_radians() + e.yaw.to_radians(), ); - + let (sr, cr) = (roll * 0.5).sin_cos(); let (sp, cp) = (pitch * 0.5).sin_cos(); let (sy, cy) = (yaw * 0.5).sin_cos(); @@ -388,12 +418,12 @@ mod tests { #[test] fn sanity_check_bmi088() { // This test confirms that we can access the BMI088 code - let accel_addr = 0x18; // Default BMI088 accelerometer address - let gyro_addr = 0x68; // Default BMI088 gyroscope address - + let accel_addr = 0x18; // Default BMI088 accelerometer address + let gyro_addr = 0x68; // Default BMI088 gyroscope address + assert_eq!(accel_addr, crate::ACCEL_ADDR); assert_eq!(gyro_addr, crate::GYRO_ADDR); - + println!("BMI088 crate version: {}", env!("CARGO_PKG_VERSION")); } -} \ No newline at end of file +} diff --git a/imu/bmi088/src/registers.rs b/imu/bmi088/src/registers.rs index eb99feb..0b0f149 100644 --- a/imu/bmi088/src/registers.rs +++ b/imu/bmi088/src/registers.rs @@ -2,34 +2,34 @@ use num_derive::{FromPrimitive, ToPrimitive}; #[derive(Debug, Clone, Copy, FromPrimitive, ToPrimitive)] pub enum AccelRegisters { - ChipId = 0x00, - Error = 0x02, - Status = 0x1E, - AccelXLsb = 0x12, - AccelXMsb = 0x13, - AccelYLsb = 0x14, - AccelYMsb = 0x15, - AccelZLsb = 0x16, - AccelZMsb = 0x17, - TempMsb = 0x22, - TempLsb = 0x23, - PowerCtrl = 0x7D, - PowerConf = 0x7C, - AccConf = 0x40, - AccRange = 0x41, - SoftReset = 0x7E, + ChipId = 0x00, + Error = 0x02, + Status = 0x1E, + AccelXLsb = 0x12, + AccelXMsb = 0x13, + AccelYLsb = 0x14, + AccelYMsb = 0x15, + AccelZLsb = 0x16, + AccelZMsb = 0x17, + TempMsb = 0x22, + TempLsb = 0x23, + PowerCtrl = 0x7D, + PowerConf = 0x7C, + AccConf = 0x40, + AccRange = 0x41, + SoftReset = 0x7E, } #[derive(Debug, Clone, Copy, FromPrimitive, ToPrimitive)] pub enum GyroRegisters { - ChipId = 0x00, - XLsb = 0x02, - XMsb = 0x03, - YLsb = 0x04, - YMsb = 0x05, - ZLsb = 0x06, - ZMsb = 0x07, - Range = 0x0F, + ChipId = 0x00, + XLsb = 0x02, + XMsb = 0x03, + YLsb = 0x04, + YMsb = 0x05, + ZLsb = 0x06, + ZMsb = 0x07, + Range = 0x0F, Bandwidth = 0x10, PowerMode = 0x11, } @@ -37,9 +37,9 @@ pub enum GyroRegisters { #[derive(Debug, Clone, Copy)] pub enum Constants { AccelChipIdValue = 0x1E, - AccelI2cAddr = 0x19, // SDO pulled high - GyroI2cAddr = 0x69, // SDO pulled high - SoftResetCmd = 0xB6, + AccelI2cAddr = 0x19, // SDO pulled high + GyroI2cAddr = 0x69, // SDO pulled high + SoftResetCmd = 0xB6, } #[derive(Debug, Clone, Copy)] @@ -54,28 +54,28 @@ pub enum AccelRange { pub enum GyroRange { Dps2000 = 0x00, Dps1000 = 0x01, - Dps500 = 0x02, - Dps250 = 0x03, - Dps125 = 0x04, + Dps500 = 0x02, + Dps250 = 0x03, + Dps125 = 0x04, } #[derive(Debug, Clone, Copy, FromPrimitive, ToPrimitive)] pub enum AccelOdr { - Hz12_5 = 0x05, - Hz25 = 0x06, - Hz50 = 0x07, - Hz100 = 0x08, - Hz200 = 0x09, - Hz400 = 0x0A, - Hz800 = 0x0B, - Hz1600 = 0x0C, + Hz12_5 = 0x05, + Hz25 = 0x06, + Hz50 = 0x07, + Hz100 = 0x08, + Hz200 = 0x09, + Hz400 = 0x0A, + Hz800 = 0x0B, + Hz1600 = 0x0C, } #[derive(Debug, Clone, Copy, FromPrimitive, ToPrimitive)] pub enum GyroOdr { - Hz100 = 0x07, - Hz200 = 0x06, - Hz400 = 0x03, - Hz1000 = 0x02, - Hz2000 = 0x01, -} \ No newline at end of file + Hz100 = 0x07, + Hz200 = 0x06, + Hz400 = 0x03, + Hz1000 = 0x02, + Hz2000 = 0x01, +} diff --git a/imu/bno055/src/lib.rs b/imu/bno055/src/lib.rs index d906e57..cec5644 100644 --- a/imu/bno055/src/lib.rs +++ b/imu/bno055/src/lib.rs @@ -394,7 +394,7 @@ impl Bno055Reader { fn start_reading_thread_with_imu( mut imu: Bno055, data: Arc>, - command_rx: mpsc::Receiver + command_rx: mpsc::Receiver, ) { thread::spawn(move || { debug!("BNO055 reading thread started"); @@ -522,4 +522,4 @@ pub enum ImuCommand { SetMode(OperationMode), Reset, Stop, -} \ No newline at end of file +} From 8bb9b58c473d5d87267e625b74c02e0f2224a368 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Tue, 11 Feb 2025 17:15:00 -0800 Subject: [PATCH 17/24] bump versions --- Cargo.toml | 2 +- imu/bno055/Cargo.toml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index c9244f7..eb97040 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.2.391" +version = "0.3.0" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" diff --git a/imu/bno055/Cargo.toml b/imu/bno055/Cargo.toml index cd6a8a1..9b59509 100644 --- a/imu/bno055/Cargo.toml +++ b/imu/bno055/Cargo.toml @@ -3,7 +3,7 @@ name = "linux_bno055" readme = "README.md" description = "Interface for interacting with BNO055 IMUs" -version = "0.0.82" +version = "0.0.7" authors.workspace = true edition.workspace = true repository.workspace = true From c7501d858f4457e14407f7d14785dad6d63ffb3e Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Wed, 12 Feb 2025 11:48:28 -0800 Subject: [PATCH 18/24] fix typo in register --- imu/bmi088/src/registers.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/imu/bmi088/src/registers.rs b/imu/bmi088/src/registers.rs index 0b0f149..4cfff53 100644 --- a/imu/bmi088/src/registers.rs +++ b/imu/bmi088/src/registers.rs @@ -4,7 +4,7 @@ use num_derive::{FromPrimitive, ToPrimitive}; pub enum AccelRegisters { ChipId = 0x00, Error = 0x02, - Status = 0x1E, + Status = 0x03, AccelXLsb = 0x12, AccelXMsb = 0x13, AccelYLsb = 0x14, From 4c92acbbe4e6f486cf5436bf9ef5bb2a3f49dc24 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Wed, 12 Feb 2025 11:48:45 -0800 Subject: [PATCH 19/24] fix remapping issue in driver --- imu/bmi088/src/lib.rs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/imu/bmi088/src/lib.rs b/imu/bmi088/src/lib.rs index df1b49b..b42b7d9 100644 --- a/imu/bmi088/src/lib.rs +++ b/imu/bmi088/src/lib.rs @@ -164,7 +164,7 @@ impl Bmi088 { }) } - /// Reads raw accelerometer data and remaps axes so that gravity appears along -Z. + /// Reads raw accelerometer data without remapping. pub fn read_raw_accelerometer(&mut self) -> Result { let mut buf = [0u8; 6]; for i in 0..6 { @@ -182,13 +182,13 @@ impl Bmi088 { let raw_y = (LittleEndian::read_i16(&buf[2..4]) as f32) * scale; let raw_z = (LittleEndian::read_i16(&buf[4..6]) as f32) * scale; Ok(Vector3 { - x: raw_y, - y: raw_z, - z: raw_x, + x: raw_x, + y: raw_y, + z: raw_z, }) } - /// Reads raw gyroscope data and remaps axes for consistency. + /// Reads raw gyroscope data without remapping. pub fn read_raw_gyroscope(&mut self) -> Result { let mut buf = [0u8; 6]; for i in 0..6 { @@ -207,9 +207,9 @@ impl Bmi088 { let raw_y = (LittleEndian::read_i16(&buf[2..4]) as f32) * scale; let raw_z = (LittleEndian::read_i16(&buf[4..6]) as f32) * scale; Ok(Vector3 { - x: raw_y, - y: raw_z, - z: raw_x, + x: raw_x, + y: raw_y, + z: raw_z, }) } From b25b38e93560fd44819bf941c4168b50ea43f17f Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Wed, 12 Feb 2025 11:49:00 -0800 Subject: [PATCH 20/24] bump version --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index eb97040..019407a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.3.0" +version = "0.3.1" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" From 93575be48d75c693d42c8cdd58b90f16937e4eb4 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Wed, 12 Feb 2025 17:05:35 -0800 Subject: [PATCH 21/24] remove euler and quat stuff --- imu/bmi088/src/lib.rs | 45 ------------------------------------------- 1 file changed, 45 deletions(-) diff --git a/imu/bmi088/src/lib.rs b/imu/bmi088/src/lib.rs index b42b7d9..015b06f 100644 --- a/imu/bmi088/src/lib.rs +++ b/imu/bmi088/src/lib.rs @@ -120,8 +120,6 @@ pub struct Bmi088 { gyro_i2c: LinuxI2CDevice, accel_range: AccelRange, gyro_range: GyroRange, - /// Used for simple Euler integration of gyro data. - simple_euler: EulerAngles, } impl Bmi088 { @@ -160,7 +158,6 @@ impl Bmi088 { gyro_i2c, accel_range: AccelRange::G3, gyro_range: GyroRange::Dps2000, - simple_euler: EulerAngles::default(), }) } @@ -224,21 +221,6 @@ impl Bmi088 { let temp_raw = (msb * 8) + (lsb / 32); Ok(((temp_raw as f32) * 0.125 + 23.0) as i8) } - - /// Updates the simple Euler integration state based on gyroscope data. - pub fn update_simple_euler(&mut self, dt: f32) -> Result<(), Error> { - let gyro = self.read_raw_gyroscope()?; - // Standard integration: roll from gyro.x, pitch from gyro.y, yaw from gyro.z. - self.simple_euler.roll += gyro.x * dt; - self.simple_euler.pitch += gyro.y * dt; - self.simple_euler.yaw += gyro.z * dt; - Ok(()) - } - - /// Returns the current integrated Euler angles. - pub fn get_simple_euler_angles(&self) -> EulerAngles { - self.simple_euler - } } /// BMI088Reader runs a background thread to update sensor data continuously. @@ -292,7 +274,6 @@ impl Bmi088Reader { } let mut imu = init_result.unwrap(); let _ = tx.send(Ok(())); - let mut simple_euler = EulerAngles::default(); while let Ok(guard) = running.read() { if !*guard { @@ -335,12 +316,6 @@ impl Bmi088Reader { } if let Ok(gyro) = imu.read_raw_gyroscope() { sensor_data.gyroscope = gyro; - let dt = 0.01; - simple_euler.roll += gyro.x * dt; - simple_euler.pitch += gyro.y * dt; - simple_euler.yaw += gyro.z * dt; - sensor_data.euler = simple_euler; - sensor_data.quaternion = euler_to_quaternion(simple_euler); } else { warn!("Failed to read gyroscope"); } @@ -393,26 +368,6 @@ impl Drop for Bmi088Reader { } } -// Add this helper function for quaternion conversion -fn euler_to_quaternion(e: EulerAngles) -> Quaternion { - let (roll, pitch, yaw) = ( - e.roll.to_radians(), - e.pitch.to_radians(), - e.yaw.to_radians(), - ); - - let (sr, cr) = (roll * 0.5).sin_cos(); - let (sp, cp) = (pitch * 0.5).sin_cos(); - let (sy, cy) = (yaw * 0.5).sin_cos(); - - Quaternion { - w: cr * cp * cy + sr * sp * sy, - x: sr * cp * cy - cr * sp * sy, - y: cr * sp * cy + sr * cp * sy, - z: cr * cp * sy - sr * sp * cy, - } -} - #[cfg(test)] mod tests { #[test] From c5ede133edab7c6b1cb8bad1d8cb5d31c2844f80 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Wed, 12 Feb 2025 17:09:11 -0800 Subject: [PATCH 22/24] bump version --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 019407a..32d3355 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.3.1" +version = "0.3.11" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" From f06cd901c2845b7752b7f56df66d28bbe1aa8733 Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Thu, 13 Feb 2025 16:13:16 -0800 Subject: [PATCH 23/24] final thouches for publish --- Cargo.toml | 2 +- imu/bmi088/Cargo.toml | 13 +++++++++---- imu/bmi088/README.md | 7 +++++++ 3 files changed, 17 insertions(+), 5 deletions(-) create mode 100644 imu/bmi088/README.md diff --git a/Cargo.toml b/Cargo.toml index 32d3355..019407a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,7 +11,7 @@ members = [ resolver = "2" [workspace.package] -version = "0.3.11" +version = "0.3.1" authors = ["Wesley Maa "] edition = "2021" description = "IMU package" diff --git a/imu/bmi088/Cargo.toml b/imu/bmi088/Cargo.toml index 1d38e1c..258255a 100644 --- a/imu/bmi088/Cargo.toml +++ b/imu/bmi088/Cargo.toml @@ -1,15 +1,20 @@ [package] name = "linux_bmi088" -version.workspace = true -authors.workspace = true -edition.workspace = true +readme = "README.md" description = "Interface for BMI088 IMU" +version = "0.0.1" +edition.workspace = true +authors = ["Ali Kuwajerwala "] license.workspace = true repository.workspace = true +[lib] +name = "linux_bmi088" +crate-type = ["rlib"] + [dependencies] byteorder = "1.5" i2cdev = "0.6" log = "0.4" num-derive = "0.4" -num-traits = "0.2" \ No newline at end of file +num-traits = "0.2" \ No newline at end of file diff --git a/imu/bmi088/README.md b/imu/bmi088/README.md new file mode 100644 index 0000000..e12dffe --- /dev/null +++ b/imu/bmi088/README.md @@ -0,0 +1,7 @@ +# BMI088 IMU + +This crate contains the code for interacting with the BMI088 IMU. + +Code based on the [embedded-hal implementation of the BMI088](https://github.com/tstellanova/bmi088). + + From ee23bdabe575881d2cd3461b6ba22428ea0ecd8f Mon Sep 17 00:00:00 2001 From: Ali Kuwajerwala Date: Thu, 13 Feb 2025 16:34:46 -0800 Subject: [PATCH 24/24] change dependency to crate instead of local folder --- imu/Cargo.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/imu/Cargo.toml b/imu/Cargo.toml index 4ff161d..6a210df 100644 --- a/imu/Cargo.toml +++ b/imu/Cargo.toml @@ -15,7 +15,7 @@ crate-type = ["rlib"] [dependencies] hexmove = "0.1.6" hiwonder = "0.2.3" -linux_bno055 = { path = "./bno055" } +linux_bno055 = "0.0.7" +linux_bmi088 = "0.0.1" socketcan = "3.3.0" log = "0.4" -linux_bmi088 = { path = "./bmi088" }