icm40609d: Change FIFO count to samples instead of bytes

As the sensor can directly report the amount of samples in the fifo, use it to simplify the logic. Also combine the fifo empty/fifo overflow checks for interrupt and polling modes. Signed-off-by: Jukka Laitinen <[email protected]>
tiiuae · Aug 13, 2024 · 279711a · 279711a
1 parent 50dc42f
commit 279711a
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 62 deletions.
diff --git 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
@@ -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 },