refactor mpu6000: use driver base class

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

@@ -46,9 +46,11 @@ static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval
 static constexpr uint32_t FIFO_GYRO_SAMPLES{FIFO_INTERVAL / (1000000 / GYRO_RATE)};
 static constexpr uint32_t FIFO_ACCEL_SAMPLES{FIFO_INTERVAL / (1000000 / ACCEL_RATE)};
 
-MPU6000::MPU6000(int bus, uint32_t device, enum Rotation rotation) :
+MPU6000::MPU6000(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)
 {
@@ -62,8 +64,6 @@ MPU6000::MPU6000(int bus, uint32_t device, enum Rotation rotation) :
 
 MPU6000::~MPU6000()
 {
-	Stop();
-
 	perf_free(_transfer_perf);
 	perf_free(_fifo_empty_perf);
 	perf_free(_fifo_overflow_perf);
@@ -76,28 +76,41 @@ int MPU6000::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;
 }
 
-bool MPU6000::Init()
+void MPU6000::exit_and_cleanup()
 {
-	if (SPI::init() != PX4_OK) {
+	if (_drdy_gpio != 0) {
-		PX4_ERR("SPI::init failed");
+		// Disable data ready callback
-		return false;
+		px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr);
+		RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN);
+	}
+
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+int MPU6000::init()
+{
+	int ret = SPI::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("SPI::init failed (%i)", ret);
+		return ret;
 	}
 
 	if (!Reset()) {
-		PX4_ERR("reset failed");
+		DEVICE_DEBUG("reset failed");
-		return false;
+		return PX4_ERROR;
 	}
 
 	Start();
 
-	return true;
+	return PX4_OK;
 }
 
 bool MPU6000::Reset()
@@ -211,33 +224,20 @@ void MPU6000::DataReady()
 
 void MPU6000::Start()
 {
-	Stop();
-
 	ResetFIFO();
 
-	// TODO: cleanup horrible DRDY define mess
+	if (_drdy_gpio == 0) {
-#if defined(GPIO_SPI1_EXTI_MPU_DRDY)
+		ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL);
-	// Setup data ready on rising edge
+
-	px4_arch_gpiosetevent(GPIO_SPI1_EXTI_MPU_DRDY, true, false, true, &MPU6000::DataReadyInterruptCallback, this);
+	} else {
-	RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN);
+		// Setup data ready on rising edge
-#else
+		px4_arch_gpiosetevent(_drdy_gpio, true, false, true, &MPU6000::DataReadyInterruptCallback, this);
-	ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL);
+		RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN);
-#endif
+	}
+
 }
 
-void MPU6000::Stop()
+void MPU6000::RunImpl()
-{
-	// TODO: cleanup horrible DRDY define mess
-#if defined(GPIO_SPI1_EXTI_MPU_DRDY)
-	// Disable data ready callback
-	px4_arch_gpiosetevent(GPIO_SPI1_EXTI_MPU_DRDY, false, false, false, nullptr, nullptr);
-	RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN);
-#else
-	ScheduleClear();
-#endif
-}
-
-void MPU6000::Run()
 {
 	// use timestamp from the data ready interrupt if available,
 	//  otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO
@@ -338,8 +338,9 @@ void MPU6000::Run()
 	_px4_accel.updateFIFO(accel);
 }
 
-void MPU6000::PrintInfo()
+void MPU6000::print_status()
 {
+	I2CSPIDriverBase::print_status();
 	perf_print_counter(_transfer_perf);
 	perf_print_counter(_fifo_empty_perf);
 	perf_print_counter(_fifo_overflow_perf);

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

@@ -48,22 +48,32 @@
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/ecl/geo/geo.h>
 #include <lib/perf/perf_counter.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <px4_platform_common/i2c_spi_buses.h>
 
 using InvenSense_MPU6000::Register;
 
-class MPU6000 : public device::SPI, public px4::ScheduledWorkItem
+class MPU6000 : public device::SPI, public I2CSPIDriver<MPU6000>
 {
 public:
-	MPU6000(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
+	MPU6000(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);
 	~MPU6000() 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:
 
 	// Transfer data
@@ -79,8 +89,6 @@ private:
 	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
 	void DataReady();
 
-	void Run() override;
-
 	uint8_t RegisterRead(Register reg);
 	void RegisterWrite(Register reg, uint8_t value);
 	void RegisterSetBits(Register reg, uint8_t setbits);
@@ -88,6 +96,8 @@ private:
 
 	void ResetFIFO();
 
+	const spi_drdy_gpio_t _drdy_gpio;
+
 	PX4Accelerometer _px4_accel;
 	PX4Gyroscope _px4_gyro;
 

src/drivers/imu/invensense/mpu6000/mpu6000_main.cpp

@@ -34,120 +34,84 @@
 #include "MPU6000.hpp"
 
 #include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
 
-namespace mpu6000
+void
+MPU6000::print_usage()
 {
-MPU6000 *g_dev{nullptr};
+	PRINT_MODULE_USAGE_NAME("mpu6000", "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) && defined(PX4_SPIDEV_MPU)
-	g_dev = new MPU6000(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, rotation);
-#elif defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU)
-	g_dev = new MPU6000(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, 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 *MPU6000::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+				       int runtime_instance)
 {
-	if (g_dev == nullptr) {
+	MPU6000 *instance = new MPU6000(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 MPU6000::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 mpu6000
-
 extern "C" int mpu6000_main(int argc, char *argv[])
 {
-	enum Rotation rotation = ROTATION_NONE;
+	int ch;
-	int myoptind = 1;
+	using ThisDriver = MPU6000;
-	int ch = 0;
+	BusCLIArguments cli{false, true};
-	const char *myoptarg = nullptr;
+	cli.default_spi_frequency = InvenSense_MPU6000::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 mpu6000::usage();
 		}
 	}
 
-	const char *verb = argv[myoptind];
+	const char *verb = cli.optarg();
 
-	if (!strcmp(verb, "start")) {
+	if (!verb) {
-		return mpu6000::start(rotation);
+		ThisDriver::print_usage();
-
+		return -1;
-	} else if (!strcmp(verb, "stop")) {
-		return mpu6000::stop();
-
-	} else if (!strcmp(verb, "status")) {
-		return mpu6000::status();
-
-	} else if (!strcmp(verb, "reset")) {
-		return mpu6000::reset();
 	}
 
-	return mpu6000::usage();
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_MPU6000);
+
+	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;
 }