refactor icm20602: use driver base class

boards/omnibus/f4sd/init/rc.board_sensors

@@ -8,7 +8,7 @@ adc start
 if ! mpu6000 -R 12 -s start
 then
 	# some boards such as the Hobbywing XRotor F4 G2 use the ICM-20602
-	icm20602 -R 6 start
+	icm20602 -s -R 6 start
 	#mpu6000 -R 12 -T 20602 -s start
 fi
 

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

@@ -43,7 +43,7 @@ fi
 # will prevent the incorrect driver from a successful initialization.
 
 # ICM20602 internal SPI bus ICM-20608-G is rotated 90 deg yaw
-if ! icm20602 -R 8 start
+if ! icm20602 -s -R 8 start
 then
 	# ICM20608 internal SPI bus ICM-20602-G is rotated 90 deg yaw
 	icm20608g -R 8 start

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

@@ -11,7 +11,7 @@ adc start
 icm20608g -R 8 start
 
 # Internal SPI bus ICM-20602
-icm20602 -R 8 start
+icm20602 -s -R 8 start
 
 # Internal SPI bus mpu9250
 mpu9250 -R 2 start

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

@@ -7,14 +7,14 @@ adc start
 
 # Internal SPI bus ICM-20602
 mpu6000 -R 8 -s -T 20602 start
-#icm20602 -R 6 start
+#icm20602 -s -R 6 start
 
 # Internal SPI bus ICM-20689
 mpu6000 -R 8 -s -T 20689 start
 #icm20689 -R 6 start
 
 # new sensor drivers (in testing)
-#icm20602 start
+#icm20602 -s start
 #icm20689 start
 
 # Internal SPI bus BMI055 accel

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

@@ -40,13 +40,16 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
 	return (msb << 8u) | lsb;
 }
 
-ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) :
+ICM20602::ICM20602(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
-	SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
+		   spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) :
-	ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
+	SPI(MODULE_NAME, nullptr, bus, device, spi_mode, bus_frequency),
+	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
+	_drdy_gpio(drdy_gpio),
 	_px4_accel(get_device_id(), ORB_PRIO_VERY_HIGH, rotation),
 	_px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation)
 {
 	set_device_type(DRV_IMU_DEVTYPE_ICM20602);
+
 	_px4_accel.set_device_type(DRV_IMU_DEVTYPE_ICM20602);
 	_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ICM20602);
 
@@ -55,8 +58,6 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) :
 
 ICM20602::~ICM20602()
 {
-	Stop();
-
 	perf_free(_transfer_perf);
 	perf_free(_bad_register_perf);
 	perf_free(_bad_transfer_perf);
@@ -66,36 +67,35 @@ ICM20602::~ICM20602()
 	perf_free(_drdy_interval_perf);
 }
 
-bool ICM20602::Init()
+int ICM20602::init()
 {
-	if (SPI::init() != PX4_OK) {
+	int ret = SPI::init();
-		PX4_ERR("SPI::init failed");
+
-		return false;
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("SPI::init failed (%i)", ret);
+		return ret;
 	}
 
-	return Reset();
+	return Reset() ? 0 : -1;
-}
-
-void ICM20602::Stop()
-{
-	// wait until stopped
-	while (_state.load() != STATE::STOPPED) {
-		_state.store(STATE::REQUEST_STOP);
-		ScheduleNow();
-		px4_usleep(10);
-	}
 }
 
 bool ICM20602::Reset()
 {
-	_state.store(STATE::RESET);
+	_state = STATE::RESET;
 	ScheduleClear();
 	ScheduleNow();
 	return true;
 }
 
