InvenSense ICM-40609-D IMU driver

This is another new InvenSense sensor with max output data rate of 32 kHz (both accel & gyro), on board anti-aliasing filter, and accel range 32G. Overall quite similar to the ICM-42688-P added in #14420, but with doubled accel range and without the optional high resolution output or clock sync.

boards/px4/fmu-v4/default.cmake

@@ -30,6 +30,7 @@ px4_add_board(
 		imu/adis16497
 		imu/invensense/icm20602
 		imu/invensense/icm20608g
+		imu/invensense/icm40609d
 		#imu/invensense/mpu9250
 		#imu/mpu6000 # legacy icm20602/icm20608g driver
 		imu/mpu9250

src/drivers/drv_sensor.h

@@ -78,6 +78,8 @@
 #define DRV_GYR_DEVTYPE_BMI160   0x25
 #define DRV_IMU_DEVTYPE_ICM42688P 0x26
 
+#define DRV_IMU_DEVTYPE_ICM40609D 0x27
+
 #define DRV_RNG_DEVTYPE_MB12XX   0x31
 #define DRV_RNG_DEVTYPE_LL40LS   0x32
 #define DRV_ACC_DEVTYPE_MPU6050  0x33

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

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__imu__invensense__icm40609d
+	MAIN icm40609d
+	COMPILE_FLAGS
+	SRCS
+		icm40609d_main.cpp
+		ICM40609D.cpp
+		ICM40609D.hpp
+		InvenSense_ICM40609D_registers.hpp
+	DEPENDS
+		px4_work_queue
+		drivers_accelerometer
+		drivers_gyroscope
+	)

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

