invensense icm20602 improvements

- refactor Run() into simple state machine
 - perform reset and configuration in sensor bus thread
 - when using data ready interrupt skip checking FIFO count

boards/px4/fmu-v5/init/rc.board_sensors

@@ -7,9 +7,11 @@ adc start
 
 # Internal SPI bus ICM-20602
 mpu6000 -R 8 -s -T 20602 start
+#icm20602
 
 # Internal SPI bus ICM-20689
 mpu6000 -R 8 -z -T 20689 start
+#icm20689 start
 
 # Internal SPI bus BMI055 accel
 bmi055 -A -R 10 start

src/drivers/imu/invensense/icm20602/CMakeLists.txt

@@ -39,6 +39,7 @@ px4_add_module(
 		ICM20602.cpp
 		ICM20602.hpp
 		icm20602_main.cpp
+		InvenSense_ICM20602_registers.hpp
 	DEPENDS
 		drivers_accelerometer
 		drivers_gyroscope

src/drivers/imu/invensense/icm20602/ICM20602.cpp

@@ -42,11 +42,6 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
 	return (msb << 8u) | lsb;
 }
 
-static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
-{
-	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
-}
-
 ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) :
 	SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
 	ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
@@ -54,8 +49,11 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation)
 {
 	set_device_type(DRV_ACC_DEVTYPE_ICM20602);
+
 	_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20602);
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602);
+
+	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
 }
 
 ICM20602::~ICM20602()
@@ -75,50 +73,6 @@ ICM20602::~ICM20602()
 	perf_free(_drdy_interval_perf);
 }
 
