diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
index 66309058a3..c7a1e6c4df 100644
--- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
+++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
@@ -104,6 +104,9 @@ ADIS16448::ADIS16448(const I2CSPIDriverConfig &config) :
 	_px4_gyro(get_device_id(), config.rotation),
 	_px4_mag(get_device_id(), config.rotation)
 {
+	if (_drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
+	}
 }
 
 ADIS16448::~ADIS16448()
@@ -112,6 +115,7 @@ ADIS16448::~ADIS16448()
 	perf_free(_bad_register_perf);
 	perf_free(_bad_transfer_perf);
 	perf_free(_perf_crc_bad);
+	perf_free(_drdy_missed_perf);
 }
 
 int ADIS16448::init()
@@ -149,6 +153,7 @@ void ADIS16448::print_status()
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
 	perf_print_counter(_perf_crc_bad);
+	perf_print_counter(_drdy_missed_perf);
 }
 
 int ADIS16448::probe()
@@ -326,7 +331,19 @@ void ADIS16448::RunImpl()
 		break;
 
 	case STATE::READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < SAMPLE_INTERVAL_US) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
 				// push backup schedule back
 				ScheduleDelayed(SAMPLE_INTERVAL_US * 2);
 			}
@@ -414,8 +431,8 @@ void ADIS16448::RunImpl()
 					}
 
 					if (imu_updated) {
-						_px4_accel.update(now, accel_x, accel_y, accel_z);
-						_px4_gyro.update(now, gyro_x, gyro_y, gyro_z);
+						_px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z);
+						_px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z);
 					}
 
 					// DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT
@@ -426,13 +443,13 @@ void ADIS16448::RunImpl()
 						const int16_t mag_x = buffer.XMAGN_OUT;
 						const int16_t mag_y = (buffer.YMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.YMAGN_OUT;
 						const int16_t mag_z = (buffer.ZMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZMAGN_OUT;
-						_px4_mag.update(now, mag_x, mag_y, mag_z);
+						_px4_mag.update(timestamp_sample, mag_x, mag_y, mag_z);
 
 						_px4_baro.set_error_count(error_count);
 						_px4_baro.set_temperature(temperature);
 
 						float pressure_pa = buffer.BARO_OUT * 0.02f; // 20 μbar per LSB
-						_px4_baro.update(now, pressure_pa);
+						_px4_baro.update(timestamp_sample, pressure_pa);
 					}
 
 					success = true;
diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp
index 2546775ac8..9e28919ab5 100644
--- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp
+++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp
@@ -105,14 +105,16 @@ private:
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
 	perf_counter_t _perf_crc_bad{nullptr};
+	perf_counter_t _drdy_missed_perf{nullptr};
 
 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
 	int _failure_count{0};
 
-	bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ)
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
+	bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ)
 	bool _self_test_passed{false};
 
 	int16_t _accel_prev[3] {};
diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
index bafa61ea0f..7df959803f 100644
--- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
+++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
@@ -47,12 +47,18 @@ ADIS16470::ADIS16470(const I2CSPIDriverConfig &config) :
 	_px4_accel(get_device_id(), config.rotation),
 	_px4_gyro(get_device_id(), config.rotation)
 {
+	if (_drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
+	}
 }
 
 ADIS16470::~ADIS16470()
 {
+	perf_free(_reset_perf);
 	perf_free(_bad_register_perf);
 	perf_free(_bad_transfer_perf);
+	perf_free(_perf_crc_bad);
+	perf_free(_drdy_missed_perf);
 }
 
 int ADIS16470::init()
@@ -87,9 +93,10 @@ void ADIS16470::print_status()
 	I2CSPIDriverBase::print_status();
 
 	perf_print_counter(_reset_perf);
-	perf_print_counter(_perf_crc_bad);
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
+	perf_print_counter(_perf_crc_bad);
+	perf_print_counter(_drdy_missed_perf);
 }
 
 int ADIS16470::probe()
