diff --git a/src/drivers/distance_sensor/srf02/SRF02.cpp b/src/drivers/distance_sensor/srf02/SRF02.cpp index ec3d432715..5e29af029e 100644 --- a/src/drivers/distance_sensor/srf02/SRF02.cpp +++ b/src/drivers/distance_sensor/srf02/SRF02.cpp @@ -33,9 +33,9 @@ #include "SRF02.hpp" -SRF02::SRF02(int bus, int address, uint8_t rotation) : - I2C("SRF02", nullptr, bus, address, 100000), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), +SRF02::SRF02(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) : + I2C("SRF02", nullptr, bus, address, 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) { _px4_rangefinder.set_max_distance(SRF02_MAX_DISTANCE); @@ -44,8 +44,6 @@ SRF02::SRF02(int bus, int address, uint8_t rotation) : SRF02::~SRF02() { - stop(); - perf_free(_sample_perf); perf_free(_comms_errors); } @@ -59,11 +57,6 @@ void SRF02::start() ScheduleDelayed(5); } -void SRF02::stop() -{ - ScheduleClear(); -} - int SRF02::init() { // I2C init (and probe) first. @@ -74,15 +67,7 @@ int SRF02::init() // XXX we should find out why we need to wait 200 ms here px4_usleep(200000); - int ret = measure(); - - if (ret != PX4_OK) { - return ret; - } - - start(); - - return PX4_OK; + return measure(); } int SRF02::collect() @@ -130,7 +115,7 @@ int SRF02::measure() return PX4_OK; } -void SRF02::Run() +void SRF02::RunImpl() { if (_collect_phase) { // Perform collection. @@ -157,8 +142,9 @@ void SRF02::Run() ScheduleDelayed(_interval); } -void SRF02::print_info() +void SRF02::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); diff --git a/src/drivers/distance_sensor/srf02/SRF02.hpp b/src/drivers/distance_sensor/srf02/SRF02.hpp index d95aa1f875..e074a02f81 100644 --- a/src/drivers/distance_sensor/srf02/SRF02.hpp +++ b/src/drivers/distance_sensor/srf02/SRF02.hpp @@ -40,7 +40,7 @@ #pragma once #include -#include +#include #include #include #include @@ -62,20 +62,25 @@ #define SRF02_CONVERSION_INTERVAL 100000 // 60ms for one sonar. #define SRF02_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100000 // 30ms between each sonar measurement (watch out for interference!). -class SRF02 : public device::I2C, public px4::ScheduledWorkItem +class SRF02 : public device::I2C, public I2CSPIDriver { public: - SRF02(int bus, int address = SRF02_BASEADDR, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + SRF02(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, + int address = SRF02_BASEADDR); ~SRF02() override; + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + int init() override; - void print_info(); + void print_status() override; + + void RunImpl(); private: - void Run() override; void start(); - void stop(); int collect(); int measure(); diff --git a/src/drivers/distance_sensor/srf02/srf02_main.cpp b/src/drivers/distance_sensor/srf02/srf02_main.cpp index 0c79611d0f..31e2bda89e 100644 --- a/src/drivers/distance_sensor/srf02/srf02_main.cpp +++ b/src/drivers/distance_sensor/srf02/srf02_main.cpp @@ -36,139 +36,73 @@ #include #include -namespace srf02 +void +SRF02::print_usage() { - -SRF02 *g_dev{nullptr}; - -static int start_bus(uint8_t rotation, int i2c_bus) -{ - if (g_dev != nullptr) { - PX4_ERR("already started"); - return PX4_ERROR; - } - - // Create the driver. - g_dev = new SRF02(rotation, i2c_bus); - - if (g_dev == nullptr) { - PX4_ERR("failed to instantiate the device"); - return PX4_ERROR; - } - - if (OK != g_dev->init()) { - PX4_ERR("failed to initialize the device"); - delete g_dev; - g_dev = nullptr; - return -1; - } - - return 0; + PRINT_MODULE_USAGE_NAME("srf02", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); + 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_DEFAULT_COMMANDS(); } -static int start(uint8_t rotation) +I2CSPIDriverBase *SRF02::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - if (g_dev != nullptr) { - PX4_WARN("already started"); - return -1; + SRF02 *instance = new SRF02(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; } - for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { - if (start_bus(rotation, i2c_bus_options[i]) == PX4_OK) { - return PX4_OK; - } + if (instance->init() != PX4_OK) { + delete instance; + return nullptr; } - return PX4_ERROR; + instance->start(); + return instance; } -static int stop() -{ - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - - } else { - return -1; - } - - return 0; -} - -static int status() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return -1; - } - - g_dev->print_info(); - - return 0; -} - -static int usage() -{ - PX4_INFO("usage: srf02 command [options]"); - PX4_INFO("options:"); - PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_EXPANSION); - PX4_INFO("\t-a --all"); - PX4_INFO("\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING); - PX4_INFO("command:"); - PX4_INFO("\tstart|stop|test|reset|info"); - return PX4_OK; -} - -} // namespace srf02 - extern "C" __EXPORT int srf02_main(int argc, char *argv[]) { - uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - int i2c_bus = PX4_I2C_BUS_EXPANSION; - bool start_all = false; int ch; - int myoptind = 1; - const char *myoptarg = nullptr; + using ThisDriver = SRF02; + BusCLIArguments cli{true, false}; + cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.default_i2c_frequency = 100000; - while ((ch = px4_getopt(argc, argv, "R:ab:", &myoptind, &myoptarg)) != EOF) { + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - rotation = (uint8_t)atoi(myoptarg); + cli.orientation = atoi(cli.optarg()); break; - - case 'b': - i2c_bus = atoi(myoptarg); - break; - - case 'a': - start_all = true; - break; - - default: - srf02::usage(); - return -1; } } - if (myoptind >= argc) { - srf02::usage(); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); return -1; } - if (!strcmp(argv[myoptind], "start")) { - if (start_all) { - return srf02::start(rotation); + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_SRF02); - } else { - return srf02::start_bus(rotation, i2c_bus); - } - - } else if (!strcmp(argv[myoptind], "stop")) { - return srf02::stop(); - - } else if (!strcmp(argv[myoptind], "status")) { - return srf02::status(); + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); } - return srf02::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 1d79681e67..ab5e1c662f 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -133,6 +133,7 @@ #define DRV_DIST_DEVTYPE_MAPPYDOT 0x71 #define DRV_DIST_DEVTYPE_MB12XX 0x72 #define DRV_DIST_DEVTYPE_SF1XX 0x73 +#define DRV_DIST_DEVTYPE_SRF02 0x74 #define DRV_DEVTYPE_UNUSED 0xff