-void ICM20602::ConfigureSampleRate(int sample_rate)
-{
-	if (sample_rate == 0) {
-		sample_rate = 1000; // default to 1 kHz
-	}
-
-	sample_rate = math::constrain(sample_rate, 250, 2000); // limit 250 - 2000 Hz
-
-	_fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 500); // round down to nearest 250 us
-	_fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES);
-
-	// recompute FIFO empty interval (us) with actual gyro sample limit
-	_fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE);
-
-	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES);
-
-	_px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us);
-	_px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us);
-
-	// FIFO watermark threshold in number of bytes
-	const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA);
-
-	for (auto &r : _register_cfg) {
-		if (r.reg == Register::FIFO_WM_TH1) {
-			r.set_bits = (fifo_watermark_threshold >> 8) & 0b00000011;
-
-		} else if (r.reg == Register::FIFO_WM_TH2) {
-			r.set_bits = fifo_watermark_threshold & 0xFF;
-		}
-	}
-}
-
-int ICM20602::probe()
-{
-	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
-
-	if (whoami != WHOAMI) {
-		PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami);
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
-}
-
 bool ICM20602::Init()
 {
 	if (SPI::init() != PX4_OK) {
@@ -134,32 +88,193 @@ bool ICM20602::Init()
 		return false;
 	}
 
-	if (!Reset()) {
+	return Reset();
-		PX4_ERR("reset failed");
+}
-		return false;
+
+void ICM20602::Stop()
+{
+	// wait until stopped
+	while (_state.load() != STATE::STOPPED) {
+		_state.store(STATE::REQUEST_STOP);
+		ScheduleNow();
+		px4_usleep(10);
 	}
-
-	Start();
-
-	return true;
 }
 
 bool ICM20602::Reset()
 {
-	// PWR_MGMT_1: Device Reset
+	_state.store(STATE::RESET);
-	RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
+
+void ICM20602::PrintInfo()
+{
+	PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
+		 static_cast<double>(1000000 / _fifo_empty_interval_us));
+
+	perf_print_counter(_transfer_perf);
+	perf_print_counter(_bad_register_perf);
+	perf_print_counter(_bad_transfer_perf);
+	perf_print_counter(_fifo_empty_perf);
+	perf_print_counter(_fifo_overflow_perf);
+	perf_print_counter(_fifo_reset_perf);
+	perf_print_counter(_drdy_interval_perf);
+
+	_px4_accel.print_status();
+	_px4_gyro.print_status();
+}
+
+int ICM20602::probe()
+{
+	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
+
+	if (whoami != WHOAMI) {
+		PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami);
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+void ICM20602::Run()
+{
+	switch (_state.load()) {
+	case STATE::RESET:
+		// PWR_MGMT_1: Device Reset
+		RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
+		_reset_timestamp = hrt_absolute_time();
+		_state.store(STATE::WAIT_FOR_RESET);
+		ScheduleDelayed(100);
+		break;
+
+	case STATE::WAIT_FOR_RESET:
 
-	for (int i = 0; i < 100; i++) {
 		// The reset value is 0x00 for all registers other than the registers below
 		//  Document Number: DS-000176 Page 31 of 57
 		if ((RegisterRead(Register::WHO_AM_I) == WHOAMI)
 		    && (RegisterRead(Register::PWR_MGMT_1) == 0x41)
 		    && (RegisterRead(Register::CONFIG) == 0x80)) {
-			return true;
-		}
-	}
 
-	return false;
+			// if reset succeeded then configure
+			_state.store(STATE::CONFIGURE);
+			ScheduleNow();
+
+		} else {
+			// RESET not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) {
+				PX4_ERR("Reset failed, retrying");
+				_state.store(STATE::RESET);
+				ScheduleDelayed(10_ms);
+
+			} else {
+				PX4_DEBUG("Reset not complete, check again in 1 ms");
+				ScheduleDelayed(1_ms);
+			}
+		}
+
+		break;
+
+	case STATE::CONFIGURE:
+		if (Configure()) {
+			// if configure succeeded then start reading from FIFO
+			_state.store(STATE::FIFO_READ);
+
+			if (DataReadyInterruptConfigure()) {
+				_data_ready_interrupt_enabled = true;
+
+				// backup schedule as a watchdog timeout
+				ScheduleDelayed(10_ms);
+
+			} else {
+				_data_ready_interrupt_enabled = false;
+				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+			}
+
+			FIFOReset();
+
+		} else {
+			PX4_DEBUG("Configure failed, retrying");
+			// try again in 1 ms
+			ScheduleDelayed(1_ms);
+		}
+
+		break;
+
+	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = 0;
+			uint8_t samples = 0;
+
+			if (_data_ready_interrupt_enabled) {
+				// re-schedule as watchdog timeout
+				ScheduleDelayed(10_ms);
+
+				// timestamp set in data ready interrupt
+				samples = _fifo_read_samples.load();
+				timestamp_sample = _fifo_watermark_interrupt_timestamp;
+			}
+
+			bool failure = false;
+
+			// manually check FIFO count if no samples from DRDY or timestamp looks bogus
+			if (!_data_ready_interrupt_enabled || (samples == 0)
+			    || (hrt_elapsed_time(&timestamp_sample) > (_fifo_empty_interval_us / 2))) {
+
+				// use the time now roughly corresponding with the last sample we'll pull from the FIFO
+				timestamp_sample = hrt_absolute_time();
+				const uint16_t fifo_count = FIFOReadCount();
+
+				if (fifo_count == 0) {
+					failure = true;
+					perf_count(_fifo_empty_perf);
+				}
+
+				samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2
+			}
+
+			if (samples > FIFO_MAX_SAMPLES) {
+				// not technically an overflow, but more samples than we expected or can publish
+				perf_count(_fifo_overflow_perf);
+				failure = true;
+				FIFOReset();
+
+			} else if (samples >= 2) {
+				// require at least 2 samples (we want at least 1 new accel sample per transfer)
+				if (!FIFORead(timestamp_sample, samples)) {
+					failure = true;
+					_px4_accel.increase_error_count();
+					_px4_gyro.increase_error_count();
+				}
+			}
+
+			if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
+				// check registers incrementally
+				if (RegisterCheck(_register_cfg[_checked_register], true)) {
+					_last_config_check_timestamp = timestamp_sample;
+					_checked_register = (_checked_register + 1) % size_register_cfg;
+
+				} else {
+					// register check failed, force reconfigure
+					PX4_DEBUG("Health check failed, reconfiguring");
+					_state.store(STATE::CONFIGURE);
+					ScheduleNow();
+				}
+			}
+		}
+
+		break;
+
+	case STATE::REQUEST_STOP:
+		DataReadyInterruptDisable();
+		ScheduleClear();
+		_state.store(STATE::STOPPED);
+		break;
+
+	case STATE::STOPPED:
+		// DO NOTHING
+		break;
+	}
 }
 
 void ICM20602::ConfigureAccel()