@@ -208,7 +215,19 @@ void ADIS16470::RunImpl()
 		break;
 
 	case STATE::READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < SAMPLE_INTERVAL_US) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
 				// push backup schedule back
 				ScheduleDelayed(SAMPLE_INTERVAL_US * 2);
 			}
@@ -233,7 +252,6 @@ void ADIS16470::RunImpl()
 			static_assert(sizeof(BurstRead) == (176 / 8), "ADIS16470 report not 176 bits");
 
 			buffer.cmd = static_cast<uint16_t>(Register::GLOB_CMD) << 8;
-
 			set_frequency(SPI_SPEED_BURST);
 
 			if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) == PX4_OK) {
@@ -291,7 +309,7 @@ void ADIS16470::RunImpl()
 				accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
 				accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
 
-				_px4_accel.update(now, accel_x, accel_y, accel_z);
+				_px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z);
 
 
 				int16_t gyro_x = buffer.X_GYRO_OUT;
@@ -301,7 +319,7 @@ void ADIS16470::RunImpl()
 				//  flip y & z to publish right handed with z down (x forward, y right, z down)
 				gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
 				gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-				_px4_gyro.update(now, gyro_x, gyro_y, gyro_z);
+				_px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z);
 
 				success = true;
 
@@ -376,6 +394,7 @@ int ADIS16470::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ADIS16470::DataReady()
 {
+	_drdy_timestamp_sample.store(hrt_absolute_time());
 	ScheduleNow();
 }
 
diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp
index 5c64b92a6e..40c6fab14f 100644
--- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp
+++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp
@@ -1,35 +1,35 @@
 /****************************************************************************
-*
-*   Copyright (c) 2021 PX4 Development Team. All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* 1. Redistributions of source code must retain the above copyright
-*    notice, this list of conditions and the following disclaimer.
-* 2. Redistributions in binary form must reproduce the above copyright
-*    notice, this list of conditions and the following disclaimer in
-*    the documentation and/or other materials provided with the
-*    distribution.
-* 3. Neither the name PX4 nor the names of its contributors may be
-*    used to endorse or promote products derived from this software
-*    without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-****************************************************************************/
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
 
 /**
  * @file ADIS16470.hpp
@@ -98,15 +98,18 @@ private:
 	PX4Gyroscope _px4_gyro;
 
 	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
-	perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))};
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
+	perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))};
+	perf_counter_t _drdy_missed_perf{nullptr};
 
 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
 	int _failure_count{0};
 
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
+
 	bool _self_test_passed{false};
 
 	enum class STATE : uint8_t {
diff --git a/src/drivers/imu/bosch/bmi055/BMI055.hpp b/src/drivers/imu/bosch/bmi055/BMI055.hpp
index 4ee625cfe2..f4fad2772e 100644
--- a/src/drivers/imu/bosch/bmi055/BMI055.hpp
+++ b/src/drivers/imu/bosch/bmi055/BMI055.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -66,7 +66,7 @@ protected:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -74,9 +74,7 @@ protected:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
 
diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp
index 0a809d4195..b73e10d3c3 100644
--- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp
+++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -162,9 +162,16 @@ void BMI055_Accelerometer::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = 0;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
@@ -192,7 +199,20 @@ void BMI055_Accelerometer::RunImpl()
 					perf_count(_fifo_empty_perf);
 
 				} else if (fifo_frame_counter >= 1) {
-					if (FIFORead(now, fifo_frame_counter)) {
+
+					uint8_t samples = fifo_frame_counter;
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						// sample timestamp set from data ready already corresponds to _fifo_samples
+						if (timestamp_sample == 0) {
+							timestamp_sample = now - FIFO_SAMPLE_DT;
+						}
+
+						samples--;
+					}
+
+					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -242,22 +262,22 @@ void BMI055_Accelerometer::ConfigureAccel()
 	const uint8_t PMU_RANGE = RegisterRead(Register::PMU_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
 
 	switch (PMU_RANGE) {
-	case range_2g:
+	case range_2g_set:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); // 1024 LSB/g, 0.98mg/LSB
 		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
 		break;
 
-	case range_4g:
+	case range_4g_set:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 512.f); // 512 LSB/g, 1.95mg/LSB
 		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
 		break;
 
-	case range_8g:
+	case range_8g_set:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 256.f); // 256 LSB/g, 3.91mg/LSB
 		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
 		break;
 
-	case range_16g:
+	case range_16g_set:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 128.f); // 128 LSB/g, 7.81mg/LSB
 		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
 		break;
@@ -318,11 +338,8 @@ int BMI055_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi
 
 void BMI055_Accelerometer::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }
 
 bool BMI055_Accelerometer::DataReadyInterruptConfigure()
@@ -437,7 +454,7 @@ void BMI055_Accelerometer::FIFOReset()
 	RegisterWrite(Register::FIFO_CONFIG_1, 0);
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 
 	// FIFO_CONFIG_0: restore FIFO watermark
 	// FIFO_CONFIG_1: re-enable FIFO
diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp
index 299db7aa09..31906c8d96 100644
--- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp
+++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -58,7 +58,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -112,7 +112,7 @@ private:
 	static constexpr uint8_t size_register_cfg{7};
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register                    | Set bits, Clear bits
-		{ Register::PMU_RANGE,         PMU_RANGE_BIT::range_16g, Bit1 | Bit0},
+		{ Register::PMU_RANGE,         PMU_RANGE_BIT::range_16g_set, PMU_RANGE_BIT::range_16g_clear},
 		{ Register::ACCD_HBW,          ACCD_HBW_BIT::data_high_bw, 0},
 		{ Register::INT_EN_1,          INT_EN_1_BIT::int_fwm_en, 0},
 		{ Register::INT_MAP_1,         INT_MAP_1_BIT::int1_fwm, 0},
diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp
index e7761b27c4..ab26469d38 100644
--- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp
+++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -162,9 +162,16 @@ void BMI055_Gyroscope::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = 0;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
@@ -192,7 +199,20 @@ void BMI055_Gyroscope::RunImpl()
 					perf_count(_fifo_empty_perf);
 
 				} else if (fifo_frame_counter >= 1) {
-					if (FIFORead(now, fifo_frame_counter)) {
+
+					uint8_t samples = fifo_frame_counter;
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						// sample timestamp set from data ready already corresponds to _fifo_samples
+						if (timestamp_sample == 0) {
+							timestamp_sample = now - FIFO_SAMPLE_DT;
+						}
+
+						samples--;
+					}
+
+					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -316,11 +336,8 @@ int BMI055_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a
 
 void BMI055_Gyroscope::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }
 
 bool BMI055_Gyroscope::DataReadyInterruptConfigure()
@@ -434,7 +451,7 @@ void BMI055_Gyroscope::FIFOReset()
 	RegisterWrite(Register::FIFO_CONFIG_1, 0);
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 
 	// FIFO_CONFIG_0: restore FIFO watermark
 	// FIFO_CONFIG_1: re-enable FIFO
diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp
index 0428b66807..13c0468f0d 100644
--- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp
+++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -58,7 +58,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
diff --git a/src/drivers/imu/bosch/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp b/src/drivers/imu/bosch/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp
index 697d248479..f95669f293 100644
--- a/src/drivers/imu/bosch/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp
+++ b/src/drivers/imu/bosch/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -97,10 +97,17 @@ enum ACCD_HBW_BIT : uint8_t {
 // PMU_RANGE
 enum PMU_RANGE_BIT : uint8_t {
 	// range<3:0>
-	range_2g  = Bit1 | Bit0, //  ́0011b ́ -> ±2g range
-	range_4g  = Bit2 | Bit0, //  ́0101b ́ -> ±4g range
-	range_8g  = Bit3,        //  ́1000b ́ -> ±8g range
-	range_16g = Bit3 | Bit2, //  ́1100b ́ -> ±16g range
+	range_16g_set   = Bit3 | Bit2, //  ́1100b ́ -> ±16g range
+	range_16g_clear = Bit1 | Bit0,
+
+	range_8g_set    = Bit3,        //  ́1000b ́ -> ±8g range
+	range_8g_clear  = Bit2 | Bit1 | Bit0,
+
+	range_4g_set    = Bit2 | Bit0, //  ́0101b ́ -> ±4g range
+	range_4g_clear  = Bit3 | Bit1,
+
+	range_2g_set    = Bit1 | Bit0, //  ́0011b ́ -> ±2g range
+	range_2g_clear  = Bit3 | Bit2,
 };
 
 // INT_EN_1
diff --git a/src/drivers/imu/bosch/bmi088/BMI088.cpp b/src/drivers/imu/bosch/bmi088/BMI088.cpp
index 63c6b895f2..0c45fd3a2d 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088.cpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/drivers/imu/bosch/bmi088/BMI088.hpp b/src/drivers/imu/bosch/bmi088/BMI088.hpp
index 7bfdd5e0f7..35187fdb72 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088.hpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -66,7 +66,7 @@ protected:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -74,9 +74,7 @@ protected:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
 
diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp
index 0db96b7547..06b33e9144 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -184,15 +184,19 @@ void BMI088_Accelerometer::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
 
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_samples;
 
 				} else {
-					samples = _fifo_samples;
+					perf_count(_drdy_missed_perf);
 				}
 
 				// push backup schedule back
@@ -214,6 +218,12 @@ void BMI088_Accelerometer::RunImpl()
 				} else {
 					samples = fifo_byte_counter / sizeof(FIFO::DATA);
 
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						timestamp_sample -= FIFO_SAMPLE_DT;
+						samples--;
+					}
+
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
@@ -226,7 +236,7 @@ void BMI088_Accelerometer::RunImpl()
 			bool success = false;
 
 			if (samples >= 1) {
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
 					if (_failure_count > 0) {
@@ -360,11 +370,8 @@ int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi
 
 void BMI088_Accelerometer::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }
 
 bool BMI088_Accelerometer::DataReadyInterruptConfigure()
@@ -561,7 +568,7 @@ void BMI088_Accelerometer::FIFOReset()
 	RegisterWrite(Register::ACC_SOFTRESET, 0xB0);
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }
 
 void BMI088_Accelerometer::UpdateTemperature()
diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp
index 3ecf1b2d87..ccf5f40d3a 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -58,7 +58,7 @@ private:
 	static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
index e841600591..792ed1e888 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -163,9 +163,16 @@ void BMI088_Gyroscope::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = 0;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
@@ -193,7 +200,20 @@ void BMI088_Gyroscope::RunImpl()
 					perf_count(_fifo_empty_perf);
 
 				} else if (fifo_frame_counter >= 1) {
-					if (FIFORead(now, fifo_frame_counter)) {
+
+					uint8_t samples = fifo_frame_counter;
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						// sample timestamp set from data ready already corresponds to _fifo_samples
+						if (timestamp_sample == 0) {
+							timestamp_sample = now - FIFO_SAMPLE_DT;
+						}
+
+						samples--;
+					}
+
+					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -317,11 +337,8 @@ int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a
 
 void BMI088_Gyroscope::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }
 
 bool BMI088_Gyroscope::DataReadyInterruptConfigure()
@@ -435,7 +452,7 @@ void BMI088_Gyroscope::FIFOReset()
 	RegisterWrite(Register::FIFO_CONFIG_1, 0);
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 
 	// FIFO_CONFIG_0: restore FIFO watermark
 	// FIFO_CONFIG_1: re-enable FIFO
diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp
index 47be30af98..59d847bf02 100644
--- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp
+++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -58,7 +58,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
diff --git a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp
index 5aca22dba6..00b8ffe6d6 100644
--- a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp
+++ b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp
index 42d9df8f40..0f1974f2cb 100644
--- a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp
+++ b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp
index 33b5f612fc..6c1734a059 100644
--- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp
+++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -72,7 +72,7 @@ protected:
 	int _total_failure_count{0};
 
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -81,9 +81,7 @@ protected:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::SELFTEST};
+	} _state{STATE::SELFTEST};
 
 	uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
 
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp
index 488a086c77..6d7e4d6739 100644
--- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp
+++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -293,11 +293,7 @@ int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi
 
 void BMI088_Accelerometer::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	ScheduleNow();
 }
 
 bool BMI088_Accelerometer::DataReadyInterruptConfigure()
@@ -588,7 +584,7 @@ void BMI088_Accelerometer::FIFOReset()
 	RegisterWrite(Register::ACC_SOFTRESET, 0xB0);
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }
 
 void BMI088_Accelerometer::UpdateTemperature()
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp
index edb540a270..189ffe9a75 100644
--- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp
+++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -59,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp
index 88bf5ad9f6..ad177c68e5 100644
--- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp
+++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -262,11 +262,8 @@ int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a
 
 void BMI088_Gyroscope::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }
 
 bool BMI088_Gyroscope::DataReadyInterruptConfigure()
@@ -382,7 +379,7 @@ void BMI088_Gyroscope::FIFOReset()
 	RegisterWrite(Register::FIFO_CONFIG_1, 0);
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 
 	// FIFO_CONFIG_0: restore FIFO watermark
 	// FIFO_CONFIG_1: re-enable FIFO
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp
index 4ccbb8e26b..33d95b3b14 100644
--- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp
+++ b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -58,7 +58,7 @@ private:
 	static constexpr uint32_t RATE{400}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
 
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_main.cpp b/src/drivers/imu/bosch/bmi088/bmi088_main.cpp
index 155ca904ae..182c1e90d4 100644
--- a/src/drivers/imu/bosch/bmi088/bmi088_main.cpp
+++ b/src/drivers/imu/bosch/bmi088/bmi088_main.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp
index a064f80ed6..d63ae34967 100644
--- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp
+++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp
@@ -237,15 +237,19 @@ void ICM20602::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
 
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;
 
 				} else {
-					samples = _fifo_gyro_samples;
+					perf_count(_drdy_missed_perf);
 				}
 
 				// push backup schedule back
@@ -265,7 +269,13 @@ void ICM20602::RunImpl()
 
 				} else {
 					// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-					samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
+					samples = fifo_count / sizeof(FIFO::DATA);
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= FIFO_SAMPLE_DT;
+						samples--;
+					}
 
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
@@ -279,7 +289,7 @@ void ICM20602::RunImpl()
 			bool success = false;
 
 			if (samples >= SAMPLES_PER_TRANSFER) {
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
 					if (_failure_count > 0) {
@@ -435,11 +445,8 @@ int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ICM20602::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }
 
 bool ICM20602::DataReadyInterruptConfigure()
@@ -530,16 +537,14 @@ bool ICM20602::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 	}
 
 	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
 
-	if (fifo_count_bytes >= FIFO::SIZE) {
+	if ((fifo_count_bytes >= FIFO::SIZE) || (fifo_count_samples > FIFO_MAX_SAMPLES)) {
 		perf_count(_fifo_overflow_perf);
 		FIFOReset();
 		return false;
-	}
 
-	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
-
-	if (fifo_count_samples == 0) {
+	} else if (fifo_count_samples == 0) {
 		perf_count(_fifo_empty_perf);
 		return false;
 	}
@@ -571,7 +576,7 @@ void ICM20602::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 
 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp
index fa33eb71d7..eb36e56d80 100644
--- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp
+++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp
@@ -71,12 +71,12 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -140,7 +140,7 @@ private:
 	hrt_abstime _last_config_check_timestamp{0};
 	int _failure_count{0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -151,7 +151,7 @@ private:
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{24};
diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
index 837ddffaad..d817cf7694 100644
--- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
+++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
@@ -228,9 +228,16 @@ void ICM20608G::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
@@ -251,16 +258,21 @@ void ICM20608G::RunImpl()
 
 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}
 
 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -402,13 +414,10 @@ int ICM20608G::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ICM20608G::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -516,8 +525,8 @@ void ICM20608G::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
 
 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);
 
 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp
index cf8906b578..9608eeb816 100644
--- a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp
+++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp
@@ -71,12 +71,12 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -138,8 +138,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -150,7 +150,7 @@ private:
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{15};
diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp
index ac15c04246..0f94e0c70d 100644
--- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp
+++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -194,9 +194,16 @@ void ICM20649::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
@@ -217,16 +224,21 @@ void ICM20649::RunImpl()
 
 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}
 
 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -395,13 +407,10 @@ int ICM20649::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ICM20649::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -487,10 +496,11 @@ void ICM20649::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits
 
 uint16_t ICM20649::FIFOReadCount()
 {
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
+
 	// read FIFO count
 	uint8_t fifo_count_buf[3] {};
 	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
-	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
 
 	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
@@ -502,9 +512,10 @@ uint16_t ICM20649::FIFOReadCount()
 
 bool ICM20649::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
+
 	FIFOTransferBuffer buffer{};
 	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE);
-	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
@@ -549,8 +560,8 @@ void ICM20649::FIFOReset()
 	RegisterClearBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET);
 
 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_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.hpp b/src/drivers/imu/invensense/icm20649/ICM20649.hpp
index cbaf1cfa32..11eb1a438a 100644
--- a/src/drivers/imu/invensense/icm20649/ICM20649.hpp
+++ b/src/drivers/imu/invensense/icm20649/ICM20649.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -71,12 +71,12 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 9000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -152,8 +152,8 @@ private:
 
 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -161,12 +161,10 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{6};
diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp
index 9c48369f6b..84a33b2270 100644
--- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp
+++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp
@@ -228,9 +228,16 @@ void ICM20689::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
@@ -251,16 +258,21 @@ void ICM20689::RunImpl()
 
 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}
 
 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -402,13 +414,10 @@ int ICM20689::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ICM20689::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -516,8 +525,8 @@ void ICM20689::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
 
 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);
 
 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp
index 4ae2cc3271..df3b8a734e 100644
--- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp
+++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp
@@ -71,12 +71,12 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -138,8 +138,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -150,7 +150,7 @@ private:
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{15};
diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp
index 0b8da3cb9b..5ebbfc71d6 100644
--- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp
+++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -230,9 +230,16 @@ void ICM20948::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
@@ -253,16 +260,21 @@ void ICM20948::RunImpl()
 
 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}
 
 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -442,13 +454,10 @@ int ICM20948::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ICM20948::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -598,8 +607,8 @@ void ICM20948::FIFOReset()
 	RegisterClearBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET);
 
 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_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.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp
index db9839649e..3bb4509c63 100644
--- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp
+++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -73,12 +73,12 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 9000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -169,8 +169,8 @@ private:
 
 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -181,7 +181,7 @@ private:
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{6};
diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
index eb4576a646..6c31240631 100644
--- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
+++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -189,15 +189,19 @@ void ICM40609D::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
 
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;
 
 				} else {
-					samples = _fifo_gyro_samples;
+					perf_count(_drdy_missed_perf);
 				}
 
 				// push backup schedule back
@@ -219,6 +223,12 @@ void ICM40609D::RunImpl()
 					// FIFO count (size in bytes)
 					samples = (fifo_count / sizeof(FIFO::DATA));
 
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= FIFO_SAMPLE_DT;
+						samples--;
+					}
+
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
@@ -231,7 +241,7 @@ void ICM40609D::RunImpl()
 			bool success = false;
 
 			if (samples >= 1) {
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
 					if (_failure_count > 0) {
@@ -409,11 +419,8 @@ int ICM40609D::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ICM40609D::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }
 
 bool ICM40609D::DataReadyInterruptConfigure()
@@ -580,7 +587,7 @@ void ICM40609D::FIFOReset()
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }
 
 void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
index d997eb5ba4..48ac7901ca 100644
--- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
+++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -75,7 +75,7 @@ private:
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -146,7 +146,7 @@ private:
 
 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -154,12 +154,10 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{10};
diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.cpp b/src/drivers/imu/invensense/icm42605/ICM42605.cpp
index be4ba11ff0..4c5727ff41 100644
--- a/src/drivers/imu/invensense/icm42605/ICM42605.cpp
+++ b/src/drivers/imu/invensense/icm42605/ICM42605.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -190,15 +190,19 @@ void ICM42605::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
 
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;
 
 				} else {
-					samples = _fifo_gyro_samples;
+					perf_count(_drdy_missed_perf);
 				}
 
 				// push backup schedule back
@@ -220,6 +224,12 @@ void ICM42605::RunImpl()
 					// FIFO count (size in bytes)
 					samples = (fifo_count / sizeof(FIFO::DATA));
 
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= FIFO_SAMPLE_DT;
+						samples--;
+					}
+
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
@@ -232,7 +242,7 @@ void ICM42605::RunImpl()
 			bool success = false;
 
 			if (samples >= 1) {
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
 					if (_failure_count > 0) {
@@ -410,11 +420,8 @@ int ICM42605::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ICM42605::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }
 
 bool ICM42605::DataReadyInterruptConfigure()
@@ -581,7 +588,7 @@ void ICM42605::FIFOReset()
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }
 
 void ICM42605::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.hpp b/src/drivers/imu/invensense/icm42605/ICM42605.hpp
index 76e4ccde94..103d05e970 100644
--- a/src/drivers/imu/invensense/icm42605/ICM42605.hpp
+++ b/src/drivers/imu/invensense/icm42605/ICM42605.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -75,7 +75,7 @@ private:
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -146,7 +146,7 @@ private:
 
 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -154,12 +154,10 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{10};
diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
index 290dc0a180..263a2223b6 100644
--- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
+++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -190,15 +190,19 @@ void ICM42688P::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
 
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;
 
 				} else {
-					samples = _fifo_gyro_samples;
+					perf_count(_drdy_missed_perf);
 				}
 
 				// push backup schedule back
@@ -220,6 +224,12 @@ void ICM42688P::RunImpl()
 					// FIFO count (size in bytes)
 					samples = (fifo_count / sizeof(FIFO::DATA));
 
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= FIFO_SAMPLE_DT;
+						samples--;
+					}
+
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
@@ -232,7 +242,7 @@ void ICM42688P::RunImpl()
 			bool success = false;
 
 			if (samples >= 1) {
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
 					if (_failure_count > 0) {
@@ -368,11 +378,8 @@ int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ICM42688P::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }
 
 bool ICM42688P::DataReadyInterruptConfigure()
@@ -553,7 +560,7 @@ void ICM42688P::FIFOReset()
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }
 
 static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
index 932b3cb28d..19829737bb 100644
--- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
+++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -75,7 +75,7 @@ private:
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -158,7 +158,7 @@ private:
 
 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -169,7 +169,7 @@ private:
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{13};
diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp
index d8915623cb..1b1ab0cc90 100644
--- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp
+++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -197,9 +197,16 @@ void MPU6000::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
@@ -220,7 +227,13 @@ void MPU6000::RunImpl()
 
 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}
 
 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
@@ -228,7 +241,7 @@ void MPU6000::RunImpl()
 					perf_count(_fifo_overflow_perf);
 
 				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -370,13 +383,10 @@ int MPU6000::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void MPU6000::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -488,8 +498,8 @@ void MPU6000::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RESET, USER_CTRL_BIT::FIFO_EN);
 
 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);
 
 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp
index f1ab397e66..2d5180013a 100644
--- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp
+++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -71,12 +71,12 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{8};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{8};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -140,8 +140,8 @@ private:
 	FIFO::DATA _fifo_sample_last_new_accel{};
 	uint32_t _fifo_accel_samples_count{0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -149,12 +149,10 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{7};
diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp
index d7d3dc1ab5..a9886bae41 100644
--- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp
+++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp
@@ -229,9 +229,16 @@ void MPU6500::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
@@ -252,16 +259,21 @@ void MPU6500::RunImpl()
 
 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}
 
 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -403,13 +415,10 @@ int MPU6500::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void MPU6500::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -521,8 +530,8 @@ void MPU6500::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
 
 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);
 
 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp
index 10bdb20d4c..6d0c6222ed 100644
--- a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp
+++ b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp
@@ -71,12 +71,12 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -138,8 +138,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -150,7 +150,7 @@ private:
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{15};
diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
index 8c0bd97398..5d1e70eaa6 100644
--- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
+++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
@@ -264,9 +264,16 @@ void MPU9250::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
@@ -287,16 +294,21 @@ void MPU9250::RunImpl()
 
 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}
 
 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -438,13 +450,10 @@ int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void MPU9250::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -594,8 +603,8 @@ void MPU9250::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
 
 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);
 
 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp
index acff91780f..b5efdcdb26 100644
--- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp
+++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp
@@ -73,12 +73,12 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -150,8 +150,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -162,7 +162,7 @@ private:
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{18};
diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
index 16191c0818..3f4c1941a7 100644
--- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
+++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -197,9 +197,16 @@ void MPU9250_I2C::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}
 
@@ -220,16 +227,21 @@ void MPU9250_I2C::RunImpl()
 
 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}
 
 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
@@ -371,13 +383,10 @@ int MPU9250_I2C::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void MPU9250_I2C::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -495,8 +504,8 @@ void MPU9250_I2C::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
 
 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);
 
 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp
index 65f6c20975..7af10d0b60 100644
--- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp
+++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -71,12 +71,12 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 1000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{1};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{1};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 1000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -136,8 +136,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -145,12 +145,10 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{9};
diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
index 8a30b4530f..7475a44f39 100644
--- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
+++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -170,11 +170,13 @@ void LSM9DS1::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			// always check current FIFO count
 			bool success = false;
 			// Number of unread words (16-bit axes) stored in FIFO.
 			const uint8_t FIFO_SRC = RegisterRead(Register::FIFO_SRC);
-			const uint8_t samples = FIFO_SRC & static_cast<uint8_t>(FIFO_SRC_BIT::FSS);
+			uint8_t samples = FIFO_SRC & static_cast<uint8_t>(FIFO_SRC_BIT::FSS);
 
 			if (FIFO_SRC & FIFO_SRC_BIT::OVRN) {
 				// overflow
@@ -185,13 +187,19 @@ void LSM9DS1::RunImpl()
 				perf_count(_fifo_empty_perf);
 
 			} else {
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}
+
 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
 				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
 						if (_failure_count > 0) {
diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp
index fdaf2ea647..c02688f0e7 100644
--- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp
+++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -74,7 +74,7 @@ private:
 	static constexpr float ACCEL_RATE{ST_LSM9DS1::LA_ODR}; // 952 Hz accel
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / 12, sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / 12, sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	struct register_config_t {
 		Register reg;
@@ -123,7 +123,7 @@ private:
 	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{6};