@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ICM40609D.hpp
+ *
+ * Driver for the Invensense ICM40609D connected via SPI.
+ *
+ */
+
+#pragma once
+
+#include "InvenSense_ICM40609D_registers.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/device/spi.h>
+#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_ICM40609D;
+
+class ICM40609D : public device::SPI, public px4::ScheduledWorkItem
+{
+public:
+	ICM40609D(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
+	~ICM40609D() override;
+
+	bool Init();
+	void Start();
+	void Stop();
+	bool Reset();
+	void PrintInfo();
+
+private:
+
+	static constexpr uint32_t GYRO_RATE{8000};  // 8 kHz gyro
+	static constexpr uint32_t ACCEL_RATE{8000}; // 8 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;
+		uint8_t INT_STATUS;
+		uint8_t FIFO_COUNTH;
+		uint8_t FIFO_COUNTL;
+		FIFO::DATA f[FIFO_MAX_SAMPLES];
+	};
+	// ensure no struct padding
+	static_assert(sizeof(TransferBuffer) == (4 * sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+
+	struct register_bank0_config_t {
+		Register::BANK_0 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	int probe() override;
+
+	void Run() 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_bank0_config_t &reg_cfg, bool notify = false);
+	uint8_t RegisterRead(Register::BANK_0 reg);
+	void RegisterWrite(Register::BANK_0 reg, uint8_t value);
+	void RegisterSetAndClearBits(Register::BANK_0 reg, uint8_t setbits, uint8_t clearbits);
+	void RegisterSetBits(Register::BANK_0 reg, uint8_t setbits);
+	void RegisterClearBits(Register::BANK_0 reg, uint8_t clearbits);
+
+	uint16_t FIFOReadCount();
+	bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
+	void FIFOReset();
+
+	void 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);
+	void UpdateTemperature();
+
+	uint8_t *_dma_data_buffer{nullptr};
+
+	PX4Accelerometer _px4_accel;
+	PX4Gyroscope _px4_gyro;
+
+	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_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 _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};
+
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		FIFO_READ,
+		REQUEST_STOP,
+		STOPPED,
+	};
+
+	px4::atomic<STATE> _state{STATE::RESET};
+
+	uint16_t _fifo_empty_interval_us{500}; // default 500 us / 2000 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_bank0{0};
+	static constexpr uint8_t size_register_bank0_cfg{11};
+	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
+		// Register                        | Set bits, Clear bits
+		{ Register::BANK_0::INT_CONFIG,    INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
+		{ Register::BANK_0::FIFO_CONFIG,   FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
+		{ Register::BANK_0::PWR_MGMT0,     PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
+		{ Register::BANK_0::GYRO_CONFIG0,  GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
+		{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
+		{ 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 },
+	};
+
+};

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

@@ -0,0 +1,238 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file InvenSense_ICM40609D_registers.hpp
+ *
+ * Invensense ICM-40609-D registers.
+ *
+ */
+
+#pragma once
+
+#include <cstdint>
+
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+namespace InvenSense_ICM40609D
+{
+static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
+static constexpr uint8_t DIR_READ = 0x80;
+
+static constexpr uint8_t WHOAMI = 0x3B;
+
+static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
+static constexpr float ROOM_TEMPERATURE_OFFSET = 25.f; // C
+
+namespace Register
+{
+
+enum class BANK_0 : uint8_t {
+	DEVICE_CONFIG     = 0x11,
+
+	INT_CONFIG        = 0x14,
+
+	FIFO_CONFIG       = 0x16,
+
+	TEMP_DATA1        = 0x1D,
+	TEMP_DATA0        = 0x1E,
+
+	INT_STATUS        = 0x2D,
+	FIFO_COUNTH       = 0x2E,
+	FIFO_COUNTL       = 0x2F,
+	FIFO_DATA         = 0x30,
+
+	SIGNAL_PATH_RESET = 0x4B,
+
+	PWR_MGMT0         = 0x4E,
+	GYRO_CONFIG0      = 0x4F,
+	ACCEL_CONFIG0     = 0x50,
+
+	FIFO_CONFIG1      = 0x5F,
+	FIFO_CONFIG2      = 0x60,
+	FIFO_CONFIG3      = 0x61,
+
+	INT_CONFIG0       = 0x63,
+
+	INT_SOURCE0       = 0x65,
+
+	WHO_AM_I          = 0x75,
+	REG_BANK_SEL      = 0x76,
+};
+
+};
+
+//---------------- BANK0 Register bits
+
+// DEVICE_CONFIG
+enum DEVICE_CONFIG_BIT : uint8_t {
+	SOFT_RESET_CONFIG = Bit0, //
+};
+
+// INT_CONFIG
+enum INT_CONFIG_BIT : uint8_t {
+	INT1_MODE           = Bit2,
+	INT1_DRIVE_CIRCUIT  = Bit1,
+	INT1_POLARITY       = Bit0,
+};
+
+// FIFO_CONFIG
+enum FIFO_CONFIG_BIT : uint8_t {
+	// 7:6 FIFO_MODE
+	FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
+};
+
+// INT_STATUS
+enum INT_STATUS_BIT : uint8_t {
+	DATA_RDY_INT   = Bit3,
+	FIFO_THS_INT   = Bit2,
+	FIFO_FULL_INT  = Bit1,
+};
+
+// SIGNAL_PATH_RESET
+enum SIGNAL_PATH_RESET_BIT : uint8_t {
+	FIFO_FLUSH      = Bit1,
+};
+
+// PWR_MGMT0
+enum PWR_MGMT0_BIT : uint8_t {
+	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
+	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
+};
+
+// GYRO_CONFIG0
+enum GYRO_CONFIG0_BIT : uint8_t {
+	// 7:5 GYRO_FS_SEL
+	GYRO_FS_SEL_2000_DPS = 0,            // 0b000 = 000: ±2000dps (default)
+	GYRO_FS_SEL_1000_DPS = Bit5,         // 0b001 = ±1000 dps
+	GYRO_FS_SEL_500_DPS  = Bit6,         // 0b010 = ±500 dps
+	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,  // 0b011 = ±250 dps
+	GYRO_FS_SEL_125_DPS  = Bit7,         // 0b100 = ±125 dps
+
+	// 3:0 GYRO_ODR
+	GYRO_ODR_32kHz       = Bit0,         // 0001: 32kHz
+	GYRO_ODR_16kHz       = Bit1,         // 0010: 16kHz
+	GYRO_ODR_8kHz        = Bit1 | Bit0,  // 0011: 8kHz
+	GYRO_ODR_4kHz        = Bit2,         // 0100: 4kHz
+	GYRO_ODR_2kHz        = Bit2 | Bit0,  // 0101: 2kHz
+	GYRO_ODR_1kHz        = Bit2 | Bit1,  // 0110: 1kHz (default)
+};
+
+// ACCEL_CONFIG0
+enum ACCEL_CONFIG0_BIT : uint8_t {
+	// 7:5 ACCEL_FS_SEL
+	ACCEL_FS_SEL_32G = 0,           // 000: ±32g (default)
+	ACCEL_FS_SEL_16G = Bit5,        // 001: ±16g
+	ACCEL_FS_SEL_8G  = Bit6,        // 010: ±8g
+	ACCEL_FS_SEL_4G  = Bit6 | Bit5, // 011: ±4g
+
+	// 3:0 ACCEL_ODR
+	ACCEL_ODR_32kHz  = Bit0,        // 0001: 32kHz
+	ACCEL_ODR_16kHz  = Bit1,        // 0010: 16kHz
+	ACCEL_ODR_8kHz   = Bit1 | Bit0, // 0011: 8kHz
+	ACCEL_ODR_4kHz   = Bit2,        // 0100: 4kHz
+	ACCEL_ODR_2kHz   = Bit2 | Bit0, // 0101: 2kHz
+	ACCEL_ODR_1kHz   = Bit2 | Bit1, // 0110: 1kHz (default)
+};
+
+// FIFO_CONFIG1
+enum FIFO_CONFIG1_BIT : uint8_t {
+	FIFO_RESUME_PARTIAL_RD = Bit6,
+	FIFO_WM_GT_TH          = Bit5,
+
+	FIFO_TEMP_EN           = Bit2,
+	FIFO_GYRO_EN           = Bit1,
+	FIFO_ACCEL_EN          = Bit0,
+};
+
+// INT_CONFIG0
+enum INT_CONFIG0_BIT : uint8_t {
+	// 3:2 FIFO_THS_INT_CLEAR
+	CLEAR_ON_FIFO_READ = Bit3,
+};
+
+// INT_SOURCE0
+enum INT_SOURCE0_BIT : uint8_t {
+	UI_DRDY_INT1_EN    = Bit3,
+	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
+	FIFO_FULL_INT1_EN  = Bit1,
+};
+
+namespace FIFO
+{
+static constexpr size_t SIZE = 2048;
+
+// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
+
+// Packet 3
+struct DATA {
+	uint8_t FIFO_Header;
+	uint8_t ACCEL_DATA_X1;
+	uint8_t ACCEL_DATA_X0;
+	uint8_t ACCEL_DATA_Y1;
+	uint8_t ACCEL_DATA_Y0;
+	uint8_t ACCEL_DATA_Z1;
+	uint8_t ACCEL_DATA_Z0;
+	uint8_t GYRO_DATA_X1;
+	uint8_t GYRO_DATA_X0;
+	uint8_t GYRO_DATA_Y1;
+	uint8_t GYRO_DATA_Y0;
+	uint8_t GYRO_DATA_Z1;
+	uint8_t GYRO_DATA_Z0;
+	uint8_t temperature;  // Temperature[7:0]
+	uint8_t timestamp_l;
+	uint8_t timestamp_h;
+};
+static_assert(sizeof(DATA) == 16, "FIFO packet 16 bytes");
+
+// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+enum FIFO_HEADER_BIT : uint8_t {
+	HEADER_MSG             = Bit7, // 1: FIFO is empty
+	HEADER_ACCEL           = Bit6,
+	HEADER_GYRO            = Bit5,
+
+	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
+	HEADER_ODR_ACCEL       = Bit1,
+	HEADER_ODR_GYRO        = Bit0,
+};
+
+}
+} // namespace InvenSense_ICM40609D

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

@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ICM40609D.hpp"
+
+#include <px4_platform_common/getopt.h>
+
+namespace icm40609d
+{
+ICM40609D *g_dev{nullptr};
+
+static int start(enum Rotation rotation)
+{
+	if (g_dev != nullptr) {
+		PX4_WARN("already started");
+		return 0;
+	}
+
+	// create the driver
+#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_ICM_40609D)
+	g_dev = new ICM40609D(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ICM_40609D, rotation);
+#endif
+
+	if (g_dev == nullptr) {
+		PX4_ERR("driver start failed");
+		return -1;
+	}
+
+	if (!g_dev->Init()) {
+		PX4_ERR("driver init failed");
+		delete g_dev;
+		g_dev = nullptr;
+		return -1;
+	}
+
+	return 0;
+}
+
+static int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_WARN("driver not running");
+		return -1;
+	}
+
+	g_dev->Stop();
+	delete g_dev;
+	g_dev = nullptr;
+
+	return 0;
+}
+
+static int reset()
+{
+	if (g_dev == nullptr) {
+		PX4_WARN("driver not running");
+		return 0;
+	}
+
+	return g_dev->Reset();
+}
+
+static int status()
+{
+	if (g_dev == nullptr) {
+		PX4_INFO("driver not running");
+		return 0;
+	}
+
+	g_dev->PrintInfo();
+
+	return 0;
+}
+
+static int usage()
+{
+	PX4_INFO("missing command: try 'start', 'stop', 'reset', 'status'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+
+	return 0;
+}
+
+} // namespace icm40609d
+
+extern "C" int icm40609d_main(int argc, char *argv[])
+{
+	enum Rotation rotation = ROTATION_NONE;
+	int myoptind = 1;
+	int ch = 0;
+	const char *myoptarg = nullptr;
+
+	// start options
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		default:
+			return icm40609d::usage();
+		}
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		return icm40609d::start(rotation);
+
+	} else if (!strcmp(verb, "stop")) {
+		return icm40609d::stop();
+
+	} else if (!strcmp(verb, "status")) {
+		return icm40609d::status();
+
+	} else if (!strcmp(verb, "reset")) {
+		return icm40609d::reset();
+	}
+
+	return icm40609d::usage();
+}