fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-04 10:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

tfmini cleanup and use PX4Rangefinder

Browse Source
This commit is contained in:
Daniel Agar
2019-09-17 21:50:59 -04:00
parent 18eaeb564d
commit 9b7ef7141f
3 changed files with 109 additions and 494 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/tfmini/tfmini.cpp
+71 -456
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2017-2018 PX4 Development Team. All rights reserved. * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -42,149 +42,78 @@
* Driver for the Benewake TFmini laser rangefinder series * Driver for the Benewake TFmini laser rangefinder series
*/ */
#include <errno.h>
#include <fcntl.h>
#include <math.h>
#include <poll.h>
#include <semaphore.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <termios.h> #include <termios.h>
#include <unistd.h>
#include <board_config.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <perf/perf_counter.h> #include <perf/perf_counter.h>
#include <px4_config.h> #include <px4_config.h>
#include <px4_getopt.h> #include <px4_getopt.h>
#include <px4_module.h> #include <px4_module.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <systemlib/err.h> #include <drivers/rangefinder/PX4Rangefinder.hpp>
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h> #include <uORB/topics/distance_sensor.h>
#ifdef __PX4_CYGWIN
#include <asm/socket.h>
#endif
#include "tfmini_parser.h" #include "tfmini_parser.h"
#define TFMINI_DEFAULT_PORT "/dev/ttyS3" #define TFMINI_DEFAULT_PORT "/dev/ttyS3"
/* Configuration Constants */ using namespace time_literals;
class TFMINI : public cdev::CDev, public px4::ScheduledWorkItem class TFMINI : public px4::ScheduledWorkItem
{ {
public: public:
TFMINI(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); TFMINI(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
virtual ~TFMINI(); virtual ~TFMINI();
virtual int init() override; int init();
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override;
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info(); void print_info();
private: private:
int collect(); int collect();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override; void Run() override;
/**
* Initialise the automatic measurement state machine and start it.
*/
void start(); void start();
/**
* Stop the automatic measurement state machine.
*/
void stop(); void stop();
/** PX4Rangefinder _px4_rangefinder;
* 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();
enum TFMINI_PARSE_STATE _parse_state {TFMINI_PARSE_STATE0_UNSYNC}; TFMINI_PARSE_STATE _parse_state {TFMINI_PARSE_STATE::STATE0_UNSYNC};
char _linebuf[10]; char _linebuf[10] {};
char _port[20]; char _port[20] {};
bool _collect_phase{false}; static constexpr int kCONVERSIONINTERVAL{9_ms};
int _class_instance{-1};
int _conversion_interval{9000};
int _fd{-1}; int _fd{-1};
int _measure_interval{0};
int _orb_class_instance{-1};
unsigned int _linebuf_index{0}; unsigned int _linebuf_index{0};
uint8_t _rotation{0};
float _max_distance{12.0f};
float _min_distance{0.30f};
hrt_abstime _last_read{0}; hrt_abstime _last_read{0};
orb_advert_t _distance_sensor_topic{nullptr}; perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "tfmini_com_err")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "tfmini_read")};
ringbuffer::RingBuffer *_reports{nullptr};
}; };
TFMINI::TFMINI(const char *port, uint8_t rotation) : TFMINI::TFMINI(const char *port, uint8_t rotation) :
CDev(RANGE_FINDER0_DEVICE_PATH),
ScheduledWorkItem(px4::wq_configurations::hp_default), ScheduledWorkItem(px4::wq_configurations::hp_default),
_rotation(rotation) _px4_rangefinder(0 /* TODO: device id */, ORB_PRIO_DEFAULT, rotation)
{ {
/* store port name */ // store port name
strncpy(_port, port, sizeof(_port) - 1); strncpy(_port, port, sizeof(_port) - 1);
/* enforce null termination */ // enforce null termination
_port[sizeof(_port) - 1] = '\0'; _port[sizeof(_port) - 1] = '\0';
} }
TFMINI::~TFMINI() TFMINI::~TFMINI()
{ {
/* make sure we are truly inactive */ // make sure we are truly inactive
stop(); 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(_sample_perf);
perf_free(_comms_errors); perf_free(_comms_errors);
} }
@@ -195,10 +124,11 @@ TFMINI::init()
int32_t hw_model = 1; // only one model so far... int32_t hw_model = 1; // only one model so far...
switch (hw_model) { switch (hw_model) {
case 1: /* TFMINI (12m, 100 Hz)*/ case 1: // TFMINI (12m, 100 Hz)
_min_distance = 0.3f; _px4_rangefinder.set_min_distance(0.3f);
_max_distance = 12.0f; _px4_rangefinder.set_max_distance(12.0f);
_conversion_interval = 9000; _px4_rangefinder.set_fov(math::radians(1.15));
break; break;
default: default:
@@ -206,12 +136,12 @@ TFMINI::init()
return -1; return -1;
} }
/* status */ // status
int ret = 0; int ret = 0;
do { /* create a scope to handle exit conditions using break */ do { // create a scope to handle exit conditions using break
/* open fd */ // open fd
_fd = ::open(_port, O_RDWR | O_NOCTTY); _fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_fd < 0) { if (_fd < 0) {
@@ -219,19 +149,17 @@ TFMINI::init()
return -1; return -1;
} }
/*baudrate 115200, 8 bits, no parity, 1 stop bit */ // baudrate 115200, 8 bits, no parity, 1 stop bit
unsigned speed = B115200; unsigned speed = B115200;
termios uart_config{};
struct termios uart_config; int termios_state{};
int termios_state;
tcgetattr(_fd, &uart_config); tcgetattr(_fd, &uart_config);
/* clear ONLCR flag (which appends a CR for every LF) */ // clear ONLCR flag (which appends a CR for every LF)
uart_config.c_oflag &= ~ONLCR; uart_config.c_oflag &= ~ONLCR;
/* set baud rate */ // set baud rate
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("CFG: %d ISPD", termios_state); PX4_ERR("CFG: %d ISPD", termios_state);
ret = -1; ret = -1;
@@ -250,19 +178,19 @@ TFMINI::init()
break; break;
} }
uart_config.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ uart_config.c_cflag |= (CLOCAL | CREAD); // ignore modem controls
uart_config.c_cflag &= ~CSIZE; uart_config.c_cflag &= ~CSIZE;
uart_config.c_cflag |= CS8; /* 8-bit characters */ uart_config.c_cflag |= CS8; // 8-bit characters
uart_config.c_cflag &= ~PARENB; /* no parity bit */ uart_config.c_cflag &= ~PARENB; // no parity bit
uart_config.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ uart_config.c_cflag &= ~CSTOPB; // only need 1 stop bit
uart_config.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ uart_config.c_cflag &= ~CRTSCTS; // no hardware flowcontrol
/* setup for non-canonical mode */ // setup for non-canonical mode
uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
uart_config.c_oflag &= ~OPOST; uart_config.c_oflag &= ~OPOST;
/* fetch bytes as they become available */ // fetch bytes as they become available
uart_config.c_cc[VMIN] = 1; uart_config.c_cc[VMIN] = 1;
uart_config.c_cc[VTIME] = 1; uart_config.c_cc[VTIME] = 1;
@@ -271,177 +199,15 @@ TFMINI::init()
ret = -1; ret = -1;
break; break;
} }
/* do regular cdev init */
ret = CDev::init();
if (ret != OK) { break; }
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
PX4_ERR("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) {
PX4_ERR("failed to create distance_sensor object. Did you start uOrb?");
}
} while (0); } while (0);
/* close the fd */ // close the fd
::close(_fd); ::close(_fd);
_fd = -1; _fd = -1;
return ret; if (ret == PX4_OK) {
} start();
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(device::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* zero would be bad */
case 0:
return -EINVAL;
/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);
/* set interval for next measurement to minimum legal value */
_measure_interval = (_conversion_interval);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_interval == 0);
/* convert hz to tick interval via microseconds */
int interval = (1000000 / arg);
/* check against maximum rate */
if (interval < _conversion_interval) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
} }
}
ssize_t
TFMINI::read(device::file_t *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<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_interval > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_reports->flush();
/* wait for it to complete */
px4_usleep(_conversion_interval);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
} while (0);
return ret; return ret;
} }
@@ -451,27 +217,30 @@ TFMINI::collect()
{ {
perf_begin(_sample_perf); perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */ // clear buffer if last read was too long ago
int64_t read_elapsed = hrt_elapsed_time(&_last_read); int64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* the buffer for read chars is buflen minus null termination */ // the buffer for read chars is buflen minus null termination
char readbuf[sizeof(_linebuf)] {}; char readbuf[sizeof(_linebuf)] {};
unsigned readlen = sizeof(readbuf) - 1; unsigned readlen = sizeof(readbuf) - 1;
int ret = 0; int ret = 0;
float distance_m = -1.0f; float distance_m = -1.0f;
/* Check the number of bytes available in the buffer*/ // Check the number of bytes available in the buffer
int bytes_available = 0; int bytes_available = 0;
::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available); ::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);
if (!bytes_available) { if (!bytes_available) {
perf_end(_sample_perf);
return -EAGAIN; return -EAGAIN;
} }
/* parse entire buffer */ // parse entire buffer
const hrt_abstime timestamp_sample = hrt_absolute_time();
do { do {
/* read from the sensor (uart buffer) */ // read from the sensor (uart buffer)
ret = ::read(_fd, &readbuf[0], readlen); ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) { if (ret < 0) {
@@ -479,8 +248,8 @@ TFMINI::collect()
perf_count(_comms_errors); perf_count(_comms_errors);
perf_end(_sample_perf); perf_end(_sample_perf);
/* only throw an error if we time out */ // only throw an error if we time out
if (read_elapsed > (_conversion_interval * 2)) { if (read_elapsed > (kCONVERSIONINTERVAL * 2)) {
/* flush anything in RX buffer */ /* flush anything in RX buffer */
tcflush(_fd, TCIFLUSH); tcflush(_fd, TCIFLUSH);
return ret; return ret;
@@ -492,40 +261,25 @@ TFMINI::collect()
_last_read = hrt_absolute_time(); _last_read = hrt_absolute_time();
/* parse buffer */ // parse buffer
for (int i = 0; i < ret; i++) { for (int i = 0; i < ret; i++) {
tfmini_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m); tfmini_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m);
} }
/* bytes left to parse */ // bytes left to parse
bytes_available -= ret; bytes_available -= ret;
} while (bytes_available > 0); } while (bytes_available > 0);
/* no valid measurement after parsing buffer */ // no valid measurement after parsing buffer
if (distance_m < 0.0f) { if (distance_m < 0.0f) {
perf_end(_sample_perf);
return -EAGAIN; return -EAGAIN;
} }
/* publish most recent valid measurement from buffer */ // publish most recent valid measurement from buffer
distance_sensor_s report {}; _px4_rangefinder.update(timestamp_sample, distance_m);
report.current_distance = distance_m;
report.id = 0; // TODO: set proper ID.
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.orientation = _rotation;
report.signal_quality = -1;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.variance = 0.0f;
/* publish it */
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
perf_end(_sample_perf); perf_end(_sample_perf);
return PX4_OK; return PX4_OK;
@@ -534,12 +288,8 @@ TFMINI::collect()
void void
TFMINI::start() TFMINI::start()
{ {
/* reset the report ring and state machine */ // schedule a cycle to start things
_collect_phase = false; ScheduleOnInterval(100_us);
_reports->flush();
/* schedule a cycle to start things */
ScheduleNow();
} }
void void
@@ -551,41 +301,19 @@ TFMINI::stop()
void void
TFMINI::Run() TFMINI::Run()
{ {
/* fds initialized? */ // fds initialized?
if (_fd < 0) { if (_fd < 0) {
/* open fd */ // open fd
_fd = ::open(_port, O_RDWR | O_NOCTTY); _fd = ::open(_port, O_RDWR | O_NOCTTY);
} }
/* collection phase? */ // perform collection
if (_collect_phase) { if (collect() == -EAGAIN) {
// reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps
/* perform collection */ ScheduleClear();
if (collect() == -EAGAIN) { ScheduleOnInterval(100_us, 87 * 9);
/* reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps */ return;
ScheduleDelayed(87 * 9);
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_interval > (_conversion_interval)) {
/* schedule a fresh cycle call when
* we are ready to measure again */
ScheduleDelayed(_measure_interval - _conversion_interval);
return;
}
} }
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
ScheduleDelayed(_conversion_interval);
} }
void void
@@ -594,8 +322,8 @@ TFMINI::print_info()
printf("Using port '%s'\n", _port); printf("Using port '%s'\n", _port);
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
printf("poll interval: %d \n", _measure_interval);
_reports->print_info("report queue"); _px4_rangefinder.print_status();
} }
/** /**
@@ -604,17 +332,13 @@ TFMINI::print_info()
namespace tfmini namespace tfmini
{ {
TFMINI *g_dev; TFMINI *g_dev{nullptr};
int start(const char *port, uint8_t rotation); int start(const char *port, uint8_t rotation);
int status(); int status();
int stop(); int stop();
int test();
int usage(); int usage();
/**
* Start the driver.
*/
int int
start(const char *port, uint8_t rotation) start(const char *port, uint8_t rotation)
{ {
@@ -638,28 +362,9 @@ start(const char *port, uint8_t rotation)
return PX4_ERROR; return PX4_ERROR;
} }
// Set the poll rate to default, starts automatic data collection.
int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("Opening device '%s' failed", port);
delete g_dev;
g_dev = nullptr;
return PX4_ERROR;
}
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
delete g_dev;
g_dev = nullptr;
return PX4_ERROR;
}
return PX4_OK; return PX4_OK;
} }
/**
* Print the driver status.
*/
int int
status() status()
{ {
@@ -674,9 +379,6 @@ status()
return 0; return 0;
} }
/**
* Stop the driver.
*/
int stop() int stop()
{ {
if (g_dev != nullptr) { if (g_dev != nullptr) {
@@ -693,77 +395,6 @@ int stop()
return PX4_OK; return PX4_OK;
} }
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
int
test()
{
int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("%s open failed (try 'tfmini start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH);
return PX4_ERROR;
}
/* do a simple demand read */
distance_sensor_s report;
ssize_t sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("immediate read failed");
close(fd);
return PX4_ERROR;
}
print_message(report);
/* start the sensor polling at 2 Hz rate */
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
PX4_ERR("failed to set 2Hz poll rate");
return PX4_ERROR;
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
px4_pollfd_struct_t fds{};
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
int ret = px4_poll(&fds, 1, 2000);
if (ret != 1) {
PX4_ERR("timed out");
break;
}
/* now go get it */
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("read failed: got %zi vs exp. %zu", sz, sizeof(report));
break;
}
print_message(report);
}
/* reset the sensor polling to the default rate */
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
PX4_ERR("failed to set default poll rate");
return PX4_ERROR;
}
PX4_INFO("PASS");
return PX4_OK;
}
/**
* Print a little info on how to use the driver.
*/
int int
usage() usage()
{ {
@@ -799,13 +430,9 @@ $ tfmini stop
} // namespace } // namespace
/**
* Driver 'main' command.
*/
extern "C" __EXPORT int tfmini_main(int argc, char *argv[]) extern "C" __EXPORT int tfmini_main(int argc, char *argv[])
{ {
int ch; int ch = 0;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
const char *device_path = TFMINI_DEFAULT_PORT; const char *device_path = TFMINI_DEFAULT_PORT;
int myoptind = 1; int myoptind = 1;
@@ -832,7 +459,6 @@ extern "C" __EXPORT int tfmini_main(int argc, char *argv[])
return tfmini::usage(); return tfmini::usage();
} }
// Start/load the driver.
if (!strcmp(argv[myoptind], "start")) { if (!strcmp(argv[myoptind], "start")) {
if (strcmp(device_path, "") != 0) { if (strcmp(device_path, "") != 0) {
return tfmini::start(device_path, rotation); return tfmini::start(device_path, rotation);
@@ -841,20 +467,9 @@ extern "C" __EXPORT int tfmini_main(int argc, char *argv[])
PX4_WARN("Please specify device path!"); PX4_WARN("Please specify device path!");
return tfmini::usage(); return tfmini::usage();
} }
} } else if (!strcmp(argv[myoptind], "stop")) {
// Stop the driver
if (!strcmp(argv[myoptind], "stop")) {
return tfmini::stop(); return tfmini::stop();
} } else if (!strcmp(argv[myoptind], "status")) {
// Test the driver/device.
if (!strcmp(argv[myoptind], "test")) {
return tfmini::test();
}
// Print driver information.
if (!strcmp(argv[myoptind], "status")) {
return tfmini::status(); return tfmini::status();
} }
src/drivers/distance_sensor/tfmini/tfmini_parser.cpp
+25 -25
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2017 PX4 Development Team. All rights reserved. * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -63,89 +63,89 @@ const char *parser_state[] = {
}; };
#endif #endif
int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TFMINI_PARSE_STATE *state, float *dist) int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, TFMINI_PARSE_STATE *state, float *dist)
{ {
int ret = -1; int ret = -1;
//char *end; //char *end;
switch (*state) { switch (*state) {
case TFMINI_PARSE_STATE6_GOT_CHECKSUM: case TFMINI_PARSE_STATE::STATE6_GOT_CHECKSUM:
if (c == 'Y') { if (c == 'Y') {
*state = TFMINI_PARSE_STATE1_SYNC_1; *state = TFMINI_PARSE_STATE::STATE1_SYNC_1;
parserbuf[*parserbuf_index] = c; parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
} else { } else {
*state = TFMINI_PARSE_STATE0_UNSYNC; *state = TFMINI_PARSE_STATE::STATE0_UNSYNC;
} }
break; break;
case TFMINI_PARSE_STATE0_UNSYNC: case TFMINI_PARSE_STATE::STATE0_UNSYNC:
if (c == 'Y') { if (c == 'Y') {
*state = TFMINI_PARSE_STATE1_SYNC_1; *state = TFMINI_PARSE_STATE::STATE1_SYNC_1;
parserbuf[*parserbuf_index] = c; parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
} }
break; break;
case TFMINI_PARSE_STATE1_SYNC_1: case TFMINI_PARSE_STATE::STATE1_SYNC_1:
if (c == 'Y') { if (c == 'Y') {
*state = TFMINI_PARSE_STATE1_SYNC_2; *state = TFMINI_PARSE_STATE::STATE1_SYNC_2;
parserbuf[*parserbuf_index] = c; parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
} else { } else {
*state = TFMINI_PARSE_STATE0_UNSYNC; *state = TFMINI_PARSE_STATE::STATE0_UNSYNC;
*parserbuf_index = 0; *parserbuf_index = 0;
} }
break; break;
case TFMINI_PARSE_STATE1_SYNC_2: case TFMINI_PARSE_STATE::STATE1_SYNC_2:
*state = TFMINI_PARSE_STATE2_GOT_DIST_L; *state = TFMINI_PARSE_STATE::STATE2_GOT_DIST_L;
parserbuf[*parserbuf_index] = c; parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
break; break;
case TFMINI_PARSE_STATE2_GOT_DIST_L: case TFMINI_PARSE_STATE::STATE2_GOT_DIST_L:
*state = TFMINI_PARSE_STATE2_GOT_DIST_H; *state = TFMINI_PARSE_STATE::STATE2_GOT_DIST_H;
parserbuf[*parserbuf_index] = c; parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
break; break;
case TFMINI_PARSE_STATE2_GOT_DIST_H: case TFMINI_PARSE_STATE::STATE2_GOT_DIST_H:
*state = TFMINI_PARSE_STATE3_GOT_STRENGTH_L; *state = TFMINI_PARSE_STATE::STATE3_GOT_STRENGTH_L;
parserbuf[*parserbuf_index] = c; parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
break; break;
case TFMINI_PARSE_STATE3_GOT_STRENGTH_L: case TFMINI_PARSE_STATE::STATE3_GOT_STRENGTH_L:
*state = TFMINI_PARSE_STATE3_GOT_STRENGTH_H; *state = TFMINI_PARSE_STATE::STATE3_GOT_STRENGTH_H;
parserbuf[*parserbuf_index] = c; parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
break; break;
case TFMINI_PARSE_STATE3_GOT_STRENGTH_H: case TFMINI_PARSE_STATE::STATE3_GOT_STRENGTH_H:
*state = TFMINI_PARSE_STATE4_GOT_RESERVED; *state = TFMINI_PARSE_STATE::STATE4_GOT_RESERVED;
parserbuf[*parserbuf_index] = c; parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
break; break;
case TFMINI_PARSE_STATE4_GOT_RESERVED: case TFMINI_PARSE_STATE::STATE4_GOT_RESERVED:
*state = TFMINI_PARSE_STATE5_GOT_QUALITY; *state = TFMINI_PARSE_STATE::STATE5_GOT_QUALITY;
parserbuf[*parserbuf_index] = c; parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++; (*parserbuf_index)++;
break; break;
case TFMINI_PARSE_STATE5_GOT_QUALITY: case TFMINI_PARSE_STATE::STATE5_GOT_QUALITY:
// Find the checksum // Find the checksum
unsigned char cksm = 0; unsigned char cksm = 0;
@@ -160,12 +160,12 @@ int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TFMINI
t2 <<= 8; t2 <<= 8;
t2 += t1; t2 += t1;
*dist = ((float)t2) / 100; *dist = ((float)t2) / 100;
*state = TFMINI_PARSE_STATE6_GOT_CHECKSUM; *state = TFMINI_PARSE_STATE::STATE6_GOT_CHECKSUM;
*parserbuf_index = 0; *parserbuf_index = 0;
ret = 0; ret = 0;
} else { } else {
*state = TFMINI_PARSE_STATE0_UNSYNC; *state = TFMINI_PARSE_STATE::STATE0_UNSYNC;
*parserbuf_index = 0; *parserbuf_index = 0;
} }
src/drivers/distance_sensor/tfmini/tfmini_parser.h
+13 -13
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2017 PX4 Development Team. All rights reserved. * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -56,17 +56,17 @@
// 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though // 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +...+Byte8. This is only a low 8bit though
enum TFMINI_PARSE_STATE { enum class TFMINI_PARSE_STATE {
TFMINI_PARSE_STATE0_UNSYNC = 0, STATE0_UNSYNC = 0,
TFMINI_PARSE_STATE1_SYNC_1, STATE1_SYNC_1,
TFMINI_PARSE_STATE1_SYNC_2, STATE1_SYNC_2,
TFMINI_PARSE_STATE2_GOT_DIST_L, STATE2_GOT_DIST_L,
TFMINI_PARSE_STATE2_GOT_DIST_H, STATE2_GOT_DIST_H,
TFMINI_PARSE_STATE3_GOT_STRENGTH_L, STATE3_GOT_STRENGTH_L,
TFMINI_PARSE_STATE3_GOT_STRENGTH_H, STATE3_GOT_STRENGTH_H,
TFMINI_PARSE_STATE4_GOT_RESERVED, STATE4_GOT_RESERVED,
TFMINI_PARSE_STATE5_GOT_QUALITY, STATE5_GOT_QUALITY,
TFMINI_PARSE_STATE6_GOT_CHECKSUM STATE6_GOT_CHECKSUM
}; };
int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TFMINI_PARSE_STATE *state, float *dist); int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, TFMINI_PARSE_STATE *state, float *dist);