From 398f0cc0eebaf8680f7ce61e9edfffd865f69850 Mon Sep 17 00:00:00 2001 From: Walter Bonetti Date: Sun, 8 Sep 2024 09:45:33 -0400 Subject: [PATCH] Fix/Add: Self-Test accelerometer and gyroscope * Fix gyroscope self-test converting u12_4 to dps * Add accelerometer self-test * Add libm for libm::fabs --- Cargo.toml | 1 + src/driver.rs | 156 ++++++++++++++++++++++++++++++++++++++--- src/fraction_helper.rs | 28 ++++---- 3 files changed, 164 insertions(+), 21 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index a7ca297..a0436c8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -28,6 +28,7 @@ targets = ["thumbv7m-none-eabi", "thumbv7em-none-eabihf"] embedded-hal = { version = "1.0.0" } bitfield = "0.15.0" log = { version = "0.4", default-features = false, optional = true } +libm = { version = "0.2" } [features] default = [] diff --git a/src/driver.rs b/src/driver.rs index e4e010d..da9946a 100644 --- a/src/driver.rs +++ b/src/driver.rs @@ -1,4 +1,5 @@ #![allow(clippy::missing_const_for_fn)] +#![allow(clippy::doc_markdown)] use embedded_hal::delay::DelayNs; use embedded_hal::i2c::{I2c, SevenBitAddress}; @@ -14,7 +15,7 @@ use crate::command::register::acceleration::{AccelerationOutput, AngularRateOutp use crate::command::register::cal::{Calibration, CalibrationRegisters}; use crate::command::register::cod_status::CODStatusRegister; use crate::command::register::ctrl1::{Ctrl1Register, IntDirection}; -use crate::command::register::ctrl2::{AccelerometerFS, Ctrl2Register}; +use crate::command::register::ctrl2::{AccelerometerFS, AccelerometerODR, Ctrl2Register}; use crate::command::register::ctrl3::{Ctrl3Register, GyroscopeFS}; use crate::command::register::ctrl5::Ctrl5Register; use crate::command::register::ctrl7::Ctrl7Register; @@ -32,7 +33,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}; +use crate::fraction_helper::{to_signed_u12_4_to_dps, to_signed_u5_11_g}; #[repr(u8)] #[derive(Debug, Clone, Copy)] @@ -65,14 +66,18 @@ pub enum Error { Communication(E), /// On-Demande Calibration Failed CoDFailed, - /// Gyroscope Startup Failed + /// Gyroscope Failed GyroscopeFailed, - /// Host command Failed, + /// Host command Failed CmdFailed, - /// Host command Failed, + /// Host command Failed CmdAckFailed, /// Gyroscope Self-Test Failed GyroscopeSelfTestFailed, + /// Accelerometer Failed + AccelerometerFailed, + /// Accelerometer Self-Test Failed + AccelerometerSelfTestFailed, } impl From for Error { @@ -779,9 +784,26 @@ where #[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)?), + x: to_signed_u12_4_to_dps(self.get_angular_rate_helper(D_VX_HIGH, D_VX_LOW)?), + y: to_signed_u12_4_to_dps(self.get_angular_rate_helper(D_VY_HIGH, D_VY_LOW)?), + z: to_signed_u12_4_to_dps(self.get_angular_rate_helper(D_VZ_HIGH, D_VZ_LOW)?), + }) + } + + /// Get COD Acceleration Output (mg) + /// + /// # 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_acceleration(&mut self) -> Result> { + Ok(AccelerationOutput { + x: to_signed_u5_11_g(self.get_acceleration_helper(D_VX_HIGH, D_VX_LOW)?), + y: to_signed_u5_11_g(self.get_acceleration_helper(D_VY_HIGH, D_VY_LOW)?), + z: to_signed_u5_11_g(self.get_acceleration_helper(D_VZ_HIGH, D_VZ_LOW)?), }) } @@ -1271,6 +1293,122 @@ where #[allow(clippy::needless_pass_by_ref_mut)] pub fn dump_register(&mut self) {} + /// Accelerometer Self-Test + /// + /// The accelerometer Self-Test (Check-Alive) is used to determine if the accelerometer is functional and working within + /// acceptable parameters. + /// It is implemented by applying an electrostatic force to actuate each of the three X, Y, and Z axis of the accelerometer. + /// If the accelerometer mechanical structure responds to this input stimulus by sensing at least 200 mg, then the + /// accelerometer can be considered functional. + /// + /// The accelerometer Self-Test data is available to be read at registers dVX_L, dVX_H, dVY_L, dVY_H, dVZ_L and + /// dVZ_H. The Host can initiate the Self-Test at any time with the following procedure. + /// + /// # Notes + /// + /// The typical time for Self-Test (from setting aST to 1, until the rising edge of INT2 if enabled, + /// or STATUSINT.bit0 is set to 1) costs about 25 ODRs: + /// * 25ms @ 1KHz ODR + /// + /// # 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 accelerometer_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)?; + } + + let ctrl2_odr_old = ctrl2.aodr(); + + // 1- Disable the sensors (CTRL7 = 0x00) + self.set_ctrl7(Ctrl7Register(0))?; + self.delay.delay_ms(10); + + let mut ctrl2 = self.get_ctrl2()?; + ctrl2.set_aodr(AccelerometerODR::NormalAODR3); // 1Khz + ctrl2.set_ast(true); + self.set_ctrl2(ctrl2)?; + + 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 { + ctrl2.set_ast(false); + ctrl2.set_aodr(ctrl2_odr_old); + self.set_ctrl2(ctrl2)?; + return Err(Error::AccelerometerFailed); + } + + // Self-Test process is about 25ms + self.delay.delay_ms(25); + try_count += 1; + } + + // 4- Set CTRL3.aST(bit7) to 0, to clear STATUSINT1.bit0 and/or INT2. + // Disable Self-Test + ctrl2.set_aodr(ctrl2_odr_old); + ctrl2.set_ast(false); + self.set_ctrl2(ctrl2)?; + + 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 >= 10 { + self.set_ctrl1(ctrl1_old)?; + return Err(Error::AccelerometerFailed); + } + + // Self-Test process is about 400ms + self.delay.delay_ms(25); + try_count += 1; + } + + // Angular rate (dps) from CoD + let cod = self.get_cod_acceleration()?; + + // 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 libm::fabsf(cod.x) > 0.2 && libm::fabsf(cod.y) > 0.2 && libm::fabsf(cod.z) > 0.2 { + Ok(()) + } else { + Err(Error::AccelerometerSelfTestFailed) + } + } + /// Gyroscope Self-Test /// /// The gyroscope Self-Test (Check-Alive) is used to determine if the gyroscope is functional. @@ -1372,7 +1510,7 @@ where // 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. { + if libm::fabsf(cod.x) > 300. && libm::fabsf(cod.y) > 300. && libm::fabsf(cod.z) > 300. { Ok(()) } else { Err(Error::GyroscopeSelfTestFailed) diff --git a/src/fraction_helper.rs b/src/fraction_helper.rs index 0c4c306..ee5e979 100644 --- a/src/fraction_helper.rs +++ b/src/fraction_helper.rs @@ -1,19 +1,23 @@ -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; +pub fn to_signed_u12_4_to_dps(raw_value: i16) -> f32 { + // Extract the integer part (top 12 bits) + let integer_part = raw_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; + // Extract the fractional part (lower 4 bits) + let fractional_mask = 0xF; // Mask to extract the lower 4 bits (0xF = 15) + let fractional_part = f32::from(raw_value & fractional_mask) * 0.0625; // 1 / 2^4 = 0.0625 dps // 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 - } +pub fn to_signed_u5_11_g(raw_value: i16) -> f32 { + // Extract the integer part (top 5 bits) + let integer_part = raw_value >> 11; + + // Extract the fractional part (lower 11 bits) + let fractional_mask = 0x7FF; // Mask to extract the lower 11 bits (0x7FF = 2047) + let fractional_part = f32::from(raw_value & fractional_mask) * 0.0005; // 0.5 mg = 0.0005 g + + // Combine the integer and fractional parts + f32::from(integer_part) + fractional_part }