From 609eafd4cdb06cb3e4eb4c666ec26da2f08281fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 18 Mar 2020 17:34:55 +0100 Subject: [PATCH] refactor l3gd20: use driver base class --- .../airmind/mindpx-v2/init/rc.board_sensors | 2 +- boards/px4/fmu-v2/init/rc.board_sensors | 6 +- boards/px4/fmu-v3/init/rc.board_sensors | 6 +- src/drivers/imu/l3gd20/L3GD20.cpp | 32 +-- src/drivers/imu/l3gd20/L3GD20.hpp | 32 ++- src/drivers/imu/l3gd20/l3gd20_main.cpp | 185 ++++++------------ 6 files changed, 89 insertions(+), 174 deletions(-) diff --git a/boards/airmind/mindpx-v2/init/rc.board_sensors b/boards/airmind/mindpx-v2/init/rc.board_sensors index 6dbec0811c..26ea9c1977 100644 --- a/boards/airmind/mindpx-v2/init/rc.board_sensors +++ b/boards/airmind/mindpx-v2/init/rc.board_sensors @@ -16,7 +16,7 @@ qmc5883 -I -R 12 start mpu6000 -s -R 8 start mpu9250 -s -R 8 start lsm303d -s -R 10 start -l3gd20 -R 14 start +l3gd20 -s -R 14 start # Internal SPI ms5611 -s start diff --git a/boards/px4/fmu-v2/init/rc.board_sensors b/boards/px4/fmu-v2/init/rc.board_sensors index 1332f2ba43..238ea20ba9 100644 --- a/boards/px4/fmu-v2/init/rc.board_sensors +++ b/boards/px4/fmu-v2/init/rc.board_sensors @@ -70,8 +70,8 @@ then # sensor heating is available, but we disable it for now param set SENS_EN_THERMAL 0 - # external L3GD20H is rotated 180 degrees yaw - l3gd20 -X -R 4 start + # (external) L3GD20H is rotated 180 degrees yaw + l3gd20 -s -R 4 start # (external) LSM303D is rotated 270 degrees yaw lsm303d -s -R 6 start @@ -105,7 +105,7 @@ else # else: On the PixhawkMini the mpu9250 has been disabled due to HW errata fi - l3gd20 start + l3gd20 -s start lsm303d -s start fi diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors index 7b32e26a78..7c7c06361c 100644 --- a/boards/px4/fmu-v3/init/rc.board_sensors +++ b/boards/px4/fmu-v3/init/rc.board_sensors @@ -78,8 +78,8 @@ then ak09916 -X -R 6 start fi - # external L3GD20H is rotated 180 degrees yaw - l3gd20 -X -R 4 start + # (external) L3GD20H is rotated 180 degrees yaw + l3gd20 -s -R 4 start # (external) LSM303D is rotated 270 degrees yaw lsm303d -s -R 6 start @@ -113,7 +113,7 @@ else # else: On the PixhawkMini the mpu9250 has been disabled due to HW errata fi - l3gd20 start + l3gd20 -s start lsm303d -s start fi diff --git a/src/drivers/imu/l3gd20/L3GD20.cpp b/src/drivers/imu/l3gd20/L3GD20.cpp index 0621484872..e12b3e0b9f 100644 --- a/src/drivers/imu/l3gd20/L3GD20.cpp +++ b/src/drivers/imu/l3gd20/L3GD20.cpp @@ -35,9 +35,10 @@ constexpr uint8_t L3GD20::_checked_registers[]; -L3GD20::L3GD20(int bus, uint32_t device, enum Rotation rotation) : - SPI("L3GD20", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())), +L3GD20::L3GD20(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode) : + SPI("L3GD20", nullptr, bus, device, spi_mode, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _errors(perf_alloc(PC_COUNT, MODULE_NAME": err")), @@ -49,10 +50,6 @@ L3GD20::L3GD20(int bus, uint32_t device, enum Rotation rotation) : L3GD20::~L3GD20() { - /* make sure we are truly inactive */ - stop(); - - /* delete the perf counter */ perf_free(_sample_perf); perf_free(_errors); perf_free(_bad_registers); @@ -226,20 +223,11 @@ L3GD20::set_samplerate(unsigned frequency) void L3GD20::start() { - /* make sure we are stopped first */ - stop(); - /* start polling at the specified rate */ uint64_t interval = 1000000 / L3GD20_DEFAULT_RATE; ScheduleOnInterval(interval - L3GD20_TIMER_REDUCTION, 10000); } -void -L3GD20::stop() -{ - ScheduleClear(); -} - void L3GD20::disable_i2c() { @@ -288,13 +276,6 @@ L3GD20::reset() _read = 0; } -void -L3GD20::Run() -{ - /* make another measurement */ - measure(); -} - void L3GD20::check_registers() { @@ -326,7 +307,7 @@ L3GD20::check_registers() } void -L3GD20::measure() +L3GD20::RunImpl() { /* status register and data as read back from the device */ #pragma pack(push, 1) @@ -412,8 +393,9 @@ L3GD20::measure() } void -L3GD20::print_info() +L3GD20::print_status() { + I2CSPIDriverBase::print_status(); printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); perf_print_counter(_errors); diff --git a/src/drivers/imu/l3gd20/L3GD20.hpp b/src/drivers/imu/l3gd20/L3GD20.hpp index e3411e0e7f..81e2505cd0 100644 --- a/src/drivers/imu/l3gd20/L3GD20.hpp +++ b/src/drivers/imu/l3gd20/L3GD20.hpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include /* Orientation on board */ #define SENSOR_BOARD_ROTATION_000_DEG 0 @@ -158,18 +158,22 @@ */ #define L3GD20_TIMER_REDUCTION 600 -class L3GD20 : public device::SPI, public px4::ScheduledWorkItem +class L3GD20 : public device::SPI, public I2CSPIDriver { public: - L3GD20(int bus, uint32_t device, enum Rotation rotation); - virtual ~L3GD20(); + L3GD20(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode); + ~L3GD20() override; - 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. - */ - void print_info(); + void RunImpl(); + + int init() override; + + void print_status() override; // print register dump void print_registers(); @@ -178,7 +182,8 @@ public: void test_error(); protected: - virtual int probe(); + void custom_method(const BusCLIArguments &cli) override; + int probe() override; private: @@ -217,21 +222,14 @@ private: uint8_t _checked_next{0}; void start(); - void stop(); void reset(); void disable_i2c(); - void Run() override; /** * check key registers for correct values */ void check_registers(); - /** - * Fetch measurements from the sensor and update the report ring. - */ - void measure(); - /** * Read a register from the L3GD20 * diff --git a/src/drivers/imu/l3gd20/l3gd20_main.cpp b/src/drivers/imu/l3gd20/l3gd20_main.cpp index baf4fcf289..91e805724e 100644 --- a/src/drivers/imu/l3gd20/l3gd20_main.cpp +++ b/src/drivers/imu/l3gd20/l3gd20_main.cpp @@ -33,161 +33,96 @@ #include "L3GD20.hpp" -/** - * Local functions in support of the shell command. - */ -namespace l3gd20 +#include +#include + +void +L3GD20::print_usage() { - -L3GD20 *g_dev{nullptr}; - -/** - * Start the driver. - * - * This function call only returns once the driver - * started or failed to detect the sensor. - */ -static void start(bool external_bus, enum Rotation rotation) -{ - if (g_dev != nullptr) { - errx(0, "already started"); - } - - /* create the driver */ - if (external_bus) { -#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_GYRO) - g_dev = new L3GD20(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_GYRO, rotation); -#else - errx(0, "External SPI not available"); -#endif - - } else { - g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_GYRO, rotation); - } - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - exit(0); -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); + PRINT_MODULE_USAGE_NAME("l3gd20", "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("regdump"); + PRINT_MODULE_USAGE_COMMAND("testerror"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -/** - * Print a little info about the driver. - */ -static void info() +I2CSPIDriverBase *L3GD20::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - if (g_dev == nullptr) { - errx(1, "driver not running\n"); + L3GD20 *instance = new L3GD20(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; } - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - -/** - * Dump the register information - */ -static void regdump() -{ - if (g_dev == nullptr) { - errx(1, "driver not running"); + if (OK != instance->init()) { + delete instance; + return nullptr; } - printf("regdump @ %p\n", g_dev); - g_dev->print_registers(); - - exit(0); + return instance; } -/** - * trigger an error - */ -static void test_error() +void L3GD20::custom_method(const BusCLIArguments &cli) { - if (g_dev == nullptr) { - errx(1, "driver not running"); + switch (cli.custom1) { + case 0: print_registers(); break; + + case 1: test_error(); break; } - - printf("regdump @ %p\n", g_dev); - g_dev->test_error(); - - exit(0); } -static void usage() +extern "C" int l3gd20_main(int argc, char *argv[]) { - warnx("missing command: try 'start', 'info', 'testerror' or 'regdump'"); - warnx("options:"); - warnx(" -X (external bus)"); - warnx(" -R rotation"); -} - -} // namespace - -extern "C" __EXPORT int l3gd20_main(int argc, char *argv[]) -{ - int myoptind = 1; int ch; - const char *myoptarg = nullptr; - bool external_bus = false; - enum Rotation rotation = ROTATION_NONE; + using ThisDriver = L3GD20; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = 11 * 1000 * 1000; - while ((ch = px4_getopt(argc, argv, "XR:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { - case 'X': - external_bus = true; - break; - case 'R': - rotation = (enum Rotation)atoi(myoptarg); + cli.rotation = (enum Rotation)atoi(cli.optarg()); break; - - default: - l3gd20::usage(); - return 0; } } - if (myoptind >= argc) { - l3gd20::usage(); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); return -1; } - const char *verb = argv[myoptind]; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_GYR_DEVTYPE_L3GD20); - /* - * Start/load the driver. - - */ if (!strcmp(verb, "start")) { - l3gd20::start(external_bus, rotation); - - } else if (!strcmp(verb, "info")) { - l3gd20::info(); - - } else if (!strcmp(verb, "regdump")) { - l3gd20::regdump(); - - } else if (!strcmp(verb, "testerror")) { - l3gd20::test_error(); + return ThisDriver::module_start(cli, iterator); } - l3gd20::usage(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + if (!strcmp(verb, "regdump")) { + cli.custom1 = 0; + return ThisDriver::module_custom_method(cli, iterator); + } + + if (!strcmp(verb, "testerror")) { + cli.custom1 = 1; + return ThisDriver::module_custom_method(cli, iterator); + } + + ThisDriver::print_usage(); return -1; }