diff --git a/Cargo.lock b/Cargo.lock index 3f52b4e..1873abb 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -88,6 +88,15 @@ version = "1.0.99" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b0674a1ddeecb70197781e945de4b3b8ffb61fa939a5597bcf48503737663100" +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + [[package]] name = "arbitrary" version = "1.4.2" @@ -1450,6 +1459,21 @@ version = "0.10.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1d87ecb2933e8aeadb3e3a02b828fed80a7528047e68b4f424523a0981a3a084" +[[package]] +name = "nalgebra" +version = "0.34.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c4d5b3eff5cd580f93da45e64715e8c20a3996342f1e466599cf7a267a0c2f5f" +dependencies = [ + "approx", + "defmt 1.0.1", + "num-complex", + "num-rational", + "num-traits", + "simba", + "typenum", +] + [[package]] name = "nb" version = "0.1.3" @@ -1499,6 +1523,15 @@ dependencies = [ "syn 2.0.106", ] +[[package]] +name = "num-complex" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" +dependencies = [ + "num-traits", +] + [[package]] name = "num-derive" version = "0.3.3" @@ -1521,6 +1554,25 @@ dependencies = [ "syn 2.0.106", ] +[[package]] +name = "num-integer" +version = "0.1.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-rational" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" +dependencies = [ + "num-integer", + "num-traits", +] + [[package]] name = "num-traits" version = "0.2.19" @@ -1553,6 +1605,12 @@ dependencies = [ "defmt 0.3.100", ] +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + [[package]] name = "petgraph" version = "0.7.1" @@ -2104,6 +2162,18 @@ dependencies = [ "windows-sys 0.52.0", ] +[[package]] +name = "simba" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c99284beb21666094ba2b75bbceda012e610f5479dfcc2d6e2426f53197ffd95" +dependencies = [ + "approx", + "num-complex", + "num-traits", + "paste", +] + [[package]] name = "smlang" version = "0.8.0" @@ -2359,6 +2429,12 @@ dependencies = [ "termcolor", ] +[[package]] +name = "typenum" +version = "1.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "562d481066bde0658276a35467c4af00bdc6ee726305698a55b86e61d7ad82bb" + [[package]] name = "ublox" version = "0.4.5" @@ -2449,6 +2525,7 @@ dependencies = [ "embassy-time 0.4.0", "embedded-io-async", "heapless 0.9.2", + "nalgebra", "ublox 0.9.0", "uor-peripherals", "uor-utils", diff --git a/common/drivers/Cargo.toml b/common/drivers/Cargo.toml index 8958d9c..9865bb2 100644 --- a/common/drivers/Cargo.toml +++ b/common/drivers/Cargo.toml @@ -30,5 +30,11 @@ ublox = { version = "0.9.0", features = [ "serde", "ubx_proto31", ], default-features = false } + +nalgebra = { version = "0.34.1", features = [ + "libm", + "defmt", +], default-features = false } + uor-peripherals = { workspace = true } uor-utils = { workspace = true } diff --git a/common/drivers/src/iim20670/config.rs b/common/drivers/src/iim20670/config.rs new file mode 100644 index 0000000..8dd606c --- /dev/null +++ b/common/drivers/src/iim20670/config.rs @@ -0,0 +1,246 @@ +/// IIM-20670 6-Axis IMU Configuration + +// Protocol Constants +/// Fixed value register content (Bank 0, offset 0x0B) +pub const FIXED_VALUE: u16 = 0xAA55; + +/// CRC seed value for the 32-bit SPI protocol +pub const CRC_SEED: u8 = 0xFF; + +// Register Offsets (5-bit address within Bank 0) +pub mod reg { + // Sensor output data (available in all banks) + pub const GYRO_X_DATA: u8 = 0x00; + pub const GYRO_Y_DATA: u8 = 0x01; + pub const GYRO_Z_DATA: u8 = 0x02; + pub const TEMP1_DATA: u8 = 0x03; + pub const ACCEL_X_DATA: u8 = 0x04; + pub const ACCEL_Y_DATA: u8 = 0x05; + pub const ACCEL_Z_DATA: u8 = 0x06; + pub const TEMP2_DATA: u8 = 0x07; + + // Bank 0 only + pub const ACCEL_X_DATA_LR: u8 = 0x08; + pub const ACCEL_Y_DATA_LR: u8 = 0x09; + pub const ACCEL_Z_DATA_LR: u8 = 0x0A; + pub const FIXED_VALUE: u8 = 0x0B; + pub const RESET_CTRL: u8 = 0x18; +} + +// Gyroscope Full-Scale Range (Table 18) +#[derive(Clone, Copy, PartialEq, Eq, Debug)] +pub enum GyroFsSel { + /// ±328 dps, 100 LSB/dps + Dps328_0 = 0b0000, + /// ±655 dps, 50 LSB/dps (DEFAULT) + Dps655 = 0b0001, + /// ±1311 dps, 25 LSB/dps + Dps1311_0 = 0b0010, + /// ±1966 dps, 16.67 LSB/dps + Dps1966 = 0b0011, + /// ±218 dps, 150 LSB/dps + Dps218 = 0b0100, + /// ±437 dps, 75 LSB/dps + Dps437 = 0b0101, + /// ±874 dps, 37.5 LSB/dps + Dps874 = 0b0110, + /// ±1311 dps, 25 LSB/dps + Dps1311_1 = 0b0111, + /// ±61 dps, 533.34 LSB/dps + Dps61 = 0b1000, + /// ±123 dps, 266.67 LSB/dps + Dps123 = 0b1001, + /// ±246 dps, 133.33 LSB/dps + Dps246 = 0b1010, + /// ±492 dps, 66.67 LSB/dps + Dps492 = 0b1011, + /// ±41 dps, 800 LSB/dps + Dps41 = 0b1100, + /// ±82 dps, 400 LSB/dps + Dps82 = 0b1101, + /// ±164 dps, 200 LSB/dps + Dps164 = 0b1110, + /// ±328 dps, 100 LSB/dps + Dps328_1 = 0b1111, +} + +impl GyroFsSel { + /// Returns the full-scale range in dps (degrees per second) + pub fn full_scale_dps(&self) -> f32 { + match self { + Self::Dps328_0 | Self::Dps328_1 => 328.0, + Self::Dps655 => 655.36, + Self::Dps1311_0 | Self::Dps1311_1 => 1311.0, + Self::Dps1966 => 1966.0, + Self::Dps218 => 218.0, + Self::Dps437 => 437.0, + Self::Dps874 => 874.0, + Self::Dps61 => 61.0, + Self::Dps123 => 123.0, + Self::Dps246 => 246.0, + Self::Dps492 => 492.0, + Self::Dps41 => 41.0, + Self::Dps82 => 82.0, + Self::Dps164 => 164.0, + } + } + + /// Returns the sensitivity in LSB/dps + pub fn sensitivity(&self) -> f32 { + match self { + Self::Dps328_0 | Self::Dps328_1 => 100.0, + Self::Dps655 => 50.0, + Self::Dps1311_0 | Self::Dps1311_1 => 25.0, + Self::Dps1966 => 16.67, + Self::Dps218 => 150.0, + Self::Dps437 => 75.0, + Self::Dps874 => 37.5, + Self::Dps61 => 533.34, + Self::Dps123 => 266.67, + Self::Dps246 => 133.33, + Self::Dps492 => 66.67, + Self::Dps41 => 800.0, + Self::Dps82 => 400.0, + Self::Dps164 => 200.0, + } + } +} + +impl Default for GyroFsSel { + fn default() -> Self { + Self::Dps655 + } +} + +// Accelerometer Full-Scale Range (Table 17) +#[derive(Clone, Copy, PartialEq, Eq, Debug)] +pub enum AccelFsSel { + /// FS = ±16.384g, FS_LR = ±32.768g + G16Lr32 = 0b000, + /// FS = ±16.384g, FS_LR = ±65.536g (DEFAULT) + G16Lr65 = 0b001, + /// FS = ±32.768g, FS_LR = ±32.768g + G32Lr32 = 0b010, + /// FS = ±32.768g, FS_LR = ±65.536g + G32Lr65 = 0b011, + /// FS = ±2.048g, FS_LR = ±4.096g + G2Lr4 = 0b100, + /// FS = ±2.048g, FS_LR = ±16.384g + G2Lr16 = 0b101, + /// FS = ±4.096g, FS_LR = ±4.096g + G4Lr4 = 0b110, + /// FS = ±4.096g, FS_LR = ±8.192g + G4Lr8 = 0b111, +} + +impl AccelFsSel { + /// Returns the high-resolution full-scale range in g + pub fn full_scale_g(&self) -> f32 { + match self { + Self::G16Lr32 | Self::G16Lr65 => 16.384, + Self::G32Lr32 | Self::G32Lr65 => 32.768, + Self::G2Lr4 | Self::G2Lr16 => 2.048, + Self::G4Lr4 | Self::G4Lr8 => 4.096, + } + } + + /// Returns the low-resolution full-scale range in g + pub fn full_scale_lr_g(&self) -> f32 { + match self { + Self::G16Lr32 => 32.768, + Self::G16Lr65 => 65.536, + Self::G32Lr32 => 32.768, + Self::G32Lr65 => 65.536, + Self::G2Lr4 => 4.096, + Self::G2Lr16 => 16.384, + Self::G4Lr4 => 4.096, + Self::G4Lr8 => 8.192, + } + } +} + +impl Default for AccelFsSel { + fn default() -> Self { + Self::G16Lr65 + } +} + +// SPI Return Status +#[derive(Clone, Copy, PartialEq, Eq, Debug)] +pub enum SpiReturnStatus { + /// 00: Reserved + Reserved, + /// 01: Successful register read/write + Success, + /// 10: Data not ready or self-test in progress + InProgress, + /// 11: Error + Error, +} + +impl From for SpiReturnStatus { + fn from(val: u8) -> Self { + match val & 0x03 { + 0b00 => Self::Reserved, + 0b01 => Self::Success, + 0b10 => Self::InProgress, + 0b11 => Self::Error, + _ => unreachable!(), + } + } +} + +// CRC Calculation (Section 5.2 of datasheet) +/// The result is inverted before returning (per datasheet). +pub fn compute_crc(input_24bit: u32) -> u8 { + let mut crc: u8 = CRC_SEED; + + for i in (0..24).rev() { + let bit = ((input_24bit >> i) & 1) as u8; + let msb = (crc >> 7) & 1; + + let mut new_crc: u8 = 0; + new_crc |= (bit ^ msb) << 0; // CRC_New[0] + new_crc |= (crc & 0x01) << 1; // CRC_New[1] = CRC[0] + new_crc |= ((crc >> 1) ^ msb) << 2; // CRC_New[2] = CRC[1] ^ CRC[7] + new_crc |= ((crc >> 2) ^ msb) << 3; // CRC_New[3] = CRC[2] ^ CRC[7] + new_crc |= ((crc >> 3) ^ msb) << 4; // CRC_New[4] = CRC[3] ^ CRC[7] + new_crc |= ((crc >> 4) & 1) << 5; // CRC_New[5] = CRC[4] + new_crc |= ((crc >> 5) & 1) << 6; // CRC_New[6] = CRC[5] + new_crc |= ((crc >> 6) & 1) << 7; // CRC_New[7] = CRC[6] + + crc = new_crc; + } + + // Final inversion + crc ^ 0xFF +} + +pub fn build_spi_command( + read: bool, + offset: u8, + data: u16, +) -> u32 { + let rw_bit: u32 = if read { 0 } else { 1 }; + let addr = (offset & 0x1F) as u32; + + // Upper 24 bits: RW(1) | ADDR(5) | RS(2, always 00 on MOSI) | DATA(16) + let upper_24 = (rw_bit << 23) | (addr << 18) | (data as u32); + let crc = compute_crc(upper_24); + + (upper_24 << 8) | (crc as u32) +} + +pub fn parse_spi_response(response: u32) -> (u8, SpiReturnStatus, u16, bool) { + let offset = ((response >> 26) & 0x1F) as u8; + let status = SpiReturnStatus::from(((response >> 24) & 0x03) as u8); + let data = ((response >> 8) & 0xFFFF) as u16; + let received_crc = (response & 0xFF) as u8; + + // Verify CRC over the upper 24 bits of the response + let upper_24 = (response >> 8) & 0x00FF_FFFF; + let computed_crc = compute_crc(upper_24); + let crc_valid = received_crc == computed_crc; + + (offset, status, data, crc_valid) +} diff --git a/common/drivers/src/iim20670/driver.rs b/common/drivers/src/iim20670/driver.rs new file mode 100644 index 0000000..9de902c --- /dev/null +++ b/common/drivers/src/iim20670/driver.rs @@ -0,0 +1,230 @@ +/// IIM-20670 6-Axis IMU Driver (Minimal) +use defmt::{error, info, warn}; +use embassy_time::Timer; +use nalgebra::Vector3; +use uor_peripherals::spi::peripheral::UORMonoCsSPI; + +use crate::iim20670::{ + config::{AccelFsSel, FIXED_VALUE, GyroFsSel, SpiReturnStatus, build_spi_command, parse_spi_response, reg}, + math::{ImuSample, RawImuSample, accel_lr_raw_to_g, accel_raw_to_g, convert_raw_sample, gyro_raw_to_dps, temp_raw_to_celsius}, +}; + +// Error Type +#[derive(Debug, defmt::Format)] +pub enum Iim20670Error { + /// SPI bus communication failure + Spi, + /// CRC mismatch on SPI response + CrcMismatch, + /// SPI status bits indicate error (RS = 11) + StatusError, + /// Data not ready (RS = 10) + DataNotReady, + /// WHO_AM_I register returned unexpected value + WhoAmIMismatch(u8), + /// Fixed value register returned unexpected value + FixedValueMismatch(u16), +} + +// Driver Struct +pub struct IIM20670<'a> { + pub spi_service: UORMonoCsSPI<'a>, + /// Current gyroscope full-scale setting (default: ±655 dps) + pub gyro_fs: GyroFsSel, + /// Current accelerometer full-scale setting (default: ±16.384g) + pub accel_fs: AccelFsSel, +} + +impl<'a> IIM20670<'a> { + // Construction & Initialization + pub async fn new(spi_service: UORMonoCsSPI<'a>) -> Self { + let mut imu = IIM20670 { + spi_service, + gyro_fs: GyroFsSel::default(), + accel_fs: AccelFsSel::default(), + }; + + imu.hard_reset().await; + // Start-up time for register read/write from power-up is 200 ms + Timer::after_millis(250).await; + + // Verify chip identity + match imu.verify_identity().await { + Ok(_) => info!("IIM-20670 identity verified"), + Err(e) => error!("IIM-20670 identity check failed: {:?}", e), + } + + imu + } + + // Low-Level SPI Communication + async fn spi_transfer_32( + &mut self, + command: u32, + ) -> Result<(SpiReturnStatus, u16), Iim20670Error> { + let tx_bytes = command.to_be_bytes(); + let mut rx_bytes = [0u8; 4]; + + match self.spi_service.transfer::(&mut rx_bytes, &tx_bytes).await { + Ok(_) => {} + Err(_) => { + warn!("SPI transfer failed for IIM-20670"); + return Err(Iim20670Error::Spi); + } + } + + let response = u32::from_be_bytes(rx_bytes); + let (_offset, status, data, crc_valid) = parse_spi_response(response); + + if !crc_valid { + warn!("CRC mismatch on IIM-20670 SPI response"); + return Err(Iim20670Error::CrcMismatch); + } + + match status { + SpiReturnStatus::Error => Err(Iim20670Error::StatusError), + _ => Ok((status, data)), + } + } + + /// Read a 16-bit register from the currently selected bank (Bank 0) + async fn read_register( + &mut self, + offset: u8, + ) -> Result { + let cmd = build_spi_command(true, offset, 0x0000); + let (status, data) = self.spi_transfer_32(cmd).await?; + + match status { + SpiReturnStatus::Success => Ok(data), + SpiReturnStatus::InProgress => Err(Iim20670Error::DataNotReady), + _ => Ok(data), + } + } + + /// Write a 16-bit value to a register in the currently selected bank (Bank 0) + async fn write_register( + &mut self, + offset: u8, + data: u16, + ) -> Result<(), Iim20670Error> { + let cmd = build_spi_command(false, offset, data); + let (status, _) = self.spi_transfer_32(cmd).await?; + + match status { + SpiReturnStatus::Success => Ok(()), + SpiReturnStatus::Error => Err(Iim20670Error::StatusError), + _ => Ok(()), + } + } + + // Reset + pub async fn hard_reset(&mut self) { + // Write bit 2 of register 0x18 in bank 0 + match self.write_register(reg::RESET_CTRL, 0x0004).await { + Ok(_) => info!("IIM-20670 hard reset requested"), + Err(_) => warn!("Failed to send hard reset to IIM-20670"), + } + } + + // Identity Verification + pub async fn verify_identity(&mut self) -> Result<(), Iim20670Error> { + let fixed = self.read_register(reg::FIXED_VALUE).await?; + if fixed != FIXED_VALUE { + error!("IIM-20670 fixed value mismatch: expected 0x{:04X}, got 0x{:04X}", FIXED_VALUE, fixed); + return Err(Iim20670Error::FixedValueMismatch(fixed)); + } + + info!("IIM-20670 identity OK (FIXED=0x{:04X})", fixed); + Ok(()) + } + + // Raw Sensor Data Reading + /// Read raw gyroscope data for all three axes. + pub async fn read_gyro_raw(&mut self) -> Result, Iim20670Error> { + let x = self.read_register(reg::GYRO_X_DATA).await? as i16; + let y = self.read_register(reg::GYRO_Y_DATA).await? as i16; + let z = self.read_register(reg::GYRO_Z_DATA).await? as i16; + Ok(Vector3::new(x, y, z)) + } + + /// Read raw accelerometer data (high-resolution) for all three axes. + pub async fn read_accel_raw(&mut self) -> Result, Iim20670Error> { + let x = self.read_register(reg::ACCEL_X_DATA).await? as i16; + let y = self.read_register(reg::ACCEL_Y_DATA).await? as i16; + let z = self.read_register(reg::ACCEL_Z_DATA).await? as i16; + Ok(Vector3::new(x, y, z)) + } + + /// Read raw accelerometer data (low-resolution) for all three axes. + pub async fn read_accel_lr_raw(&mut self) -> Result, Iim20670Error> { + let x = self.read_register(reg::ACCEL_X_DATA_LR).await? as i16; + let y = self.read_register(reg::ACCEL_Y_DATA_LR).await? as i16; + let z = self.read_register(reg::ACCEL_Z_DATA_LR).await? as i16; + Ok(Vector3::new(x, y, z)) + } + + /// Read raw temperature sensor 1 data. + pub async fn read_temp1_raw(&mut self) -> Result { + Ok(self.read_register(reg::TEMP1_DATA).await? as i16) + } + + /// Read raw temperature sensor 2 data. + pub async fn read_temp2_raw(&mut self) -> Result { + Ok(self.read_register(reg::TEMP2_DATA).await? as i16) + } + + /// Read a complete raw IMU sample (all sensors in one call). + pub async fn read_raw_sample(&mut self) -> Result { + let gyro = self.read_gyro_raw().await?; + let temp1 = self.read_temp1_raw().await?; + let accel = self.read_accel_raw().await?; + let temp2 = self.read_temp2_raw().await?; + let accel_lr = self.read_accel_lr_raw().await?; + + Ok(RawImuSample { + gyro, + accel, + accel_lr, + temp1, + temp2, + }) + } + + // Converted Sensor Data Reading + /// Read gyroscope data converted to degrees per second. + pub async fn read_gyro_dps(&mut self) -> Result, Iim20670Error> { + let raw = self.read_gyro_raw().await?; + Ok(gyro_raw_to_dps(&raw, self.gyro_fs)) + } + + /// Read accelerometer data (high-resolution) converted to g. + pub async fn read_accel_g(&mut self) -> Result, Iim20670Error> { + let raw = self.read_accel_raw().await?; + Ok(accel_raw_to_g(&raw, self.accel_fs)) + } + + /// Read accelerometer data (low-resolution) converted to g. + pub async fn read_accel_lr_g(&mut self) -> Result, Iim20670Error> { + let raw = self.read_accel_lr_raw().await?; + Ok(accel_lr_raw_to_g(&raw, self.accel_fs)) + } + + /// Read temperature sensor 1 in degrees Celsius. + pub async fn read_temperature_1(&mut self) -> Result { + let raw = self.read_temp1_raw().await?; + Ok(temp_raw_to_celsius(raw)) + } + + /// Read temperature sensor 2 in degrees Celsius. + pub async fn read_temperature_2(&mut self) -> Result { + let raw = self.read_temp2_raw().await?; + Ok(temp_raw_to_celsius(raw)) + } + + /// Read a complete IMU sample with all values converted to physical units. + pub async fn read_sample(&mut self) -> Result { + let raw = self.read_raw_sample().await?; + Ok(convert_raw_sample(&raw, self.gyro_fs, self.accel_fs)) + } +} diff --git a/common/drivers/src/iim20670/math.rs b/common/drivers/src/iim20670/math.rs new file mode 100644 index 0000000..821ee0f --- /dev/null +++ b/common/drivers/src/iim20670/math.rs @@ -0,0 +1,108 @@ +/// IIM-20670 Math / Conversion Functions +use nalgebra::Vector3; + +use crate::iim20670::config::{AccelFsSel, GyroFsSel}; + +// Raw Data Container +/// Complete raw IMU sample (16-bit signed, 2's complement from registers) +#[derive(Clone, Copy, Default, Debug)] +pub struct RawImuSample { + pub gyro: Vector3, + pub accel: Vector3, + pub accel_lr: Vector3, + pub temp1: i16, + pub temp2: i16, +} + +// Converted Data Container +/// Complete converted IMU sample in physical units +#[derive(Clone, Copy, Debug)] +pub struct ImuSample { + /// Angular rate in degrees per second + pub gyro_dps: Vector3, + /// Acceleration in g (high-resolution) + pub accel_g: Vector3, + /// Acceleration in g (low-resolution) + pub accel_lr_g: Vector3, + /// Temperature from sensor 1 in degrees Celsius + pub temperature_1_c: f32, + /// Temperature from sensor 2 in degrees Celsius + pub temperature_2_c: f32, +} + +impl Default for ImuSample { + fn default() -> Self { + Self { + gyro_dps: Vector3::zeros(), + accel_g: Vector3::zeros(), + accel_lr_g: Vector3::zeros(), + temperature_1_c: 0.0, + temperature_2_c: 0.0, + } + } +} + +// Gyroscope Conversion (Section 4.7.1) +/// Convert raw gyroscope vector to degrees per second. +/// Formula per axis: ω = gyro_data / 2^15 * FS +pub fn gyro_raw_to_dps( + raw: &Vector3, + fs_sel: GyroFsSel, +) -> Vector3 { + let scale = fs_sel.full_scale_dps() / 32768.0; + raw.cast::() * scale +} + +/// Convert gyroscope dps to radians per second. +pub fn dps_to_rads(dps: &Vector3) -> Vector3 { + dps * (core::f32::consts::PI / 180.0) +} + +// Accelerometer Conversion (Section 4.8.1) +/// Convert raw accelerometer vector (high-resolution) to g +/// Formula per axis: a = accel_data / 2^15 * FS +pub fn accel_raw_to_g( + raw: &Vector3, + fs_sel: AccelFsSel, +) -> Vector3 { + let scale = fs_sel.full_scale_g() / 32768.0; + raw.cast::() * scale +} + +/// Convert raw accelerometer vector (low-resolution) to g +/// Formula per axis: a_lr = accel_data_lr / 2^15 * FS_LR +pub fn accel_lr_raw_to_g( + raw: &Vector3, + fs_sel: AccelFsSel, +) -> Vector3 { + let scale = fs_sel.full_scale_lr_g() / 32768.0; + raw.cast::() * scale +} + +/// Convert acceleration from g to m/s². +pub fn g_to_ms2(g: &Vector3) -> Vector3 { + g * 9.80665 +} + +// Temperature Conversion (Section 4.9) +/// Convert a raw temperature register value to degrees Celsius. +/// Formula: TEMPERATURE (°C) = 25 + temp_data / 20 +pub fn temp_raw_to_celsius(raw: i16) -> f32 { + 25.0 + (raw as f32 / 20.0) +} + +// Full Sample Conversion +/// Convert a complete raw IMU sample to physical units. +pub fn convert_raw_sample( + raw: &RawImuSample, + gyro_fs: GyroFsSel, + accel_fs: AccelFsSel, +) -> ImuSample { + ImuSample { + gyro_dps: gyro_raw_to_dps(&raw.gyro, gyro_fs), + accel_g: accel_raw_to_g(&raw.accel, accel_fs), + accel_lr_g: accel_lr_raw_to_g(&raw.accel_lr, accel_fs), + temperature_1_c: temp_raw_to_celsius(raw.temp1), + temperature_2_c: temp_raw_to_celsius(raw.temp2), + } +} diff --git a/common/drivers/src/iim20670/mod.rs b/common/drivers/src/iim20670/mod.rs new file mode 100644 index 0000000..60a6fd2 --- /dev/null +++ b/common/drivers/src/iim20670/mod.rs @@ -0,0 +1,3 @@ +pub mod config; +pub mod driver; +pub mod math; diff --git a/common/drivers/src/lib.rs b/common/drivers/src/lib.rs index a507a61..7e62e24 100644 --- a/common/drivers/src/lib.rs +++ b/common/drivers/src/lib.rs @@ -3,3 +3,4 @@ pub mod ejection_channel; pub mod ms561101; pub mod rfd900x; +pub mod iim20670; \ No newline at end of file