yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

tfmini cleanup and use PX4Rangefinder

Browse Source
This commit is contained in:
Daniel Agar
2019-09-11 16:34:35 -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

527
src/drivers/distance_sensor/tfmini/tfmini.cpp
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
* modification, are permitted provided that the following conditions
@@ -42,149 +42,78 @@
* 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 <unistd.h>
#include <board_config.h>
#include <drivers/device/device.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <perf/perf_counter.h>
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_module.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <drivers/rangefinder/PX4Rangefinder.hpp>
#include <uORB/topics/distance_sensor.h>
#ifdef __PX4_CYGWIN
#include <asm/socket.h>
#endif
#include "tfmini_parser.h"
#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:
TFMINI(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
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();
private:
int collect();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void Run() override;
/**
* Initialise the automatic measurement state machine and start it.
*/
void start();
/**
* 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();
PX4Rangefinder _px4_rangefinder;
enum TFMINI_PARSE_STATE _parse_state {TFMINI_PARSE_STATE0_UNSYNC};
TFMINI_PARSE_STATE _parse_state {TFMINI_PARSE_STATE::STATE0_UNSYNC};
char _linebuf[10];
char _port[20];
char _linebuf[10] {};
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 _measure_interval{0};
int _orb_class_instance{-1};
unsigned int _linebuf_index{0};
uint8_t _rotation{0};
float _max_distance{12.0f};
float _min_distance{0.30f};
hrt_abstime _last_read{0};
orb_advert_t _distance_sensor_topic{nullptr};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, 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) :
CDev(RANGE_FINDER0_DEVICE_PATH),
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);
/* enforce null termination */
// enforce null termination
_port[sizeof(_port) - 1] = '\0';
}
TFMINI::~TFMINI()
{
/* make sure we are truly inactive */
// 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);
}
@@ -195,10 +124,11 @@ TFMINI::init()
int32_t hw_model = 1; // only one model so far...
switch (hw_model) {
case 1: /* TFMINI (12m, 100 Hz)*/
_min_distance = 0.3f;
_max_distance = 12.0f;
_conversion_interval = 9000;
case 1: // TFMINI (12m, 100 Hz)
_px4_rangefinder.set_min_distance(0.3f);
_px4_rangefinder.set_max_distance(12.0f);
_px4_rangefinder.set_fov(math::radians(1.15));
break;
default:
@@ -206,12 +136,12 @@ TFMINI::init()
return -1;
}
/* status */
// status
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);
if (_fd < 0) {
@@ -219,19 +149,17 @@ TFMINI::init()
return -1;
}
/*baudrate 115200, 8 bits, no parity, 1 stop bit */
// baudrate 115200, 8 bits, no parity, 1 stop bit
unsigned speed = B115200;
struct termios uart_config;
int termios_state;
termios uart_config{};
int termios_state{};
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;
/* set baud rate */
// set baud rate
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("CFG: %d ISPD", termios_state);
ret = -1;
@@ -250,19 +178,19 @@ TFMINI::init()
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 |= 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 */
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 */
// 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 */
// fetch bytes as they become available
uart_config.c_cc[VMIN] = 1;
uart_config.c_cc[VTIME] = 1;
@@ -271,177 +199,15 @@ TFMINI::init()
ret = -1;
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);
/* close the fd */
// close the fd
::close(_fd);
_fd = -1;
return ret;
}
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);
if (ret == PX4_OK) {
start();
}
}
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;
}
@@ -451,27 +217,30 @@ TFMINI::collect()
{
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);
/* the buffer for read chars is buflen minus null termination */
// the buffer for read chars is buflen minus null termination
char readbuf[sizeof(_linebuf)] {};
unsigned readlen = sizeof(readbuf) - 1;
int ret = 0;
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;
::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);
if (!bytes_available) {
perf_end(_sample_perf);
return -EAGAIN;
}
/* parse entire buffer */
// parse entire buffer
const hrt_abstime timestamp_sample = hrt_absolute_time();
do {
/* read from the sensor (uart buffer) */
// read from the sensor (uart buffer)
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
@@ -479,8 +248,8 @@ TFMINI::collect()
perf_count(_comms_errors);
perf_end(_sample_perf);
/* only throw an error if we time out */
if (read_elapsed > (_conversion_interval * 2)) {
// only throw an error if we time out
if (read_elapsed > (kCONVERSIONINTERVAL * 2)) {
/* flush anything in RX buffer */
tcflush(_fd, TCIFLUSH);
return ret;
@@ -492,40 +261,25 @@ TFMINI::collect()
_last_read = hrt_absolute_time();
/* parse buffer */
// parse buffer
for (int i = 0; i < ret; i++) {
tfmini_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m);
}
/* bytes left to parse */
// bytes left to parse
bytes_available -= ret;
} while (bytes_available > 0);
/* no valid measurement after parsing buffer */
// no valid measurement after parsing buffer
if (distance_m < 0.0f) {
perf_end(_sample_perf);
return -EAGAIN;
}
/* publish most recent valid measurement from buffer */
distance_sensor_s report {};
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 most recent valid measurement from buffer
_px4_rangefinder.update(timestamp_sample, distance_m);
/* 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);
return PX4_OK;
@@ -534,12 +288,8 @@ TFMINI::collect()
void
TFMINI::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
ScheduleNow();
// schedule a cycle to start things
ScheduleOnInterval(100_us);
}
void
@@ -551,41 +301,19 @@ TFMINI::stop()
void
TFMINI::Run()
{
/* fds initialized? */
// fds initialized?
if (_fd < 0) {
/* open fd */
// open fd
_fd = ::open(_port, O_RDWR | O_NOCTTY);
}
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (collect() == -EAGAIN) {
/* reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps */
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;
}
// perform collection
if (collect() == -EAGAIN) {
// reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps
ScheduleClear();
ScheduleOnInterval(100_us, 87 * 9);
return;
}
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
ScheduleDelayed(_conversion_interval);
}
void
@@ -594,8 +322,8 @@ TFMINI::print_info()
printf("Using port '%s'\n", _port);
perf_print_counter(_sample_perf);
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
{
TFMINI *g_dev;
TFMINI *g_dev{nullptr};
int start(const char *port, uint8_t rotation);
int status();
int stop();
int test();
int usage();
/**
* Start the driver.
*/
int
start(const char *port, uint8_t rotation)
{
@@ -638,28 +362,9 @@ start(const char *port, uint8_t rotation)
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;
}
/**
* Print the driver status.
*/
int
status()
{
@@ -674,9 +379,6 @@ status()
return 0;
}
/**
* Stop the driver.
*/
int stop()
{
if (g_dev != nullptr) {
@@ -693,77 +395,6 @@ int stop()
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
usage()
{
@@ -799,13 +430,9 @@ $ tfmini stop
} // namespace
/**
* Driver 'main' command.
*/
extern "C" __EXPORT int tfmini_main(int argc, char *argv[])
{
int ch;
int ch = 0;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
const char *device_path = TFMINI_DEFAULT_PORT;
int myoptind = 1;
@@ -832,7 +459,6 @@ extern "C" __EXPORT int tfmini_main(int argc, char *argv[])
return tfmini::usage();
}
// Start/load the driver.
if (!strcmp(argv[myoptind], "start")) {
if (strcmp(device_path, "") != 0) {
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!");
return tfmini::usage();
}
}
// Stop the driver
if (!strcmp(argv[myoptind], "stop")) {
} else if (!strcmp(argv[myoptind], "stop")) {
return tfmini::stop();
}
// Test the driver/device.
if (!strcmp(argv[myoptind], "test")) {
return tfmini::test();
}
// Print driver information.
if (!strcmp(argv[myoptind], "status")) {
} else if (!strcmp(argv[myoptind], "status")) {
return tfmini::status();
}

50
src/drivers/distance_sensor/tfmini/tfmini_parser.cpp
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
* modification, are permitted provided that the following conditions
@@ -63,89 +63,89 @@ const char *parser_state[] = {
};
#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;
//char *end;
switch (*state) {
case TFMINI_PARSE_STATE6_GOT_CHECKSUM:
case TFMINI_PARSE_STATE::STATE6_GOT_CHECKSUM:
if (c == 'Y') {
*state = TFMINI_PARSE_STATE1_SYNC_1;
*state = TFMINI_PARSE_STATE::STATE1_SYNC_1;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = TFMINI_PARSE_STATE0_UNSYNC;
*state = TFMINI_PARSE_STATE::STATE0_UNSYNC;
}
break;
case TFMINI_PARSE_STATE0_UNSYNC:
case TFMINI_PARSE_STATE::STATE0_UNSYNC:
if (c == 'Y') {
*state = TFMINI_PARSE_STATE1_SYNC_1;
*state = TFMINI_PARSE_STATE::STATE1_SYNC_1;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
}
break;
case TFMINI_PARSE_STATE1_SYNC_1:
case TFMINI_PARSE_STATE::STATE1_SYNC_1:
if (c == 'Y') {
*state = TFMINI_PARSE_STATE1_SYNC_2;
*state = TFMINI_PARSE_STATE::STATE1_SYNC_2;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = TFMINI_PARSE_STATE0_UNSYNC;
*state = TFMINI_PARSE_STATE::STATE0_UNSYNC;
*parserbuf_index = 0;
}
break;
case TFMINI_PARSE_STATE1_SYNC_2:
*state = TFMINI_PARSE_STATE2_GOT_DIST_L;
case TFMINI_PARSE_STATE::STATE1_SYNC_2:
*state = TFMINI_PARSE_STATE::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;
case TFMINI_PARSE_STATE::STATE2_GOT_DIST_L:
*state = TFMINI_PARSE_STATE::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;
case TFMINI_PARSE_STATE::STATE2_GOT_DIST_H:
*state = TFMINI_PARSE_STATE::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;
case TFMINI_PARSE_STATE::STATE3_GOT_STRENGTH_L:
*state = TFMINI_PARSE_STATE::STATE3_GOT_STRENGTH_H;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
break;
case TFMINI_PARSE_STATE3_GOT_STRENGTH_H:
*state = TFMINI_PARSE_STATE4_GOT_RESERVED;
case TFMINI_PARSE_STATE::STATE3_GOT_STRENGTH_H:
*state = TFMINI_PARSE_STATE::STATE4_GOT_RESERVED;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
break;
case TFMINI_PARSE_STATE4_GOT_RESERVED:
*state = TFMINI_PARSE_STATE5_GOT_QUALITY;
case TFMINI_PARSE_STATE::STATE4_GOT_RESERVED:
*state = TFMINI_PARSE_STATE::STATE5_GOT_QUALITY;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
break;
case TFMINI_PARSE_STATE5_GOT_QUALITY:
case TFMINI_PARSE_STATE::STATE5_GOT_QUALITY:
// Find the checksum
unsigned char cksm = 0;
@@ -160,12 +160,12 @@ int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TFMINI
t2 <<= 8;
t2 += t1;
*dist = ((float)t2) / 100;
*state = TFMINI_PARSE_STATE6_GOT_CHECKSUM;
*state = TFMINI_PARSE_STATE::STATE6_GOT_CHECKSUM;
*parserbuf_index = 0;
ret = 0;
} else {
*state = TFMINI_PARSE_STATE0_UNSYNC;
*state = TFMINI_PARSE_STATE::STATE0_UNSYNC;
*parserbuf_index = 0;
}

26
src/drivers/distance_sensor/tfmini/tfmini_parser.h
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
* 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
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
enum class TFMINI_PARSE_STATE {
STATE0_UNSYNC = 0,
STATE1_SYNC_1,
STATE1_SYNC_2,
STATE2_GOT_DIST_L,
STATE2_GOT_DIST_H,
STATE3_GOT_STRENGTH_L,
STATE3_GOT_STRENGTH_H,
STATE4_GOT_RESERVED,
STATE5_GOT_QUALITY,
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);