From 5fa4cd10196da8bc75234f3d7adef605f269c34e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 23 Mar 2020 11:21:16 +0100 Subject: [PATCH] refactor fxas21002c: use driver base class --- boards/nxp/fmuk66-v3/init/rc.board_sensors | 2 +- src/drivers/imu/fxas21002c/FXAS21002C.cpp | 32 +-- src/drivers/imu/fxas21002c/FXAS21002C.hpp | 43 ++-- .../imu/fxas21002c/fxas21002c_main.cpp | 221 ++++++------------ 4 files changed, 88 insertions(+), 210 deletions(-) diff --git a/boards/nxp/fmuk66-v3/init/rc.board_sensors b/boards/nxp/fmuk66-v3/init/rc.board_sensors index 6b98b2ae52..9bb57737ed 100644 --- a/boards/nxp/fmuk66-v3/init/rc.board_sensors +++ b/boards/nxp/fmuk66-v3/init/rc.board_sensors @@ -22,4 +22,4 @@ mpl3115a2 -I start fxos8701cq start # Internal SPI (gyro) -fxas21002c start +fxas21002c -s start diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.cpp b/src/drivers/imu/fxas21002c/FXAS21002C.cpp index a8accbe7e9..46e1d90d15 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.cpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.cpp @@ -186,9 +186,10 @@ static constexpr uint8_t _checked_registers[] { using namespace time_literals; -FXAS21002C::FXAS21002C(int bus, uint32_t device, enum Rotation rotation) : - SPI("FXAS21002C", nullptr, bus, device, SPIDEV_MODE0, 2 * 1000 * 1000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())), +FXAS21002C::FXAS21002C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode) : + SPI("FXAS21002C", 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(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _errors(perf_alloc(PC_COUNT, MODULE_NAME": err")), @@ -200,10 +201,6 @@ FXAS21002C::FXAS21002C(int bus, uint32_t device, enum Rotation rotation) : FXAS21002C::~FXAS21002C() { - /* make sure we are truly inactive */ - stop(); - - /* delete the perf counter */ perf_free(_sample_perf); perf_free(_errors); perf_free(_bad_registers); @@ -471,26 +468,10 @@ FXAS21002C::set_onchip_lowpass_filter(int frequency_hz) void FXAS21002C::start() { - /* make sure we are stopped first */ - stop(); - /* start polling at the specified rate */ ScheduleOnInterval((1_s / FXAS21002C_DEFAULT_RATE) - FXAS21002C_TIMER_REDUCTION, 10000); } -void -FXAS21002C::stop() -{ - ScheduleClear(); -} - -void -FXAS21002C::Run() -{ - /* make another measurement */ - measure(); -} - void FXAS21002C::check_registers(void) { @@ -522,7 +503,7 @@ FXAS21002C::check_registers(void) } void -FXAS21002C::measure() +FXAS21002C::RunImpl() { // start the performance counter perf_begin(_sample_perf); @@ -589,8 +570,9 @@ FXAS21002C::measure() } void -FXAS21002C::print_info() +FXAS21002C::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/fxas21002c/FXAS21002C.hpp b/src/drivers/imu/fxas21002c/FXAS21002C.hpp index 4e50b806ce..85d4def8a1 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.hpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.hpp @@ -43,34 +43,31 @@ #include #include #include -#include +#include -class FXAS21002C : public device::SPI, public px4::ScheduledWorkItem +class FXAS21002C : public device::SPI, public I2CSPIDriver { public: - FXAS21002C(int bus, uint32_t device, enum Rotation rotation); + FXAS21002C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode); virtual ~FXAS21002C(); - 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(); + int init() override; - /** - * dump register values - */ - void print_registers(); + void print_status() override; - /** - * deliberately trigger an error - */ - void test_error(); + void RunImpl(); + void print_registers(); + void test_error(); protected: - virtual int probe(); + void custom_method(const BusCLIArguments &cli); + int probe() override; private: @@ -101,11 +98,6 @@ private: */ void start(); - /** - * Stop automatic measurement. - */ - void stop(); - /** * Reset chip. * @@ -118,18 +110,11 @@ private: */ void set_standby(int rate, bool standby_true); - void Run() override; - /** * check key registers for correct values */ void check_registers(void); - /** - * Fetch accel measurements from the sensor and update the report ring. - */ - void measure(); - /** * Read a register from the FXAS21002C * diff --git a/src/drivers/imu/fxas21002c/fxas21002c_main.cpp b/src/drivers/imu/fxas21002c/fxas21002c_main.cpp index 1bc6facf36..5f439d3d1b 100644 --- a/src/drivers/imu/fxas21002c/fxas21002c_main.cpp +++ b/src/drivers/imu/fxas21002c/fxas21002c_main.cpp @@ -33,187 +33,98 @@ #include "FXAS21002C.hpp" -extern "C" { __EXPORT int fxas21002c_main(int argc, char *argv[]); } - -/** - * Local functions in support of the shell command. - */ -namespace fxas21002c -{ - -FXAS21002C *g_dev; - -void start(bool external_bus, enum Rotation rotation); -void info(); -void regdump(); -void usage(); -void test_error(); - -/** - * Start the driver. - * - * This function call only returns once the driver is - * up and running or failed to detect the sensor. - */ -void -start(bool external_bus, enum Rotation rotation) -{ - if (g_dev != nullptr) { - PX4_INFO("already started"); - exit(0); - } - - /* create the driver */ - if (external_bus) { -#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_GYRO) - g_dev = new FXAS21002C(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_GYRO, rotation); -#else - PX4_ERR("External SPI not available"); - exit(0); -#endif - - } else { - g_dev = new FXAS21002C(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_GYRO, rotation); - } - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating FXAS21002C obj"); - 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 a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running\n"); - exit(1); - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - -/** - * dump registers from device - */ -void -regdump() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running\n"); - exit(1); - } - - printf("regdump @ %p\n", g_dev); - g_dev->print_registers(); - - exit(0); -} - -/** - * trigger an error - */ -void -test_error() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running\n"); - exit(1); - } - - g_dev->test_error(); - - exit(0); -} +#include +#include void -usage() +FXAS21002C::print_usage() { - PX4_INFO("missing command: try 'start', 'info', 'testerror' or 'regdump'"); - PX4_INFO("options:"); - PX4_INFO(" -X (external bus)"); - PX4_INFO(" -R rotation"); + PRINT_MODULE_USAGE_NAME("fxas21002c", "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(); } -} // namespace - -int -fxas21002c_main(int argc, char *argv[]) +I2CSPIDriverBase *FXAS21002C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - bool external_bus = false; - enum Rotation rotation = ROTATION_NONE; + FXAS21002C *instance = new FXAS21002C(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode); - int ch = 0; - int myoptind = 1; - const char *myoptarg = NULL; + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } - while ((ch = px4_getopt(argc, argv, "XR:", &myoptind, &myoptarg)) != EOF) { + if (OK != instance->init()) { + delete instance; + return nullptr; + } + + return instance; +} + +void FXAS21002C::custom_method(const BusCLIArguments &cli) +{ + switch (cli.custom1) { + case 0: print_registers(); break; + + case 1: test_error(); break; + } + +} + +extern "C" int fxas21002c_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = FXAS21002C; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = 2 * 1000 * 1000; + cli.spi_mode = SPIDEV_MODE0; + + 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: - fxas21002c::usage(); - return 0; } } - const char *verb = argv[myoptind]; + const char *verb = cli.optarg(); - /* - * Start/load the driver. + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_GYR_DEVTYPE_FXAS2100C); - */ if (!strcmp(verb, "start")) { - fxas21002c::start(external_bus, rotation); + return ThisDriver::module_start(cli, iterator); } - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - fxas21002c::info(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - /* - * dump device registers - */ if (!strcmp(verb, "regdump")) { - fxas21002c::regdump(); + cli.custom1 = 0; + return ThisDriver::module_custom_method(cli, iterator); } - /* - * trigger an error - */ if (!strcmp(verb, "testerror")) { - fxas21002c::test_error(); + cli.custom1 = 1; + return ThisDriver::module_custom_method(cli, iterator); } - PX4_WARN("unrecognized command, try 'start', 'info', 'testerror' or 'regdump'"); + ThisDriver::print_usage(); return -1; }