@@ -216,54 +331,126 @@ void ICM20602::ConfigureGyro()
 	}
 }
 
-void ICM20602::ResetFIFO()
+void ICM20602::ConfigureSampleRate(int sample_rate)
 {
-	perf_count(_fifo_reset_perf);
+	if (sample_rate == 0) {
+		sample_rate = 1000; // default to 1 kHz
+	}
 
-	// USER_CTRL: disable FIFO and reset all signal paths
+	_fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 250); // round down to nearest 250 us
-	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST,
+	_fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES);
-				USER_CTRL_BIT::FIFO_EN);
 
-	// FIFO_EN: enable both gyro and accel
+	// recompute FIFO empty interval (us) with actual gyro sample limit
-	RegisterSetBits(Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN);
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE);
 
-	// USER_CTRL: re-enable FIFO
+	_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES);
-	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN,
+
-				USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST);
+	_px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us);
+	_px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us);
+
+	// FIFO watermark threshold in number of bytes
+	const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA);
+
+	for (auto &r : _register_cfg) {
+		if (r.reg == Register::FIFO_WM_TH1) {
+			r.set_bits = (fifo_watermark_threshold >> 8) & 0b00000011;
+
+		} else if (r.reg == Register::FIFO_WM_TH2) {
+			r.set_bits = fifo_watermark_threshold & 0xFF;
+		}
+	}
 }
 
-bool ICM20602::Configure(bool notify)
+bool ICM20602::Configure()
 {
 	bool success = true;
 
 	for (const auto &reg : _register_cfg) {
-		if (!CheckRegister(reg, notify)) {
+		if (!RegisterCheck(reg)) {
 			success = false;
 		}
 	}
 
+	ConfigureAccel();
+	ConfigureGyro();
+
 	return success;
 }
 
-bool ICM20602::CheckRegister(const register_config_t &reg_cfg, bool notify)
+int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<ICM20602 *>(arg)->DataReady();
+	return 0;
+}
+
+void ICM20602::DataReady()
+{
+	_fifo_watermark_interrupt_timestamp = hrt_absolute_time();
+	_fifo_read_samples.store(_fifo_gyro_samples);
+	ScheduleNow();
+	perf_count(_drdy_interval_perf);
+}
+
+bool ICM20602::DataReadyInterruptConfigure()
+{
+	int ret_setevent = -1;
+
+	// Setup data ready on rising edge
+	// TODO: cleanup horrible DRDY define mess
+#if defined(GPIO_DRDY_PORTC_PIN14)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback,
+					     this);
+#elif defined(GPIO_SPI1_DRDY1_ICM20602)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback,
+					     this);
+#elif defined(GPIO_SPI1_DRDY4_ICM20602)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback,
+					     this);
+#elif defined(GPIO_SPI1_DRDY1_ICM20602)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback,
+					     this);
+#elif defined(GPIO_DRDY_ICM_2060X)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback,
+					     this);
+#endif
+
+	return (ret_setevent == 0);
+}
+
+bool ICM20602::DataReadyInterruptDisable()
+{
+	int ret_setevent = -1;
+
+	// Disable data ready callback
+	// TODO: cleanup horrible DRDY define mess
+#if defined(GPIO_DRDY_PORTC_PIN14)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr);
+#elif defined(GPIO_SPI1_DRDY1_ICM20602)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
+#elif defined(GPIO_SPI1_DRDY4_ICM20602)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr);
+#elif defined(GPIO_SPI1_DRDY1_ICM20602)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
+#elif defined(GPIO_DRDY_ICM_2060X)
+	ret_setevent = px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr);
+#endif
+
+	return (ret_setevent == 0);
+}
+
+bool ICM20602::RegisterCheck(const register_config_t &reg_cfg, bool notify)
 {
 	bool success = true;
 
 	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
 
 	if (reg_cfg.set_bits && !(reg_value & reg_cfg.set_bits)) {
-		if (notify) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-			PX4_ERR("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
-		}
-
 		success = false;
 	}
 
 	if (reg_cfg.clear_bits && (reg_value & reg_cfg.clear_bits)) {
-		if (notify) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-			PX4_ERR("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
-		}
-
 		success = false;
 	}
 
@@ -279,6 +466,8 @@ bool ICM20602::CheckRegister(const register_config_t &reg_cfg, bool notify)
 
 		if (notify) {
 			perf_count(_bad_register_perf);
+			_px4_accel.increase_error_count();
+			_px4_gyro.increase_error_count();
 		}
 	}
 
@@ -325,148 +514,22 @@ void ICM20602::RegisterClearBits(Register reg, uint8_t clearbits)
 	RegisterSetAndClearBits(reg, 0, clearbits);
 }
 
-int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg)
+uint16_t ICM20602::FIFOReadCount()
 {
-	ICM20602 *dev = reinterpret_cast<ICM20602 *>(arg);
-	dev->DataReady();
-	return 0;
-}
-
-void ICM20602::DataReady()
-{
-	perf_count(_drdy_interval_perf);
-
-	// make another measurement
-	ScheduleNow();
-}
-
-void ICM20602::Start()
-{
-	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
-
-	// attempt to configure 3 times
-	for (int i = 0; i < 3; i++) {
-		if (Configure(false)) {
-			break;
-		}
-	}
-
-	// TODO: cleanup horrible DRDY define mess
-#if defined(GPIO_DRDY_PORTC_PIN14)
-	_using_data_ready_interrupt_enabled = true;
-	// Setup data ready on rising edge
-	px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
-#elif defined(GPIO_SPI1_DRDY1_ICM20602)
-	_using_data_ready_interrupt_enabled = true;
-	// Setup data ready on rising edge
-	px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
-#elif defined(GPIO_SPI1_DRDY4_ICM20602)
-	_using_data_ready_interrupt_enabled = true;
-	// Setup data ready on rising edge
-	px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
-#elif defined(GPIO_SPI1_DRDY1_ICM20602)
-	_using_data_ready_interrupt_enabled = true;
-	// Setup data ready on rising edge
-	px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
-#elif defined(GPIO_DRDY_ICM_2060X)
-	_using_data_ready_interrupt_enabled = true;
-	// Setup data ready on rising edge
-	px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback, this);
-#else
-	_using_data_ready_interrupt_enabled = false;
-	ScheduleOnInterval(FIFO_EMPTY_INTERVAL_US, FIFO_EMPTY_INTERVAL_US);
-#endif
-
-	ResetFIFO();
-
-	// schedule as watchdog
-	if (_using_data_ready_interrupt_enabled) {
-		ScheduleDelayed(100_ms);
-	}
-}
-
-void ICM20602::Stop()
-{
-	Reset();
-
-	// TODO: cleanup horrible DRDY define mess
-#if defined(GPIO_DRDY_PORTC_PIN14)
-	// Disable data ready callback
-	px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr);
-#elif defined(GPIO_SPI1_DRDY1_ICM20602)
-	// Disable data ready callback
-	px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
-#elif defined(GPIO_SPI1_DRDY4_ICM20602)
-	// Disable data ready callback
-	px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr);
-#elif defined(GPIO_SPI1_DRDY1_ICM20602)
-	// Disable data ready callback
-	px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr);
-#elif defined(GPIO_DRDY_ICM_2060X)
-	// Disable data ready callback
-	px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr);
-#endif
-
-	ScheduleClear();
-}
-
-void ICM20602::Run()
-{
-	// use the time now roughly corresponding with the last sample we'll pull from the FIFO
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
-
 	// read FIFO count
 	uint8_t fifo_count_buf[3] {};
 	fifo_count_buf[0] = static_cast<uint8_t>(Register::FIFO_COUNTH) | DIR_READ;
 
 	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
+		return 0;
 	}
 
