diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp
index d2d0dcde9f..d2b10a69d2 100644
--- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp
+++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp
@@ -188,10 +188,13 @@ void MPU6000::RunImpl()
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = 0;
 
-			if (_data_ready_interrupt_enabled && (hrt_elapsed_time(&timestamp_sample) < (_fifo_empty_interval_us / 2))) {
+			if (_data_ready_interrupt_enabled) {
 				// re-schedule as watchdog timeout
 				ScheduleDelayed(10_ms);
+			}
 
+			if (_data_ready_interrupt_enabled && (hrt_elapsed_time(&timestamp_sample) < (_fifo_empty_interval_us / 2))) {
+				// use timestamp from data ready interrupt if enabled and seems valid
 				timestamp_sample = _fifo_watermark_interrupt_timestamp;
 
 			} else {
diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp
index 4395330107..21287d6086 100644
--- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp
+++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp
@@ -188,10 +188,13 @@ void MPU6500::RunImpl()
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = 0;
 
-			if (_data_ready_interrupt_enabled && (hrt_elapsed_time(&timestamp_sample) < (_fifo_empty_interval_us / 2))) {
+			if (_data_ready_interrupt_enabled) {
 				// re-schedule as watchdog timeout
 				ScheduleDelayed(10_ms);
+			}
 
+			if (_data_ready_interrupt_enabled && (hrt_elapsed_time(&timestamp_sample) < (_fifo_empty_interval_us / 2))) {
+				// use timestamp from data ready interrupt if enabled and seems valid
 				timestamp_sample = _fifo_watermark_interrupt_timestamp;
 
 			} else {
diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
index a47b0d1876..52e37b7ff0 100644
--- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
+++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp
@@ -221,10 +221,13 @@ void MPU9250::RunImpl()
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = 0;
 
-			if (_data_ready_interrupt_enabled && (hrt_elapsed_time(&timestamp_sample) < (_fifo_empty_interval_us / 2))) {
+			if (_data_ready_interrupt_enabled) {
 				// re-schedule as watchdog timeout
 				ScheduleDelayed(10_ms);
+			}
 
+			if (_data_ready_interrupt_enabled && (hrt_elapsed_time(&timestamp_sample) < (_fifo_empty_interval_us / 2))) {
+				// use timestamp from data ready interrupt if enabled and seems valid
 				timestamp_sample = _fifo_watermark_interrupt_timestamp;
 
 			} else {