diff --git a/src/drivers/distance_sensor/CMakeLists.txt b/src/drivers/distance_sensor/CMakeLists.txt index ef2c207696..648391adb5 100644 --- a/src/drivers/distance_sensor/CMakeLists.txt +++ b/src/drivers/distance_sensor/CMakeLists.txt @@ -49,3 +49,4 @@ add_subdirectory(vl53l0x) add_subdirectory(vl53l1x) add_subdirectory(gy_us42) add_subdirectory(tfmini_i2c) +add_subdirectory(rds02uf) diff --git a/src/drivers/distance_sensor/Kconfig b/src/drivers/distance_sensor/Kconfig index d967c67386..96aa22c407 100644 --- a/src/drivers/distance_sensor/Kconfig +++ b/src/drivers/distance_sensor/Kconfig @@ -19,6 +19,7 @@ menu "Distance sensors" select DRIVERS_DISTANCE_SENSOR_VL53L1X select DRIVERS_DISTANCE_SENSOR_GY_US42 select DRIVERS_DISTANCE_SENSOR_TFMINI_I2C + select DRIVERS_DISTANCE_SENSOR_RDS02UF ---help--- Enable default set of distance sensor drivers diff --git a/src/drivers/distance_sensor/rds02uf/CMakeLists.txt b/src/drivers/distance_sensor/rds02uf/CMakeLists.txt new file mode 100644 index 0000000000..78d48778d5 --- /dev/null +++ b/src/drivers/distance_sensor/rds02uf/CMakeLists.txt @@ -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 + ) diff --git a/src/drivers/distance_sensor/rds02uf/Kconfig b/src/drivers/distance_sensor/rds02uf/Kconfig new file mode 100644 index 0000000000..e0b91ed8b3 --- /dev/null +++ b/src/drivers/distance_sensor/rds02uf/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_RDS02UF + bool "tfmini" + default n + ---help--- + Enable support for rds02uf diff --git a/src/drivers/distance_sensor/rds02uf/module.yaml b/src/drivers/distance_sensor/rds02uf/module.yaml new file mode 100644 index 0000000000..72f67f8205 --- /dev/null +++ b/src/drivers/distance_sensor/rds02uf/module.yaml @@ -0,0 +1,26 @@ +module_name: Benewake RDS02UF Rangefinder +serial_config: + - command: rds02uf start -d ${SERIAL_DEV} -R SENS_RDS02UF_ROT + port_config_param: + name: SENS_RDS02UF_CFG + group: Sensors + +parameters: + - group: Sensors + definitions: + SENS_RDS02UF_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 diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf.cpp b/src/drivers/distance_sensor/rds02uf/rds02uf.cpp new file mode 100644 index 0000000000..8a02a3506b --- /dev/null +++ b/src/drivers/distance_sensor/rds02uf/rds02uf.cpp @@ -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 +#include +#include + +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_RDS02UF; + 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_RADAR); +} + +Rds02uf::~Rds02uf() +{ + // make sure we are truly inactive + stop(); + + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int +Rds02uf::init() +{ + param_t _param_rds02uf_rot = param_find("SENS_RDS02UF_ROT"); + int32_t rotation; + if (param_get(_param_rds02uf_rot, &rotation) == PX4_OK) + { + _px4_rangefinder.set_orientation(rotation); + } + + _px4_rangefinder.set_min_distance(0.4f); + _px4_rangefinder.set_max_distance(20.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 20Hz, but we run a bit faster to avoid missing data) + ScheduleOnInterval(47_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 21 bytes @ 115200 bps + ScheduleClear(); + ScheduleOnInterval(57_ms, 87 * 21); + return; + } +} + +void +Rds02uf::print_info() +{ + printf("Using port '%s'\n", _port); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf.hpp b/src/drivers/distance_sensor/rds02uf/rds02uf.hpp new file mode 100644 index 0000000000..556f0bec4e --- /dev/null +++ b/src/drivers/distance_sensor/rds02uf/rds02uf.hpp @@ -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 + +#include +#include +#include +#include +#include +#include +#include + +#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_SYNC_1}; + + char _linebuf[21] {}; + char _port[20] {}; + + static constexpr int kCONVERSIONINTERVAL{50_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")}; + +}; diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf_main.cpp b/src/drivers/distance_sensor/rds02uf/rds02uf_main.cpp new file mode 100644 index 0000000000..3286fbe594 --- /dev/null +++ b/src/drivers/distance_sensor/rds02uf/rds02uf_main.cpp @@ -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 +#include + +/** + * 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(); +} diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp b/src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp new file mode 100644 index 0000000000..8f121e5b85 --- /dev/null +++ b/src/drivers/distance_sensor/rds02uf/rds02uf_parser.cpp @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * 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 + * @author Chuong Nguyen + * @author Ayush Gaud + * @author Zebulon + * + * Declarations of parser for the Benewake Rds02UF rangefinder series + */ + +#include "rds02uf_parser.h" +#include +#include + +#define RDS02UF_HEAD_LEN 2 +#define RDS02UF_PRE_DATA_LEN 6 +#define RDS02UF_DATA_LEN 10 +// #define RDS02UF_DEBUG + +#ifdef RDS02UF_DEBUG +#include + +const char *parser_state[] = { + + "0_STATE1_SYNC_1", + "1_STATE2_SYNC_2", + "2_STATE3_ADDRESS", + "3_STATE4_ERROR_CODE", + "4_STATE5_FC_CODE_L", + "5_STATE6_FC_CODE_H", + "6_STATE7_LENGTH_L", + "7_STATE8_LENGTH_H", + "8_STATE9_REAL_DATA", + "9_STATE10_CRC", + "10_STATE11_END_1", + "11_STATE12_END_2" +}; +#endif + +int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist) +{ + int ret = -1; + char data = c; + uint8_t crc_data = 0; + + switch (*state) { + case RDS02UF_PARSE_STATE::STATE0_SYNC_1: + if (data == RDS02_HEAD1) + { + parserbuf[*parserbuf_index] = data; + (*parserbuf_index)++; + *state = RDS02UF_PARSE_STATE::STATE1_SYNC_2; + } + break; + case RDS02UF_PARSE_STATE::STATE1_SYNC_2: + if (data == RDS02_HEAD2) + { + parserbuf[*parserbuf_index] = data; + (*parserbuf_index)++; + *state = RDS02UF_PARSE_STATE::STATE2_ADDRESS; + }else{ + *parserbuf_index = 0; + *state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; + } + break; + case RDS02UF_PARSE_STATE::STATE2_ADDRESS: // address + parserbuf[*parserbuf_index] = data; + (*parserbuf_index)++; + *state = RDS02UF_PARSE_STATE::STATE3_ERROR_CODE; + break; + case RDS02UF_PARSE_STATE::STATE3_ERROR_CODE: // error_code + parserbuf[*parserbuf_index] = data; + (*parserbuf_index)++; + *state = RDS02UF_PARSE_STATE::STATE4_FC_CODE_L; + break; + case RDS02UF_PARSE_STATE::STATE4_FC_CODE_L: // fc_code low + parserbuf[*parserbuf_index] = data; + (*parserbuf_index)++; + *state = RDS02UF_PARSE_STATE::STATE5_FC_CODE_H; + break; + case RDS02UF_PARSE_STATE::STATE5_FC_CODE_H: // fc_code high + parserbuf[*parserbuf_index] = data; + (*parserbuf_index)++; + *state = RDS02UF_PARSE_STATE::STATE6_LENGTH_L; + break; + case RDS02UF_PARSE_STATE::STATE6_LENGTH_L: // lengh_low + parserbuf[*parserbuf_index] = data; + (*parserbuf_index)++; + *state = RDS02UF_PARSE_STATE::STATE7_LENGTH_H; + break; + case RDS02UF_PARSE_STATE::STATE7_LENGTH_H: // lengh_high + { + uint8_t read_len = data << 8 | parserbuf[*parserbuf_index-1]; + if ( read_len == RDS02UF_DATA_LEN) // rds02uf data length is 10 + { + parserbuf[*parserbuf_index] = data; + (*parserbuf_index)++; + *state = RDS02UF_PARSE_STATE::STATE8_REAL_DATA; + }else{ + *parserbuf_index = 0; + *state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; + } + break; + } + + case RDS02UF_PARSE_STATE::STATE8_REAL_DATA: // real_data + parserbuf[*parserbuf_index] = data; + (*parserbuf_index)++; + if ((*parserbuf_index) == (RDS02UF_HEAD_LEN + RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN)) + { + *state = RDS02UF_PARSE_STATE::STATE9_CRC; + } + break; + case RDS02UF_PARSE_STATE::STATE9_CRC: // crc + crc_data = crc8(&parserbuf[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN); + parserbuf[*parserbuf_index] = data; + if (crc_data == data || data == 0xff) + { + (*parserbuf_index)++; + *state = RDS02UF_PARSE_STATE::STATE10_END_1; + }else{ + *parserbuf_index = 0; + *state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; + } + break; + case RDS02UF_PARSE_STATE::STATE10_END_1: // + if (data == RDS02_END) + { + parserbuf[*parserbuf_index] = data; + (*parserbuf_index)++; + *state = RDS02UF_PARSE_STATE::STATE11_END_2; + }else{ + *parserbuf_index = 0; + *state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; + } + + break; + case RDS02UF_PARSE_STATE::STATE11_END_2: // + { + uint16_t fc_code = (parserbuf[STATE5_FC_CODE_H] << 8 | parserbuf[STATE4_FC_CODE_L]); + uint8_t err_code = parserbuf[STATE3_ERROR_CODE]; + if (data == RDS02_END) + { + if (fc_code == 0x03ff && err_code == 0) // get targer information + { + if(parserbuf[RDS02_DATA_START_INDEX] == RDS02_TARGET_INFO) + { + float distance = (parserbuf[RDS02_DATA_Y_INDEX + 1] * 256 + parserbuf[RDS02_DATA_Y_INDEX]) / 100.0f; + *dist = distance; + ret = true; + } + } + } + + *parserbuf_index = 0; + *state = RDS02UF_PARSE_STATE::STATE0_SYNC_1; + + break; + } + } + +#ifdef RDS02UF_DEBUG + static u_int16_t cnt; + cnt += 1; + static RDS02UF_PARSE_STATE last_state = RDS02UF_PARSE_STATE::STATE12_END_2; + if (*state != last_state || cnt > 500) + { + printf("state: %s,read: %02x\n", parser_state[*state],data); + last_state = *state; + cnt = 0; + } +#endif + + return ret; +} + +uint8_t crc8(char* pbuf, int32_t len) +{ + char* data = pbuf; + uint8_t crc = 0; + while ( len-- ) + crc = crc8_table[crc^*(data++)]; + return crc; +} diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf_parser.h b/src/drivers/distance_sensor/rds02uf/rds02uf_parser.h new file mode 100644 index 0000000000..7bcb0236b5 --- /dev/null +++ b/src/drivers/distance_sensor/rds02uf/rds02uf_parser.h @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 + * @author Chuong Nguyen + * @author Ayush Gaud + * @author Zebulon + * + * Declarations of parser for the Benewake Rds02UF rangefinder series + */ + +#pragma once + +#define RDS02_HEAD1 0x55 +#define RDS02_HEAD2 0x55 +#define RDS02_END 0xAA + +#define RDS02_BUFFER_SIZE 50 +#define RDS02_DATA_START_INDEX 8 +#define RDS02_DATA_Y_INDEX 13 +#define RDS02_DATA_FC_INDEX RDS02_DATA_START_INDEX + + +#define RDS02_TARGET_INFO 0x0C +// Data Format for Benewake Rds02UF +// =============================== +// 21 bytes total per message: +// 1) 0x55 +// 2) 0x55 +// 3) address +// 4) error_code +// 5) FC_CODE_L (low 8bit) +// 6) FC_CODE_H (high 8bit) +// 7) LENGTH_L (low 8bit) +// 8) LENGTH_H (high 8bit) +// 9) REAL_DATA (10Byte) +// 10) CRC8 +// 11) END_1 0xAA +// 12) END_2 0xAA + + + +enum RDS02UF_PARSE_STATE { + STATE0_SYNC_1 = 0, + STATE1_SYNC_2, + STATE2_ADDRESS, + STATE3_ERROR_CODE, + STATE4_FC_CODE_L, + STATE5_FC_CODE_H, + STATE6_LENGTH_L, + STATE7_LENGTH_H, + STATE8_REAL_DATA, + STATE9_CRC, + STATE10_END_1, + STATE11_END_2 +}; + +int rds02uf_parse(char c, char *parserbuf, unsigned *parserbuf_index, RDS02UF_PARSE_STATE *state, float *dist); +uint8_t crc8(char* pbuf, int32_t len); + +const uint8_t crc8_table[256] = { + 0x93,0x98,0xE4,0x46,0xEB,0xBA,0x04,0x4C, + 0xFA,0x40,0xB8,0x96,0x0E,0xB2,0xB7,0xC0, + 0x0C,0x32,0x9B,0x80,0xFF,0x30,0x7F,0x9D, + 0xB3,0x81,0x58,0xE7,0xF1,0x19,0x7E,0xB6, + 0xCD,0xF7,0xB4,0xCB,0xBC,0x5C,0xD6,0x09, + 0x20,0x0A,0xE0,0x37,0x51,0x67,0x24,0x95, + 0xE1,0x62,0xF8,0x5E,0x38,0x15,0x54,0x77, + 0x63,0x57,0x6D,0xE9,0x89,0x76,0xBE,0x41, + 0x5D,0xF9,0xB1,0x4D,0x6C,0x53,0x9C,0xA2, + 0x23,0xC4,0x8E,0xC8,0x05,0x42,0x61,0x71, + 0xC5,0x00,0x18,0x6F,0x5F,0xFB,0x7B,0x11, + 0x65,0x2D,0x8C,0xED,0x14,0xAB,0x88,0xD5, + 0xD9,0xC2,0x36,0x34,0x7C,0x5B,0x3C,0xF6, + 0x48,0x0B,0xEE,0x02,0x83,0x79,0x17,0xE6, + 0xA8,0x78,0xF5,0xD3,0x4E,0x50,0x52,0x91, + 0xD8,0xC6,0x22,0xEC,0x3B,0xE5,0x3F,0x86, + 0x06,0xCF,0x2B,0x2F,0x3D,0x59,0x1C,0x87, + 0xEF,0x4F,0x10,0xD2,0x7D,0xDA,0x72,0xA0, + 0x9F,0xDE,0x6B,0x75,0x56,0xBD,0xC7,0xC1, + 0x70,0x1D,0x25,0x92,0xA5,0x31,0xE2,0xD7, + 0xD0,0x9A,0xAF,0xA9,0xC9,0x97,0x08,0x33, + 0x5A,0x99,0xC3,0x16,0x84,0x82,0x8A,0xF3, + 0x4A,0xCE,0xDB,0x29,0x0F,0xAE,0x6E,0xE3, + 0x8B,0x07,0x3A,0x74,0x47,0xB0,0xBB,0xB5, + 0x7A,0xAA,0x2C,0xD4,0x03,0x3E,0x1A,0xA7, + 0x27,0x64,0x06,0xBF,0x55,0x73,0x1E,0xFE, + 0x49,0x01,0x39,0x28,0xF4,0x26,0xDF,0xDD, + 0x44,0x0D,0x21,0xF2,0x85,0xB9,0xEA,0x4B, + 0xDC,0x6A,0xCA,0xAC,0x12,0xFC,0x2E,0x2A, + 0xA3,0xF0,0x66,0xE8,0x60,0x45,0xA1,0x8D, + 0x68,0x35,0xFD,0x8F,0x9E,0x1F,0x13,0xD1, + 0xAD,0x69,0xCC,0xA4,0x94,0x90,0x1B,0x43, + }; diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 69b9e8978c..9deb5deccb 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -218,6 +218,7 @@ #define DRV_BARO_DEVTYPE_ICP10111 0xC1 #define DRV_DIST_DEVTYPE_TFMINI_I2C 0xC2 +#define DRV_DIST_DEVTYPE_RDS02UF 0xC3 #define DRV_DEVTYPE_UNUSED 0xff