-	if (_using_data_ready_interrupt_enabled) {
+	return combine(fifo_count_buf[1], fifo_count_buf[2]);
-		// re-schedule as watchdog
+}
-		ScheduleDelayed(100_ms);
-	}
-
-	// check registers
-	if (hrt_elapsed_time(&_last_config_check) > 100_ms) {
-		_checked_register = (_checked_register + 1) % size_register_cfg;
-
-		if (CheckRegister(_register_cfg[_checked_register])) {
-			// delay next register check if current succeeded
-			_last_config_check = hrt_absolute_time();
-
-		} else {
-			// if register check failed reconfigure all
-			Configure();
-			ResetFIFO();
-			return;
-		}
-	}
-
-	const uint16_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]);
-	const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2
-
-	if (samples < 2) {
-		perf_count(_fifo_empty_perf);
-		return;
-
-	} else if (samples > FIFO_MAX_SAMPLES) {
-		// not technically an overflow, but more samples than we expected or can publish
-		perf_count(_fifo_overflow_perf);
-		ResetFIFO();
-
-		return;
-	}
-
-	// Transfer data
-	struct TransferBuffer {
-		uint8_t cmd;
-		FIFO::DATA f[FIFO_MAX_SAMPLES];
-	};
-	// ensure no struct padding
-	static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA)));
 
+bool ICM20602::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
+{
 	TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
 	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
 	memset(report, 0, transfer_size);
@@ -477,18 +540,70 @@ void ICM20602::Run()
 	if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
 		perf_end(_transfer_perf);
 		perf_count(_bad_transfer_perf);
-		return;
+		return false;
 	}
 
 	perf_end(_transfer_perf);
 
+	bool bad_data = false;
 
