From 008efff42ea0fec64173941bd8a746ec0e7ecfa4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Mar 2014 20:00:06 +0100 Subject: Added support for driver rangefinder --- src/drivers/sf0x/sf0x.cpp | 829 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 829 insertions(+) create mode 100644 src/drivers/sf0x/sf0x.cpp (limited to 'src/drivers/sf0x/sf0x.cpp') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp new file mode 100644 index 000000000..40be85b27 --- /dev/null +++ b/src/drivers/sf0x/sf0x.cpp @@ -0,0 +1,829 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sf0x.cpp + * @author Lorenz Meier + * + * Driver for the Lightware SF0x 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 + +/* Configuration Constants */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +#define SF0X_CONVERSION_INTERVAL 84000 +#define SF0X_TAKE_RANGE_REG 'D' +#define SF02F_MIN_DISTANCE 0.0f +#define SF02F_MAX_DISTANCE 40.0f + +class SF0X : public device::CDev +{ +public: + SF0X(const char* port="/dev/ttyS1"); + virtual ~SF0X(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _fd; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * 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 cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int sf0x_main(int argc, char *argv[]); + +SF0X::SF0X(const char *port) : + CDev("SF0X", RANGE_FINDER_DEVICE_PATH), + _min_distance(SF02F_MIN_DISTANCE), + _max_distance(SF02F_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), + _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows")) +{ + /* open fd */ + _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); + + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +SF0X::~SF0X() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } +} + +int +SF0X::init() +{ + int ret = ERROR; + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + goto out; + } + + /* get a publish handle on the range finder topic */ + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +SF0X::probe() +{ + return measure(); +} + +void +SF0X::set_minimum_distance(float min) +{ + _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(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(SF0X_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_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(SF0X_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +ssize_t +SF0X::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 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 */ + usleep(SF0X_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. + */ + char cmd = SF0X_TAKE_RANGE_REG; + ret = ::write(_fd, &cmd, 1); + + if (OK != sizeof(cmd)) { + perf_count(_comms_errors); + log("serial transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +SF0X::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + float val; + + perf_begin(_sample_perf); + + char buf[16]; + ret = ::read(_fd, buf, sizeof(buf)); + printf("ret: %d, val: %s", ret, buf); + + if (ret < 0) { + log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + float si_units = 0.0f; + struct range_finder_report report; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +SF0X::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +SF0X::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +SF0X::cycle_trampoline(void *arg) +{ + SF0X *dev = (SF0X *)arg; + + dev->cycle(); +} + +void +SF0X::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(SF0X_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(SF0X_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + log("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + USEC2TICK(SF0X_CONVERSION_INTERVAL)); +} + +void +SF0X::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace sf0x +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +SF0X *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new SF0X(); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* 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; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +sf0x_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + sf0x::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + sf0x::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + sf0x::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + sf0x::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + sf0x::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} -- cgit v1.2.3 From 7cc33b9ca6c0facd6e2f38f0bc515e9769e13073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Mar 2014 21:06:29 +0100 Subject: Complete output parsing, pending testing --- src/drivers/sf0x/sf0x.cpp | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) (limited to 'src/drivers/sf0x/sf0x.cpp') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 40be85b27..d3043a26b 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -33,7 +33,8 @@ /** * @file sf0x.cpp - * @author Lorenz Meier + * @author Lorenz Meier + * @author Greg Hulands * * Driver for the Lightware SF0x laser rangefinder series */ @@ -459,25 +460,26 @@ SF0X::measure() int SF0X::collect() { - int ret = -EIO; - - /* read from the sensor */ - float val; + int ret; perf_begin(_sample_perf); char buf[16]; + /* read from the sensor (uart buffer) */ ret = ::read(_fd, buf, sizeof(buf)); - printf("ret: %d, val: %s", ret, buf); - if (ret < 0) { + if (ret < 1) { log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - float si_units = 0.0f; + printf("ret: %d, val (str): %s\n", ret, buf); + char* end; + float si_units = strtod(buf,&end); + printf("val (float): %8.4f\n", si_units); + struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ @@ -538,7 +540,7 @@ SF0X::stop() void SF0X::cycle_trampoline(void *arg) { - SF0X *dev = (SF0X *)arg; + SF0X *dev = static_cast(arg); dev->cycle(); } @@ -598,7 +600,7 @@ SF0X::print_info() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); + printf("poll interval: %d ticks\n", _measure_ticks); _reports->print_info("report queue"); } @@ -694,7 +696,6 @@ test() { struct range_finder_report report; ssize_t sz; - int ret; int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); @@ -725,7 +726,7 @@ test() /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; - ret = poll(&fds, 1, 2000); + int ret = poll(&fds, 1, 2000); if (ret != 1) { errx(1, "timed out waiting for sensor data"); -- cgit v1.2.3 From 171af566f716a99b078eaa4ef0f90405091e8d19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Mar 2014 21:45:22 +0100 Subject: Allow to set the UART via start argument, cleanups --- src/drivers/sf0x/sf0x.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) (limited to 'src/drivers/sf0x/sf0x.cpp') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d3043a26b..f25856689 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -87,11 +87,12 @@ static const int ERROR = -1; #define SF0X_TAKE_RANGE_REG 'D' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f +#define SF0X_DEFAULT_PORT "/dev/ttyS2" class SF0X : public device::CDev { public: - SF0X(const char* port="/dev/ttyS1"); + SF0X(const char* port=SF0X_DEFAULT_PORT); virtual ~SF0X(); virtual int init(); @@ -628,7 +629,7 @@ void info(); * Start the driver. */ void -start() +start(const char* port) { int fd; @@ -637,7 +638,7 @@ start() } /* create the driver */ - g_dev = new SF0X(); + g_dev = new SF0X(port); if (g_dev == nullptr) { goto fail; @@ -795,7 +796,11 @@ sf0x_main(int argc, char *argv[]) * Start/load the driver. */ if (!strcmp(argv[1], "start")) { - sf0x::start(); + if (argc > 2) { + sf0x::start(argv[2]); + } else { + sf0x::start(SF0X_DEFAULT_PORT); + } } /* -- cgit v1.2.3 From d69d895f0240a0dda5fda2f8f046179b069439a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Mar 2014 00:46:13 +0100 Subject: Add missing CDev init step --- src/drivers/sf0x/sf0x.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src/drivers/sf0x/sf0x.cpp') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index f25856689..5dd1f59de 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -209,6 +209,10 @@ SF0X::init() { int ret = ERROR; + /* do regular cdev init */ + if (CDev::init() != OK) + goto out; + /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); @@ -447,7 +451,7 @@ SF0X::measure() char cmd = SF0X_TAKE_RANGE_REG; ret = ::write(_fd, &cmd, 1); - if (OK != sizeof(cmd)) { + if (ret != sizeof(cmd)) { perf_count(_comms_errors); log("serial transfer returned %d", ret); return ret; -- cgit v1.2.3 From d2905b8d8eba3a3b1e4258343e7a384071c55073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Mar 2014 11:18:02 +0100 Subject: SF02/F operational, but not cleaned up / optimized - good enough for first logging --- src/drivers/sf0x/sf0x.cpp | 137 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 106 insertions(+), 31 deletions(-) (limited to 'src/drivers/sf0x/sf0x.cpp') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 5dd1f59de..7b4b49813 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -83,8 +84,8 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -#define SF0X_CONVERSION_INTERVAL 84000 -#define SF0X_TAKE_RANGE_REG 'D' +#define SF0X_CONVERSION_INTERVAL 90000 +#define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f #define SF0X_DEFAULT_PORT "/dev/ttyS2" @@ -186,8 +187,39 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); - // enable debug() calls - _debug_enabled = true; + if (_fd < 0) { + warnx("FAIL: laser fd"); + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + unsigned speed = B9600; + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERR CFG: %d ISPD", termios_state); + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERR CFG: %d OSPD\n", termios_state); + } + + if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR baud %d ATTR", termios_state); + } + + // disable debug() calls + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -208,6 +240,7 @@ int SF0X::init() { int ret = ERROR; + unsigned i = 0; /* do regular cdev init */ if (CDev::init() != OK) @@ -217,6 +250,7 @@ SF0X::init() _reports = new RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) { + warnx("mem err"); goto out; } @@ -226,12 +260,33 @@ SF0X::init() _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + warnx("advert err"); } - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; + /* attempt to get a measurement 5 times */ + while (ret != OK && i < 5) { + + if (measure()) { + ret = ERROR; + _sensor_ok = false; + } + + usleep(100000); + if (collect()) { + ret = ERROR; + _sensor_ok = false; + } else { + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; + } + + i++; + } + + /* close the fd */ + ::close(_fd); + _fd = -1; out: return ret; } @@ -453,7 +508,7 @@ SF0X::measure() if (ret != sizeof(cmd)) { perf_count(_comms_errors); - log("serial transfer returned %d", ret); + log("write fail %d", ret); return ret; } @@ -473,17 +528,30 @@ SF0X::collect() /* read from the sensor (uart buffer) */ ret = ::read(_fd, buf, sizeof(buf)); - if (ret < 1) { - log("error reading from sensor: %d", ret); + if (ret < 3) { + log("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - printf("ret: %d, val (str): %s\n", ret, buf); + if (buf[ret - 2] != '\r' || buf[ret - 1] != '\n') { + /* incomplete read */ + return -1; + } + char* end; - float si_units = strtod(buf,&end); - printf("val (float): %8.4f\n", si_units); + float si_units; + bool valid; + + if (buf[0] == '-' && buf[1] == '-' && buf[2] == '.') { + si_units = -1.0f; + valid = false; + } else { + si_units = strtod(buf,&end); + valid = true; + } + debug("val (float): %8.4f, raw: %s\n", si_units, buf); struct range_finder_report report; @@ -491,7 +559,7 @@ SF0X::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; - report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); @@ -519,21 +587,21 @@ SF0X::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1); - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_RANGEFINDER - }; - static orb_advert_t pub = -1; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } + // /* notify about state change */ + // struct subsystem_info_s info = { + // true, + // true, + // true, + // SUBSYSTEM_TYPE_RANGEFINDER + // }; + // static orb_advert_t pub = -1; + + // if (pub > 0) { + // orb_publish(ORB_ID(subsystem_info), pub, &info); + + // } else { + // pub = orb_advertise(ORB_ID(subsystem_info), &info); + // } } void @@ -553,6 +621,12 @@ SF0X::cycle_trampoline(void *arg) void SF0X::cycle() { + /* fds initialized? */ + if (_fd < 0) { + /* open fd */ + _fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK); + } + /* collection phase? */ if (_collect_phase) { @@ -653,9 +727,10 @@ start(const char* port) } /* set the poll rate to default, starts automatic data collection */ - fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + fd = open(RANGE_FINDER_DEVICE_PATH, 0); if (fd < 0) { + warnx("device open fail"); goto fail; } -- cgit v1.2.3 From 1c488a95da934817999a0a40ccb87e07bf5e0307 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Mar 2014 18:46:22 +0100 Subject: Added laser range finder logging, needs testing --- src/drivers/sf0x/sf0x.cpp | 30 ++++++++++++++++++++++++------ 1 file changed, 24 insertions(+), 6 deletions(-) (limited to 'src/drivers/sf0x/sf0x.cpp') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 7b4b49813..685b96646 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -118,6 +118,9 @@ private: int _measure_ticks; bool _collect_phase; int _fd; + char _linebuf[10]; + unsigned _linebuf_index; + hrt_abstime _last_read; orb_advert_t _range_finder_topic; @@ -179,6 +182,9 @@ SF0X::SF0X(const char *port) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), + _fd(-1), + _linebuf_index(0), + _last_read(0), _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), @@ -187,6 +193,10 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); + /* tell it to stop auto-triggering */ + char stop_auto = ' '; + ::write(_fd, &stop_auto, 1); + if (_fd < 0) { warnx("FAIL: laser fd"); } @@ -219,7 +229,7 @@ SF0X::SF0X(const char *port) : } // disable debug() calls - _debug_enabled = false; + _debug_enabled = true; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -524,18 +534,26 @@ SF0X::collect() perf_begin(_sample_perf); - char buf[16]; + /* clear buffer if last read was too long ago */ + if (hrt_elapsed(&_last_read) > (SF0X_CONVERSION_INTERVAL * 2)) { + _linebuf_index = 0; + } + /* read from the sensor (uart buffer) */ - ret = ::read(_fd, buf, sizeof(buf)); + ret = ::read(_fd, _linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); - if (ret < 3) { + if (ret < 1) { log("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - if (buf[ret - 2] != '\r' || buf[ret - 1] != '\n') { + _linebuf_index += ret; + + _last_read = hrt_absolute_time(); + + if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { /* incomplete read */ return -1; } @@ -544,7 +562,7 @@ SF0X::collect() float si_units; bool valid; - if (buf[0] == '-' && buf[1] == '-' && buf[2] == '.') { + if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; } else { -- cgit v1.2.3 From 7851472ae9f19341a1a8905eed63c2c907f16385 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 10:52:53 +0100 Subject: Laser range finder ready to roll, logging tested --- src/drivers/sf0x/sf0x.cpp | 68 ++++++++++++++++++++++++++++++++++++----------- 1 file changed, 52 insertions(+), 16 deletions(-) (limited to 'src/drivers/sf0x/sf0x.cpp') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 685b96646..dace02cf8 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -84,7 +84,7 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -#define SF0X_CONVERSION_INTERVAL 90000 +#define SF0X_CONVERSION_INTERVAL 83334 #define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f @@ -193,14 +193,16 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); - /* tell it to stop auto-triggering */ - char stop_auto = ' '; - ::write(_fd, &stop_auto, 1); - if (_fd < 0) { warnx("FAIL: laser fd"); } + /* tell it to stop auto-triggering */ + char stop_auto = ' '; + (void)::write(_fd, &stop_auto, 1); + usleep(100); + (void)::write(_fd, &stop_auto, 1); + struct termios uart_config; int termios_state; @@ -229,7 +231,7 @@ SF0X::SF0X(const char *port) : } // disable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -535,41 +537,58 @@ SF0X::collect() perf_begin(_sample_perf); /* clear buffer if last read was too long ago */ - if (hrt_elapsed(&_last_read) > (SF0X_CONVERSION_INTERVAL * 2)) { + uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; } /* read from the sensor (uart buffer) */ - ret = ::read(_fd, _linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); + ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); - if (ret < 1) { - log("read err: %d", ret); + if (ret < 0) { + _linebuf[sizeof(_linebuf) - 1] = '\0'; + debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); perf_count(_comms_errors); perf_end(_sample_perf); - return ret; + + /* only throw an error if we time out */ + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { + return ret; + } else { + return -EAGAIN; + } } _linebuf_index += ret; + if (_linebuf_index >= sizeof(_linebuf)) { + _linebuf_index = 0; + } _last_read = hrt_absolute_time(); if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - /* incomplete read */ - return -1; + /* incomplete read, reschedule ourselves */ + return -EAGAIN; } char* end; float si_units; bool valid; + /* enforce line ending */ + _linebuf[sizeof(_linebuf) - 1] = '\0'; + if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; } else { - si_units = strtod(buf,&end); + si_units = strtod(_linebuf,&end); valid = true; } - debug("val (float): %8.4f, raw: %s\n", si_units, buf); + debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); + + /* done with this chunk, resetting */ + _linebuf_index = 0; struct range_finder_report report; @@ -649,7 +668,19 @@ SF0X::cycle() if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + int collect_ret = collect(); + + if (collect_ret == -EAGAIN) { + /* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + USEC2TICK(1100)); + return; + } + + if (OK != collect_ret) { log("collection error"); /* restart the measurement state machine */ start(); @@ -842,6 +873,11 @@ test() warnx("time: %lld", report.timestamp); } + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + errx(0, "PASS"); } -- cgit v1.2.3 From b7662537df656f8546acecc9cca6ab2ac40714bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 10:54:48 +0100 Subject: Fix code style of laser rangefinder --- src/drivers/sf0x/sf0x.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) (limited to 'src/drivers/sf0x/sf0x.cpp') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index dace02cf8..70cd1ab1e 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -93,7 +93,7 @@ static const int ERROR = -1; class SF0X : public device::CDev { public: - SF0X(const char* port=SF0X_DEFAULT_PORT); + SF0X(const char *port = SF0X_DEFAULT_PORT); virtual ~SF0X(); virtual int init(); @@ -255,8 +255,9 @@ SF0X::init() unsigned i = 0; /* do regular cdev init */ - if (CDev::init() != OK) + if (CDev::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); @@ -284,9 +285,11 @@ SF0X::init() } usleep(100000); + if (collect()) { ret = ERROR; _sensor_ok = false; + } else { ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -538,6 +541,7 @@ SF0X::collect() /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; } @@ -550,16 +554,18 @@ SF0X::collect() debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); perf_count(_comms_errors); perf_end(_sample_perf); - + /* only throw an error if we time out */ if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { return ret; + } else { return -EAGAIN; } } _linebuf_index += ret; + if (_linebuf_index >= sizeof(_linebuf)) { _linebuf_index = 0; } @@ -571,7 +577,7 @@ SF0X::collect() return -EAGAIN; } - char* end; + char *end; float si_units; bool valid; @@ -581,10 +587,12 @@ SF0X::collect() if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; + } else { - si_units = strtod(_linebuf,&end); + si_units = strtod(_linebuf, &end); valid = true; } + debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); /* done with this chunk, resetting */ @@ -756,7 +764,7 @@ void info(); * Start the driver. */ void -start(const char* port) +start(const char *port) { int fd; @@ -931,6 +939,7 @@ sf0x_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (argc > 2) { sf0x::start(argv[2]); + } else { sf0x::start(SF0X_DEFAULT_PORT); } -- cgit v1.2.3