diff --git a/src/drivers/distance_sensor/tfmini/tfmini.cpp b/src/drivers/distance_sensor/tfmini/tfmini.cpp index 6c429334ba45..5be96a7ed872 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini.cpp +++ b/src/drivers/distance_sensor/tfmini/tfmini.cpp @@ -42,41 +42,38 @@ * Driver for the Benewake TFmini laser rangefinder series */ -#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 #include -#ifdef __PX4_CYGWIN -#include -#endif - -#include -#include +#include -#include -#include +#include #include #include - +#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include +#ifdef __PX4_CYGWIN +#include +#endif #include "tfmini_parser.h" @@ -90,91 +87,80 @@ class TFMINI : public cdev::CDev, public px4::ScheduledWorkItem 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; + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; + /** * Diagnostics - print some basic information about the driver. */ 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 TFMINI_PARSE_STATE _parse_state; - - hrt_abstime _last_read; - int _class_instance; - int _orb_class_instance; + int collect(); - orb_advert_t _distance_sensor_topic; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void Run() override; /** * Initialise the automatic measurement state machine and start it. */ - void start(); + void start(); /** * Stop the automatic measurement state machine. */ - void stop(); + 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 TFMINI_MIN_DISTANCE * and TFMINI_MAX_DISTANCE */ - void set_minimum_distance(float min); - void set_maximum_distance(float max); - float get_minimum_distance(); - float get_maximum_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 collect(); + enum TFMINI_PARSE_STATE _parse_state {TFMINI_PARSE_STATE0_UNSYNC}; + + char _linebuf[10]; + char _port[20]; + + bool _collect_phase{false}; + + int _class_instance{-1}; + int _conversion_interval{9000}; + int _fd{-1}; + int _measure_interval{0}; + int _orb_class_instance{-1}; + + unsigned int _linebuf_index{0}; + + uint8_t _rotation{0}; + + float _max_distance{12.0f}; + float _min_distance{0.30f}; + + hrt_abstime _last_read{0}; + + orb_advert_t _distance_sensor_topic{nullptr}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "tfmini_com_err")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "tfmini_read")}; + + ringbuffer::RingBuffer *_reports{nullptr}; }; -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int tfmini_main(int argc, char *argv[]); TFMINI::TFMINI(const char *port, uint8_t rotation) : CDev(RANGE_FINDER0_DEVICE_PATH), ScheduledWorkItem(px4::wq_configurations::hp_default), - _rotation(rotation), - _min_distance(0.30f), - _max_distance(12.0f), - _conversion_interval(9000), - _reports(nullptr), - _measure_interval(0), - _collect_phase(false), - _fd(-1), - _linebuf_index(0), - _parse_state(TFMINI_PARSE_STATE0_UNSYNC), - _last_read(0), - _class_instance(-1), - _orb_class_instance(-1), - _distance_sensor_topic(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "tfmini_read")), - _comms_errors(perf_alloc(PC_COUNT, "tfmini_com_err")) + _rotation(rotation) { /* store port name */ strncpy(_port, port, sizeof(_port) - 1); @@ -467,7 +453,7 @@ TFMINI::collect() int64_t read_elapsed = hrt_elapsed_time(&_last_read); /* the buffer for read chars is buflen minus null termination */ - char readbuf[sizeof(_linebuf)]; + char readbuf[sizeof(_linebuf)] {}; unsigned readlen = sizeof(readbuf) - 1; int ret = 0; @@ -520,18 +506,16 @@ TFMINI::collect() } /* publish most recent valid measurement from buffer */ - distance_sensor_s report{}; - - report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - report.orientation = _rotation; + distance_sensor_s report {}; 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; + report.id = 0; // TODO: set proper ID. + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.orientation = _rotation; + report.signal_quality = -1; + report.timestamp = hrt_absolute_time(); + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + report.variance = 0.0f; /* publish it */ orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); @@ -540,11 +524,9 @@ TFMINI::collect() /* notify anyone waiting for data */ poll_notify(POLLIN); - - ret = OK; - perf_end(_sample_perf); - return ret; + + return PX4_OK; } void @@ -577,12 +559,9 @@ TFMINI::Run() if (_collect_phase) { /* perform collection */ - int collect_ret = collect(); - - if (collect_ret == -EAGAIN) { + if (collect() == -EAGAIN) { /* reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps */ ScheduleDelayed(87 * 9); - return; } @@ -596,7 +575,6 @@ TFMINI::Run() /* schedule a fresh cycle call when * we are ready to measure again */ ScheduleDelayed(_measure_interval - _conversion_interval); - return; } } @@ -627,10 +605,10 @@ namespace tfmini TFMINI *g_dev; int start(const char *port, uint8_t rotation); +int status(); int stop(); int test(); -int info(); -void usage(); +int usage(); /** * Start the driver. @@ -638,51 +616,64 @@ void usage(); int start(const char *port, uint8_t rotation) { - int fd; - if (g_dev != nullptr) { PX4_ERR("already started"); - return 1; + return PX4_OK; } - /* create the driver */ + // Instantiate the driver. g_dev = new TFMINI(port, rotation); if (g_dev == nullptr) { - goto fail; + PX4_ERR("driver start failed"); + return PX4_ERROR; } if (OK != g_dev->init()) { - goto fail; + PX4_ERR("driver start failed"); + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; } - /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); + // Set the poll rate to default, starts automatic data collection. + int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); if (fd < 0) { PX4_ERR("Opening device '%s' failed", port); - goto fail; + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; } - return 0; - -fail: + return PX4_OK; +} - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; +/** + * Print the driver status. + */ +int +status() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; } - PX4_ERR("driver start failed"); - return 1; + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return 0; } /** - * Stop the driver + * Stop the driver. */ int stop() { @@ -697,7 +688,7 @@ int stop() return 1; } - return 0; + return PX4_OK; } /** @@ -708,23 +699,21 @@ int stop() int test() { - struct distance_sensor_s report; - ssize_t sz; - int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); if (fd < 0) { PX4_ERR("%s open failed (try 'tfmini start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH); - return 1; + return PX4_ERROR; } /* do a simple demand read */ - sz = px4_read(fd, &report, sizeof(report)); + distance_sensor_s report; + ssize_t sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { PX4_ERR("immediate read failed"); close(fd); - return 1; + return PX4_ERROR; } print_message(report); @@ -732,7 +721,7 @@ test() /* start the sensor polling at 2 Hz rate */ if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { PX4_ERR("failed to set 2Hz poll rate"); - return 1; + return PX4_ERROR; } /* read the sensor 5x and report each value */ @@ -763,34 +752,17 @@ test() /* reset the sensor polling to the default rate */ if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { PX4_ERR("failed to set default poll rate"); - return 1; + return PX4_ERROR; } PX4_INFO("PASS"); - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - return 0; + return PX4_OK; } /** * Print a little info on how to use the driver. */ -void +int usage() { PRINT_MODULE_DESCRIPTION( @@ -818,13 +790,17 @@ Stop driver 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("test","Test driver (basic functional tests)"); - PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information"); + PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status"); + return PX4_OK; } } // namespace -int -tfmini_main(int argc, char *argv[]) + +/** + * Driver 'main' command. + */ +extern "C" __EXPORT int tfmini_main(int argc, char *argv[]) { int ch; uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; @@ -844,52 +820,41 @@ tfmini_main(int argc, char *argv[]) default: PX4_WARN("Unknown option!"); - return -1; + return PX4_ERROR; } } if (myoptind >= argc) { - goto out_error; + PX4_ERR("unrecognized command"); + return tfmini::usage(); } - /* - * Start/load the driver. - */ + // Start/load the driver. if (!strcmp(argv[myoptind], "start")) { if (strcmp(device_path, "") != 0) { return tfmini::start(device_path, rotation); } else { PX4_WARN("Please specify device path!"); - tfmini::usage(); - return -1; + return tfmini::usage(); } } - /* - * Stop the driver - */ + // Stop the driver if (!strcmp(argv[myoptind], "stop")) { return tfmini::stop(); } - /* - * Test the driver/device. - */ + // Test the driver/device. if (!strcmp(argv[myoptind], "test")) { return tfmini::test(); } - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { - tfmini::info(); - return 0; + // Print driver information. + if (!strcmp(argv[myoptind], "info") || + !strcmp(argv[myoptind], "status")) { + return tfmini::status(); } -out_error: - PX4_ERR("unrecognized command"); - tfmini::usage(); - return -1; + return tfmini::usage(); }