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

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()

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()

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

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);
 }
 

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

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);
 }
 

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()

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)

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)

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);
 }
 

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)

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);
 }
 

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);
 }
 

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);
 }
 

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();
 	}
 }

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);
 }
 

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)

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)

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 &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)

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 &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)