diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp
index d63ae34967..70a6e4d8be 100644
--- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp
+++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp
@@ -271,24 +271,25 @@ void ICM20602::RunImpl()
 					// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
 					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_gyro_samples) {
+						// grab desired number of samples, but reschedule next cycle sooner
+						int extra_samples = samples - _fifo_gyro_samples;
+						timestamp_sample -= extra_samples * FIFO_SAMPLE_DT;
+						samples = _fifo_gyro_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);
-						samples = 0;
+						ScheduleOnInterval(_fifo_empty_interval_us,
+								   _fifo_empty_interval_us - (extra_samples * FIFO_SAMPLE_DT));
+
+					} else if (samples < _fifo_gyro_samples) {
+						// reschedule next cycle to catch the desired number of samples
+						ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * FIFO_SAMPLE_DT);
 					}
 				}
 			}
 
 			bool success = false;
 
-			if (samples >= SAMPLES_PER_TRANSFER) {
+			if (samples == _fifo_gyro_samples) {
 				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp
index 0f94e0c70d..68835d0576 100644
--- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp
+++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp
@@ -226,18 +226,21 @@ void ICM20649::RunImpl()
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
 				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_gyro_samples) {
+					// grab desired number of samples, but reschedule next cycle sooner
+					int extra_samples = samples - _fifo_gyro_samples;
+					timestamp_sample -= extra_samples * FIFO_SAMPLE_DT;
+					samples = _fifo_gyro_samples;
+
+					ScheduleOnInterval(_fifo_empty_interval_us,
+							   _fifo_empty_interval_us - (extra_samples * FIFO_SAMPLE_DT));
+
+				} else if (samples < _fifo_gyro_samples) {
+					// reschedule next cycle to catch the desired number of samples
+					ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * FIFO_SAMPLE_DT);
 				}
 
-				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 >= SAMPLES_PER_TRANSFER) {
+				if (samples == _fifo_gyro_samples) {
 					if (FIFORead(timestamp_sample, samples)) {
 						success = true;
 
diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp
index 5ebbfc71d6..8f61ad9879 100644
--- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp
+++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp
@@ -262,18 +262,21 @@ void ICM20948::RunImpl()
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
 				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_gyro_samples) {
+					// grab desired number of samples, but reschedule next cycle sooner
+					int extra_samples = samples - _fifo_gyro_samples;
+					timestamp_sample -= extra_samples * FIFO_SAMPLE_DT;
+					samples = _fifo_gyro_samples;
+
+					ScheduleOnInterval(_fifo_empty_interval_us,
+							   _fifo_empty_interval_us - (extra_samples * FIFO_SAMPLE_DT));
+
+				} else if (samples < _fifo_gyro_samples) {
+					// reschedule next cycle to catch the desired number of samples
+					ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * FIFO_SAMPLE_DT);
 				}
 
-				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 >= SAMPLES_PER_TRANSFER) {
+				if (samples == _fifo_gyro_samples) {
 					if (FIFORead(timestamp_sample, samples)) {
 						success = true;