-void ICM20602::PrintInfo()
+void ICM20602::exit_and_cleanup()
 {
+	DataReadyInterruptDisable();
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void ICM20602::print_status()
+{
+	I2CSPIDriverBase::print_status();
 	PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
 		 static_cast<double>(1000000 / _fifo_empty_interval_us));
 
@@ -116,21 +116,21 @@ int ICM20602::probe()
 	const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
 
 	if (whoami != WHOAMI) {
-		PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami);
+		DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
 		return PX4_ERROR;
 	}
 
 	return PX4_OK;
 }
 
-void ICM20602::Run()
+void ICM20602::RunImpl()
 {
-	switch (_state.load()) {
+	switch (_state) {
 	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);
+		_state = STATE::WAIT_FOR_RESET;
 		ScheduleDelayed(100);
 		break;
 
@@ -143,14 +143,14 @@ void ICM20602::Run()
 		    && (RegisterRead(Register::CONFIG) == 0x80)) {
 
 			// if reset succeeded then configure
-			_state.store(STATE::CONFIGURE);
+			_state = STATE::CONFIGURE;
 			ScheduleNow();
 
 		} else {
 			// RESET not complete
 			if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) {
 				PX4_ERR("Reset failed, retrying");
-				_state.store(STATE::RESET);
+				_state = STATE::RESET;
 				ScheduleDelayed(10_ms);
 
 			} else {
@@ -164,7 +164,7 @@ void ICM20602::Run()
 	case STATE::CONFIGURE:
 		if (Configure()) {
 			// if configure succeeded then start reading from FIFO
-			_state.store(STATE::FIFO_READ);
+			_state = STATE::FIFO_READ;
 
 			if (DataReadyInterruptConfigure()) {
 				_data_ready_interrupt_enabled = true;
@@ -242,23 +242,13 @@ void ICM20602::Run()
 				} else {
 					// register check failed, force reconfigure
 					PX4_DEBUG("Health check failed, reconfiguring");
-					_state.store(STATE::CONFIGURE);
+					_state = STATE::CONFIGURE;
 					ScheduleNow();
 				}
 			}
 		}
 
 		break;
-
-	case STATE::REQUEST_STOP:
-		DataReadyInterruptDisable();
-		ScheduleClear();
-		_state.store(STATE::STOPPED);
-		break;
-
-	case STATE::STOPPED:
-		// DO NOTHING
-		break;
 	}
 }
 
@@ -378,49 +368,21 @@ void ICM20602::DataReady()
 
 bool ICM20602::DataReadyInterruptConfigure()
 {
-	int ret_setevent = -1;
+	if (_drdy_gpio == 0) {
+		return false;
+	}
 
 	// Setup data ready on rising edge
-	// TODO: cleanup horrible DRDY define mess
+	return px4_arch_gpiosetevent(_drdy_gpio, true, false, true, &ICM20602::DataReadyInterruptCallback, this) == 0;
-#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;
+	if (_drdy_gpio == 0) {
+		return false;
+	}
 
-	// Disable data ready callback
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
-	// 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)

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

@@ -49,22 +49,32 @@
 #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>
+#include <px4_platform_common/i2c_spi_buses.h>
 
 using namespace InvenSense_ICM20602;
 
-class ICM20602 : public device::SPI, public px4::ScheduledWorkItem
+class ICM20602 : public device::SPI, public I2CSPIDriver<ICM20602>
 {
 public:
-	ICM20602(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
+	ICM20602(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
+		 spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
 	~ICM20602() override;
 
-	bool Init();
+	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-	void Start();
+					     int runtime_instance);
-	void Stop();
+	static void print_usage();
-	bool Reset();
-	void PrintInfo();
 
+	void RunImpl();
+
+	int init() override;
+	void print_status() override;
+
+	void Start();
+	bool Reset();
+
+protected:
+	void custom_method(const BusCLIArguments &cli) override;
+	void exit_and_cleanup() override;
 private:
 
 	// Sensor Configuration
@@ -88,8 +98,6 @@ private:
 
 	int probe() override;
 
-	void Run() override;
-
 	bool Configure();
 	void ConfigureAccel();
 	void ConfigureGyro();
@@ -116,6 +124,8 @@ private:
 	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
 	bool ProcessTemperature(const FIFOTransferBuffer &buffer, uint8_t samples);
 
+	const spi_drdy_gpio_t _drdy_gpio;
+
 	PX4Accelerometer _px4_accel;
 	PX4Gyroscope _px4_gyro;
 
@@ -141,11 +151,9 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-		REQUEST_STOP,
-		STOPPED,
 	};
 
-	px4::atomic<STATE> _state{STATE::RESET};
+	STATE _state{STATE::RESET};
 
 	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))};

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

