From d15fa828417178737b35432bf02e589b9965c77e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 17 Mar 2020 18:02:05 +0100 Subject: [PATCH] refactor adis16448: use driver base class --- src/drivers/drv_sensor.h | 4 +- src/drivers/imu/adis16448/ADIS16448.cpp | 34 ++--- src/drivers/imu/adis16448/ADIS16448.h | 36 ++---- src/drivers/imu/adis16448/adis16448_main.cpp | 129 +++++++------------ 4 files changed, 72 insertions(+), 131 deletions(-) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index f40b6381a8..a9539f130a 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -112,9 +112,7 @@ #define DRV_ACC_DEVTYPE_FXOS8701C 0x52 #define DRV_GYR_DEVTYPE_FXAS2100C 0x54 -#define DRV_ACC_DEVTYPE_ADIS16448 0x55 -#define DRV_MAG_DEVTYPE_ADIS16448 0x56 -#define DRV_GYR_DEVTYPE_ADIS16448 0x57 +#define DRV_IMU_DEVTYPE_ADIS16448 0x57 #define DRV_BARO_DEVTYPE_LPS22HB 0x58 #define DRV_ACC_DEVTYPE_ADIS16477 0x59 diff --git a/src/drivers/imu/adis16448/ADIS16448.cpp b/src/drivers/imu/adis16448/ADIS16448.cpp index 0467fcf9b2..e0b7c3c19d 100644 --- a/src/drivers/imu/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/adis16448/ADIS16448.cpp @@ -37,18 +37,19 @@ #include "ADIS16448.h" -ADIS16448::ADIS16448(int bus, uint32_t device, enum Rotation rotation) : - SPI("ADIS16448", nullptr, bus, device, SPIDEV_MODE3, 1000000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode) : + SPI("ADIS16448", nullptr, bus, device, spi_mode, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), _px4_baro(get_device_id(), ORB_PRIO_MAX), _px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation), _px4_mag(get_device_id(), ORB_PRIO_MAX, rotation) { - _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16448); - _px4_baro.set_device_type(DRV_ACC_DEVTYPE_ADIS16448); // not a typo, baro uses accel device id - _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16448); - _px4_mag.set_device_type(DRV_MAG_DEVTYPE_ADIS16448); + _px4_accel.set_device_type(DRV_IMU_DEVTYPE_ADIS16448); + _px4_baro.set_device_type(DRV_IMU_DEVTYPE_ADIS16448); + _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ADIS16448); + _px4_mag.set_device_type(DRV_IMU_DEVTYPE_ADIS16448); _px4_accel.set_scale(ADIS16448_ACCEL_SENSITIVITY); _px4_gyro.set_scale(ADIS16448_GYRO_INITIAL_SENSITIVITY); @@ -59,9 +60,6 @@ ADIS16448::ADIS16448(int bus, uint32_t device, enum Rotation rotation) : ADIS16448::~ADIS16448() { - // Ensure the driver is inactive. - stop(); - // Delete the perf counter. perf_free(_perf_read); perf_free(_perf_transfer); @@ -209,7 +207,7 @@ ADIS16448::probe() } if (!reset_success) { - PX4_ERR("unable to successfully reset"); + DEVICE_DEBUG("unable to successfully reset"); return PX4_ERROR; } @@ -307,8 +305,9 @@ ADIS16448::set_gyro_dyn_range(uint16_t desired_gyro_dyn_range) } void -ADIS16448::print_info() +ADIS16448::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_perf_read); perf_print_counter(_perf_transfer); perf_print_counter(_perf_bad_transfer); @@ -361,19 +360,10 @@ ADIS16448::write_reg16(unsigned reg, uint16_t value) void ADIS16448::start() { - // Make sure we are stopped first. - stop(); - // Start polling at the specified interval ScheduleOnInterval((1_s / _sample_rate), 10000); } -void -ADIS16448::stop() -{ - ScheduleClear(); -} - // computes the CCITT CRC16 on the data received from a burst read static uint16_t ComputeCRC16(uint16_t burstData[13]) { @@ -527,7 +517,7 @@ ADIS16448::measure() } void -ADIS16448::Run() +ADIS16448::RunImpl() { // Make another measurement. measure(); diff --git a/src/drivers/imu/adis16448/ADIS16448.h b/src/drivers/imu/adis16448/ADIS16448.h index 8e9378a828..0b0b167996 100644 --- a/src/drivers/imu/adis16448/ADIS16448.h +++ b/src/drivers/imu/adis16448/ADIS16448.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include using namespace time_literals; @@ -98,21 +98,24 @@ static constexpr float ADIS16448_MAG_SENSITIVITY{1.0 / 7.0 / 1000.0}; // 7 static constexpr float ADIS16448_ACCEL_GYRO_UPDATE_RATE{819.2}; // accel and gryo max update 819.2 samples per second static constexpr float ADIS16448_MAG_BARO_UPDATE_RATE{51.2}; // xMAGN_OUT and BARO_OUT registers update at 51.2 samples per second -class ADIS16448 : public device::SPI, public px4::ScheduledWorkItem +class ADIS16448 : public device::SPI, public I2CSPIDriver { public: - ADIS16448(int bus, uint32_t device, enum Rotation rotation); + ADIS16448(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode); virtual ~ADIS16448(); - virtual int init(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); - /** - * Diagnostics - print some basic information about the driver and sensor. - */ - void print_info(); + int init() override; + void print_status() override; + + void RunImpl(); protected: - virtual int probe(); + int probe() override; private: @@ -135,16 +138,6 @@ private: */ int measure(); - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - void Run(); - /** * Modify a register in the ADIS16448 * Bits are cleared before bits are set. @@ -196,11 +189,6 @@ private: */ void start(); - /** - * Stop automatic measurement. - */ - void stop(); - PX4Accelerometer _px4_accel; PX4Barometer _px4_baro; PX4Gyroscope _px4_gyro; diff --git a/src/drivers/imu/adis16448/adis16448_main.cpp b/src/drivers/imu/adis16448/adis16448_main.cpp index 44d9865649..fbf3cdfc66 100644 --- a/src/drivers/imu/adis16448/adis16448_main.cpp +++ b/src/drivers/imu/adis16448/adis16448_main.cpp @@ -34,109 +34,74 @@ #include "ADIS16448.h" #include +#include -namespace adis16448 +void +ADIS16448::print_usage() { -ADIS16448 *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_EXT) - g_dev = new ADIS16448(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, rotation); -#elif defined(PX4_SPIDEV_EXTERNAL1_1) - g_dev = new ADIS16448(PX4_SPI_BUS_EXTERNAL1, PX4_SPIDEV_EXTERNAL1_1, rotation); -#else - PX4_ERR("External SPI not available"); - return -1; -#endif - - if (g_dev == nullptr) { - PX4_ERR("driver start failed"); - return -1; - } - - if (g_dev->init() != PX4_OK) { - PX4_ERR("driver init failed"); - delete g_dev; - g_dev = nullptr; - return -1; - } - - return 0; + PRINT_MODULE_USAGE_NAME("adis16448", "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_DEFAULT_COMMANDS(); } -static int stop() +I2CSPIDriverBase *ADIS16448::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - if (g_dev == nullptr) { - PX4_WARN("driver not running"); - return -1; + ADIS16448 *instance = new ADIS16448(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; } - delete g_dev; - g_dev = nullptr; - - return 0; -} - -static int status() -{ - if (g_dev == nullptr) { - PX4_INFO("driver not running"); - return 0; + if (OK != instance->init()) { + delete instance; + return nullptr; } - g_dev->print_info(); - - return 0; + return instance; } -static int usage() -{ - PX4_INFO("missing command: try 'start', 'stop', 'status'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); - - return 0; -} - -} // namespace adis16448 - extern "C" int adis16448_main(int argc, char *argv[]) { - enum Rotation rotation = ROTATION_NONE; - int myoptind = 1; - int ch = 0; - const char *myoptarg = nullptr; + int ch; + using ThisDriver = ADIS16448; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = 1000000; - // 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 adis16448::usage(); } } - const char *verb = argv[myoptind]; + const char *verb = cli.optarg(); - if (!strcmp(verb, "start")) { - return adis16448::start(rotation); - - } else if (!strcmp(verb, "stop")) { - return adis16448::stop(); - - } else if (!strcmp(verb, "status")) { - return adis16448::status(); + if (!verb) { + ThisDriver::print_usage(); + return -1; } - return adis16448::usage(); + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ADIS16448); + + 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); + } + + ThisDriver::print_usage(); + return -1; }