8 changed files with 855 additions and 0 deletions
@ -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 |
||||
) |
@ -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 |
@ -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); |
||||
} |
@ -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")}; |
||||
|
||||
}; |
@ -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(); |
||||
} |
@ -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; |
||||
} |
@ -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…
Reference in new issue