From 9f2bb6c7f9908819e624dd02c6da1f97d2cf1757 Mon Sep 17 00:00:00 2001 From: Ayush Date: Sun, 31 Dec 2017 23:20:37 +0530 Subject: [PATCH] Added support for TFmini-LiDAR --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 + src/drivers/tfmini/CMakeLists.txt | 44 + src/drivers/tfmini/tfmini.cpp | 986 ++++++++++++++++++++ src/drivers/tfmini/tfmini_parser.cpp | 180 ++++ src/drivers/tfmini/tfmini_parser.h | 72 ++ src/modules/sensors/sensor_params.c | 10 + 6 files changed, 1293 insertions(+) create mode 100644 src/drivers/tfmini/CMakeLists.txt create mode 100644 src/drivers/tfmini/tfmini.cpp create mode 100644 src/drivers/tfmini/tfmini_parser.cpp create mode 100644 src/drivers/tfmini/tfmini_parser.h diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 75cb0d4db3..2a55d8ce36 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -59,6 +59,7 @@ set(config_module_list #drivers/ulanding drivers/vmount modules/sensors + drivers/tfmini # # System commands diff --git a/src/drivers/tfmini/CMakeLists.txt b/src/drivers/tfmini/CMakeLists.txt new file mode 100644 index 0000000000..69aebd90f6 --- /dev/null +++ b/src/drivers/tfmini/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2017 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. +# +############################################################################ +px4_add_module( + MODULE drivers__tfmini + MAIN tfmini + COMPILE_FLAGS + -Wno-sign-compare + SRCS + tfmini.cpp + tfmini_parser.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/tfmini/tfmini.cpp b/src/drivers/tfmini/tfmini.cpp new file mode 100644 index 0000000000..7dee05ae5b --- /dev/null +++ b/src/drivers/tfmini/tfmini.cpp @@ -0,0 +1,986 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 tfmini.cpp + * @author Lorenz Meier + * @author Greg Hulands + * @author Ayush Gaud + * + * 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 + +#include + +#include "tfmini_parser.h" + +/* Configuration Constants */ + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +// designated SERIAL4/5 on Pixhawk +#define TFMINI_DEFAULT_PORT "/dev/ttyS6" + +class TFMINI : public device::CDev +{ +public: + TFMINI(const char *port = TFMINI_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + virtual ~TFMINI(); + + 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(); + +private: + char _port[20]; + uint8_t _rotation; + float _min_distance; + float _max_distance; + int _conversion_interval; + work_s _work; + ringbuffer::RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + 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; + + orb_advert_t _distance_sensor_topic; + + 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 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(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + 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 tfmini_main(int argc, char *argv[]); + +TFMINI::TFMINI(const char *port, uint8_t rotation) : + CDev("TFMINI", RANGE_FINDER0_DEVICE_PATH), + _rotation(rotation), + _min_distance(0.30f), + _max_distance(12.0f), + _conversion_interval(10000), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(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), + _consecutive_fail_count(0), + _sample_perf(perf_alloc(PC_ELAPSED, "tfmini_read")), + _comms_errors(perf_alloc(PC_COUNT, "tfmini_com_err")) +{ + /* store port name */ + strncpy(_port, port, sizeof(_port)); + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; + + /* open fd */ + _fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC); + + /*baudrate 115200, 8 bits, no parity, 1 stop bit */ + unsigned speed = B115200; + + struct termios uart_config; + + int termios_state; + + tcgetattr(_fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* 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); + } + + uart_config.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ + uart_config.c_cflag &= ~CSIZE; + uart_config.c_cflag |= CS8; /* 8-bit characters */ + uart_config.c_cflag &= ~PARENB; /* no parity bit */ + uart_config.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ + uart_config.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ + + /* setup for non-canonical mode */ + uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); + uart_config.c_oflag &= ~OPOST; + + /* fetch bytes as they become available */ + uart_config.c_cc[VMIN] = 1; + uart_config.c_cc[VTIME] = 1; + + if (_fd < 0) { + warnx("FAIL: laser fd"); + } + + // disable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +TFMINI::~TFMINI() +{ + /* 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); +} + +int +TFMINI::init() +{ + int hw_model; + param_get(param_find("SENS_EN_TFMINI"), &hw_model); + + switch (hw_model) { + case 0: + DEVICE_LOG("disabled."); + return 0; + + case 1: /* TFMINI (12m, 100 Hz)*/ + _min_distance = 0.3f; + _max_distance = 12.0f; + _conversion_interval = 10000; + break; + + default: + DEVICE_LOG("invalid HW model %d.", hw_model); + return -1; + } + + /* status */ + int ret = 0; + + 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) { + warnx("mem err"); + 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) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); + } + + } while (0); + + /* close the fd */ + ::close(_fd); + _fd = -1; + + return OK; +} + +void +TFMINI::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +TFMINI::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +TFMINI::get_minimum_distance() +{ + return _min_distance; +} + +float +TFMINI::get_maximum_distance() +{ + return _max_distance; +} + +int +TFMINI::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(_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(_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 = px4_enter_critical_section(); + + if (!_reports->resize(arg)) { + px4_leave_critical_section(flags); + return -ENOMEM; + } + + px4_leave_critical_section(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 +TFMINI::read(struct file *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_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(); + + /* wait for it to complete */ + 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 +TFMINI::collect() +{ + int ret; + + perf_begin(_sample_perf); + + /* clear buffer if last read was too long ago */ + uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + + /* the buffer for read chars is buflen minus null termination */ + char readbuf[sizeof(_linebuf)]; + unsigned readlen = sizeof(readbuf) - 1; + + /* read from the sensor (uart buffer) */ + ret = ::read(_fd, &readbuf[0], readlen); + + if (ret < 0) { + DEVICE_DEBUG("read err: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + + /* only throw an error if we time out */ + if (read_elapsed > (_conversion_interval * 2)) { + return ret; + + } else { + return -EAGAIN; + } + + } else if (ret == 0) { + return -EAGAIN; + } + + _last_read = hrt_absolute_time(); + + float distance_m = -1.0f; + bool valid = false; + + for (int i = 0; i < ret; i++) { + if (OK == tfmini_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) { + valid = true; + } + } + + if (!valid) { + return -EAGAIN; + } + + DEVICE_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.covariance = 0.0f; + /* 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; + + perf_end(_sample_perf); + return ret; +} + +void +TFMINI::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)&TFMINI::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = {}; + info.present = true; + info.enabled = true; + info.ok = true; + info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER; + + static orb_advert_t pub = nullptr; + + if (pub != nullptr) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + + } +} + +void +TFMINI::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +TFMINI::cycle_trampoline(void *arg) +{ + TFMINI *dev = static_cast(arg); + + dev->cycle(); +} + +void +TFMINI::cycle() +{ + /* fds initialized? */ + if (_fd < 0) { + /* open fd */ + _fd = ::open(TFMINI_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_SYNC); + } + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + int collect_ret = collect(); + + if (collect_ret == -EAGAIN) { + /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ + work_queue(HPWORK, + &_work, + (worker_t)&TFMINI::cycle_trampoline, + this, + USEC2TICK(1042 * 8)); + return; + } + + if (OK != collect_ret) { + + /* we know the sensor needs about four seconds to initialize */ + if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { + DEVICE_LOG("collection error #%u", _consecutive_fail_count); + } + + _consecutive_fail_count++; + + /* restart the measurement state machine */ + start(); + return; + + } else { + /* apparently success */ + _consecutive_fail_count = 0; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(_conversion_interval)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&TFMINI::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(_conversion_interval)); + + return; + } + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&TFMINI::cycle_trampoline, + this, + USEC2TICK(_conversion_interval)); +} + +void +TFMINI::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + printf("poll interval: %d ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace tfmini +{ + +TFMINI *g_dev; + +void start(const char *port, uint8_t rotation); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start(const char *port, uint8_t rotation) +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new TFMINI(port, rotation); + + 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_FINDER0_DEVICE_PATH, 0); + + if (fd < 0) { + warnx("device open fail"); + 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 distance_sensor_s report; + ssize_t sz; + + int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'tfmini start' if the driver is not running", RANGE_FINDER0_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.current_distance); + warnx("time: %llu", report.timestamp); + + /* start the sensor polling at 2 Hz rate */ + 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; + int ret = poll(&fds, 1, 2000); + + if (ret != 1) { + warnx("timed out"); + break; + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warnx("read failed: got %d vs exp. %d", sz, sizeof(report)); + break; + } + + warnx("read #%u", i); + warnx("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + warnx("measurement: %0.3f m", (double)report.current_distance); + warnx("time: %llu", report.timestamp); + } + + /* reset the sensor polling to the default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "ERR: DEF RATE"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER0_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 +tfmini_main(int argc, char *argv[]) +{ + // check for optional arguments + int ch; + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + int myoptind = 1; + const char *myoptarg = NULL; + + + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (uint8_t)atoi(myoptarg); + PX4_INFO("Setting distance sensor orientation to %d", (int)rotation); + break; + + default: + PX4_WARN("Unknown option!"); + } + } + + /* + * Start/load the driver. + */ + if (!strcmp(argv[myoptind], "start")) { + if (argc > myoptind + 1) { + tfmini::start(argv[myoptind + 1], rotation); + + } else { + tfmini::start(TFMINI_DEFAULT_PORT, rotation); + } + } + + /* + * Stop the driver + */ + if (!strcmp(argv[myoptind], "stop")) { + tfmini::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[myoptind], "test")) { + tfmini::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[myoptind], "reset")) { + tfmini::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) { + tfmini::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/tfmini/tfmini_parser.cpp b/src/drivers/tfmini/tfmini_parser.cpp new file mode 100644 index 0000000000..510ded0b25 --- /dev/null +++ b/src/drivers/tfmini/tfmini_parser.cpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 modified from sf0x_parser.cpp + * @author Lorenz Meier + * @author Chuong Nguyen + * @author Ayush Gaud + * + * Declarations of parser for the Benewake TFmini laser rangefinder series + */ + +#include "tfmini_parser.h" +#include +#include + +// #define TFMINI_DEBUG + +#ifdef TFMINI_DEBUG +#include + +const char *parser_state[] = { + "0_UNSYNC", + "1_SYNC_1", + "2_SYNC_2", + "3_GOT_DIST_L", + "4_GOT_DIST_H", + "5_GOT_STRENGTH_L", + "6_GOT_STRENGTH_H", + "7_GOT_PRESERVED", + "8_GOT_QUALITY" + "9_GOT_CHECKSUM" +}; +#endif + +int tfmini_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum TFMINI_PARSE_STATE *state, float *dist) +{ + int ret = -1; + //char *end; + + switch (*state) { + case TFMINI_PARSE_STATE6_GOT_CHECKSUM: + if (c == 'Y') { + *state = TFMINI_PARSE_STATE1_SYNC_1; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = TFMINI_PARSE_STATE0_UNSYNC; + } + + break; + + case TFMINI_PARSE_STATE0_UNSYNC: + if (c == 'Y') { + *state = TFMINI_PARSE_STATE1_SYNC_1; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } + + break; + + case TFMINI_PARSE_STATE1_SYNC_1: + if (c == 'Y') { + *state = TFMINI_PARSE_STATE1_SYNC_2; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + } else { + *state = TFMINI_PARSE_STATE0_UNSYNC; + *parserbuf_index = 0; + } + + break; + + case TFMINI_PARSE_STATE1_SYNC_2: + *state = TFMINI_PARSE_STATE2_GOT_DIST_L; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + break; + + case TFMINI_PARSE_STATE2_GOT_DIST_L: + *state = TFMINI_PARSE_STATE2_GOT_DIST_H; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + break; + + case TFMINI_PARSE_STATE2_GOT_DIST_H: + *state = TFMINI_PARSE_STATE3_GOT_STRENGTH_L; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + break; + + case TFMINI_PARSE_STATE3_GOT_STRENGTH_L: + *state = TFMINI_PARSE_STATE3_GOT_STRENGTH_H; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + break; + + case TFMINI_PARSE_STATE3_GOT_STRENGTH_H: + *state = TFMINI_PARSE_STATE4_GOT_RESERVED; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + break; + + case TFMINI_PARSE_STATE4_GOT_RESERVED: + *state = TFMINI_PARSE_STATE5_GOT_QUALITY; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + + break; + + case TFMINI_PARSE_STATE5_GOT_QUALITY: + // Find the checksum + unsigned char cksm = 0; + + for (int i = 0; i < 8; i++) { + cksm += parserbuf[i]; + } + + if (c == cksm) { + parserbuf[*parserbuf_index] = '\0'; + unsigned int t1 = parserbuf[2]; + unsigned int t2 = parserbuf[3]; + t2 <<= 8; + t2 += t1; + *dist = ((float)t2) / 100; + *state = TFMINI_PARSE_STATE6_GOT_CHECKSUM; + *parserbuf_index = 0; + ret = 0; + + } else { + *state = TFMINI_PARSE_STATE0_UNSYNC; + *parserbuf_index = 0; + } + + break; + } + +#ifdef TFMINI_DEBUG + printf("state: TFMINI_PARSE_STATE%s\n", parser_state[*state]); +#endif + + return ret; +} diff --git a/src/drivers/tfmini/tfmini_parser.h b/src/drivers/tfmini/tfmini_parser.h new file mode 100644 index 0000000000..88e417afbb --- /dev/null +++ b/src/drivers/tfmini/tfmini_parser.h @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 modified from sf0x_parser.cpp + * @author Lorenz Meier + * @author Chuong Nguyen + * @author Ayush Gaud + * + * Declarations of parser for the Benewake TFmini laser rangefinder series + */ + +#pragma once + +// Data Format for Benewake TFmini +// =============================== +// 9 bytes total per message: +// 1) 0x59 +// 2) 0x59 +// 3) Dist_L (low 8bit) +// 4) Dist_H (high 8bit) +// 5) Strength_L (low 8bit) +// 6) Strength_H (high 8bit) +// 7) Reserved bytes +// 8) Original signal quality degree +// 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though + + +enum TFMINI_PARSE_STATE { + TFMINI_PARSE_STATE0_UNSYNC = 0, + TFMINI_PARSE_STATE1_SYNC_1, + TFMINI_PARSE_STATE1_SYNC_2, + TFMINI_PARSE_STATE2_GOT_DIST_L, + TFMINI_PARSE_STATE2_GOT_DIST_H, + TFMINI_PARSE_STATE3_GOT_STRENGTH_L, + TFMINI_PARSE_STATE3_GOT_STRENGTH_H, + TFMINI_PARSE_STATE4_GOT_RESERVED, + TFMINI_PARSE_STATE5_GOT_QUALITY, + TFMINI_PARSE_STATE6_GOT_CHECKSUM +}; + +int tfmini_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum TFMINI_PARSE_STATE *state, float *dist); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 01609fbe83..1378c297c0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -315,3 +315,13 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f); * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f); + +/** + * Benewake TFmini laser rangefinder + * + * @reboot_required true + * + * @boolean + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_EN_TFMINI, 0);