+	ProcessGyro(timestamp_sample, report, samples);
+
+	if (!ProcessAccel(timestamp_sample, report, samples)) {
+		bad_data = true;
+	}
+
+	// limit temperature updates to 1 Hz
+	if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) {
+		_temperature_update_timestamp = timestamp_sample;
+
+		if (!ProcessTemperature(report, samples)) {
+			bad_data = true;
+		}
+	}
+
+	return !bad_data;
+}
+
+void ICM20602::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// FIFO_EN: disable FIFO
+	RegisterWrite(Register::FIFO_EN, 0);
+
+	// USER_CTRL: disable FIFO and reset all signal paths
+	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST,
+				USER_CTRL_BIT::FIFO_EN);
+
+	// reset while FIFO is disabled
+	_fifo_watermark_interrupt_timestamp = 0;
+	_fifo_read_samples.store(0);
+
+	// FIFO_EN: enable both gyro and accel
+	// USER_CTRL: re-enable FIFO
+	for (const auto &r : _register_cfg) {
+		if ((r.reg == Register::FIFO_EN) || (r.reg == Register::USER_CTRL)) {
+			RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
+		}
+	}
+}
+
+static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
+{
+	return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
+}
+
+bool ICM20602::ProcessAccel(const hrt_abstime &timestamp_sample, const TransferBuffer *const report, uint8_t samples)
+{
 	PX4Accelerometer::FIFOSample accel;
 	accel.timestamp_sample = timestamp_sample;
 	accel.dt = _fifo_empty_interval_us / _fifo_accel_samples;
 
+	bool bad_data = false;
+
 	// accel data is doubled in FIFO, but might be shifted
-	int accel_first_sample = 0;
+	int accel_first_sample = 1;
 
 	if (samples >= 3) {
 		if (fifo_accel_equal(report->f[0], report->f[1])) {
@@ -503,7 +618,7 @@ void ICM20602::Run()
 
 		} else {
 			perf_count(_bad_transfer_perf);
-			return;
+			bad_data = true;
 		}
 	}
 
@@ -515,7 +630,8 @@ void ICM20602::Run()
 		int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
 		int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
 
-		// sensor's frame is +x forward, +y left, +z up, flip y & z to publish right handed (x forward, y right, z down)
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
 		accel.x[accel_samples] = accel_x;
 		accel.y[accel_samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
 		accel.z[accel_samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
@@ -524,30 +640,44 @@ void ICM20602::Run()
 
 	accel.samples = accel_samples;
 
+	_px4_accel.updateFIFO(accel);
 
+	return !bad_data;
+}
+
+void ICM20602::ProcessGyro(const hrt_abstime &timestamp_sample, const TransferBuffer *const report, uint8_t samples)
+{
 	PX4Gyroscope::FIFOSample gyro;
 	gyro.timestamp_sample = timestamp_sample;
 	gyro.samples = samples;
 	gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
 
-	int16_t temperature[samples];
-
 	for (int i = 0; i < samples; i++) {
 		const FIFO::DATA &fifo_sample = report->f[i];
 
-		temperature[i] = combine(fifo_sample.TEMP_OUT_H, fifo_sample.TEMP_OUT_L);
-
 		const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
 		const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
 		const int16_t gyro_z = combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L);
 
-		// sensor's frame is +x forward, +y left, +z up, flip y & z to publish right handed (x forward, y right, z down)
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
 		gyro.x[i] = gyro_x;
 		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
 		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
 	}
 
-	// Temperature
+	_px4_gyro.updateFIFO(gyro);
+}
+
+bool ICM20602::ProcessTemperature(const TransferBuffer *const report, uint8_t samples)
+{
+	int16_t temperature[samples];
+
+	for (int i = 0; i < samples; i++) {
+		const FIFO::DATA &fifo_sample = report->f[i];
+		temperature[i] = combine(fifo_sample.TEMP_OUT_H, fifo_sample.TEMP_OUT_L);
+	}
+
 	int32_t temperature_sum{0};
 
 	for (auto t : temperature) {
@@ -560,7 +690,7 @@ void ICM20602::Run()
 		// temperature changing wildly is an indication of a transfer error
 		if (fabsf(t - temperature_avg) > 1000) {
 			perf_count(_bad_transfer_perf);
-			return;
+			return false;
 		}
 	}
 
@@ -569,24 +699,5 @@ void ICM20602::Run()
 	_px4_accel.set_temperature(temperature_C);
 	_px4_gyro.set_temperature(temperature_C);
 
-
+	return true;
-	_px4_gyro.updateFIFO(gyro);
-	_px4_accel.updateFIFO(accel);
-}
-
-void ICM20602::PrintInfo()
-{
-	PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
-		 static_cast<double>(1000000 / _fifo_empty_interval_us));
-
-	perf_print_counter(_transfer_perf);
-	perf_print_counter(_bad_register_perf);
-	perf_print_counter(_bad_transfer_perf);
-	perf_print_counter(_fifo_empty_perf);
-	perf_print_counter(_fifo_overflow_perf);
-	perf_print_counter(_fifo_reset_perf);
-	perf_print_counter(_drdy_interval_perf);
-
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }

src/drivers/imu/invensense/icm20602/ICM20602.hpp

@@ -48,6 +48,7 @@
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/ecl/geo/geo.h>
 #include <lib/perf/perf_counter.h>
+#include <px4_platform_common/atomic.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
 using namespace InvenSense_ICM20602;
@@ -66,6 +67,19 @@ public:
 
 private:
 
