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.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; }