From 83b367f00056c2d57dd8a109ae20249f0775958e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Mar 2020 15:03:57 +0100 Subject: [PATCH] refactor teraranger: use driver base class --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- .../distance_sensor/teraranger/TERARANGER.cpp | 20 +- .../distance_sensor/teraranger/TERARANGER.hpp | 19 +- .../teraranger/teraranger_main.cpp | 171 +++++------------- src/drivers/drv_sensor.h | 1 + 5 files changed, 65 insertions(+), 148 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 777a075f4a..1dbe8cb403 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -103,7 +103,7 @@ fi # Teraranger one tof sensor if param greater -s SENS_EN_TRANGER 0 then - teraranger start -a + teraranger start -X fi # Possible external pmw3901 optical flow sensor diff --git a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp b/src/drivers/distance_sensor/teraranger/TERARANGER.cpp index a130881a72..386f288871 100644 --- a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp +++ b/src/drivers/distance_sensor/teraranger/TERARANGER.cpp @@ -73,9 +73,9 @@ static uint8_t crc8(uint8_t *p, uint8_t len) return crc & 0xFF; } -TERARANGER::TERARANGER(const int bus, const int address, const uint8_t rotation) : - I2C("TERARANGER", nullptr, bus, address, 100000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +TERARANGER::TERARANGER(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency) : + I2C("TERARANGER", nullptr, bus, TERARANGER_ONE_BASEADDR, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation) { // up the retries since the device misses the first measure attempts @@ -84,8 +84,6 @@ TERARANGER::TERARANGER(const int bus, const int address, const uint8_t rotation) TERARANGER::~TERARANGER() { - stop(); - perf_free(_sample_perf); perf_free(_comms_errors); } @@ -211,8 +209,6 @@ int TERARANGER::init() return PX4_ERROR; } - start(); - return PX4_OK; } @@ -253,7 +249,7 @@ int TERARANGER::probe() return -EIO; } -void TERARANGER::Run() +void TERARANGER::RunImpl() { // Perform data collection. collect(); @@ -267,13 +263,9 @@ void TERARANGER::start() ScheduleOnInterval(TERARANGER_MEASUREMENT_INTERVAL); } -void TERARANGER::stop() -{ - ScheduleClear(); -} - -void TERARANGER::print_info() +void TERARANGER::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); diff --git a/src/drivers/distance_sensor/teraranger/TERARANGER.hpp b/src/drivers/distance_sensor/teraranger/TERARANGER.hpp index 0f8e27c61a..6351f30cd4 100644 --- a/src/drivers/distance_sensor/teraranger/TERARANGER.hpp +++ b/src/drivers/distance_sensor/teraranger/TERARANGER.hpp @@ -42,7 +42,7 @@ #include #include -#include +#include #include #include @@ -72,24 +72,26 @@ using namespace time_literals; #define TERARANGER_MEASUREMENT_INTERVAL 10_ms -class TERARANGER : public device::I2C, public px4::ScheduledWorkItem +class TERARANGER : public device::I2C, public I2CSPIDriver { public: - TERARANGER(const int bus, const int address = TERARANGER_ONE_BASEADDR, - const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + TERARANGER(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency); ~TERARANGER() override; - virtual int init() override; - void print_info(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + virtual int init() override; + void print_status() override; + + void RunImpl(); protected: virtual int probe() override; private: - void start(); - void stop(); int collect(); int measure(); @@ -102,7 +104,6 @@ private: */ int probe_address(const uint8_t address); - void Run() override; PX4Rangefinder _px4_rangefinder; diff --git a/src/drivers/distance_sensor/teraranger/teraranger_main.cpp b/src/drivers/distance_sensor/teraranger/teraranger_main.cpp index b3f734128e..4c1c6be528 100644 --- a/src/drivers/distance_sensor/teraranger/teraranger_main.cpp +++ b/src/drivers/distance_sensor/teraranger/teraranger_main.cpp @@ -36,77 +36,8 @@ #include #include -namespace teraranger -{ - -TERARANGER *g_dev{nullptr}; - -static int start_bus(const int i2c_bus, const uint8_t orientation) -{ - if (g_dev != nullptr) { - PX4_ERR("already started"); - return PX4_OK; - } - - // Instantiate the driver. - g_dev = new TERARANGER(i2c_bus, TERARANGER_ONE_BASEADDR, orientation); - - if (g_dev == nullptr) { - delete g_dev; - return PX4_ERROR; - } - - // Initialize the sensor. - if (g_dev->init() != PX4_OK) { - delete g_dev; - g_dev = nullptr; - return PX4_ERROR; - } - - return PX4_OK; -} - -static int start(const uint8_t orientation) -{ - if (g_dev != nullptr) { - PX4_ERR("already started"); - return PX4_ERROR; - } - - for (size_t i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { - if (start_bus(i2c_bus_options[i], orientation) == PX4_OK) { - return PX4_OK; - } - } - - return PX4_ERROR; -} - -static int stop() -{ - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - - } - - PX4_INFO("driver stopped"); - return PX4_OK; -} - -static int status() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return PX4_ERROR; - } - - g_dev->print_info(); - - return PX4_OK; -} - -static int usage() +void +TERARANGER::print_usage() { PRINT_MODULE_DESCRIPTION( R"DESCR_STR( @@ -117,79 +48,71 @@ I2C bus driver for TeraRanger rangefinders. The sensor/driver must be enabled using the parameter SENS_EN_TRANGER. Setup/usage information: https://docs.px4.io/en/sensor/rangefinders.html#teraranger-rangefinders - -### Examples -Start driver on any bus (start on bus where first sensor found). -$ teraranger start -a -Start driver on specified bus -$ teraranger start -b 1 -Stop driver -$ teraranger stop )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("teraranger", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); - PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver"); - PRINT_MODULE_USAGE_PARAM_FLAG('a', "Attempt to start driver on all I2C buses (first one found)", true); - PRINT_MODULE_USAGE_PARAM_INT('b', 1, 1, 2000, "Start driver on specific I2C bus", true); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver"); - PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver information"); - - return PX4_OK; + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -} // namespace +I2CSPIDriverBase *TERARANGER::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + TERARANGER *instance = new TERARANGER(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (instance->init() != PX4_OK) { + delete instance; + return nullptr; + } + + instance->start(); + return instance; +} extern "C" __EXPORT int teraranger_main(int argc, char *argv[]) { - const char *myoptarg = nullptr; - int ch; - int i2c_bus = PX4_I2C_BUS_EXPANSION; - int myoptind = 1; + using ThisDriver = TERARANGER; + BusCLIArguments cli{true, false}; + cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.default_i2c_frequency = 100000; - uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - - bool start_all = false; - - while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { - case 'a': - start_all = true; - break; - - case 'b': - i2c_bus = atoi(myoptarg); - break; - case 'R': - rotation = (uint8_t)atoi(myoptarg); + cli.orientation = atoi(cli.optarg()); break; - - default: - PX4_WARN("Unknown option!"); - return teraranger::usage(); } } - if (myoptind >= argc) { - return teraranger::usage(); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; } - if (!strcmp(argv[myoptind], "start")) { + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_TERARANGER); - if (start_all) { - return teraranger::start(rotation); - - } else { - return teraranger::start_bus(i2c_bus, rotation); - } - } else if (!strcmp(argv[myoptind], "status")) { - return teraranger::status(); - } else if (!strcmp(argv[myoptind], "stop")) { - return teraranger::stop(); + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); } - return teraranger::usage(); + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index ab5e1c662f..7f61e7297b 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -134,6 +134,7 @@ #define DRV_DIST_DEVTYPE_MB12XX 0x72 #define DRV_DIST_DEVTYPE_SF1XX 0x73 #define DRV_DIST_DEVTYPE_SRF02 0x74 +#define DRV_DIST_DEVTYPE_TERARANGER 0x75 #define DRV_DEVTYPE_UNUSED 0xff