+	// Sensor Configuration
+	static constexpr uint32_t GYRO_RATE{8000};  // 8 kHz gyro
+	static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel
+	static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
+
+	// Transfer data
+	struct TransferBuffer {
+		uint8_t cmd;
+		FIFO::DATA f[FIFO_MAX_SAMPLES];
+	};
+	// ensure no struct padding
+	static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+
 	struct register_config_t {
 		Register reg;
 		uint8_t set_bits{0};
@@ -74,17 +88,19 @@ private:
 
 	int probe() override;
 
-	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
-	void DataReady();
-
 	void Run() override;
 
-	void ConfigureSampleRate(int sample_rate);
+	bool Configure();
-	bool CheckRegister(const register_config_t &reg_cfg, bool notify = true);
-	bool Configure(bool notify = true);
-
 	void ConfigureAccel();
 	void ConfigureGyro();
+	void ConfigureSampleRate(int sample_rate);
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
+
+	bool RegisterCheck(const register_config_t &reg_cfg, bool notify = false);
 
 	uint8_t RegisterRead(Register reg);
 	void RegisterWrite(Register reg, uint8_t value);
@@ -92,7 +108,13 @@ private:
 	void RegisterSetBits(Register reg, uint8_t setbits);
 	void RegisterClearBits(Register reg, uint8_t clearbits);
 
-	void ResetFIFO();
+	uint16_t FIFOReadCount();
+	bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
+	void FIFOReset();
+
+	bool ProcessAccel(const hrt_abstime &timestamp_sample, const TransferBuffer *const buffer, uint8_t samples);
+	void ProcessGyro(const hrt_abstime &timestamp_sample, const TransferBuffer *const buffer, uint8_t samples);
+	bool ProcessTemperature(const TransferBuffer *const report, uint8_t samples);
 
 	uint8_t *_dma_data_buffer{nullptr};
 
@@ -102,21 +124,30 @@ private:
 	perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")};
+	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
-	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo overflow")};
+	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
-	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo reset")};
+	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
-	perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")};
+	perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval")};
 
-	hrt_abstime _last_config_check{0};
+	hrt_abstime _reset_timestamp{0};
+	hrt_abstime _last_config_check_timestamp{0};
+	hrt_abstime _fifo_watermark_interrupt_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
 
+	px4::atomic<uint8_t> _fifo_read_samples{0};
+	bool _data_ready_interrupt_enabled{false};
 	uint8_t _checked_register{0};
 
-	bool _using_data_ready_interrupt_enabled{false};
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		FIFO_READ,
+		REQUEST_STOP,
+		STOPPED,
+	};
 
-	// Sensor Configuration
+	px4::atomic<STATE> _state{STATE::RESET};
-	static constexpr uint32_t GYRO_RATE{8000};  // 8 kHz gyro
-	static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel
-	static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
 
 	uint16_t _fifo_empty_interval_us{1000}; // 1000 us / 1000 Hz transfer interval
 	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
@@ -135,6 +166,6 @@ private:
 		{ Register::FIFO_WM_TH2,   0, 0 }, // FIFO_WM_TH[7:0]
 		{ Register::USER_CTRL,     USER_CTRL_BIT::FIFO_EN, 0 },
 		{ Register::FIFO_EN,       FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, 0 },
-		{ Register::INT_ENABLE,    INT_ENABLE_BIT::FIFO_OFLOW_EN, INT_ENABLE_BIT::DATA_RDY_INT_EN }
+		{ Register::INT_ENABLE,    0, INT_ENABLE_BIT::DATA_RDY_INT_EN }
 	};
 };

src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp

@@ -54,8 +54,7 @@ static constexpr uint8_t Bit7 = (1 << 7);
 
 namespace InvenSense_ICM20602
 {
-static constexpr uint32_t SPI_SPEED = 10 * 1000 *
+static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
-				      1000; // 10MHz SPI serial interface for communicating with all registers
 static constexpr uint8_t DIR_READ = 0x80;
 
 static constexpr uint8_t WHOAMI = 0x12;

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -58,7 +58,8 @@ public:
 
 	void set_device_id(uint32_t device_id) { _device_id = device_id; }
 	void set_device_type(uint8_t devtype);
-	void set_error_count(uint64_t error_count) { _error_count += error_count; }
+	void set_error_count(uint64_t error_count) { _error_count = error_count; }
+	void increase_error_count() { _error_count++; }
 	void set_range(float range) { _range = range; UpdateClipLimit(); }
 	void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
 	void set_temperature(float temperature) { _temperature = temperature; }

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -60,7 +60,8 @@ public:
 
 	void set_device_id(uint32_t device_id) { _device_id = device_id; }
 	void set_device_type(uint8_t devtype);
-	void set_error_count(uint64_t error_count) { _error_count += error_count; }
+	void set_error_count(uint64_t error_count) { _error_count = error_count; }
+	void increase_error_count() { _error_count++; }
 	void set_range(float range) { _range = range; UpdateClipLimit(); }
 	void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
 	void set_temperature(float temperature) { _temperature = temperature; }