From de650dc83ca162d925b32b289043bb2992c23480 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 14 Sep 2021 18:42:22 -0400 Subject: [PATCH] drivers/imu: data ready scheduling only schedule cycle if thread is ready - this provides the thread with a bit more control to block new scheduling by the interupt during resets or other exceptional events --- .../imu/analog_devices/adis16470/ADIS16470.cpp | 8 ++++++-- .../imu/bosch/bmi055/BMI055_Accelerometer.cpp | 14 +++++++++----- src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp | 14 +++++++++----- .../imu/bosch/bmi088/BMI088_Accelerometer.cpp | 10 +++++++--- src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp | 14 +++++++++----- .../imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp | 9 +++++++-- .../imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp | 14 +++++++++----- src/drivers/imu/invensense/icm20602/ICM20602.cpp | 14 +++++++++----- src/drivers/imu/invensense/icm20608g/ICM20608G.cpp | 9 ++++++--- src/drivers/imu/invensense/icm20649/ICM20649.cpp | 7 +++++-- src/drivers/imu/invensense/icm20689/ICM20689.cpp | 9 ++++++--- src/drivers/imu/invensense/icm20948/ICM20948.cpp | 7 +++++-- src/drivers/imu/invensense/icm40609d/ICM40609D.cpp | 10 +++++++--- src/drivers/imu/invensense/icm42605/ICM42605.cpp | 10 +++++++--- src/drivers/imu/invensense/icm42670p/ICM42670P.cpp | 5 +++-- src/drivers/imu/invensense/icm42688p/ICM42688P.cpp | 10 +++++++--- src/drivers/imu/invensense/mpu6000/MPU6000.cpp | 9 ++++++--- src/drivers/imu/invensense/mpu6500/MPU6500.cpp | 9 ++++++--- src/drivers/imu/invensense/mpu9250/MPU9250.cpp | 9 ++++++--- src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp | 9 ++++++--- 20 files changed, 135 insertions(+), 65 deletions(-) diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp index 7df959803f..76238d303a 100644 --- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp +++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp @@ -394,8 +394,12 @@ int ADIS16470::DataReadyInterruptCallback(int irq, void *context, void *arg) void ADIS16470::DataReady() { - _drdy_timestamp_sample.store(hrt_absolute_time()); - ScheduleNow(); + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; + + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { + ScheduleNow(); + } } bool ADIS16470::DataReadyInterruptConfigure() diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp index 77cf537302..31d62375c6 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp @@ -338,8 +338,12 @@ int BMI055_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi void BMI055_Accelerometer::DataReady() { - _drdy_timestamp_sample.store(hrt_absolute_time()); - ScheduleNow(); + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; + + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { + ScheduleNow(); + } } bool BMI055_Accelerometer::DataReadyInterruptConfigure() @@ -453,9 +457,6 @@ void BMI055_Accelerometer::FIFOReset() // FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1 RegisterWrite(Register::FIFO_CONFIG_1, 0); - // reset while FIFO is disabled - _drdy_timestamp_sample.store(0); - // FIFO_CONFIG_0: restore FIFO watermark // FIFO_CONFIG_1: re-enable FIFO for (const auto &r : _register_cfg) { @@ -463,6 +464,9 @@ void BMI055_Accelerometer::FIFOReset() RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); } } + + // clear sample timestamp to allow data ready scheduling to resume + _drdy_timestamp_sample.store(0); } void BMI055_Accelerometer::UpdateTemperature() diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp index d2ea514530..dd7ce2dcca 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp @@ -336,8 +336,12 @@ int BMI055_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a void BMI055_Gyroscope::DataReady() { - _drdy_timestamp_sample.store(hrt_absolute_time()); - ScheduleNow(); + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; + + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { + ScheduleNow(); + } } bool BMI055_Gyroscope::DataReadyInterruptConfigure() @@ -450,9 +454,6 @@ void BMI055_Gyroscope::FIFOReset() // FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1 RegisterWrite(Register::FIFO_CONFIG_1, 0); - // reset while FIFO is disabled - _drdy_timestamp_sample.store(0); - // FIFO_CONFIG_0: restore FIFO watermark // FIFO_CONFIG_1: re-enable FIFO for (const auto &r : _register_cfg) { @@ -460,6 +461,9 @@ void BMI055_Gyroscope::FIFOReset() RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); } } + + // clear sample timestamp to allow data ready scheduling to resume + _drdy_timestamp_sample.store(0); } } // namespace Bosch::BMI055::Gyroscope diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp index 8f0dc0068b..59786f103e 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp @@ -370,8 +370,12 @@ int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi void BMI088_Accelerometer::DataReady() { - _drdy_timestamp_sample.store(hrt_absolute_time()); - ScheduleNow(); + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; + + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { + ScheduleNow(); + } } bool BMI088_Accelerometer::DataReadyInterruptConfigure() @@ -567,7 +571,7 @@ void BMI088_Accelerometer::FIFOReset() // ACC_SOFTRESET: trigger a FIFO reset by writing 0xB0 to ACC_SOFTRESET (register 0x7E). RegisterWrite(Register::ACC_SOFTRESET, 0xB0); - // reset while FIFO is disabled + // clear sample timestamp to allow data ready scheduling to resume _drdy_timestamp_sample.store(0); } diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp index 7dac23e578..7ffbd1d3f6 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp @@ -337,8 +337,12 @@ int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a void BMI088_Gyroscope::DataReady() { - _drdy_timestamp_sample.store(hrt_absolute_time()); - ScheduleNow(); + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; + + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { + ScheduleNow(); + } } bool BMI088_Gyroscope::DataReadyInterruptConfigure() @@ -451,9 +455,6 @@ void BMI088_Gyroscope::FIFOReset() // FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1 RegisterWrite(Register::FIFO_CONFIG_1, 0); - // reset while FIFO is disabled - _drdy_timestamp_sample.store(0); - // FIFO_CONFIG_0: restore FIFO watermark // FIFO_CONFIG_1: re-enable FIFO for (const auto &r : _register_cfg) { @@ -461,6 +462,9 @@ void BMI088_Gyroscope::FIFOReset() RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); } } + + // clear sample timestamp to allow data ready scheduling to resume + _drdy_timestamp_sample.store(0); } } // namespace Bosch::BMI088::Gyroscope diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp index 6d7e4d6739..9f36bf239d 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp @@ -293,7 +293,12 @@ int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi void BMI088_Accelerometer::DataReady() { - ScheduleNow(); + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; + + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { + ScheduleNow(); + } } bool BMI088_Accelerometer::DataReadyInterruptConfigure() @@ -583,7 +588,7 @@ void BMI088_Accelerometer::FIFOReset() // ACC_SOFTRESET: trigger a FIFO reset by writing 0xB0 to ACC_SOFTRESET (register 0x7E). RegisterWrite(Register::ACC_SOFTRESET, 0xB0); - // reset while FIFO is disabled + // clear sample timestamp to allow data ready scheduling to resume _drdy_timestamp_sample.store(0); } diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp index ad177c68e5..4263ee2fb8 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp @@ -262,8 +262,12 @@ int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a void BMI088_Gyroscope::DataReady() { - _drdy_timestamp_sample.store(hrt_absolute_time()); - ScheduleNow(); + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; + + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { + ScheduleNow(); + } } bool BMI088_Gyroscope::DataReadyInterruptConfigure() @@ -378,9 +382,6 @@ void BMI088_Gyroscope::FIFOReset() // FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1 RegisterWrite(Register::FIFO_CONFIG_1, 0); - // reset while FIFO is disabled - _drdy_timestamp_sample.store(0); - // FIFO_CONFIG_0: restore FIFO watermark // FIFO_CONFIG_1: re-enable FIFO for (const auto &r : _register_cfg) { @@ -388,6 +389,9 @@ void BMI088_Gyroscope::FIFOReset() RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); } } + + // clear sample timestamp to allow data ready scheduling to resume + _drdy_timestamp_sample.store(0); } bool BMI088_Gyroscope::SelfTest() diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index c13ab0b800..fb9654cf7d 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -452,8 +452,12 @@ int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20602::DataReady() { - _drdy_timestamp_sample.store(hrt_absolute_time()); - ScheduleNow(); + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; + + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { + ScheduleNow(); + } } bool ICM20602::DataReadyInterruptConfigure() @@ -582,9 +586,6 @@ void ICM20602::FIFOReset() // USER_CTRL: reset FIFO RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN); - // reset while FIFO is disabled - _drdy_timestamp_sample.store(0); - // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO for (const auto &r : _register_cfg) { @@ -592,6 +593,9 @@ void ICM20602::FIFOReset() RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); } } + + // clear sample timestamp to allow data ready scheduling to resume + _drdy_timestamp_sample.store(0); } static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index 06fe4b048e..9c915545ff 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -415,8 +415,9 @@ int ICM20608G::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20608G::DataReady() { // at least the required number of samples in the FIFO - if (++_drdy_count >= _fifo_gyro_samples) { - _drdy_timestamp_sample.store(hrt_absolute_time()); + uint64_t expected = 0; + + if ((++_drdy_count >= _fifo_gyro_samples) && _drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } @@ -526,7 +527,6 @@ void ICM20608G::FIFOReset() // reset while FIFO is disabled _drdy_count = 0; - _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO @@ -535,6 +535,9 @@ void ICM20608G::FIFOReset() RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); } } + + // clear sample timestamp to allow data ready scheduling to resume + _drdy_timestamp_sample.store(0); } static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp index e38c9381b8..fab6cbc7a9 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp @@ -417,8 +417,9 @@ int ICM20649::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20649::DataReady() { // at least the required number of samples in the FIFO - if (++_drdy_count >= _fifo_gyro_samples) { - _drdy_timestamp_sample.store(hrt_absolute_time()); + uint64_t expected = 0; + + if ((++_drdy_count >= _fifo_gyro_samples) && _drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } @@ -570,6 +571,8 @@ void ICM20649::FIFOReset() // reset while FIFO is disabled _drdy_count = 0; + + // clear sample timestamp to allow data ready scheduling to resume _drdy_timestamp_sample.store(0); } diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index 8337ba2cc4..5c8188276d 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -415,8 +415,9 @@ int ICM20689::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20689::DataReady() { // at least the required number of samples in the FIFO - if (++_drdy_count >= _fifo_gyro_samples) { - _drdy_timestamp_sample.store(hrt_absolute_time()); + uint64_t expected = 0; + + if ((++_drdy_count >= _fifo_gyro_samples) && _drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } @@ -526,7 +527,6 @@ void ICM20689::FIFOReset() // reset while FIFO is disabled _drdy_count = 0; - _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO @@ -535,6 +535,9 @@ void ICM20689::FIFOReset() RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); } } + + // clear sample timestamp to allow data ready scheduling to resume + _drdy_timestamp_sample.store(0); } static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index d95cb636c3..9388d9773a 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -464,8 +464,9 @@ int ICM20948::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20948::DataReady() { // at least the required number of samples in the FIFO - if (++_drdy_count >= _fifo_gyro_samples) { - _drdy_timestamp_sample.store(hrt_absolute_time()); + uint64_t expected = 0; + + if ((++_drdy_count >= _fifo_gyro_samples) && _drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } @@ -617,6 +618,8 @@ void ICM20948::FIFOReset() // reset while FIFO is disabled _drdy_count = 0; + + // clear sample timestamp to allow data ready scheduling to resume _drdy_timestamp_sample.store(0); } diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 894c9cd7c2..ac59add638 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -419,8 +419,12 @@ int ICM40609D::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM40609D::DataReady() { - _drdy_timestamp_sample.store(hrt_absolute_time()); - ScheduleNow(); + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; + + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { + ScheduleNow(); + } } bool ICM40609D::DataReadyInterruptConfigure() @@ -586,7 +590,7 @@ void ICM40609D::FIFOReset() // SIGNAL_PATH_RESET: FIFO flush RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); - // reset while FIFO is disabled + // clear sample timestamp to allow data ready scheduling to resume _drdy_timestamp_sample.store(0); } diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.cpp b/src/drivers/imu/invensense/icm42605/ICM42605.cpp index 88ddb8b30e..c28fc9e4b9 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.cpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.cpp @@ -420,8 +420,12 @@ int ICM42605::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM42605::DataReady() { - _drdy_timestamp_sample.store(hrt_absolute_time()); - ScheduleNow(); + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; + + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { + ScheduleNow(); + } } bool ICM42605::DataReadyInterruptConfigure() @@ -587,7 +591,7 @@ void ICM42605::FIFOReset() // SIGNAL_PATH_RESET: FIFO flush RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); - // reset while FIFO is disabled + // clear sample timestamp to allow data ready scheduling to resume _drdy_timestamp_sample.store(0); } diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp index f66eb13e2e..c27150a5ff 100644 --- a/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp +++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp @@ -411,9 +411,10 @@ int ICM42670P::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM42670P::DataReady() { - uint32_t expected = 0; + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { ScheduleNow(); } } diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index 0c8e66f862..14dd9a5c3d 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -378,8 +378,12 @@ int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM42688P::DataReady() { - _drdy_timestamp_sample.store(hrt_absolute_time()); - ScheduleNow(); + // schedule transfer if sample timestamp has been cleared (thread ready for next transfer) + uint64_t expected = 0; + + if (_drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { + ScheduleNow(); + } } bool ICM42688P::DataReadyInterruptConfigure() @@ -559,7 +563,7 @@ void ICM42688P::FIFOReset() // SIGNAL_PATH_RESET: FIFO flush RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); - // reset while FIFO is disabled + // clear sample timestamp to allow data ready scheduling to resume _drdy_timestamp_sample.store(0); } diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index ffdb9bafb0..4932ef7aaa 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -384,8 +384,9 @@ int MPU6000::DataReadyInterruptCallback(int irq, void *context, void *arg) void MPU6000::DataReady() { // at least the required number of samples in the FIFO - if (++_drdy_count >= _fifo_gyro_samples) { - _drdy_timestamp_sample.store(hrt_absolute_time()); + uint64_t expected = 0; + + if ((++_drdy_count >= _fifo_gyro_samples) && _drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } @@ -499,7 +500,6 @@ void MPU6000::FIFOReset() // reset while FIFO is disabled _drdy_count = 0; - _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO @@ -508,6 +508,9 @@ void MPU6000::FIFOReset() RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); } } + + // clear sample timestamp to allow data ready scheduling to resume + _drdy_timestamp_sample.store(0); } static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index 2f29db0f79..71f6408ab8 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -416,8 +416,9 @@ int MPU6500::DataReadyInterruptCallback(int irq, void *context, void *arg) void MPU6500::DataReady() { // at least the required number of samples in the FIFO - if (++_drdy_count >= _fifo_gyro_samples) { - _drdy_timestamp_sample.store(hrt_absolute_time()); + uint64_t expected = 0; + + if ((++_drdy_count >= _fifo_gyro_samples) && _drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } @@ -531,7 +532,6 @@ void MPU6500::FIFOReset() // reset while FIFO is disabled _drdy_count = 0; - _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO @@ -540,6 +540,9 @@ void MPU6500::FIFOReset() RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); } } + + // clear sample timestamp to allow data ready scheduling to resume + _drdy_timestamp_sample.store(0); } static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index 501f5f97db..5e398e7063 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -451,8 +451,9 @@ int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg) void MPU9250::DataReady() { // at least the required number of samples in the FIFO - if (++_drdy_count >= _fifo_gyro_samples) { - _drdy_timestamp_sample.store(hrt_absolute_time()); + uint64_t expected = 0; + + if ((++_drdy_count >= _fifo_gyro_samples) && _drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } @@ -604,7 +605,6 @@ void MPU9250::FIFOReset() // reset while FIFO is disabled _drdy_count = 0; - _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO @@ -613,6 +613,9 @@ void MPU9250::FIFOReset() RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); } } + + // clear sample timestamp to allow data ready scheduling to resume + _drdy_timestamp_sample.store(0); } void MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp index 73a87a71db..1f5310e768 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp @@ -384,8 +384,9 @@ int MPU9250_I2C::DataReadyInterruptCallback(int irq, void *context, void *arg) void MPU9250_I2C::DataReady() { // at least the required number of samples in the FIFO - if (++_drdy_count >= _fifo_gyro_samples) { - _drdy_timestamp_sample.store(hrt_absolute_time()); + uint64_t expected = 0; + + if ((++_drdy_count >= _fifo_gyro_samples) && _drdy_timestamp_sample.compare_exchange(&expected, hrt_absolute_time())) { _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } @@ -505,7 +506,6 @@ void MPU9250_I2C::FIFOReset() // reset while FIFO is disabled _drdy_count = 0; - _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO @@ -514,6 +514,9 @@ void MPU9250_I2C::FIFOReset() RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); } } + + // clear sample timestamp to allow data ready scheduling to resume + _drdy_timestamp_sample.store(0); } bool MPU9250_I2C::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)