From 74db018012c595e6ae6d7aa8d196d1a14bf02ee7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 17 Mar 2020 18:08:44 +0100 Subject: [PATCH] refactor lsm303agr: use driver base class --- boards/av/x-v1/init/rc.board_sensors | 2 +- .../magnetometer/lsm303agr/LSM303AGR.cpp | 38 ++--- .../magnetometer/lsm303agr/LSM303AGR.hpp | 47 ++---- .../magnetometer/lsm303agr/lsm303agr_main.cpp | 155 +++++------------- 4 files changed, 70 insertions(+), 172 deletions(-) diff --git a/boards/av/x-v1/init/rc.board_sensors b/boards/av/x-v1/init/rc.board_sensors index 10fc3c162f..862de804f2 100644 --- a/boards/av/x-v1/init/rc.board_sensors +++ b/boards/av/x-v1/init/rc.board_sensors @@ -9,7 +9,7 @@ adis16477 -s -R 8 start lps22hb -s start -lsm303agr -R 4 start +lsm303agr -s -R 4 start ms4525_airspeed -T 4515 -I -b 3 start diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp index 208670d30f..ae1f619930 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp @@ -61,9 +61,10 @@ static constexpr uint8_t LSM303AGR_WHO_AM_I_M = 0x40; */ #define LSM303AGR_TIMER_REDUCTION 200 -LSM303AGR::LSM303AGR(int bus, const char *path, uint32_t device, enum Rotation rotation) : - SPI("LSM303AGR", path, bus, device, SPIDEV_MODE3, 8 * 1000 * 1000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +LSM303AGR::LSM303AGR(I2CSPIBusOption bus_option, int bus, int device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode) : + SPI("LSM303AGR", nullptr, bus, device, spi_mode, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _mag_sample_perf(perf_alloc(PC_ELAPSED, "LSM303AGR_mag_read")), _bad_registers(perf_alloc(PC_COUNT, "LSM303AGR_bad_reg")), _bad_values(perf_alloc(PC_COUNT, "LSM303AGR_bad_val")), @@ -83,9 +84,6 @@ LSM303AGR::LSM303AGR(int bus, const char *path, uint32_t device, enum Rotation r LSM303AGR::~LSM303AGR() { - /* make sure we are truly inactive */ - stop(); - if (_class_instance != -1) { unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance); } @@ -99,12 +97,12 @@ LSM303AGR::~LSM303AGR() int LSM303AGR::init() { - int ret = PX4_OK; - /* do SPI init (and probe) first */ - if (SPI::init() != OK) { - PX4_ERR("SPI init failed"); - return PX4_ERROR; + int ret = SPI::init(); + + if (ret != OK) { + DEVICE_DEBUG("SPI init failed (%i)", ret); + return ret; } _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); @@ -118,6 +116,9 @@ LSM303AGR::init() /* fill report structures */ measure(); + _measure_interval = CONVERSION_INTERVAL; + start(); + return ret; } @@ -350,17 +351,7 @@ LSM303AGR::start() } void -LSM303AGR::stop() -{ - if (_measure_interval > 0) { - /* ensure no new items are queued while we cancel this one */ - _measure_interval = 0; - ScheduleClear(); - } -} - -void -LSM303AGR::Run() +LSM303AGR::RunImpl() { if (_measure_interval == 0) { return; @@ -476,8 +467,9 @@ LSM303AGR::collect() } void -LSM303AGR::print_info() +LSM303AGR::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_mag_sample_perf); perf_print_counter(_bad_registers); perf_print_counter(_bad_values); diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp index 6342c098ad..b771738347 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp @@ -31,8 +31,7 @@ * ****************************************************************************/ -#ifndef DRIVERS_IMU_LSM303AGR_LSM303AGR_HPP_ -#define DRIVERS_IMU_LSM303AGR_LSM303AGR_HPP_ +#pragma once #include #include @@ -42,7 +41,7 @@ #include #include #include -#include +#include #include // Register mapping @@ -88,23 +87,26 @@ static constexpr uint8_t OUTY_H_REG_M = 0x6B; static constexpr uint8_t OUTZ_L_REG_M = 0x6C; static constexpr uint8_t OUTZ_H_REG_M = 0x6D; -class LSM303AGR : public device::SPI, public px4::ScheduledWorkItem +class LSM303AGR : public device::SPI, public I2CSPIDriver { public: - LSM303AGR(int bus, const char *path, uint32_t device, enum Rotation rotation); + LSM303AGR(I2CSPIBusOption bus_option, int bus, int device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode); virtual ~LSM303AGR(); - virtual int init(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + int init() override; - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + void print_status() override; + + void RunImpl(); protected: - virtual int probe(); + int probe() override; private: @@ -137,11 +139,6 @@ private: */ void start(); - /** - * Stop automatic measurement. - */ - void stop(); - /** * Reset chip. * @@ -164,21 +161,6 @@ private: */ int collect(); - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - * - * This is the heart of the measurement state machine. This function - * alternately starts a measurement, or collects the data from the - * previous measurement. - * - * When the interval between measurements is greater than the minimum - * measurement interval, a gap is inserted between collection - * and measurement to provide the most recent measurement possible - * at the next interval. - */ - void Run() override; - /** * Read a register from the LSM303AGR * @@ -200,4 +182,3 @@ private: LSM303AGR operator=(const LSM303AGR &); }; -#endif /* DRIVERS_IMU_LSM303AGR_LSM303AGR_HPP_ */ diff --git a/src/drivers/magnetometer/lsm303agr/lsm303agr_main.cpp b/src/drivers/magnetometer/lsm303agr/lsm303agr_main.cpp index 0fdf462f9e..ca99918702 100644 --- a/src/drivers/magnetometer/lsm303agr/lsm303agr_main.cpp +++ b/src/drivers/magnetometer/lsm303agr/lsm303agr_main.cpp @@ -33,149 +33,74 @@ #include "LSM303AGR.hpp" -#include -#include #include +#include -#define LSM303AGR_DEVICE_PATH_MAG "/dev/lsm303agr_mag" - -extern "C" { __EXPORT int lsm303agr_main(int argc, char *argv[]); } - -/** - * Local functions in support of the shell command. - */ -namespace lsm303agr -{ - -LSM303AGR *g_dev; - -void start(enum Rotation rotation); -void info(); -void usage(); - -/** - * Start the driver. - * - * This function call only returns once the driver is - * up and running or failed to detect the sensor. - */ void -start(enum Rotation rotation) +LSM303AGR::print_usage() { - int fd_mag = -1; + PRINT_MODULE_USAGE_NAME("lsm303agr", "driver"); + 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(); +} - if (g_dev != nullptr) { - errx(0, "already started"); - } +I2CSPIDriverBase *LSM303AGR::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + LSM303AGR *instance = new LSM303AGR(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode); - /* create the driver */ -#if defined(PX4_SPIDEV_LSM303A_M) && defined(PX4_SPIDEV_LSM303A_X) - g_dev = new LSM303AGR(PX4_SPI_BUS_SENSOR5, LSM303AGR_DEVICE_PATH_MAG, PX4_SPIDEV_LSM303A_M, rotation); -#else - errx(0, "External SPI not available"); -#endif - - if (g_dev == nullptr) { + if (!instance) { PX4_ERR("alloc failed"); - goto fail; + return nullptr; } - if (OK != g_dev->init()) { - goto fail; + if (OK != instance->init()) { + delete instance; + return nullptr; } - fd_mag = px4_open(LSM303AGR_DEVICE_PATH_MAG, O_RDONLY); - - /* don't fail if open cannot be opened */ - if (0 <= fd_mag) { - if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - } - - px4_close(fd_mag); - - exit(0); -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); + return instance; } -/** - * Print a little info about the driver. - */ -void -info() +extern "C" int lsm303agr_main(int argc, char *argv[]) { - if (g_dev == nullptr) { - errx(1, "driver not running\n"); - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - -void -usage() -{ - PX4_INFO("missing command: try 'start', 'info', 'reset'"); - PX4_INFO("options:"); - PX4_INFO(" -X (external bus)"); - PX4_INFO(" -R rotation"); -} - -} // namespace - -int -lsm303agr_main(int argc, char *argv[]) -{ - int myoptind = 1; int ch; - const char *myoptarg = nullptr; + using ThisDriver = LSM303AGR; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = 8 * 1000 * 1000; - enum Rotation rotation = ROTATION_NONE; - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "XR:a:", &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: - lsm303agr::usage(); - exit(0); } } - if (myoptind >= argc) { - lsm303agr::usage(); - exit(0); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; } - const char *verb = argv[myoptind]; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LSM303AGR); - /* - * Start/load the driver. - - */ if (!strcmp(verb, "start")) { - lsm303agr::start(rotation); + return ThisDriver::module_start(cli, iterator); } - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - lsm303agr::info(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - errx(1, "unrecognized command, try 'start', 'info'"); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; }