From 0424495d942aeb068913ca6c53b637fa3f4dd553 Mon Sep 17 00:00:00 2001 From: Walter Bonetti Date: Sat, 7 Sep 2024 16:31:02 -0400 Subject: [PATCH] Fixes/Add: Gyroscope SelfTest and FIFO reads * Fix crtl type replaced by ctrl * Add missing documentation on different fn * Add Gyroscope SelfTest * Add private helper for U12.4 format and f32_abs * Fix FIFO sample count in `read_fifo` --- Cargo.toml | 19 +++- src/command.rs | 138 +++++++++++++++++++++++++++ src/driver.rs | 211 ++++++++++++++++++++++++++++++++++++----- src/fraction_helper.rs | 19 ++++ src/lib.rs | 2 +- 5 files changed, 360 insertions(+), 29 deletions(-) create mode 100644 src/fraction_helper.rs diff --git a/Cargo.toml b/Cargo.toml index 34f4bdf..a7ca297 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -3,26 +3,35 @@ name = "qmi8658" categories = ["embedded", "no-std"] version = "0.1.0" edition = "2021" -authors = [ - "Walter Bonetti " -] +authors = ["Walter Bonetti "] license = "MIT OR Apache-2.0" description = "QMI8658 Sensor/Gyroscope" keywords = ["no-std", "qmi8658", "embedded", "embedded-hal-driver", "sensor"] -include = ["src/**/*.rs", "crates-io.md", "README.md", "LICENSE-APACHE", "LICENSE-MIT"] +include = [ + "src/**/*.rs", + "crates-io.md", + "README.md", + "LICENSE-APACHE", + "LICENSE-MIT", +] repository = "https://github.com/IniterWorker/qmi8658" readme = "README.md" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [package.metadata.docs.rs] -targets = [ "thumbv7m-none-eabi", "thumbv7em-none-eabihf" ] +targets = ["thumbv7m-none-eabi", "thumbv7em-none-eabihf"] [dependencies] # Embedded HAL abstraction # We use this layer to abstract hardware i2c/spi embedded-hal = { version = "1.0.0" } bitfield = "0.15.0" +log = { version = "0.4", default-features = false, optional = true } + +[features] +default = [] +loglib = ["log"] [dev-dependencies.cargo-husky] version = "1" diff --git a/src/command.rs b/src/command.rs index bca2096..df1a03f 100644 --- a/src/command.rs +++ b/src/command.rs @@ -410,6 +410,16 @@ pub mod register { assert_eq!(data.aodr(), AccelerometerODR::NormalAODR7); } + #[test] + fn test_struct_bit_range_bis() { + let register: u8 = 0b0000_0111; + + let data = Ctrl2Register(register); + assert!(!data.ast()); + assert_eq!(data.afs(), AccelerometerFS::FS2G); + assert_eq!(data.aodr(), AccelerometerODR::NormalAODR7); + } + #[test] fn test_struct_set_bit_range() { let register_result: u8 = 0b1000_0111; @@ -421,6 +431,18 @@ pub mod register { data.set_aodr(AccelerometerODR::NormalAODR7); assert_eq!(data.0, register_result); } + + #[test] + fn test_struct_set_bit_range_bis() { + let register_result: u8 = 0b0000_0111; + let register_default: u8 = 0b0000_0000; + + let mut data = Ctrl2Register(register_default); + data.set_ast(false); + data.set_afs(AccelerometerFS::FS2G); + data.set_aodr(AccelerometerODR::NormalAODR7); + assert_eq!(data.0, register_result); + } } } @@ -493,12 +515,120 @@ pub mod register { #[derive(Clone, Copy, PartialEq, Eq)] pub struct Ctrl3Register(u8); impl Debug; + #[doc = "Set Gyroscope Self-Test Enable/Disable"] pub bool, gst, set_gst: 7; #[doc = "Set Gyroscope Full-scale"] pub GyroscopeFS, gfs, set_gfs: 6, 4; #[doc = "Set Gyroscope Output Data Rate (ODR)"] pub GyroscopeODR, godr, set_godr: 3, 0; } + + #[cfg(test)] + mod test { + use bitfield::{BitRange, BitRangeMut}; + + use super::Ctrl3Register; + use super::GyroscopeFS; + use super::GyroscopeODR; + + #[test] + fn test_gfs_bit_range() { + // Test DPS16 + let value_dps16: u8 = 0b0000_0000; + assert_eq!( + >::bit_range(&value_dps16, 6, 4), + GyroscopeFS::DPS16 + ); + + // Test DPS32 + let value_dps32: u8 = 0b0001_0000; + assert_eq!( + >::bit_range(&value_dps32, 6, 4), + GyroscopeFS::DPS32 + ); + + // Test DPS64 + let value_dps64: u8 = 0b0010_0000; + assert_eq!( + >::bit_range(&value_dps64, 6, 4), + GyroscopeFS::DPS64 + ); + + // Test DPS128 + let value_dps128: u8 = 0b0011_0000; + assert_eq!( + >::bit_range(&value_dps128, 6, 4), + GyroscopeFS::DPS128 + ); + + // Test DPS256 + let value_dps256: u8 = 0b0100_0000; + assert_eq!( + >::bit_range(&value_dps256, 6, 4), + GyroscopeFS::DPS256 + ); + + // Test DPS512 + let value_dps512: u8 = 0b0101_0000; + assert_eq!( + >::bit_range(&value_dps512, 6, 4), + GyroscopeFS::DPS512 + ); + + // Test DPS1024 + let value_dps1024: u8 = 0b0110_0000; + assert_eq!( + >::bit_range(&value_dps1024, 6, 4), + GyroscopeFS::DPS1024 + ); + + // Test DPS2048 + let value_dps2048: u8 = 0b0111_0000; + assert_eq!( + >::bit_range(&value_dps2048, 6, 4), + GyroscopeFS::DPS2048 + ); + } + + #[test] + fn test_gfs_set_bit_range() { + let mut value: u8 = 0; + + // Set DPS16 + value.set_bit_range(6, 4, GyroscopeFS::DPS16); + assert_eq!(value, 0b0000_0000); + + // Set DPS32 + value.set_bit_range(6, 4, GyroscopeFS::DPS32); + assert_eq!(value, 0b0001_0000); + + // Set DPS32 + value.set_bit_range(6, 4, GyroscopeFS::DPS64); + assert_eq!(value, 0b0010_0000); + } + + #[test] + fn test_struct_bit_range() { + let register: u8 = 0b1000_0111; + + let data = Ctrl3Register(register); + assert!(data.gst()); + assert_eq!(data.gfs(), GyroscopeFS::DPS16); + assert_eq!(data.godr(), GyroscopeODR::NormalGORD7); + } + + #[test] + fn test_struct_set_bit_range() { + let register_result: u8 = 0b1000_0111; + let register_default: u8 = 0b0000_0000; + + let mut data = Ctrl3Register(register_default); + data.set_gst(true); + data.set_gfs(GyroscopeFS::DPS16); + data.set_godr(GyroscopeODR::NormalGORD7); + assert_eq!(data.0, register_result); + } + } } pub mod ctrl5 { @@ -855,6 +985,14 @@ pub mod register { let data = SensorDataAvailableAndLockRegister(register); assert_eq!(data.ctrl9_cmd_done(), Ctrl9CmdDone::Done); } + + #[test] + fn test_cmd_available_read() { + let register: u8 = 0b0000_0001; + + let data = SensorDataAvailableAndLockRegister(register); + assert!(data.available()); + } } } diff --git a/src/driver.rs b/src/driver.rs index 37fffa4..e4e010d 100644 --- a/src/driver.rs +++ b/src/driver.rs @@ -5,10 +5,10 @@ use embedded_hal::i2c::{I2c, SevenBitAddress}; use crate::command::constants::{ AX_HIGH, AX_LOW, AY_HIGH, AZ_HIGH, AZ_LOW, CAL1_HIGH, CAL1_LOW, CAL2_HIGH, CAL2_LOW, CAL3_HIGH, CAL3_LOW, CAL4_HIGH, CAL4_LOW, COD_STATUS, CTRL1, CTRL2, CTRL3, CTRL5, CTRL7, CTRL8, CTRL9, - FIFO_CTRL, FIFO_DATA, FIFO_SMPL_CNT, FIFO_STATUS, FIFO_WTM_TH, GX_HIGH, GX_LOW, GY_HIGH, - GY_LOW, GZ_HIGH, GZ_LOW, RESET, STATUS0, STATUS1, STATUSINT, STEP_CNT_HIGH, STEP_CNT_LOW, - STEP_CNT_MID, TAP_STATUS, TEMP_HIGH, TEMP_LOW, TIMESTAMP_HIGH, TIMESTAMP_LOW, TIMESTAMP_MID, - WHO_AM_I, + D_VX_HIGH, D_VX_LOW, D_VY_HIGH, D_VY_LOW, D_VZ_HIGH, D_VZ_LOW, FIFO_CTRL, FIFO_DATA, + FIFO_SMPL_CNT, FIFO_STATUS, FIFO_WTM_TH, GX_HIGH, GX_LOW, GY_HIGH, GY_LOW, GZ_HIGH, GZ_LOW, + RESET, REVISION_ID, STATUS0, STATUS1, STATUSINT, STEP_CNT_HIGH, STEP_CNT_LOW, STEP_CNT_MID, + TAP_STATUS, TEMP_HIGH, TEMP_LOW, TIMESTAMP_HIGH, TIMESTAMP_LOW, TIMESTAMP_MID, WHO_AM_I, }; use crate::command::register::acceleration::{AccelerationOutput, AngularRateOutput}; use crate::command::register::cal::{Calibration, CalibrationRegisters}; @@ -32,6 +32,7 @@ use crate::command::register::status_int::{Ctrl9CmdDone, SensorDataAvailableAndL use crate::command::register::tap_status::TapStatusRegister; use crate::command::register::temp::Temperature; use crate::command::register::timestamp::SampleTimeStamp; +use crate::fraction_helper::{abs_f32, to_signed_u12_4_f32}; #[repr(u8)] #[derive(Debug, Clone, Copy)] @@ -70,6 +71,8 @@ pub enum Error { CmdFailed, /// Host command Failed, CmdAckFailed, + /// Gyroscope Self-Test Failed + GyroscopeSelfTestFailed, } impl From for Error { @@ -181,7 +184,7 @@ where pub fn get_device_revision_id(&mut self) -> Result> { let mut buffer = [0u8; 1]; self.interface - .write_read(self.addr.into(), &[WHO_AM_I], &mut buffer)?; + .write_read(self.addr.into(), &[REVISION_ID], &mut buffer)?; Ok(buffer[0]) } @@ -194,7 +197,7 @@ where /// /// Possible errors include: /// - Communication error: This can occur if there is a problem communicating with the device over the interface. - pub fn get_crtl1(&mut self) -> Result> { + pub fn get_ctrl1(&mut self) -> Result> { let mut buffer = [0u8; 1]; self.interface .write_read(self.addr.into(), &[CTRL1], &mut buffer)?; @@ -210,7 +213,7 @@ where /// /// Possible errors include: /// - Communication error: This can occur if there is a problem communicating with the device over the interface. - pub fn set_crtl1(&mut self, value: Ctrl1Register) -> Result<(), Error> { + pub fn set_ctrl1(&mut self, value: Ctrl1Register) -> Result<(), Error> { self.write_register(CTRL1, value.0)?; Ok(()) } @@ -239,7 +242,7 @@ where /// /// Possible errors include: /// - Communication error: This can occur if there is a problem communicating with the device over the interface. - pub fn set_crtl2(&mut self, value: Ctrl2Register) -> Result<(), Error> { + pub fn set_ctrl2(&mut self, value: Ctrl2Register) -> Result<(), Error> { self.write_register(CTRL2, value.0)?; // Calculate G/ADC(tick) @@ -277,7 +280,7 @@ where /// /// Possible errors include: /// - Communication error: This can occur if there is a problem communicating with the device over the interface. - pub fn set_crtl3(&mut self, value: Ctrl3Register) -> Result<(), Error> { + pub fn set_ctrl3(&mut self, value: Ctrl3Register) -> Result<(), Error> { self.write_register(CTRL3, value.0)?; // Calculate DPS/ADC(tick) @@ -319,7 +322,7 @@ where /// /// Possible errors include: /// - Communication error: This can occur if there is a problem communicating with the device over the interface. - pub fn set_crtl5(&mut self, value: Ctrl5Register) -> Result<(), Error> { + pub fn set_ctrl5(&mut self, value: Ctrl5Register) -> Result<(), Error> { self.write_register(CTRL5, value.0)?; Ok(()) } @@ -348,7 +351,7 @@ where /// /// Possible errors include: /// - Communication error: This can occur if there is a problem communicating with the device over the interface. - pub fn set_crtl7(&mut self, value: Ctrl7Register) -> Result<(), Error> { + pub fn set_ctrl7(&mut self, value: Ctrl7Register) -> Result<(), Error> { self.write_register(CTRL7, value.0)?; Ok(()) } @@ -377,7 +380,7 @@ where /// /// Possible errors include: /// - Communication error: This can occur if there is a problem communicating with the device over the interface. - pub fn set_crtl8(&mut self, value: Ctrl8Register) -> Result<(), Error> { + pub fn set_ctrl8(&mut self, value: Ctrl8Register) -> Result<(), Error> { self.write_register(CTRL8, value.0)?; Ok(()) } @@ -425,7 +428,7 @@ where /// /// Possible errors include: /// - Communication error: This can occur if there is a problem communicating with the device over the interface. - pub fn set_crtl9(&mut self, value: Ctrl9Register) -> Result<(), Error> { + pub fn set_ctrl9(&mut self, value: Ctrl9Register) -> Result<(), Error> { self.write_register(CTRL9, value as u8)?; Ok(()) } @@ -759,9 +762,26 @@ where #[allow(clippy::cast_lossless)] pub fn get_angular_rate(&mut self) -> Result> { Ok(AngularRateOutput { - x: self.get_angular_rate_helper(GX_HIGH, GX_LOW)? as f32 * self.gyroscope_scale, - y: self.get_angular_rate_helper(GY_HIGH, GY_LOW)? as f32 * self.gyroscope_scale, - z: self.get_angular_rate_helper(GZ_HIGH, GZ_LOW)? as f32 * self.gyroscope_scale, + x: self.get_angular_rate_helper(GX_HIGH, GX_LOW)? as f32 / self.gyroscope_scale, + y: self.get_angular_rate_helper(GY_HIGH, GY_LOW)? as f32 / self.gyroscope_scale, + z: self.get_angular_rate_helper(GZ_HIGH, GZ_LOW)? as f32 / self.gyroscope_scale, + }) + } + + /// Get COD Angular Rate Output (dps) + /// + /// # Errors + /// + /// This function can return an error if there is an issue during the communication process. + /// + /// Possible errors include: + /// - Communication error: This can occur if there is a problem communicating with the device over the interface. + #[allow(clippy::cast_lossless)] + pub fn get_cod_angular_rate(&mut self) -> Result> { + Ok(AngularRateOutput { + x: to_signed_u12_4_f32(self.get_angular_rate_helper(D_VX_HIGH, D_VX_LOW)?), + y: to_signed_u12_4_f32(self.get_angular_rate_helper(D_VY_HIGH, D_VY_LOW)?), + z: to_signed_u12_4_f32(self.get_angular_rate_helper(D_VZ_HIGH, D_VZ_LOW)?), }) } @@ -920,7 +940,7 @@ where let mut ctrl7 = self.get_ctrl7()?; ctrl7.set_accelerometer_enable(accelrometer); ctrl7.set_gyroscope_enable(gyroscope); - self.set_crtl7(ctrl7) + self.set_ctrl7(ctrl7) } /// Get enabled sensors @@ -1028,7 +1048,11 @@ where /// Get FIFO Sample Count (MSB+LSB) /// - /// # Errors + /// # Notes + /// + /// The sample count is 6xSensorNumberx2bytes. + /// + /// # Errors /// /// This function can return an error if there is an issue during the communication process. /// @@ -1141,11 +1165,18 @@ where let sample_cnt = self.get_fifo_sample_cnt()?; let (gyro_en, acc_en) = self.get_sensors_enable()?; + let sample_per_iter = match (gyro_en, acc_en) { + (true, true) => 12, + _ => 6, + }; + + let iter_cnt = sample_cnt / sample_per_iter; + // Send CTRL_CMD_REQ_FIFO (0x05) by CTRL9 command, to enable FIFO read mode. // Refer to CTRL_CMD_REQ_FIFO for details. self.send_command(Ctrl9Register::CtrlCmdReqFifo)?; - for i in 0..sample_cnt { + for i in 0..iter_cnt { match (gyro_en, acc_en) { (true, true) => { // this line will collect the acceleration output registers @@ -1196,9 +1227,9 @@ where let (gyro_en, acc_en) = self.get_sensors_enable()?; self.set_sensors_enable(false, false)?; - let mut ctrl1 = self.get_crtl1()?; + let mut ctrl1 = self.get_ctrl1()?; ctrl1.set_fifo_int_sel(fifo_int_dir); - self.set_crtl1(ctrl1)?; + self.set_ctrl1(ctrl1)?; let mut fifo_ctrl = FIFOControlRegister(0); fifo_ctrl.set_fifo_mode(fifo_mode); @@ -1214,6 +1245,140 @@ where Ok(()) } + #[cfg(feature = "loglib")] + pub fn dump_register(&mut self) { + if let Ok(value) = self.get_ctrl1() { + log::warn!("{:?}", value); + } + if let Ok(value) = self.get_ctrl2() { + log::warn!("{:?}", value); + } + if let Ok(value) = self.get_ctrl3() { + log::warn!("{:?}", value); + } + if let Ok(value) = self.get_ctrl5() { + log::warn!("{:?}", value); + } + if let Ok(value) = self.get_ctrl7() { + log::warn!("{:?}", value); + } + if let Ok(value) = self.get_ctrl8() { + log::warn!("{:?}", value); + } + } + + #[cfg(not(feature = "loglib"))] + #[allow(clippy::needless_pass_by_ref_mut)] + pub fn dump_register(&mut self) {} + + /// Gyroscope Self-Test + /// + /// The gyroscope Self-Test (Check-Alive) is used to determine if the gyroscope is functional. + /// It is implemented by applying an electrostatic force to actuate each of the three X, Y, and Z axis of the gyroscope and + /// measures the mechanical response on the corresponding X, Y, and Z axis. If the equivalent magnitude of the gyroscope + /// output is greater than 300dps for each axis, the gyroscope can be considered as functional. + /// + /// The gyroscope Self-Test data is available to be read at output registers `dVX_L`, `dVX_H`, `dVY_L`, `dVY_H`, `dVZ_L` & + /// `dVZ_H`. The Host can initiate the Self-Test anytime with the following procedure. + /// + /// # Notes + /// + /// Self-Test process is about 400ms + /// + /// # Errors + /// + /// Possible errors include: + /// - Communication error: This can occur if there is a problem communicating with the device over the interface. + /// - Driver's error: This can occur if the `SensorDataAvailableAndLockRegister` host command doesn't acknowledge properly during the command process. + pub fn gyroscope_test(&mut self) -> Result<(), Error> { + let mut try_count = 0; + + let mut ctrl1 = self.get_ctrl1()?; + let ctrl1_old = ctrl1; + ctrl1.set_int1_enable(false); + ctrl1.set_int2_enable(false); + self.set_ctrl1(ctrl1)?; + + // The default values does not seem right at least a power-on-reset + // To prevent failing test, check if ast/gst is already true + let mut ctrl2 = self.get_ctrl2()?; + let mut ctrl3 = self.get_ctrl3()?; + + if ctrl2.ast() { + ctrl2.set_ast(false); + self.set_ctrl2(ctrl2)?; + } + + if ctrl3.gst() { + ctrl3.set_gst(false); + self.set_ctrl3(ctrl3)?; + } + + // 1- Disable the sensors (CTRL7 = 0x00) + self.set_ctrl7(Ctrl7Register(0))?; + self.delay.delay_ms(10); + + let mut ctrl3 = self.get_ctrl3()?; + ctrl3.set_gst(true); + self.set_ctrl3(ctrl3)?; + + loop { + // 3- Wait for QMI8658A to drive INT2 to High, if INT2 is enabled, or STATUSINT.bit0 is set to 1 + let value = self.get_sensor_data_available_and_lock()?; + if value.available() { + break; + } + + if try_count >= 10 { + ctrl3.set_gst(false); + self.set_ctrl3(ctrl3)?; + return Err(Error::GyroscopeSelfTestFailed); + } + + // Self-Test process is about 400ms + self.delay.delay_ms(100); + try_count += 1; + } + + // 4- Set CTRL3.aST(bit7) to 0, to clear STATUSINT1.bit0 and/or INT2. + // Disable Self-Test + ctrl3.set_gst(false); + self.set_ctrl3(ctrl3)?; + + try_count = 0; + + // 5- Check for QMI8658A drives INT2 back to Low, or sets STATUSINT1.bit0 to 0. + loop { + let value = self.get_sensor_data_available_and_lock()?; + if !value.available() { + break; + } + if try_count >= 8 { + self.set_ctrl1(ctrl1_old)?; + return Err(Error::GyroscopeSelfTestFailed); + } + + // Self-Test process is about 400ms + self.delay.delay_ms(100); + try_count += 1; + } + + // Angular rate (dps) from CoD + let cod = self.get_cod_angular_rate()?; + + // Revert changes + self.set_ctrl1(ctrl1_old)?; + + // If the absolute results of all three axes are higher than 300dps, + // the gyroscope can be considered functional. Otherwise, + // the gyroscope cannot be considered functional. + if abs_f32(cod.x) > 300. && abs_f32(cod.y) > 300. && abs_f32(cod.z) > 300. { + Ok(()) + } else { + Err(Error::GyroscopeSelfTestFailed) + } + } + /// Send Command /// /// # Errors @@ -1226,7 +1391,7 @@ where pub fn send_command(&mut self, value: Ctrl9Register) -> Result<(), Error> { let mut try_count = 0; - self.set_crtl9(value)?; + self.set_ctrl9(value)?; loop { let value = self.get_sensor_data_available_and_lock()?; @@ -1240,7 +1405,7 @@ where try_count += 1; } - self.set_crtl9(Ctrl9Register::CtrlCmdAck)?; + self.set_ctrl9(Ctrl9Register::CtrlCmdAck)?; loop { let value = self.get_sensor_data_available_and_lock()?; diff --git a/src/fraction_helper.rs b/src/fraction_helper.rs new file mode 100644 index 0000000..0c4c306 --- /dev/null +++ b/src/fraction_helper.rs @@ -0,0 +1,19 @@ +pub fn to_signed_u12_4_f32(value: i16) -> f32 { + // Extract the integer part by shifting right 4 bits (keeping the sign) + let integer_part = value >> 4; + + // Extract the fractional part by masking the lower 4 bits (as a positive value) + let fractional_part = f32::from(value & 0xF) / 16.0; + + // Combine the integer and fractional parts + f32::from(integer_part) + fractional_part +} + +#[allow(clippy::suboptimal_flops)] +pub fn abs_f32(value: f32) -> f32 { + if value < 0.0 { + -value + } else { + value + } +} diff --git a/src/lib.rs b/src/lib.rs index 019608e..33f2ec3 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -63,8 +63,8 @@ pub mod command; pub mod prelude; mod bitfield_helper; - mod driver; +mod fraction_helper; // export the driver and interface pub use driver::Qmi8658;