Browse Source

add rds02uf radar rangefinder sersor,step1

tf_driver-1.12
zbr3550 3 years ago
parent
commit
51c82baccf
  1. 1
      src/drivers/distance_sensor/CMakeLists.txt
  2. 45
      src/drivers/distance_sensor/rds02uf/CMakeLists.txt
  3. 27
      src/drivers/distance_sensor/rds02uf/module.yaml
  4. 266
      src/drivers/distance_sensor/rds02uf/rds02uf.cpp
  5. 96
      src/drivers/distance_sensor/rds02uf/rds02uf.hpp
  6. 189
      src/drivers/distance_sensor/rds02uf/rds02uf_main.cpp
  7. 159
      src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp
  8. 72
      src/drivers/distance_sensor/rds02uf/rds02uf_parser.h

1
src/drivers/distance_sensor/CMakeLists.txt

@ -49,3 +49,4 @@ add_subdirectory(vl53l0x) @@ -49,3 +49,4 @@ add_subdirectory(vl53l0x)
add_subdirectory(vl53l1x)
add_subdirectory(gy_us42)
add_subdirectory(tfmini_i2c)
add_subdirectory(rds02uf)

45
src/drivers/distance_sensor/rds02uf/CMakeLists.txt

@ -0,0 +1,45 @@ @@ -0,0 +1,45 @@
############################################################################
#
# 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
# 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__distance_sensor__rds02uf
MAIN rds02uf
SRCS
rds02uf.cpp
rds02uf.hpp
rds02uf_main.cpp
rds02uf_parser.cpp
MODULE_CONFIG
module.yaml
DEPENDS
drivers_rangefinder
)

27
src/drivers/distance_sensor/rds02uf/module.yaml

@ -0,0 +1,27 @@ @@ -0,0 +1,27 @@
module_name: Benewake RDS02UF Rangefinder
serial_config:
- command: rds02uf start -d ${SERIAL_DEV} -R SENS_RDS02_ROT
port_config_param:
name: SENS_RDS02UF_CFG
group: Sensors
parameters:
- group: Sensors
definitions:
SENS_RDS02_ROT:
description:
short: Distance Sensor Rotation
long: |
Distance Sensor Rotation as MAV_SENSOR_ORIENTATION enum
type: enum
values:
25: ROTATION_DOWNWARD_FACING
24: ROTATION_UPWARD_FACING
12: ROTATION_BACKWARD_FACING
0: ROTATION_FORWARD_FACING
6: ROTATION_LEFT_FACING
2: ROTATION_RIGHT_FACING
reboot_required: true
default: 25

266
src/drivers/distance_sensor/rds02uf/rds02uf.cpp

