From 0c9161f004531b5c8a342ca2a075fbbfd97823ae Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 29 Dec 2019 12:23:55 -0500 Subject: [PATCH] sf0x: move to PX4Rangefinder and cleanup --- .../distance_sensor/sf0x/CMakeLists.txt | 3 + src/drivers/distance_sensor/sf0x/SF0X.cpp | 329 +++--------------- src/drivers/distance_sensor/sf0x/SF0X.hpp | 108 ++---- .../distance_sensor/sf0x/sf0x_main.cpp | 235 +++---------- 4 files changed, 119 insertions(+), 556 deletions(-) diff --git a/src/drivers/distance_sensor/sf0x/CMakeLists.txt b/src/drivers/distance_sensor/sf0x/CMakeLists.txt index 62fa50cf36..bb34b6ed42 100644 --- a/src/drivers/distance_sensor/sf0x/CMakeLists.txt +++ b/src/drivers/distance_sensor/sf0x/CMakeLists.txt @@ -40,6 +40,9 @@ px4_add_module( SF0X.hpp sf0x_main.cpp sf0x_parser.cpp + DEPENDS + drivers_rangefinder + px4_work_queue MODULE_CONFIG module.yaml ) diff --git a/src/drivers/distance_sensor/sf0x/SF0X.cpp b/src/drivers/distance_sensor/sf0x/SF0X.cpp index 587426b446..c05d129e02 100644 --- a/src/drivers/distance_sensor/sf0x/SF0X.cpp +++ b/src/drivers/distance_sensor/sf0x/SF0X.cpp @@ -33,26 +33,16 @@ #include "SF0X.hpp" +#include + +/* Configuration Constants */ +#define SF0X_TAKE_RANGE_REG 'd' + SF0X::SF0X(const char *port, uint8_t rotation) : - CDev(RANGE_FINDER0_DEVICE_PATH), ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), - _rotation(rotation), - _min_distance(0.30f), - _max_distance(40.0f), - _conversion_interval(83334), - _reports(nullptr), - _measure_interval(0), - _collect_phase(false), - _fd(-1), - _linebuf_index(0), - _parse_state(SF0X_PARSE_STATE0_UNSYNC), - _last_read(0), - _class_instance(-1), - _orb_class_instance(-1), - _distance_sensor_topic(nullptr), - _consecutive_fail_count(0), - _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), - _comms_errors(perf_alloc(PC_COUNT, "sf0x_com_err")) + _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) { /* store port name */ strncpy(_port, port, sizeof(_port) - 1); @@ -60,22 +50,13 @@ SF0X::SF0X(const char *port, uint8_t rotation) : /* enforce null termination */ _port[sizeof(_port) - 1] = '\0'; + _px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); } SF0X::~SF0X() { - /* make sure we are truly inactive */ stop(); - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - - if (_class_instance != -1) { - unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); - } - perf_free(_sample_perf); perf_free(_comms_errors); } @@ -83,40 +64,39 @@ SF0X::~SF0X() int SF0X::init() { - int hw_model; + int32_t hw_model = 0; param_get(param_find("SENS_EN_SF0X"), &hw_model); switch (hw_model) { - case 1: /* SF02 (40m, 12 Hz)*/ - _min_distance = 0.3f; - _max_distance = 40.0f; - _conversion_interval = 83334; + _px4_rangefinder.set_min_distance(0.30f); + _px4_rangefinder.set_max_distance(40.0f); + _interval = 83334; break; case 2: /* SF10/a (25m 32Hz) */ - _min_distance = 0.01f; - _max_distance = 25.0f; - _conversion_interval = 31250; + _px4_rangefinder.set_min_distance(0.01f); + _px4_rangefinder.set_max_distance(25.0f); + _interval = 31250; break; case 3: /* SF10/b (50m 32Hz) */ - _min_distance = 0.01f; - _max_distance = 50.0f; - _conversion_interval = 31250; + _px4_rangefinder.set_min_distance(0.01f); + _px4_rangefinder.set_max_distance(50.0f); + _interval = 31250; break; case 4: /* SF10/c (100m 16Hz) */ - _min_distance = 0.01f; - _max_distance = 100.0f; - _conversion_interval = 62500; + _px4_rangefinder.set_min_distance(0.01f); + _px4_rangefinder.set_max_distance(100.0f); + _interval = 62500; break; case 5: /* SF11/c (120m 20Hz) */ - _min_distance = 0.01f; - _max_distance = 120.0f; - _conversion_interval = 50000; + _px4_rangefinder.set_min_distance(0.01f); + _px4_rangefinder.set_max_distance(120.0f); + _interval = 50000; break; default: @@ -124,197 +104,16 @@ SF0X::init() return -1; } - /* status */ - int ret = 0; + start(); - do { /* create a scope to handle exit conditions using break */ - - /* do regular cdev init */ - ret = CDev::init(); - - if (ret != OK) { break; } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); - - if (_reports == nullptr) { - PX4_ERR("alloc failed"); - ret = -1; - break; - } - - _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; - - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_HIGH); - - if (_distance_sensor_topic == nullptr) { - PX4_ERR("failed to create distance_sensor object"); - } - - } while (0); - - return ret; + return PX4_OK; } -void -SF0X::set_minimum_distance(float min) +int SF0X::measure() { - _min_distance = min; -} - -void -SF0X::set_maximum_distance(float max) -{ - _max_distance = max; -} - -float -SF0X::get_minimum_distance() -{ - return _min_distance; -} - -float -SF0X::get_maximum_distance() -{ - return _max_distance; -} - -int -SF0X::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_interval = (_conversion_interval); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* convert hz to tick interval via microseconds */ - int interval = (1000000 / arg); - - /* check against maximum rate */ - if (interval < (_conversion_interval)) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } -} - -ssize_t -SF0X::read(device::file_t *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct distance_sensor_s); - struct distance_sensor_s *rbuf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_measure_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(rbuf)) { - ret += sizeof(*rbuf); - rbuf++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - do { - _reports->flush(); - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - px4_usleep(_conversion_interval); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - if (_reports->get(rbuf)) { - ret = sizeof(*rbuf); - } - - } while (0); - - return ret; -} - -int -SF0X::measure() -{ - int ret; - - /* - * Send the command to begin a measurement. - */ + // Send the command to begin a measurement. char cmd = SF0X_TAKE_RANGE_REG; - ret = ::write(_fd, &cmd, 1); + int ret = ::write(_fd, &cmd, 1); if (ret != sizeof(cmd)) { perf_count(_comms_errors); @@ -322,13 +121,10 @@ SF0X::measure() return ret; } - ret = OK; - - return ret; + return PX4_OK; } -int -SF0X::collect() +int SF0X::collect() { perf_begin(_sample_perf); @@ -340,6 +136,7 @@ SF0X::collect() unsigned readlen = sizeof(readbuf) - 1; /* read from the sensor (uart buffer) */ + const hrt_abstime timestamp_sample = hrt_absolute_time(); int ret = ::read(_fd, &readbuf[0], readlen); if (ret < 0) { @@ -348,7 +145,7 @@ SF0X::collect() perf_end(_sample_perf); /* only throw an error if we time out */ - if (read_elapsed > (_conversion_interval * 2)) { + if (read_elapsed > (_interval * 2)) { return ret; } else { @@ -376,52 +173,28 @@ SF0X::collect() PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO")); - struct distance_sensor_s report; - - report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - report.orientation = _rotation; - report.current_distance = distance_m; - report.min_distance = get_minimum_distance(); - report.max_distance = get_maximum_distance(); - report.variance = 0.0f; - report.signal_quality = -1; - /* TODO: set proper ID */ - report.id = 0; - - /* publish it */ - orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); - - _reports->force(&report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - ret = OK; + _px4_rangefinder.update(timestamp_sample, distance_m); perf_end(_sample_perf); - return ret; + + return PX4_OK; } -void -SF0X::start() +void SF0X::start() { /* reset the report ring and state machine */ _collect_phase = false; - _reports->flush(); /* schedule a cycle to start things */ ScheduleNow(); } -void -SF0X::stop() +void SF0X::stop() { ScheduleClear(); } -void -SF0X::Run() +void SF0X::Run() { /* fds initialized? */ if (_fd < 0) { @@ -447,7 +220,7 @@ SF0X::Run() uart_config.c_cflag &= ~(CSTOPB | PARENB); /* if distance sensor model is SF11/C, then set baudrate 115200, else 9600 */ - int hw_model; + int32_t hw_model = 0; param_get(param_find("SENS_EN_SF0X"), &hw_model); @@ -507,17 +280,6 @@ SF0X::Run() /* next phase is measurement */ _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_interval > (_conversion_interval)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - _conversion_interval); - - return; - } } /* measurement phase */ @@ -529,14 +291,13 @@ SF0X::Run() _collect_phase = true; /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(_conversion_interval); + ScheduleDelayed(_interval); } -void -SF0X::print_info() +void SF0X::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); - printf("poll interval: %d\n", _measure_interval); - _reports->print_info("report queue"); + + _px4_rangefinder.print_status(); } diff --git a/src/drivers/distance_sensor/sf0x/SF0X.hpp b/src/drivers/distance_sensor/sf0x/SF0X.hpp index 41185dad0d..62fd103799 100644 --- a/src/drivers/distance_sensor/sf0x/SF0X.hpp +++ b/src/drivers/distance_sensor/sf0x/SF0X.hpp @@ -43,111 +43,47 @@ #include #include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - +#include #include -#include -#include -#include #include - -#include -#include +#include #include "sf0x_parser.h" -/* Configuration Constants */ - -#define SF0X_TAKE_RANGE_REG 'd' - -// designated SERIAL4/5 on Pixhawk -#define SF0X_DEFAULT_PORT "/dev/ttyS6" - -class SF0X : public cdev::CDev, public px4::ScheduledWorkItem +class SF0X : public px4::ScheduledWorkItem { public: - SF0X(const char *port = SF0X_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); - virtual ~SF0X(); + SF0X(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + ~SF0X() override; - virtual int init() override; - - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - - /** - * Diagnostics - print some basic information about the driver. - */ + int init(); void print_info(); private: - char _port[20]; - uint8_t _rotation; - float _min_distance; - float _max_distance; - int _conversion_interval; - ringbuffer::RingBuffer *_reports; - int _measure_interval; - bool _collect_phase; - int _fd; - char _linebuf[10]; - unsigned _linebuf_index; - enum SF0X_PARSE_STATE _parse_state; - hrt_abstime _last_read; - int _class_instance; - int _orb_class_instance; + void start(); + void stop(); + void Run() override; + int measure(); + int collect(); - orb_advert_t _distance_sensor_topic; + + PX4Rangefinder _px4_rangefinder; + + char _port[20] {}; + int _interval{100000}; + bool _collect_phase{false}; + int _fd{-1}; + char _linebuf[10] {}; + unsigned _linebuf_index{0}; + enum SF0X_PARSE_STATE _parse_state {SF0X_PARSE_STATE0_UNSYNC}; + hrt_abstime _last_read{0}; unsigned _consecutive_fail_count; perf_counter_t _sample_perf; perf_counter_t _comms_errors; - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * Set the min and max distance thresholds if you want the end points of the sensors - * range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE - * and SF0X_MAX_DISTANCE - */ - void set_minimum_distance(float min); - void set_maximum_distance(float max); - float get_minimum_distance(); - float get_maximum_distance(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; - int measure(); - int collect(); }; diff --git a/src/drivers/distance_sensor/sf0x/sf0x_main.cpp b/src/drivers/distance_sensor/sf0x/sf0x_main.cpp index b6713bc613..0284037d50 100644 --- a/src/drivers/distance_sensor/sf0x/sf0x_main.cpp +++ b/src/drivers/distance_sensor/sf0x/sf0x_main.cpp @@ -34,34 +34,15 @@ #include "SF0X.hpp" #include +#include -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int sf0x_main(int argc, char *argv[]); - -/** - * Local functions in support of the shell command. - */ namespace sf0x { -SF0X *g_dev; +SF0X *g_dev{nullptr}; -int start(const char *port, uint8_t rotation); -int stop(); -int test(); -int reset(); -int info(); - -/** - * Start the driver. - */ -int -start(const char *port, uint8_t rotation) +static int start(const char *port, uint8_t rotation) { - int fd; - if (g_dev != nullptr) { PX4_WARN("already started"); return -1; @@ -71,41 +52,19 @@ start(const char *port, uint8_t rotation) g_dev = new SF0X(port, rotation); if (g_dev == nullptr) { - goto fail; + return -1; } - if (OK != g_dev->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = open(RANGE_FINDER0_DEVICE_PATH, 0); - - if (fd < 0) { - PX4_ERR("device open fail (%i)", errno); - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; + if (g_dev->init() != PX4_OK) { + delete g_dev; + g_dev = nullptr; + return -1; } return 0; - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - return -1; } -/** - * Stop the driver - */ -int stop() +static int stop() { if (g_dev != nullptr) { delete g_dev; @@ -118,124 +77,53 @@ int stop() return 0; } -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -int -test() -{ - struct distance_sensor_s report; - ssize_t sz; - - int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH); - return -1; - } - - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("immediate read failed"); - return -1; - } - - print_message(report); - - /* start the sensor polling at 2 Hz rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - PX4_ERR("failed to set 2Hz poll rate"); - return -1; - } - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - int ret = poll(&fds, 1, 2000); - - if (ret != 1) { - PX4_ERR("timed out"); - break; - } - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("read failed: got %zi vs exp. %zu", sz, sizeof(report)); - break; - } - - print_message(report); - } - - /* reset the sensor polling to the default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - PX4_ERR("ioctl SENSORIOCSPOLLRATE failed"); - return -1; - } - - return 0; -} - -/** - * Reset the driver. - */ -int -reset() -{ - int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("open failed (%i)", errno); - return -1; - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - return -1; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - return -1; - } - - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() +static int status() { if (g_dev == nullptr) { PX4_ERR("driver not running"); return -1; } - printf("state @ %p\n", g_dev); g_dev->print_info(); return 0; } +static int usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Serial bus driver for the LightWare SF02/F, SF10/a, SF10/b, SF10/c, SF11/c Laser rangefinders. + +Most boards are configured to enable/start the driver on a specified UART using the SENS_SF0X_CFG parameter. + +Setup/usage information: https://docs.px4.io/en/sensor/sfxx_lidar.html + +### Examples + +Attempt to start driver on a specified serial device. +$ sf0x start -d /dev/ttyS1 +Stop driver +$ sf0x stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sf0x", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", 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"); + return PX4_OK; +} + } // namespace -int -sf0x_main(int argc, char *argv[]) +extern "C" __EXPORT int sf0x_main(int argc, char *argv[]) { uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - const char *device_path = SF0X_DEFAULT_PORT; + const char *device_path = nullptr; int ch; int myoptind = 1; const char *myoptarg = nullptr; @@ -251,51 +139,26 @@ sf0x_main(int argc, char *argv[]) break; default: - PX4_WARN("Unknown option!"); + sf0x::usage(); return -1; } } if (myoptind >= argc) { - goto out_error; + sf0x::usage(); + return -1; } - /* - * Start/load the driver. - */ if (!strcmp(argv[myoptind], "start")) { return sf0x::start(device_path, rotation); - } - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { + } else if (!strcmp(argv[myoptind], "stop")) { return sf0x::stop(); + + } else if (!strcmp(argv[myoptind], "status")) { + return sf0x::status(); } - /* - * Test the driver/device. - */ - if (!strcmp(argv[myoptind], "test")) { - return sf0x::test(); - } - - /* - * Reset the driver. - */ - if (!strcmp(argv[myoptind], "reset")) { - return sf0x::reset(); - } - - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { - return sf0x::info(); - } - -out_error: - PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); + sf0x::usage(); return -1; }