icm20608g/icm20689: force FIFO count check sooner

The InvenSense icm20608g and icm20689 don't have a FIFO watermark interrupt, but they do have a data ready interrupt and the ability to get the current FIFO count in the same large transfer as the actual FIFO data. So instead of manually checking the FIFO count before every transfer (costs ~ 1-3% cpu) we trust the data ready counts, verify things are in sync after the large transfer (fifo count + fifo contents), and force a manual check before the next transfer if necessary.

As a precaution this change lowers the threshold for forcing a manual FIFO count check before the large transfer. Forcing the check if out of sync by 2 (or more) samples makes sense because we always do transfers in multiples of 2 (gyro samples per accel).

The other small cosmetic changes are keeping the icm20608g and icm20689 in sync (they're nearly identical).

src/drivers/imu/invensense/icm20608g/ICM20608G.cpp

@@ -265,22 +265,22 @@ void ICM20608G::ConfigureAccel()
 	switch (ACCEL_FS_SEL) {
 	case ACCEL_FS_SEL_2G:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2 * CONSTANTS_ONE_G);
+		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
 		break;
 
 	case ACCEL_FS_SEL_4G:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4 * CONSTANTS_ONE_G);
+		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
 		break;
 
 	case ACCEL_FS_SEL_8G:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8 * CONSTANTS_ONE_G);
+		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
 		break;
 
 	case ACCEL_FS_SEL_16G:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16 * CONSTANTS_ONE_G);
+		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
 		break;
 	}
 }
@@ -374,7 +374,7 @@ bool ICM20608G::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &ICM20608G::DataReadyInterruptCallback, this) == 0;
+	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
 }
 
 bool ICM20608G::DataReadyInterruptDisable()
@@ -493,8 +493,8 @@ bool ICM20608G::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
 		// force check if there is somehow fewer samples actually in the FIFO (potentially a serious error)
 		_force_fifo_count_check = true;
 
-	} else if (fifo_count_samples > samples + 4) {
-		// if we're more than a few samples behind force FIFO_COUNT check
+	} else if (fifo_count_samples >= samples + 2) {
+		// if we're more than a couple samples behind force FIFO_COUNT check
 		_force_fifo_count_check = true;
 
 	} else {
@@ -504,9 +504,15 @@ bool ICM20608G::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
 
 	if (valid_samples > 0) {
 		ProcessGyro(timestamp_sample, buffer, valid_samples);
-		return ProcessAccel(timestamp_sample, buffer, valid_samples);
+
+		if (ProcessAccel(timestamp_sample, buffer, valid_samples)) {
+			return true;
+		}
 	}
 
+	// force FIFO count check if there was any other error
+	_force_fifo_count_check = true;
+
 	return false;
 }
 

src/drivers/imu/invensense/icm20689/ICM20689.cpp

@@ -264,23 +264,23 @@ void ICM20689::ConfigureAccel()
 
 	switch (ACCEL_FS_SEL) {
 	case ACCEL_FS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384);
-		_px4_accel.set_range(2 * CONSTANTS_ONE_G);
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
+		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
 		break;
 
 	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192);
-		_px4_accel.set_range(4 * CONSTANTS_ONE_G);
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
 		break;
 
 	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096);
-		_px4_accel.set_range(8 * CONSTANTS_ONE_G);
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
+		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
 		break;
 
 	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048);
-		_px4_accel.set_range(16 * CONSTANTS_ONE_G);
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
 		break;
 	}
 }
@@ -374,7 +374,7 @@ bool ICM20689::DataReadyInterruptConfigure()
 	}
 
 	// Setup data ready on falling edge
-	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &ICM20689::DataReadyInterruptCallback, this) == 0;
+	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
 }
 
 bool ICM20689::DataReadyInterruptDisable()
@@ -493,8 +493,8 @@ bool ICM20689::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
 		// force check if there is somehow fewer samples actually in the FIFO (potentially a serious error)
 		_force_fifo_count_check = true;
 
-	} else if (fifo_count_samples > samples + 4) {
-		// if we're more than a few samples behind force FIFO_COUNT check
+	} else if (fifo_count_samples >= samples + 2) {
+		// if we're more than a couple samples behind force FIFO_COUNT check
 		_force_fifo_count_check = true;
 
 	} else {
@@ -504,9 +504,15 @@ bool ICM20689::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
 
 	if (valid_samples > 0) {
 		ProcessGyro(timestamp_sample, buffer, valid_samples);
-		return ProcessAccel(timestamp_sample, buffer, valid_samples);
+
+		if (ProcessAccel(timestamp_sample, buffer, valid_samples)) {
+			return true;
+		}
 	}
 
+	// force FIFO count check if there was any other error
+	_force_fifo_count_check = true;
+
 	return false;
 }
 

src/drivers/imu/invensense/icm20689/ICM20689.hpp

@@ -74,9 +74,9 @@ private:
 
 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{125.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
-	static constexpr float GYRO_RATE{1000000 / FIFO_SAMPLE_DT}; // 8 kHz gyro
-	static constexpr float ACCEL_RATE{GYRO_RATE / 2.f};         // 4 kHz accel
+	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};       // ensure at least 1 new accel sample per transfer
+	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8 kHz gyro
+	static constexpr float ACCEL_RATE{GYRO_RATE / 2.f};      // 4 kHz accel
 
 	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};