@ -0,0 +1,266 @@ @@ -0,0 +1,266 @@
/****************************************************************************
*
* Copyright (c) 2017-2019, 2021 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.
*
****************************************************************************/
#include "rds02uf.hpp"
#include <lib/drivers/device/Device.hpp>
#include <fcntl.h>
Rds02uf::Rds02uf(const char *port, uint8_t rotation) :
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
_px4_rangefinder(0, rotation)
{
// store port name
strncpy(_port, port, sizeof(_port) - 1);
// enforce null termination
_port[sizeof(_port) - 1] = '\0';
device::Device::DeviceId device_id;
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_TFMINI;
device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
uint8_t bus_num = atoi(&_port[strlen(_port) - 1]); // Assuming '/dev/ttySx'
if (bus_num < 10) {
device_id.devid_s.bus = bus_num;
}
_px4_rangefinder.set_device_id(device_id.devid);
_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
}
Rds02uf::~Rds02uf()
{
// make sure we are truly inactive
stop();
perf_free(_sample_perf);
perf_free(_comms_errors);
}
int
Rds02uf::init()
{
_px4_rangefinder.set_min_distance(0.4f);
_px4_rangefinder.set_max_distance(12.0f);
_px4_rangefinder.set_fov(math::radians(1.15f));
// status
int ret = 0;
do { // create a scope to handle exit conditions using break
// open fd
_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_fd < 0) {
PX4_ERR("Error opening fd");
return -1;
}
// baudrate 115200, 8 bits, no parity, 1 stop bit
unsigned speed = B115200;
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) {
PX4_ERR("CFG: %d ISPD", termios_state);
ret = -1;
break;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
PX4_ERR("CFG: %d OSPD\n", termios_state);
ret = -1;
break;
}
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("baud %d ATTR", termios_state);
ret = -1;
break;
}
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) {
PX4_ERR("FAIL: laser fd");
ret = -1;
break;
}
} while (0);
// close the fd
::close(_fd);
_fd = -1;
if (ret == PX4_OK) {
start();
}
return ret;
}
int
Rds02uf::collect()
{
perf_begin(_sample_perf);
// 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
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
int bytes_available = 0;
::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);
if (!bytes_available) {
perf_end(_sample_perf);
return 0;
}
// parse entire buffer
const hrt_abstime timestamp_sample = hrt_absolute_time();
do {
// read from the sensor (uart buffer)
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
PX4_ERR("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
// only throw an error if we time out
if (read_elapsed > (kCONVERSIONINTERVAL * 2)) {
/* flush anything in RX buffer */
tcflush(_fd, TCIFLUSH);
return ret;
} else {
return -EAGAIN;
}
}
_last_read = hrt_absolute_time();
// parse buffer
for (int i = 0; i < ret; i++) {
rds02uf_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m);
}
// bytes left to parse
bytes_available -= ret;
} while (bytes_available > 0);
// no valid measurement after parsing buffer
if (distance_m < 0.0f) {
perf_end(_sample_perf);
return -EAGAIN;
}
// publish most recent valid measurement from buffer
_px4_rangefinder.update(timestamp_sample, distance_m);
perf_end(_sample_perf);
return PX4_OK;
}
void
Rds02uf::start()
{
// schedule a cycle to start things (the sensor sends at 100Hz, but we run a bit faster to avoid missing data)
ScheduleOnInterval(7_ms);
}
void
Rds02uf::stop()
{
ScheduleClear();
}
void
Rds02uf::Run()
{
// fds initialized?
if (_fd < 0) {
// open fd
_fd = ::open(_port, O_RDWR | O_NOCTTY);
}
// perform collection
if (collect() == -EAGAIN) {
// reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps
ScheduleClear();
ScheduleOnInterval(7_ms, 87 * 9);
return;
}
}
void
Rds02uf::print_info()
{
printf("Using port '%s'\n", _port);
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
}

96
src/drivers/distance_sensor/rds02uf/rds02uf.hpp

@ -0,0 +1,96 @@ @@ -0,0 +1,96 @@
/****************************************************************************
*
* Copyright (c) 2017-2019, 2021 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 rds02uf.cpp
* @author
*
* Driver for the Benewake RDS02UF rangefinder series
*/
#pragma once
#include <termios.h>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
#include <uORB/topics/distance_sensor.h>
#include "rds02uf_parser.h"
#define RDS02UF_DEFAULT_PORT "/dev/ttyS3"
using namespace time_literals;
class Rds02uf : public px4::ScheduledWorkItem
{
public:
Rds02uf(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
virtual ~Rds02uf();
int init();
void print_info();
private:
int collect();
void Run() override;
void start();
void stop();
PX4Rangefinder _px4_rangefinder;
RDS02UF_PARSE_STATE _parse_state {RDS02UF_PARSE_STATE::STATE0_UNSYNC};
char _linebuf[10] {};
char _port[20] {};
static constexpr int kCONVERSIONINTERVAL{9_ms};
int _fd{-1};
unsigned int _linebuf_index{0};
hrt_abstime _last_read{0};
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")};
};

189
src/drivers/distance_sensor/rds02uf/rds02uf_main.cpp

@ -0,0 +1,189 @@ @@ -0,0 +1,189 @@
/****************************************************************************
*
* 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
* 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.
*
****************************************************************************/
#include "rds02uf.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
/**
* Local functions in support of the shell command.
*/
namespace rds02uf
{
Rds02uf *g_dev{nullptr};
int start(const char *port, uint8_t rotation);
int status();
int stop();
int usage();
int
start(const char *port, uint8_t rotation)
{
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_OK;
}
// Instantiate the driver.
g_dev = new Rds02uf(port, rotation);
if (g_dev == nullptr) {
PX4_ERR("driver start failed");
return PX4_ERROR;
}
if (OK != g_dev->init()) {
PX4_ERR("driver start failed");
delete g_dev;
g_dev = nullptr;
return PX4_ERROR;
}
return PX4_OK;
}
int
status()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running");
return 1;
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
return 0;
}
int stop()
{
if (g_dev != nullptr) {
PX4_INFO("stopping driver");
delete g_dev;
g_dev = nullptr;
PX4_INFO("driver stopped");
} else {
PX4_ERR("driver not running");
return 1;
}
return PX4_OK;
}
int
usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Serial bus driver for the Benewake RDS02UF radar.
Most boards are configured to enable/start the driver on a specified UART using the SENS_RDS02_CFG parameter.
Setup/usage information:
### Examples
Attempt to start driver on a specified serial device.
$ rds02uf start -d /dev/ttyS1
Stop driver
$ rds02uf stop
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("rds02uf", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("status","Driver status");
PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("test","Test driver (basic functional tests)");
PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status");
return PX4_OK;
}
} // namespace
extern "C" __EXPORT int rds02uf_main(int argc, char *argv[])
{
int ch = 0;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
const char *device_path = RDS02UF_DEFAULT_PORT;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
break;
case 'd':
device_path = myoptarg;
break;
default:
PX4_WARN("Unknown option!");
return PX4_ERROR;
}
}
if (myoptind >= argc) {
PX4_ERR("unrecognized command");
return rds02uf::usage();
}
if (!strcmp(argv[myoptind], "start")) {
if (strcmp(device_path, "") != 0) {
return rds02uf::start(device_path, rotation);
} else {
PX4_WARN("Please specify device path!");
return rds02uf::usage();
}
} else if (!strcmp(argv[myoptind], "stop")) {
return rds02uf::stop();
} else if (!strcmp(argv[myoptind], "status")) {
return rds02uf::status();
}
return rds02uf::usage();
}

