invensense/mpu6000 refactor to be consistent with new icm20602, icm20608g, etc

- always check FIFO count before transfer even with data ready
 - interupt pin set active low and latch
 - relax retry timeout if configure failed
 - improve configured empty rate (sample rate) rounding
 - check FIFO count as part of full transfer and reset or adjust timing if necessary

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

@@ -39,6 +39,7 @@ px4_add_module(
 		MPU6000.cpp
 		MPU6000.hpp
 		mpu6000_main.cpp
+		InvenSense_MPU6000_registers.hpp
 	DEPENDS
 		drivers_accelerometer
 		drivers_gyroscope

src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp

@@ -40,6 +40,8 @@
 
 #pragma once
 
+#include <cstdint>
+
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
 static constexpr uint8_t Bit1 = (1 << 1);
@@ -52,25 +54,33 @@ static constexpr uint8_t Bit7 = (1 << 7);
 
 namespace InvenSense_MPU6000
 {
-static constexpr uint32_t SPI_SPEED = 20 * 1000 * 1000;
+static constexpr uint32_t SPI_SPEED = 1 * 1000 * 1000;
+static constexpr uint32_t SPI_SPEED_SENSOR = 10 * 1000 * 1000; // 20MHz for reading sensor and interrupt registers
 static constexpr uint8_t DIR_READ = 0x80;
 
 static constexpr uint8_t WHOAMI = 0x68;
 
+static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C
+static constexpr float ROOM_TEMPERATURE_OFFSET = 25.f; // C
+
 enum class Register : uint8_t {
 	CONFIG        = 0x1A,
 	GYRO_CONFIG   = 0x1B,
 	ACCEL_CONFIG  = 0x1C,
 
 	FIFO_EN       = 0x23,
+	I2C_MST_CTRL  = 0x24,
+
+	INT_PIN_CFG   = 0x37,
+	INT_ENABLE    = 0x38,
 
 	INT_STATUS    = 0x3A,
 
-	INT_ENABLE    = 0x38,
-
 	TEMP_OUT_H    = 0x41,
 	TEMP_OUT_L    = 0x42,
 
+	SIGNAL_PATH_RESET = 0x68,
+
 	USER_CTRL     = 0x6A,
 	PWR_MGMT_1    = 0x6B,
 
@@ -112,6 +122,14 @@ enum FIFO_EN_BIT : uint8_t {
 	ACCEL_FIFO_EN = Bit3,
 };
 
+// INT_PIN_CFG
+enum INT_PIN_CFG_BIT : uint8_t {
+	INT_LEVEL    = Bit7,
+
+	INT_RD_CLEAR = Bit4,
+
+};
+
 // INT_ENABLE
 enum INT_ENABLE_BIT : uint8_t {
 	FIFO_OFLOW_EN   = Bit4,
@@ -124,22 +142,34 @@ enum INT_STATUS_BIT : uint8_t {
 	DATA_RDY_INT   = Bit0,
 };
 
+// SIGNAL_PATH_RESET
+enum SIGNAL_PATH_RESET_BIT : uint8_t {
+	GYRO_RESET  = Bit2,
+	ACCEL_RESET = Bit1,
+	TEMP_RESET  = Bit0,
+};
+
 // USER_CTRL
 enum USER_CTRL_BIT : uint8_t {
 	FIFO_EN        = Bit6,
+	I2C_MST_EN     = Bit5,
+	I2C_IF_DIS     = Bit4,
+
 	FIFO_RESET     = Bit2,
+
+	SIG_COND_RESET = Bit0,
 };
 
 // PWR_MGMT_1
 enum PWR_MGMT_1_BIT : uint8_t {
 	DEVICE_RESET = Bit7,
+	SLEEP        = Bit6,
 
 	CLKSEL_2     = Bit2,
 	CLKSEL_1     = Bit1,
 	CLKSEL_0     = Bit0,
 };
 
-
 namespace FIFO
 {
 static constexpr size_t SIZE = 1024;

src/drivers/imu/invensense/mpu6000/MPU6000.hpp

@@ -48,9 +48,10 @@
 #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/i2c_spi_buses.h>
 
-using InvenSense_MPU6000::Register;
+using namespace InvenSense_MPU6000;
 
 class MPU6000 : public device::SPI, public I2CSPIDriver<MPU6000>
 {
@@ -75,6 +76,13 @@ protected:
 	void custom_method(const BusCLIArguments &cli) override;
 	void exit_and_cleanup() override;
 private:
+	// Sensor Configuration
+	static constexpr float FIFO_SAMPLE_DT{125.f};
+	static constexpr uint32_t SAMPLES_PER_TRANSFER{8};       // 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 / 8.f};      // 1 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]))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -82,19 +90,41 @@ private:
 		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
 	};
 	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+
+	struct register_config_t {
+		Register reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
 
 	int probe() override;
 
+	bool Configure();
+	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);
-	void RegisterSetBits(Register reg, uint8_t setbits);
+	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
-	void RegisterClearBits(Register reg, uint8_t clearbits);
+	void RegisterSetBits(Register reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
+	void RegisterClearBits(Register reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, 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 FIFOTransferBuffer &buffer, const uint8_t samples);
+	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
+	void UpdateTemperature();
 
 	const spi_drdy_gpio_t _drdy_gpio;
 
@@ -102,12 +132,45 @@ private:
 	PX4Gyroscope _px4_gyro;
 
 	perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
-	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")};
+	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
-	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo overflow")};
+	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
-	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo reset")};
+	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
-	perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")};
+	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 _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval")};
 
-	hrt_abstime _time_data_ready{0};
+	hrt_abstime _reset_timestamp{0};
-	hrt_abstime _time_last_temperature_update{0};
+	hrt_abstime _last_config_check_timestamp{0};
-	int _data_ready_count{0};
+	hrt_abstime _fifo_watermark_interrupt_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
+
+	px4::atomic<uint8_t> _data_ready_count{0};
+	px4::atomic<uint8_t> _fifo_read_samples{0};
+	bool _data_ready_interrupt_enabled{false};
+
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		FIFO_READ,
+	};
+
+	STATE _state{STATE::RESET};
+
+	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
+	uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
+
+	uint8_t _checked_register{0};
+	static constexpr uint8_t size_register_cfg{7};
+	register_config_t _register_cfg[size_register_cfg] {
+		// Register               | Set bits, Clear bits
+		{ Register::PWR_MGMT_1,    PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP },
+		{ Register::ACCEL_CONFIG,  ACCEL_CONFIG_BIT::AFS_SEL_16G, 0 },
+		{ Register::GYRO_CONFIG,   GYRO_CONFIG_BIT::FS_SEL_2000_DPS, 0 },
+		{ Register::USER_CTRL,     USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, USER_CTRL_BIT::I2C_MST_EN},
+		{ Register::FIFO_EN,       FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, FIFO_EN_BIT::TEMP_FIFO_EN },
+		{ Register::INT_PIN_CFG,   INT_PIN_CFG_BIT::INT_LEVEL, 0 },
+		{ Register::INT_ENABLE,    INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 }
+	};
 };