@@ -34,120 +34,84 @@
 #include "ICM20602.hpp"
 
 #include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
 
-namespace icm20602
+void
+ICM20602::print_usage()
 {
-ICM20602 *g_dev{nullptr};
+	PRINT_MODULE_USAGE_NAME("icm20602", "driver");
-
+	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
-static int start(enum Rotation rotation)
+	PRINT_MODULE_USAGE_COMMAND("start");
-{
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
-	if (g_dev != nullptr) {
+	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
-		PX4_WARN("already started");
+	PRINT_MODULE_USAGE_COMMAND("reset");
-		return 0;
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-	}
-
-	// create the driver
-#if defined(PX4_SPI_BUS_SENSORS)
-	g_dev = new ICM20602(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ICM_20602, rotation);
-#elif defined(PX4_SPI_BUS_SENSORS1)
-	g_dev = new ICM20602(PX4_SPI_BUS_SENSORS1, PX4_SPIDEV_ICM_20602, 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()
+I2CSPIDriverBase *ICM20602::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+					int runtime_instance)
 {
-	if (g_dev == nullptr) {
+	ICM20602 *instance = new ICM20602(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
-		PX4_WARN("driver not running");
+					  cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
-		return -1;
+
+	if (!instance) {
+		PX4_ERR("alloc failed");
+		return nullptr;
 	}
 
-	g_dev->Stop();
+	if (OK != instance->init()) {
-	delete g_dev;
+		delete instance;
-	g_dev = nullptr;
+		return nullptr;
-
-	return 0;
-}
-
-static int reset()
-{
-	if (g_dev == nullptr) {
-		PX4_WARN("driver not running");
-		return 0;
 	}
 
-	return g_dev->Reset();
+	return instance;
 }
 
-static int status()
+void ICM20602::custom_method(const BusCLIArguments &cli)
 {
-	if (g_dev == nullptr) {
+	Reset();
-		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 icm20602
-
 extern "C" int icm20602_main(int argc, char *argv[])
 {
-	enum Rotation rotation = ROTATION_NONE;
+	int ch;
-	int myoptind = 1;
+	using ThisDriver = ICM20602;
-	int ch = 0;
+	BusCLIArguments cli{false, true};
-	const char *myoptarg = nullptr;
+	cli.default_spi_frequency = SPI_SPEED;
 
-	// start options
+	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
-	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'R':
-			rotation = (enum Rotation)atoi(myoptarg);
+			cli.rotation = (enum Rotation)atoi(cli.optarg());
 			break;
-
-		default:
-			return icm20602::usage();
 		}
 	}
 
-	const char *verb = argv[myoptind];
+	const char *verb = cli.optarg();
 
-	if (!strcmp(verb, "start")) {
+	if (!verb) {
-		return icm20602::start(rotation);
+		ThisDriver::print_usage();
-
+		return -1;
-	} else if (!strcmp(verb, "stop")) {
-		return icm20602::stop();
-
-	} else if (!strcmp(verb, "status")) {
-		return icm20602::status();
-
-	} else if (!strcmp(verb, "reset")) {
-		return icm20602::reset();
 	}
 
-	return icm20602::usage();
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM20602);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	if (!strcmp(verb, "reset")) {
+		return ThisDriver::module_custom_method(cli, iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
 }