159
src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp

@ -0,0 +1,159 @@ @@ -0,0 +1,159 @@
/****************************************************************************
*
* 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
* 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
*
* Declarations of parser for the Benewake TFmini laser rangefinder series
*/
#include "rds02uf_parser.h"
#include <string.h>
#include <stdlib.h>
int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist)
{
int ret = -1;
//char *end;
switch (*state) {
case RDS02UF_PARSE_STATE::STATE6_GOT_CHECKSUM:
if (c == 'Y') {
*state = RDS02UF_PARSE_STATE::STATE1_SYNC_1;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = RDS02UF_PARSE_STATE::STATE0_UNSYNC;
}
break;
case RDS02UF_PARSE_STATE::STATE0_UNSYNC:
if (c == 'Y') {
*state = RDS02UF_PARSE_STATE::STATE1_SYNC_1;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
}
break;
case RDS02UF_PARSE_STATE::STATE1_SYNC_1:
if (c == 'Y') {
*state = RDS02UF_PARSE_STATE::STATE1_SYNC_2;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = RDS02UF_PARSE_STATE::STATE0_UNSYNC;
*parserbuf_index = 0;
}
break;
case RDS02UF_PARSE_STATE::STATE1_SYNC_2:
*state = RDS02UF_PARSE_STATE::STATE2_GOT_DIST_L;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
break;
case RDS02UF_PARSE_STATE::STATE2_GOT_DIST_L:
*state = RDS02UF_PARSE_STATE::STATE2_GOT_DIST_H;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
break;
case RDS02UF_PARSE_STATE::STATE2_GOT_DIST_H:
*state = RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_L;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
break;
case RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_L:
*state = RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_H;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
break;
case RDS02UF_PARSE_STATE::STATE3_GOT_STRENGTH_H:
*state = RDS02UF_PARSE_STATE::STATE4_GOT_RESERVED;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
break;
case RDS02UF_PARSE_STATE::STATE4_GOT_RESERVED:
*state = RDS02UF_PARSE_STATE::STATE5_GOT_QUALITY;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
break;
case RDS02UF_PARSE_STATE::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 = RDS02UF_PARSE_STATE::STATE6_GOT_CHECKSUM;
*parserbuf_index = 0;
ret = 0;
} else {
*state = RDS02UF_PARSE_STATE::STATE0_UNSYNC;
*parserbuf_index = 0;
}
break;
}
#ifdef RDS02UF_DEBUG
printf("state: RDS02UF_PARSE_STATE%s\n", parser_state[*state]);
#endif
return ret;
}

72
src/drivers/distance_sensor/rds02uf/rds02uf_parser.h

@ -0,0 +1,72 @@ @@ -0,0 +1,72 @@
/****************************************************************************
*
* 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
* 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 class RDS02UF_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 rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist);
Loading…
Cancel
Save