changes

Signed-off-by: Jukka Laitinen <[email protected]>

src/drivers/imu/invensense/icm40609d/ICM40609D.cpp

@@ -173,6 +173,9 @@ void ICM40609D::RunImpl()
 			// if configure succeeded then start reading from FIFO
 			_state = STATE::FIFO_READ;
 
+			// Reset fifo
+			FIFOReset();
+
 			if (DataReadyInterruptConfigure()) {
 				_data_ready_interrupt_enabled = true;
 
@@ -184,8 +187,6 @@ void ICM40609D::RunImpl()
 				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
 			}
 
-			FIFOReset();
-
 		} else {
 			// CONFIGURE not complete
 			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
@@ -223,19 +224,16 @@ void ICM40609D::RunImpl()
 
 			if (samples == 0) {
 				// check current FIFO count
-				const uint16_t fifo_count = FIFOReadCount();
+				samples = FIFOReadCount();
 
-				if (fifo_count >= FIFO::SIZE) {
+				if (samples * sizeof(FIFO::DATA) >= FIFO::SIZE) {
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
 
-				} else if (fifo_count == 0) {
+				} else if (samples == 0) {
 					perf_count(_fifo_empty_perf);
 
 				} else {
-					// 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 -= static_cast<int>(FIFO_SAMPLE_DT);
@@ -374,17 +372,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,16 +529,14 @@ 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);
+	const uint16_t fifo_count_samples = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
 
-	if (fifo_count_bytes >= FIFO::SIZE) {
+	if (fifo_count_samples * sizeof(FIFO::DATA) >= 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;
@@ -555,7 +545,7 @@ bool ICM40609D::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 	// 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 < math::min((uint16_t)samples, fifo_count_samples); i++) {
 		bool valid = true;
 
 		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
@@ -596,6 +586,9 @@ void ICM40609D::FIFOReset()
 {
 	perf_count(_fifo_reset_perf);
 
+	// Read INT_STATUS to clear
+	RegisterRead(Register::BANK_0::INT_STATUS);
+
 	// SIGNAL_PATH_RESET: FIFO flush
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
 

src/drivers/imu/invensense/icm40609d/ICM40609D.hpp

@@ -173,7 +173,6 @@ class ICM40609D : public device::SPI, public I2CSPIDriver<ICM40609D>
 		{ Register::BANK_0::FIFO_CONFIG1,  FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TEMP_EN },
 
 		{ Register::BANK_0::FIFO_CONFIG2,  0, 0 }, // FIFO_WM[7:0] set at runtime
-		{ Register::BANK_0::FIFO_CONFIG3,  0, 0 }, // FIFO_WM[11:8] set at runtime
 		{ Register::BANK_0::INT_CONFIG0,   INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
 		{ Register::BANK_0::INT_SOURCE0,   INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
 	};