diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
index 55db372d5d39..28fce0691ce9 100644
--- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
+++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
@@ -203,57 +203,45 @@ void ICM40609D::RunImpl()
 
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = now;
-			uint8_t samples = 0;
+			uint8_t samples = FIFOReadCount();
 
-			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) < _fifo_empty_interval_us) {
-					timestamp_sample = drdy_timestamp_sample;
-					samples = _fifo_gyro_samples;
+			if (samples == 0) {
+				perf_count(_fifo_empty_perf);
 
-				} else {
-					perf_count(_drdy_missed_perf);
+			} else {
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+					samples--;
 				}
 
-				// push backup schedule back
-				ScheduleDelayed(_fifo_empty_interval_us * 2);
-			}
-
-			if (samples == 0) {
-				// check current FIFO count
-				const uint16_t fifo_count = FIFOReadCount();
-
-				if (fifo_count >= FIFO::SIZE) {
+				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;
+				}
+			}
 
-				} else if (fifo_count == 0) {
-					perf_count(_fifo_empty_perf);
+			bool success = false;
 
-				} else {
-					// FIFO count (size in bytes)
-					samples = (fifo_count / sizeof(FIFO::DATA));
+			if (samples > 0) {
+				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);
 
-					// tolerate minor jitter, leave sample to next iteration if behind by only 1
-					if (samples == _fifo_gyro_samples + 1) {
-						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
-						samples--;
-					}
+					if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+						timestamp_sample = drdy_timestamp_sample;
+						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;
+					} else {
+						perf_count(_drdy_missed_perf);
 					}
-				}
-			}
 
-			bool success = false;
+					// push backup schedule back
+					ScheduleDelayed(_fifo_empty_interval_us * 2);
+				}
 
-			if (samples >= 1) {
 				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
@@ -374,17 +362,11 @@ void ICM40609D::ConfigureSampleRate(int sample_rate)
 
 void ICM40609D::ConfigureFIFOWatermark(uint8_t samples)
 {
-	// FIFO watermark threshold in number of bytes
-	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
-
 	for (auto &r : _register_bank0_cfg) {
 		if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
 			// FIFO_WM[7:0]  FIFO_CONFIG2
-			r.set_bits = fifo_watermark_threshold & 0xFF;
+			r.set_bits = samples & 0xFF;
 
-		} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
-			// FIFO_WM[11:8] FIFO_CONFIG3
-			r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
 		}
 	}
 }
@@ -537,25 +519,10 @@ bool ICM40609D::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		return false;
 	}
 
-	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
-
-	if (fifo_count_bytes >= FIFO::SIZE) {
-		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) {
-		perf_count(_fifo_empty_perf);
-		return false;
-	}
-
 	// check FIFO header in every sample
 	uint8_t valid_samples = 0;
 
-	for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
+	for (int i = 0; i < samples; i++) {
 		bool valid = true;
 
 		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
index 882e11a91ee8..8dee6c2fb793 100644
--- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
+++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
@@ -164,7 +164,7 @@ class ICM40609D : public device::SPI, public I2CSPIDriver<ICM40609D>
 	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
 		// Register                        | Set bits, Clear bits
 		{ Register::BANK_0::INT_CONFIG,    INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
-		{ Register::BANK_0::INTF_CONFIG0,  INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0},
+		{ Register::BANK_0::INTF_CONFIG0,  INTF_CONFIG0_BIT::FIFO_COUNT_REC | INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0},
 		{ Register::BANK_0::FIFO_CONFIG,   FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
 		{ Register::BANK_0::PWR_MGMT0,     PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
 		{ Register::BANK_0::GYRO_CONFIG0,  GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },