diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index adcea0fbf7..1585417891 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/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) : - SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +MPU6000::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) : + 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) { - PX4_ERR("SPI::init failed"); - return false; + if (_drdy_gpio != 0) { + // Disable data ready callback + 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"); - return false; + DEVICE_DEBUG("reset failed"); + 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 defined(GPIO_SPI1_EXTI_MPU_DRDY) - // Setup data ready on rising edge - px4_arch_gpiosetevent(GPIO_SPI1_EXTI_MPU_DRDY, true, false, true, &MPU6000::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); -#else - ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL); -#endif + if (_drdy_gpio == 0) { + ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL); + + } else { + // Setup data ready on rising edge + px4_arch_gpiosetevent(_drdy_gpio, true, false, true, &MPU6000::DataReadyInterruptCallback, this); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + } + } -void MPU6000::Stop() -{ - // 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() +void MPU6000::RunImpl() { // 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); diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp index 22d0cc3817..91f08101b1 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp @@ -48,22 +48,32 @@ #include #include #include -#include +#include using InvenSense_MPU6000::Register; -class MPU6000 : public device::SPI, public px4::ScheduledWorkItem +class MPU6000 : public device::SPI, public I2CSPIDriver { 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(); - void Start(); - void Stop(); - bool Reset(); - void PrintInfo(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + 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; diff --git a/src/drivers/imu/invensense/mpu6000/mpu6000_main.cpp b/src/drivers/imu/invensense/mpu6000/mpu6000_main.cpp index fc05c025e0..495ae4f538 100644 --- a/src/drivers/imu/invensense/mpu6000/mpu6000_main.cpp +++ b/src/drivers/imu/invensense/mpu6000/mpu6000_main.cpp @@ -34,120 +34,84 @@ #include "MPU6000.hpp" #include +#include -namespace mpu6000 +void +MPU6000::print_usage() { -MPU6000 *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_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; + PRINT_MODULE_USAGE_NAME("mpu6000", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_COMMAND("reset"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -static int stop() +I2CSPIDriverBase *MPU6000::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - if (g_dev == nullptr) { - PX4_WARN("driver not running"); - return -1; + MPU6000 *instance = new MPU6000(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; } - 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; + if (OK != instance->init()) { + delete instance; + return nullptr; } - return g_dev->Reset(); + return instance; } -static int status() +void MPU6000::custom_method(const BusCLIArguments &cli) { - if (g_dev == nullptr) { - PX4_INFO("driver not running"); - return 0; - } - - g_dev->PrintInfo(); - - return 0; + Reset(); } -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 myoptind = 1; - int ch = 0; - const char *myoptarg = nullptr; + int ch; + using ThisDriver = MPU6000; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = InvenSense_MPU6000::SPI_SPEED; - // start options - while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "R:")) != 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")) { - return mpu6000::start(rotation); - - } else if (!strcmp(verb, "stop")) { - return mpu6000::stop(); - - } else if (!strcmp(verb, "status")) { - return mpu6000::status(); - - } else if (!strcmp(verb, "reset")) { - return mpu6000::reset(); + if (!verb) { + ThisDriver::print_usage(); + return -1; } - 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; }