forked from Archive/PX4-Autopilot
Added support for TFmini-LiDAR
This commit is contained in:
parent
ad532d0510
commit
9f2bb6c7f9
|
@ -59,6 +59,7 @@ set(config_module_list
|
|||
#drivers/ulanding
|
||||
drivers/vmount
|
||||
modules/sensors
|
||||
drivers/tfmini
|
||||
|
||||
#
|
||||
# System commands
|
||||
|
|
|
@ -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 :
|
|
@ -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 <lm@inf.ethz.ch>
|
||||
* @author Greg Hulands
|
||||
* @author Ayush Gaud <ayush.gaud@gmail.com>
|
||||
*
|
||||
* Driver for the Benewake TFmini laser rangefinder series
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <px4_getopt.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <semaphore.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/wqueue.h>
|
||||
#include <nuttx/clock.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_range_finder.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
#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<struct distance_sensor_s *>(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<TFMINI *>(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'");
|
||||
}
|
|
@ -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 <lm@inf.ethz.ch>
|
||||
* @author Chuong Nguyen <chnguye7@asu.edu>
|
||||
* @author Ayush Gaud <ayush.gaud@gmail.com>
|
||||
*
|
||||
* Declarations of parser for the Benewake TFmini laser rangefinder series
|
||||
*/
|
||||
|
||||
#include "tfmini_parser.h"
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
// #define TFMINI_DEBUG
|
||||
|
||||
#ifdef TFMINI_DEBUG
|
||||
#include <stdio.h>
|
||||
|
||||
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;
|
||||
}
|
|
@ -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 <lm@inf.ethz.ch>
|
||||
* @author Chuong Nguyen <chnguye7@asu.edu>
|
||||
* @author Ayush Gaud <ayush.gaud@gmail.com>
|
||||
*
|
||||
* 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);
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue