From 591e8fde5d01204588180da7a88a808ce5834901 Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Thu, 13 Dec 2018 16:30:11 +0100 Subject: [PATCH] Initial commit. Integrated new distance sensor hardware. TODO: Parsing the data Signed-off-by: Claudio Micheli --- src/drivers/distance_sensor/CMakeLists.txt | 1 + .../distance_sensor/airlango/CMakeLists.txt | 40 + .../distance_sensor/airlango/isl2950.cpp | 797 ++++++++++++++++++ .../airlango/isl2950_parser.cpp | 55 ++ .../distance_sensor/airlango/isl2950_parser.h | 53 ++ .../airlango/repo/CMakeLists_old.txt | 44 + .../airlango/repo/lanbao_isl.cpp | 305 +++++++ .../airlango/repo/lanbao_isl.h | 155 ++++ .../airlango/repo/lanbao_isl_v2.cpp | 178 ++++ .../airlango/repo/lanbao_isl_v2.h | 23 + .../distance_sensor/airlango/repo/tof.cpp | 345 ++++++++ .../distance_sensor/airlango/repo/tof.h | 173 ++++ .../airlango/repo/tof_main.cpp | 370 ++++++++ 13 files changed, 2539 insertions(+) create mode 100644 src/drivers/distance_sensor/airlango/CMakeLists.txt create mode 100644 src/drivers/distance_sensor/airlango/isl2950.cpp create mode 100644 src/drivers/distance_sensor/airlango/isl2950_parser.cpp create mode 100644 src/drivers/distance_sensor/airlango/isl2950_parser.h create mode 100644 src/drivers/distance_sensor/airlango/repo/CMakeLists_old.txt create mode 100644 src/drivers/distance_sensor/airlango/repo/lanbao_isl.cpp create mode 100644 src/drivers/distance_sensor/airlango/repo/lanbao_isl.h create mode 100644 src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.cpp create mode 100644 src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.h create mode 100644 src/drivers/distance_sensor/airlango/repo/tof.cpp create mode 100644 src/drivers/distance_sensor/airlango/repo/tof.h create mode 100644 src/drivers/distance_sensor/airlango/repo/tof_main.cpp diff --git a/src/drivers/distance_sensor/CMakeLists.txt b/src/drivers/distance_sensor/CMakeLists.txt index 11e14117fda6..093e6cf573d6 100644 --- a/src/drivers/distance_sensor/CMakeLists.txt +++ b/src/drivers/distance_sensor/CMakeLists.txt @@ -43,3 +43,4 @@ add_subdirectory(ulanding) add_subdirectory(leddar_one) add_subdirectory(vl53lxx) add_subdirectory(pga460) +add_subdirectory(airlango) diff --git a/src/drivers/distance_sensor/airlango/CMakeLists.txt b/src/drivers/distance_sensor/airlango/CMakeLists.txt new file mode 100644 index 000000000000..056faae748ae --- /dev/null +++ b/src/drivers/distance_sensor/airlango/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2015 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__isl2950 + MAIN isl2950 + SRCS + isl2950.cpp + isl2950_parser.cpp + DEPENDS + ) diff --git a/src/drivers/distance_sensor/airlango/isl2950.cpp b/src/drivers/distance_sensor/airlango/isl2950.cpp new file mode 100644 index 000000000000..e2a6be9fbda6 --- /dev/null +++ b/src/drivers/distance_sensor/airlango/isl2950.cpp @@ -0,0 +1,797 @@ +/**************************************************************************** + * + * Copyright (c) 2014-2015 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 landbao15L2950.cpp + * @author Claudio Micheli + * + * Driver for the Lanbao ISL2950 + */ + + #include + #include + #include + + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + + #include + + #include + #include + #include + #include + + #include + #include + + #include "isl2950_parser.h" + + /* Configuration Constants */ + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +#define ISL2950_TAKE_RANGE_REG 'd' + +// designated serial port on Pixhawk + #define ISL2950_DEFAULT_PORT "/dev/ttyS1" // Its baudrate is 115200 + // #define LANBAO_DEFAULT_BAUDRATE 115200 + + + class ISL2950 : public cdev::CDev + { + public: + // Constructor + ISL2950(const char *port = ISL2950_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + // Virtual destructor + virtual ~ISL2950(); + + virtual int init(); + //virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + private: + char _port[20]; + uint8_t _rotation; + float _min_distance; + float _max_distance; + int _conversion_interval; + work_s _work{}; + ringbuffer::RingBuffer *_reports; + int _measure_ticks; + bool _collect_phase; + int _fd; + char _linebuf[50]; + unsigned _linebuf_index; + enum ISL2950_PARSE_STATE _parse_state; + hrt_abstime _last_read; + + int _class_instance; + int _orb_class_instance; + + orb_advert_t _distance_sensor_topic; + + unsigned _consecutive_fail_count; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + + }; + + + /* + * Driver 'main' command. + */ + extern "C" __EXPORT int isl2950_main(int argc, char *argv[]); + +/** +* Method : Constructor +* +* @note This method initializes the class variables +*/ + + ISL2950::ISL2950(const char *port, uint8_t rotation) : + CDev(RANGE_FINDER0_DEVICE_PATH), + _rotation(rotation), + _min_distance(0.30f), + _max_distance(40.0f), + _conversion_interval(83334), + _reports(nullptr), + _measure_ticks(0), + _collect_phase(false), + _fd(-1), + _linebuf_index(0), + _parse_state(ISL2950_PARSE_STATE0_UNSYNC), + _last_read(0), + _class_instance(-1), + _orb_class_instance(-1), + _distance_sensor_topic(nullptr), + _consecutive_fail_count(0), + _sample_perf(perf_alloc(PC_ELAPSED, "isl2950_read")), + _comms_errors(perf_alloc(PC_COUNT, "isl2950_com_err")) + { + /* store port name */ + strncpy(_port, port, sizeof(_port)); + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; + +} + +// Destructor +ISL2950::~ISL2950() +{ + /* 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); +} + +/** +* Method : init() +* +* This method setup the general driver for a range finder sensor. +*/ + +int +ISL2950::init() +{ + /* status */ +int ret = 0; + +do { /* create a scope to handle exit conditions using 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("alloc failed"); + 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"); + } + + } while (0); + + return ret; +} + +int +ISL2950::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_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(_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_ticks == 0); + + /* convert hz to tick interval via microseconds */ + int ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(_conversion_interval)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +/* +ssize_t +ISL2950::read(device::file_t *filp, char *buffer, size_t buflen) +{ + // SOME STUFFS + +}*/ + +int +ISL2950::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + char cmd = ISL2950_TAKE_RANGE_REG; + ret = ::write(_fd, &cmd, 1); + + if (ret != sizeof(cmd)) { + perf_count(_comms_errors); + printf("write fail %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int + +/* + * Method: collect() + * + * This method reads data from serial UART and places it into a buffer +*/ +ISL2950::collect() +{ + int ret; + + perf_begin(_sample_perf); + + /* clear buffer if last read was too long ago */ + //int64_t read_elapsed = hrt_elapsed_time(&_last_read); + + // ----------------------- LANBAO SPECIFIC --------------------------- + //printf("enter ISL2950::collect() - reads from serial"); + uint8_t buffer[50]; + int bytes_available = 0; + // int bytes_processed = 0; + int bytes_read = 0; + // bool full_frame; +bytes_read = bytes_read; + + bytes_read = ::read(_fd, buffer + bytes_available, 50 - bytes_available); + printf("read() returns %02X %02X %02X %02X \n", buffer[0], buffer[1],buffer[2],buffer[3] ); + + + //-------------------------------------------------------------------- + + _last_read = hrt_absolute_time(); + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +ISL2950::start() +{ + PX4_INFO("ISL2950::start() - launch the work queue"); + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&ISL2950::cycle_trampoline, this, 1); + +} + +void +ISL2950::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +ISL2950::cycle_trampoline(void *arg) +{ + ISL2950 *dev = static_cast(arg); + + dev->cycle(); +} + +void +ISL2950::cycle() +{ + PX4_DEBUG("ISL2950::cycle() - in the cycle"); + /* fds initialized? */ + if (_fd < 0) { + /* open fd */ + _fd = ::open(_port,O_RDWR); + + if (_fd < 0) { + PX4_ERR("ISL2950::cycle() - open failed (%i)", errno); + return; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + unsigned speed = B115200; + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("CFG: %d ISPD", termios_state); + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("CFG: %d OSPD", termios_state); + } + + if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("baud %d ATTR", termios_state); + } + } + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + int collect_ret = collect(); + + if (collect_ret == -EAGAIN) { + /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ + work_queue(HPWORK, + &_work, + (worker_t)&ISL2950::cycle_trampoline, + this, + USEC2TICK(1042 * 8)); + return; + } + + if (OK != collect_ret) { + + /* we know the sensor needs about four seconds to initialize */ + if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { + PX4_ERR("collection error #%u", _consecutive_fail_count); + } + + _consecutive_fail_count++; + + /* restart the measurement state machine */ + start(); + return; + + } else { + /* apparently success */ + _consecutive_fail_count = 0; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(_conversion_interval)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&ISL2950::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(_conversion_interval)); + + return; + } + } + + /* measurement phase */ +/* if (OK != measure()) { + PX4_DEBUG("measure error"); + } +*/ + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&ISL2950::cycle_trampoline, + this, + USEC2TICK(_conversion_interval)); +} + +void +ISL2950::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + printf("poll interval: %d ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace isl2950 +{ + +ISL2950 *g_dev; + +int start(const char *port, uint8_t rotation); +int stop(); +int test(); +int reset(); +int info(); + +/** + * Start the driver. + */ +int +start(const char *port, uint8_t rotation) +{ + int fd; + + if (g_dev != nullptr) { + PX4_WARN("already started"); + return -1; + } + + /* create the driver */ + g_dev = new ISL2950(port, rotation); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(RANGE_FINDER0_DEVICE_PATH, 0); + + if (fd < 0) { + PX4_ERR("device open fail (%i)", errno); + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("failed to set baudrate %d", B115200); + goto fail; + } + PX4_DEBUG("isl2950::start() succeeded"); + return 0; + +fail: + PX4_DEBUG("isl2950::start() failed"); + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + return -1; +} + +/** + * Stop the driver + */ +int stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + return -1; + } + + return 0; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +int +test() +{ + struct distance_sensor_s report; + ssize_t sz; + + int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + PX4_ERR("%s open failed (try 'isl2950 start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH); + return -1; + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_ERR("immediate read failed"); + return -1; + } + + print_message(report); + + /* start the sensor polling at 2 Hz rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + PX4_ERR("failed to set 2Hz poll rate"); + return -1; + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + int ret = poll(&fds, 1, 2000); + + if (ret != 1) { + PX4_ERR("timed out"); + break; + } + + /* now go get it */ + sz = 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 != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + PX4_ERR("ioctl SENSORIOCSPOLLRATE failed"); + return -1; + } + + return 0; +} + +/** + * Reset the driver. + */ +int +reset() +{ + int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + PX4_ERR("open failed (%i)", errno); + return -1; + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + PX4_ERR("driver reset failed"); + return -1; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("driver poll restart failed"); + return -1; + } + + return 0; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return -1; + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return 0; +} + +} // namespace + +int +isl2950_main(int argc, char *argv[]) +{ + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + const char *device_path = ISL2950_DEFAULT_PORT; + int ch; + 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 -1; + } + } + + if (myoptind >= argc) { + goto out_error; + } + + /* + * Start/load the driver. + */ + if (!strcmp(argv[myoptind], "start")) { + return isl2950::start(device_path, rotation); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[myoptind], "stop")) { + return isl2950::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[myoptind], "test")) { + return isl2950::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[myoptind], "reset")) { + return isl2950::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { + return isl2950::info(); + } + +out_error: + PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); + return -1; +} diff --git a/src/drivers/distance_sensor/airlango/isl2950_parser.cpp b/src/drivers/distance_sensor/airlango/isl2950_parser.cpp new file mode 100644 index 000000000000..2f50974040cb --- /dev/null +++ b/src/drivers/distance_sensor/airlango/isl2950_parser.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 isl2950_parser.cpp + * @author Claudio Micheli claudio@auterion.com + * + */ + +#include "isl2950_parser.h" +#include +#include + +#include "isl2950_parser.h" +#include +#include + + +int isl2950_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum ISL2950_PARSE_STATE *state, float *dist) +{ + int bytes_processed = 0; + +// SOME STUFFS + return bytes_processed; +} diff --git a/src/drivers/distance_sensor/airlango/isl2950_parser.h b/src/drivers/distance_sensor/airlango/isl2950_parser.h new file mode 100644 index 000000000000..6e4b3a9f9da6 --- /dev/null +++ b/src/drivers/distance_sensor/airlango/isl2950_parser.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 sf0x_parser.cpp + * @author Lorenz Meier + * + * Declarations of parser for the Lightware SF0x laser rangefinder series + */ + +#pragma once + +enum ISL2950_PARSE_STATE { + ISL2950_PARSE_STATE0_UNSYNC = 0, + ISL2950_PARSE_STATE1_SYNC, + ISL2950_PARSE_STATE2_GOT_DIGIT0, + ISL2950_PARSE_STATE3_GOT_DOT, + ISL2950_PARSE_STATE4_GOT_DIGIT1, + ISL2950_PARSE_STATE5_GOT_DIGIT2, + ISL2950_PARSE_STATE6_GOT_CARRIAGE_RETURN +}; + +int isl2950_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum ISL2950_PARSE_STATE *state, float *dist); diff --git a/src/drivers/distance_sensor/airlango/repo/CMakeLists_old.txt b/src/drivers/distance_sensor/airlango/repo/CMakeLists_old.txt new file mode 100644 index 000000000000..2194da350b38 --- /dev/null +++ b/src/drivers/distance_sensor/airlango/repo/CMakeLists_old.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2016 Airlango Inc. All rights reserved. +# +############################################################################ + +# set(TOOLS_ERROR_MSG +# "The AIRLANGO_SDK must be installed and the environment variable AIRLANG_SDK must be set" +# "(e.g. export AIRLANGO_SDK=/opt/airlangoSDK)") + +# if ("$ENV{AIRLANGO_SDK}" STREQUAL "") +# message(FATAL_ERROR ${TOOLS_ERROR_MSG}) +# else() +# set(AIRLANGO_SDK $ENV{AIRLANGO_SDK}) +# endif() + +# AirlangoSDK library header files +# include_directories( +# ${AIRLANGO_SDK}/hexagon/include +# ) + +# add_library(libtof SHARED IMPORTED GLOBAL) +# set_target_properties(libtof PROPERTIES IMPORTED_LOCATION ${AIRLANGO_SDK}/hexagon/lib/libtof.so) + +px4_add_module( + MODULE drivers__airlangotof + MAIN tof_main + STACK_MAIN 1200 + COMPILE_FLAGS + -Weffc++ + SRCS + lanbao_isl_v2.cpp + lanbao_isl.cpp + tof.cpp + tof_main.cpp + + DEPENDS + ) + +# set(module_external_libraries +# ${module_external_libraries} +# libtof +# CACHE INTERNAL "module_external_libraries" +# ) diff --git a/src/drivers/distance_sensor/airlango/repo/lanbao_isl.cpp b/src/drivers/distance_sensor/airlango/repo/lanbao_isl.cpp new file mode 100644 index 000000000000..36fbbee93438 --- /dev/null +++ b/src/drivers/distance_sensor/airlango/repo/lanbao_isl.cpp @@ -0,0 +1,305 @@ +// +// Copyright (c) 2016 Airlango Ltd. All rights reserved. +// +// @file lanbao_isl.cpp +// +// Device driver implementaion for Lanbao ISL29501 +// +#include +#include +#include +#include +#include +#include +#include "lanbao_isl.h" + +static const char _height_instructions[] = { + 0xA5, 0xF0, 0x0F, 0x5A, 0xFE, 0xA5, 0x05, 0x02, 0x5A, 0x06 +}; + +static const int _height_instructions_len = + sizeof(_height_instructions) / sizeof(_height_instructions[0]); + +static enum DSPAL_SERIAL_BITRATES IntToDspalSerialBitratesEnum(int bitrate) { + // NOTE: here we only support the data rate we possibly use + switch (bitrate) { + case 115200: + return DSPAL_SIO_BITRATE_115200; + default: + return DSPAL_SIO_BITRATE_MAX; + } +} + +LanbaoIsl::LanbaoIsl() + : callback_(NULL), + context_(NULL), + fd_(-1), + rx_should_stop_(false) { + PX4_DEBUG("LanbaoIsl ctor"); +} + +LanbaoIsl::~LanbaoIsl() { + PX4_DEBUG("LanbaoIsl dtor"); + PX4_DEBUG("waiting on rx thread"); + rx_should_stop_ = true; + pthread_join(rx_thread_, NULL); + PX4_DEBUG("RX thread done"); + + Stop(); + CloseDevice(); +} + +int LanbaoIsl::SetDeviceMode(IslWorkingMode mode) { + if (mode == KEEP_HEIGHT) { // actively config mode to HEIGHT + int ret = write(fd_, _height_instructions, _height_instructions_len); + if (ret != _height_instructions_len) { + PX4_ERR("failed to write working mode command to device"); + return -1; + } + } + return 0; +} + +int LanbaoIsl::InitializeInternal(const char* device, int baudrate) { + PX4_DEBUG("LanbaoIsl::InitializeInternal()"); + + fd_ = open(device, O_RDWR); + if (fd_ < 0) { + PX4_ERR("failed to open %s", device); + return -1; + } + + // set the serial baud rate + struct dspal_serial_ioctl_data_rate rate_cfg = { + .bit_rate = IntToDspalSerialBitratesEnum(baudrate) + }; + + if (rate_cfg.bit_rate == DSPAL_SIO_BITRATE_MAX) { + PX4_ERR("baudrate not supported: %d", baudrate); + goto failure; + } + + if (ioctl(fd_, SERIAL_IOCTL_SET_DATA_RATE, (void*)&rate_cfg) < 0) { + PX4_ERR("failed to set baudrate %d on %s", baudrate, device); + goto failure; + } + +#if 0 + { + // This device requires us to manually change its working mode + IslWorkingMode mode = KEEP_HEIGHT; + if (SetDeviceMode(mode) < 0) { + PX4_ERR("failed to set device working mode as KEEP_HEIGHT"); + goto failure; + } + } +#endif + + PX4_DEBUG("LanbaoIsl::InitializeInternal() succeeded"); + return 0; + +failure: + PX4_DEBUG("LanbaoIsl::InitializeInternal() failed"); + + CloseDevice(); + return -1; +} + +void* LanbaoIsl::RxTrampoline(void *arg) { + LanbaoIsl* instance = reinterpret_cast(arg); + return instance->RxMain(); +} + +void* LanbaoIsl::RxMain() { + PX4_DEBUG("enter LanbaoIsl::RxMain()"); + uint8_t buffer[50]; + int bytes_available = 0; + int bytes_processed = 0; + int bytes_read = 0; + bool full_frame; + + while (!rx_should_stop_) { + // sleep for a short while. This is b/c the + // dspal serial driver does not support blocking + // read. + usleep(10 * 1000); + + // read incoming bytes into buffer + // On Eagle board, `read()` returns: + // 0 means there's no more data available to read + // < 0 means something error on hardware + // > 0 means successfully get data + bytes_read = read(fd_, buffer + bytes_available, 50 - bytes_available); + PX4_DEBUG("read() returns %d", bytes_read); + + if (bytes_read < 0) { + PX4_ERR("error on read(): %d", bytes_read); + rx_should_stop_ = true; + } else if (bytes_read == 0) { + continue; + } + + bytes_available += bytes_read; + + // parse the buffer data + full_frame = false; + bytes_processed = Parse(buffer, bytes_available, &full_frame); + + PX4_DEBUG("Parse() processed %d bytes, full_frame %d", + bytes_processed, full_frame); + + // discard the processed bytes and move the buffer content to the head + bytes_available -= bytes_processed; + memcpy(buffer, buffer + bytes_processed, bytes_available); + + // if full frame is identified, invoke the + // callback_ if available + if (full_frame) { + int raw_measurement = data_.distance_mm; + + // TODO:ATTENTION: + // +#if 0 + raw_measurement -= 80; + if (raw_measurement < 0) { + raw_measurement = 0; + } +#endif + + data_.raw_distance_mm = raw_measurement; + data_.distance_mm = distance_filter_.Filter(raw_measurement); + PX4_DEBUG("tof measurement data, raw: %d mm, filtered: %d mm", + raw_measurement, data_.distance_mm); + if (callback_) { + callback_(&data_, context_); + } + } + } + + PX4_DEBUG("exit RxMain()"); + return NULL; +} + +void LanbaoIsl::CloseDevice() { + PX4_DEBUG("LanbaoIsl::CloseDevice()"); + + if (fd_ >= 0) { + PX4_DEBUG("close device handle %d", fd_); + close(fd_); + fd_ = -1; + } +} + +int LanbaoIsl::StartRxThread(DataReadyCallback callback, void* context) { + PX4_DEBUG("LanbaoIsl::StartRxThread()"); + int ret; + + callback_ = callback; + context_ = context; + + rx_should_stop_ = false; + + pthread_attr_t attr; + size_t stacksize = -1; + pthread_attr_init(&attr); + pthread_attr_getstacksize(&attr, &stacksize); + PX4_DEBUG("RX thread stack size: %d", stacksize); + stacksize = 8 * 1024; + + PX4_DEBUG("setting the thread stack size to[%d]", stacksize); + pthread_attr_setstacksize(&attr, stacksize); + + ret = pthread_create(&rx_thread_, &attr, &LanbaoIsl::RxTrampoline, this); + if (ret != 0) { + PX4_ERR("Failed to create RX thread in LanbaoIsl: %d", ret); + return -1; + } + + PX4_DEBUG("RX thread created in LanbaoIsl"); + return 0; +} + +int LanbaoIsl::SendMeasurementCommand() { + PX4_DEBUG("SendMeasurementCommand(), nothing need to do since device will."); + return 0; +} + +int LanbaoIsl::Parse(const uint8_t* buffer, int length, bool* full_frame) { + static TofFramingState state = TFS_NOT_STARTED; + static uint16_t crc16 = 0; + int bytes_processed = 0; + + PX4_DEBUG("LanbaoTof::Parse()"); + + while (bytes_processed < length) { + uint8_t b = buffer[bytes_processed++]; + PX4_DEBUG("parse byte 0x%02X", b); + + switch (state) { + case TFS_NOT_STARTED: + if (b == TOF_SFD1) { + crc16 = b; + state = TFS_GOT_SFD1; + PX4_DEBUG("Got SFD1"); + } + break; + + case TFS_GOT_SFD1: + if (b == TOF_SFD2) { + crc16 += b; + state = TFS_GOT_SFD2; + PX4_DEBUG("Got SFD2"); + } else if (b == TOF_SFD1) { + crc16 = b; + state = TFS_GOT_SFD1; + PX4_DEBUG("Discard previous SFD1, Got new SFD1"); + } else { + state = TFS_NOT_STARTED; + } + break; + + case TFS_GOT_SFD2: + crc16 += b; + data_.distance_mm = b; + state = TFS_GOT_DATA1; + PX4_DEBUG("Got DATA1 0x%02X", b); + break; + + case TFS_GOT_DATA1: + crc16 += b; + data_.distance_mm = (data_.distance_mm << 8) + b; + state = TFS_GOT_DATA2; + PX4_DEBUG("Got DATA2 0x%02X", b); + break; + + case TFS_GOT_DATA2: + if (b == (crc16 >> 8)) { + state = TFS_GOT_CHECKSUM1; + } else { + PX4_DEBUG("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X", + b, crc16); + state = TFS_NOT_STARTED; + } + break; + + case TFS_GOT_CHECKSUM1: + // Here, reset state to `NOT-STARTED` no matter crc ok or not + state = TFS_NOT_STARTED; + if (b == (crc16 & 0xFF)) { + PX4_DEBUG("Checksum verified"); + *full_frame = true; + return bytes_processed; + } else { + PX4_DEBUG("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X", + b, crc16); + } + break; + + default: + PX4_DEBUG("This should never happen.") + break; + } + } + + return bytes_processed; +} diff --git a/src/drivers/distance_sensor/airlango/repo/lanbao_isl.h b/src/drivers/distance_sensor/airlango/repo/lanbao_isl.h new file mode 100644 index 000000000000..9f52686454f8 --- /dev/null +++ b/src/drivers/distance_sensor/airlango/repo/lanbao_isl.h @@ -0,0 +1,155 @@ +// +// Copyright (c) 2016 Airlango Ltd. All rights reserved. +// +// @file lanbao_isl.h +// +// Device driver implementation for ISL29501 from www.shlanbao.com +// +#pragma once +#include +#include +#include +#include "tof.h" +#include "adiag.h" + +// Average filter +template +class AverageFilter { + public: + AverageFilter(int window_size = 12) + : window_full_(false), + window_size_(window_size), + item_index_(0), + items_(nullptr) { + // ASSERT(window_size > 0); + items_ = new T[window_size]; + if (items_ != nullptr) { + for (int i = 0; i < window_size; i++) { + items_[i] = 0; + } + } + } + + ~AverageFilter() { + if (items_ != nullptr) { + delete[] items_; + } + } + + public: + T Filter(const T& item) { +#if 0 + sum_ -= items_[item_index_]; + sum_ += item; + items_[item_index_] = item; + if (++item_index_ == window_size_) { + item_index_ = 0; + window_full_ = true; + } + + T result = sum_ / (window_full_ ? window_size_ : item_index_); + return result; +#else + items_[item_index_] = item; + if (++item_index_ == window_size_) { + item_index_ = 0; + window_full_ = true; + } + + int num = NumOfSamplingForCalc(item); + T result = CalcAverage(num); + + return result; +#endif + } + + protected: + T CalcAverage(int num) { + int valid_index; // (latest) valid index + + if (item_index_ == 0) { + valid_index = window_size_ - 1; + } else { + valid_index = item_index_ - 1; + } + + // IF data not fully collected, we return the original sampling data, + // this wil only occur at the very earlier stage + if (!window_full_) { + return items_[valid_index]; + } + + sum_ = 0; + + for (int i = 0; i < num; i++) { + sum_ += items_[valid_index]; + + if (--valid_index < 0) { + valid_index = window_size_ - 1; + } + } + + return sum_ / num; + } + + int NumOfSamplingForCalc(const T& item) { + int num = (item / 1000) + 3; + if (num > window_size_) { + num = window_size_; + } + return num; + } + + protected: + bool window_full_ = false; + int window_size_ = 0; + int item_index_ = 0; + T* items_ = nullptr; + T sum_ = 0; +}; + +// frame start delimiter +#define TOF_SFD1 0xA5 +#define TOF_SFD2 0x5A + +typedef enum { + TFS_NOT_STARTED = 0, + TFS_GOT_SFD1, + TFS_GOT_SFD2, + TFS_GOT_DATA1, + TFS_GOT_DATA2, + TFS_GOT_CHECKSUM1, + TFS_GOT_CHECKSUM2, +} TofFramingState; + +enum IslWorkingMode { + KEEP_HEIGHT = 0, + NUM_WORKING_MODE +}; + +// Lanbao ISL29501 to provide tof functionality +class LanbaoIsl : public Tof { + public: + LanbaoIsl(); + virtual ~LanbaoIsl(); + virtual TofModel model() const { return LANBAO_ISL; }; + + protected: + virtual int InitializeInternal(const char* device, int baudrate); + virtual int StartRxThread(DataReadyCallback callback, void* context); + virtual int SendMeasurementCommand(); + virtual int Parse(const uint8_t* buffer, int length, bool* full_frame); + + static void* RxTrampoline(void* arg); + void* RxMain(); + void CloseDevice(); + int SetDeviceMode(IslWorkingMode mode = KEEP_HEIGHT); + + DataReadyCallback callback_; + void* context_; + int fd_; + pthread_t rx_thread_; + volatile bool rx_should_stop_; + struct TofData data_; + AverageFilter distance_filter_; +}; diff --git a/src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.cpp b/src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.cpp new file mode 100644 index 000000000000..ab1d3b350e25 --- /dev/null +++ b/src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.cpp @@ -0,0 +1,178 @@ +// +// Copyright (c) 2016 Airlango Ltd. All rights reserved. +// +// @file lanbao_isl.cpp +// +// Device driver implementaion for Lanbao ISL29501 hardware version 2, which +// uses crc-16 for checksum calculation. +// Also see http://zhangxu1018.blog.sohu.com/161752060.html +// +#include +#include "lanbao_isl_v2.h" + +typedef unsigned char UCHAR; +typedef unsigned short USHORT; + +static const UCHAR aucCRCHi[] = { + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40 +}; + +static const UCHAR aucCRCLo[] = { + 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, + 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, + 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, + 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, + 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, + 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, + 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, + 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, + 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, + 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, + 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, + 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, + 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, + 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, + 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, + 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, + 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, + 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, + 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, + 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, + 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, + 0x41, 0x81, 0x80, 0x40 +}; + +USHORT usMBCRC16(UCHAR* pucFrame, USHORT usLen) { + UCHAR ucCRCHi = 0xFF; + UCHAR ucCRCLo = 0xFF; + int iIndex; + while (usLen--) { + iIndex = ucCRCLo ^ *(pucFrame++); + ucCRCLo = (UCHAR)(ucCRCHi ^ aucCRCHi[iIndex]); + ucCRCHi = aucCRCLo[iIndex]; + } + return (USHORT)(ucCRCHi << 8 | ucCRCLo); +} + +LanbaoIslV2::LanbaoIslV2() { + PX4_DEBUG("LanbaoIslV2 ctor"); +} + +LanbaoIslV2::~LanbaoIslV2() { + PX4_DEBUG("LanbaoIslV2 dtor"); +} + +// TOF frame format +// +// 1B 1B 1B 1B 2B +// | 0xA5 | 0x5A | distance-MSB | distance-LSB | crc-16 | +// +// Frame data saved for CRC calculation +const static int TOF_DISTANCE_MSB_POS = 2; +const static int TOF_DISTANCE_LSB_POS = 3; +const static int TOF_CRC_CALC_DATA_LEN = 4; +static unsigned char frame_data[TOF_CRC_CALC_DATA_LEN] = { + TOF_SFD1, TOF_SFD2, 0, 0 +}; + +int LanbaoIslV2::Parse(const uint8_t* buffer, int length, bool* full_frame) { + static TofFramingState state = TFS_NOT_STARTED; + static uint16_t crc16 = 0; + int bytes_processed = 0; + + PX4_DEBUG("LanbaoTofV2::Parse()"); + + while (bytes_processed < length) { + uint8_t b = buffer[bytes_processed++]; + PX4_DEBUG("parse byte 0x%02X", b); + + switch (state) { + case TFS_NOT_STARTED: + if (b == TOF_SFD1) { + state = TFS_GOT_SFD1; + PX4_DEBUG("Got SFD1"); + } + break; + + case TFS_GOT_SFD1: + if (b == TOF_SFD2) { + state = TFS_GOT_SFD2; + PX4_DEBUG("Got SFD2"); + } else if (b == TOF_SFD1) { + state = TFS_GOT_SFD1; + PX4_DEBUG("Discard previous SFD1, Got new SFD1"); + } else { + state = TFS_NOT_STARTED; + } + break; + + case TFS_GOT_SFD2: + frame_data[TOF_DISTANCE_MSB_POS] = b; + state = TFS_GOT_DATA1; + PX4_DEBUG("Got DATA1 0x%02X", b); + break; + + case TFS_GOT_DATA1: + frame_data[TOF_DISTANCE_LSB_POS] = b; + state = TFS_GOT_DATA2; + PX4_DEBUG("Got DATA2 0x%02X", b); + // do crc calculation + crc16 = usMBCRC16(frame_data, TOF_CRC_CALC_DATA_LEN); + // convert endian + crc16 = (crc16 >> 8) | (crc16 << 8); + break; + + case TFS_GOT_DATA2: + if (b == (crc16 >> 8)) { + state = TFS_GOT_CHECKSUM1; + } else { + PX4_DEBUG("Checksum invalid on high byte: 0x%02X, calculated: 0x%04X", + b, crc16); + state = TFS_NOT_STARTED; + } + break; + + case TFS_GOT_CHECKSUM1: + // Here, reset state to `NOT-STARTED` no matter crc ok or not + state = TFS_NOT_STARTED; + if (b == (crc16 & 0xFF)) { + PX4_DEBUG("Checksum verified"); + data_.distance_mm = (frame_data[TOF_DISTANCE_MSB_POS] << 8) + | frame_data[TOF_DISTANCE_LSB_POS]; + *full_frame = true; + return bytes_processed; + } else { + PX4_DEBUG("Checksum invalidon low byte: 0x%02X, calculated: 0x%04X", + b, crc16); + } + break; + + default: + PX4_DEBUG("This should never happen.") + break; + } + } + + return bytes_processed; +} diff --git a/src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.h b/src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.h new file mode 100644 index 000000000000..4247a19b2d55 --- /dev/null +++ b/src/drivers/distance_sensor/airlango/repo/lanbao_isl_v2.h @@ -0,0 +1,23 @@ +// +// Copyright (c) 2016 Airlango Ltd. All rights reserved. +// +// @file lanbao_isl_v2.h +// +// Lanbao TOF, hw version 2, with CRC-16 support +// +#ifndef EAGLE_TOF_LANBAO_ISL_V2_H_ +#define EAGLE_TOF_LANBAO_ISL_V2_H_ + +#include "lanbao_isl.h" + +class LanbaoIslV2 : public LanbaoIsl { + public: + LanbaoIslV2(); + virtual ~LanbaoIslV2(); + virtual TofModel model() const { return LANBAO_ISL_V2; } + + protected: + virtual int Parse(const uint8_t* buffer, int length, bool* full_frame); +}; + +#endif diff --git a/src/drivers/distance_sensor/airlango/repo/tof.cpp b/src/drivers/distance_sensor/airlango/repo/tof.cpp new file mode 100644 index 000000000000..cdc5220535b9 --- /dev/null +++ b/src/drivers/distance_sensor/airlango/repo/tof.cpp @@ -0,0 +1,345 @@ +// +// Copyright (c) 2016 Airlango Ltd. All rights reserved. +// +// @file tof.cpp +// +// Basic implementation for TOF device driver. +// +#include +#include +#include +#include +#include +#include "tof.h" +#include "lanbao_isl.h" +#include "lanbao_isl_v2.h" + + +#define TOF_MEASUREMENT_TIMEOUT_USEC 200000 +#define TOF_MEASUREMENT_TIMER_SIGNAL (SIGRTMAX-1) + +#define TIME_DIFF_USEC(start, end) \ + (((end).tv_sec - (start).tv_sec)*1E6 + ((end).tv_nsec - (start).tv_nsec)/1E3) + +Tof* Tof::instance_ = NULL; + +Tof::Tof() + : is_initialized_(false), + callback_(NULL), + context_(NULL), + tx_thread_(0), + measurement_interval_ms_(0), + measurement_should_stop_(true), + periodic_measurement_running_(false) { + PX4_DEBUG("Tof ctor"); + + memset(&last_measurement_ts_, 0, sizeof(struct timespec)); + memset(&last_measurement_, 0, sizeof(struct TofData)); + + // NOTE(jintao): Must initialize mutex with the following functions. + // Using INITIALIZER results in DSP crash while calling + // and pthread_mutex_destory. + pthread_mutex_init(&mutex_, NULL); +} + +Tof::~Tof() { + PX4_DEBUG("Tof dtor"); + instance_ = NULL; + + pthread_mutex_destroy(&mutex_); +} + +Tof* Tof::GetInstance(TofModel model) { + PX4_DEBUG("Tof::GetInstance() for model %s", TofModelToStr(model)); + + if (model >= TOF_MODEL_NUM) { + PX4_ERR("Unknown model"); + return NULL; + } + + if (instance_ != NULL) { + TofModel curr_model = instance_->model(); + if (curr_model != model) { + PX4_ERR("Tof already initialized for model %s", + TofModelToStr(curr_model)); + return NULL; + } + + PX4_DEBUG("return existing instance %s", TofModelToStr(curr_model)); + return instance_; + } + + switch (model) { + case LANBAO_ISL: + instance_ = new LanbaoIsl(); + break; + case LANBAO_ISL_V2: + instance_ = new LanbaoIslV2(); + break; + default: + return NULL; + } + + if (instance_ == NULL) { + PX4_ERR("Failed to instantiate Tof model %s", TofModelToStr(model)); + return NULL; + } + + PX4_DEBUG("Created instance for %s", TofModelToStr(model)); + return instance_; +} + +int Tof::Initialize(const char* device, int baudrate) { + int ret; + + if (is_initialized_) { + PX4_ERR("Initialize() can be called only once!"); + return -1; + } + + // Initialize the specified serial device with baudrate + ret = InitializeInternal(device, baudrate); + if (ret < 0) { + PX4_ERR("Failed to initialize device %s baudrate %d", device, baudrate); + return -1; + } + + // Start RX thread to parse the incoming frame + ret = StartRxThread(Tof::DataReadyCb, (void*)this); + if (ret < 0) { + PX4_ERR("Failed to register Data Ready Callback. Fatal!"); + return -1; + } + + is_initialized_ = true; + return 0; +} + +void Tof::DataReadyCb(const struct TofData* data, void* context) { + PX4_DEBUG("Tof::DataReadyCb(): new data %d mm", data->distance_mm); + Tof* obj = (Tof*)context; + obj->HandleNewMeasurement(data); +} + +void Tof::HandleNewMeasurement(const struct TofData* data) { + PX4_DEBUG("Tof::HandleNewMeasurement()"); + + struct TofData m; + int ret; + struct timespec ts; + + // remember the latest measurement result. Call user callback + // if available. + + pthread_mutex_lock(&mutex_); + + memcpy(&last_measurement_, data, sizeof(struct TofData)); + clock_gettime(CLOCK_REALTIME, &last_measurement_ts_); + + // save a snapshot to avoid race condition. + m = last_measurement_; + ts = last_measurement_ts_; + + pthread_mutex_unlock(&mutex_); + + PX4_DEBUG("last_measurement updated distance %d mm, ts %llu ms", + m.distance_mm, ts.tv_sec*1000+(uint64_t)(ts.tv_nsec/1E6)); + + if (callback_) { + PX4_DEBUG("Notifying user of new measurement result"); + callback_(&m, context_); + } +} + +int Tof::Start(int interval_ms, DataReadyCallback callback, void* context) { + PX4_DEBUG("Tof::Start() internval %d ms", interval_ms); + int ret; + + if (!is_initialized_) { + PX4_ERR("Start() cannot be called without device initialized!"); + return -1; + } + + if (interval_ms <= 0) { + PX4_ERR("Invalid measurement interval %d", interval_ms); + return -1; + } + + callback_ = callback; + context_ = context; + measurement_interval_ms_ = interval_ms; + + measurement_should_stop_ = false; + periodic_measurement_running_ = true; + + // If device requires user to actively send measurement commands, we create + // a thread (timer) to do that periodically + + if (NeedTriggerManually()) { + pthread_attr_t attr; + size_t stacksize = -1; + pthread_attr_init(&attr); + pthread_attr_getstacksize(&attr, &stacksize); + PX4_DEBUG("TX thread stack size: %d", stacksize); + stacksize = 8 * 1024; + + PX4_DEBUG("setting the thread stack size to[%d]", stacksize); + pthread_attr_setstacksize(&attr, stacksize); + ret = pthread_create(&tx_thread_, &attr, &Tof::TxTrampoline, this); + if (ret != 0) { + periodic_measurement_running_ = false; + PX4_ERR("Failed to create TX thread in Tof: %d", ret); + return -1; + } + } + + return 0; +} + +void* Tof::TxTrampoline(void* arg) { + Tof* obj = (Tof*)arg; + return obj->DoPeriodicMeasurement(); +} + +void* Tof::DoPeriodicMeasurement() { + struct itimerspec timer_spec; + struct sigevent sigev; + sigset_t set; + timer_t timer_id; + int sig; + int rv; + + sigev.sigev_notify = SIGEV_SIGNAL; + sigev.sigev_signo = TOF_MEASUREMENT_TIMER_SIGNAL; + sigev.sigev_value.sival_int = TOF_MEASUREMENT_TIMER_SIGNAL; + sigev.sigev_notify_function = 0; + sigev.sigev_notify_attributes = 0; + + // create timer + if (timer_create(CLOCK_REALTIME, &sigev, &timer_id) != 0) { + PX4_ERR("timer_create failed"); + return NULL; + } + + timer_spec.it_value.tv_sec = 0; + timer_spec.it_value.tv_nsec = measurement_interval_ms_*1E6; + timer_spec.it_interval.tv_sec = 0; + timer_spec.it_interval.tv_nsec = measurement_interval_ms_*1E6; + + // start the timer + if (timer_settime(timer_id, 0, &timer_spec, NULL) != 0) { + PX4_ERR("timer_settime failed"); + timer_delete(timer_id); + return NULL; + } + + sigemptyset(&set); + sigaddset(&set, TOF_MEASUREMENT_TIMER_SIGNAL); + + PX4_DEBUG("start periodic measurement"); + while(!measurement_should_stop_) { + rv = sigwait(&set, &sig); + if (rv != 0 || sig != TOF_MEASUREMENT_TIMER_SIGNAL) { + PX4_ERR("sigwait failed rv %d sig %d", rv, sig); + continue; + } + PX4_DEBUG("waken up by signal %d", sig); + + rv = SendMeasurementCommand(); + if (rv < 0) { + PX4_ERR("SendMeasurementCommand() failed: %d", rv); + } else { + PX4_DEBUG("Sent measurement command"); + } + } + + PX4_DEBUG("stop periodic measurement"); + + // delete the timer + timer_delete(timer_id); + return NULL; +} + +int Tof::Stop() { + PX4_DEBUG("Tof::Stop()"); + PX4_DEBUG("stopping measurement thread"); + + measurement_should_stop_ = true; + if (tx_thread_ != 0) { + pthread_join(tx_thread_, NULL); + tx_thread_ = 0; + } + + periodic_measurement_running_ = false; + + PX4_DEBUG("measurement thread stopped"); + return 0; +} + +int Tof::DoMeasurement() { + int ret; + struct timespec ts_start, ts_now, old_ts; + bool stop_wait = false; + uint64_t time_lapse_usec; + int i; + + PX4_DEBUG("Tof::DoMeasurement()"); + + if (!is_initialized_) { + PX4_ERR("DoMeasurement() cannot be called without device initialized!"); + return -1; + } + + // get a snapshot of previous last_measurement_ts_ + pthread_mutex_lock(&mutex_); + old_ts = last_measurement_ts_; + pthread_mutex_unlock(&mutex_); + + + // Send measurement command only if the device is not in + // periodic measurement mode + if (!periodic_measurement_running_) { + ret = SendMeasurementCommand(); + if (ret < 0) { + PX4_DEBUG("SendMeasurementCommand() failed: %d", ret); + return -1; + } + } + + clock_gettime(CLOCK_REALTIME, &ts_start); + + // Wait on new data arrival until new measurement is ready or time out occurs. + // Periodically poll the last measurement result. If the last measurement + // timestamp is within max RTT 20ms, it is considered the new measurement + // data. + + while (!stop_wait) { + pthread_mutex_lock(&mutex_); + + if (last_measurement_ts_.tv_sec != old_ts.tv_sec || + last_measurement_ts_.tv_nsec != old_ts.tv_nsec) { + stop_wait = true; + ret = last_measurement_.distance_mm; + pthread_mutex_unlock(&mutex_); + break; + } + + pthread_mutex_unlock(&mutex_); + + clock_gettime(CLOCK_REALTIME, &ts_now); + time_lapse_usec = TIME_DIFF_USEC(ts_start, ts_now); + + PX4_DEBUG("time_lapse_usec %llu", time_lapse_usec); + + if (time_lapse_usec > TOF_MEASUREMENT_TIMEOUT_USEC) { + stop_wait = true; + ret = -2; + } + + usleep(10000); + } + + PX4_DEBUG("DoMeasurement() return %d", ret); + + return ret; +} diff --git a/src/drivers/distance_sensor/airlango/repo/tof.h b/src/drivers/distance_sensor/airlango/repo/tof.h new file mode 100644 index 000000000000..7564ca4f391c --- /dev/null +++ b/src/drivers/distance_sensor/airlango/repo/tof.h @@ -0,0 +1,173 @@ +// +// Copyright (c) 2016 Airlango Ltd. All rights reserved. +// +// @file tof.h +// +// TOF (time of flight) device driver interfaces +// +#pragma once +#include + +// Data info reported to up-layer user +struct TofData { + int distance_mm; + int raw_distance_mm; +}; + +// Supported TOF device model +enum TofModel { + LANBAO_ISL, + LANBAO_ISL_V2, // hw version 2, with crc16 + BENEWAKE_TF_MINI, // Benewake TF_MINI + TOF_MODEL_NUM +}; + +static inline const char* TofModelToStr(enum TofModel model) { + switch (model) { + case LANBAO_ISL: + return "LANBAO_ISL"; + case LANBAO_ISL_V2: + return "LANBAO_ISL_V2"; + case BENEWAKE_TF_MINI: + return "BENEWAKE_TF_MINI"; + default: + return "Unknown"; + } +} + +typedef void (*DataReadyCallback)(const struct TofData* data, void* context); + +// Tof is a base class to define interfaces for TOF device driver. Sub-classes +// shall provide detailed implementation for specific device to complete +// the measurement functionality. +class Tof { + public: + + // Tof class method to get the singleton Tof instance. + // If no device model has been instantiated, this function instantiates + // specified model and return the pointer to the new instance. If the + // specified type has been instantiated, this function returns the + // pointer to the existing instance. If a different model has been + // instantiated, this function returns NULL. + // NOTE: + // 1. This function only instantiates the singleton object. To initialize + // the tof device, need to call Initialize(). + // 2. Tof driver is currently not implemented to be thread-safe. We assume + // there is up to 1 user that uses tof driver at a moment. + // + // @param[in] model the tof model enumeration value + // + // @return + // - pointer to the instance on success + // - NULL on error. + static Tof* GetInstance(TofModel model); + + virtual ~Tof(); + + // @brief + // Initialize the Tof driver instance for the specified device path. On + // successful initialization, the Tof device is ready to do measurement. + // For periodic measurement, see Start() function. + // + // @param[in] device + // the dspal serial device path which the tof is connected to + // @param[in] baudrate + // the UART baud rate in bit per second + // @return + // - 0 on success + // - -1 on error + int Initialize(const char* device, int baudrate); + + // @brief + // Start periodic measurement at specified interval_ms. + // @param[in] interval_ms measurement interval_ms in millisecond + // @param[in] callback data ready interrupt service routine. This is + // the callback function to be invoked when new measurement data + // is ready. + // @param[in] context address where the context data for callback is stored at + // @return + // - 0 successfully started the measurement + // - -1 device not initialized + // - -2 other errors + int Start(int interval_ms, DataReadyCallback callback, void* context); + + // Stop the periodic measurement. If this function is called when measurement + // is not running, this function takes no effect. + // @return + // - 0 on success + // - -1 on error + int Stop(); + + // Do one time measurement and return the measurement results. This is a + // blocking call. + // + // @return + // - positive integer indicating the distance to the object in millimeter + // - 0 if no object is detected + // - -1 on error + // - -2 on timeout + int DoMeasurement(); + + virtual TofModel model() const = 0; + + protected: + Tof(); + + static void DataReadyCb(const struct TofData* data, void* context); + + void HandleNewMeasurement(const struct TofData* data); + + // Initialize the specified serial device with given baudrate. The + // actual initialization operation is model specific, thus this virtual + // method should be implemented by the tof subclass. + // This function is called in Initialize(). + // + // @return + // - 0 if device is successfully initialized. + // - -1 on error. + virtual int InitializeInternal(const char* device, int baudrate) = 0; + + // Start RX thread to process incoming byte stream amd register the provided + // callback function as data ready callback. On success, when + // As the tof data parser is model dependent, thus this virtual method + // shall be implemented by the tof subclass. When new tof measurement + // data is ready, the subclass instance invokes the callback immediately. + // context is pass to the callback function as argument. + // This method is called in Initialize(). + // + // @return + // - 0 on success + // -1 on error. + virtual int StartRxThread(DataReadyCallback callback, void* context) = 0; + + // Send one time measurement command to tof device. This tof communication + // protocol is device specific and thus this virtual method needs to be + // implemented by tof subclass. + // + // @return + // - 0 on success, + // - -1 on error + virtual int SendMeasurementCommand() = 0; + + // Some devices will periodically response measurement data once it gets + // initialized. While some other devices might need user trigger measurement + // operation manually. + virtual bool NeedTriggerManually() const { return false; } + + static void* TxTrampoline(void* arg); + + void* DoPeriodicMeasurement(); + + private: + static Tof* instance_; + bool is_initialized_; + pthread_mutex_t mutex_; + struct TofData last_measurement_; + struct timespec last_measurement_ts_; + DataReadyCallback callback_; + void* context_; + pthread_t tx_thread_; + int measurement_interval_ms_; + bool measurement_should_stop_; + bool periodic_measurement_running_; +}; diff --git a/src/drivers/distance_sensor/airlango/repo/tof_main.cpp b/src/drivers/distance_sensor/airlango/repo/tof_main.cpp new file mode 100644 index 000000000000..abd8d1ba72a6 --- /dev/null +++ b/src/drivers/distance_sensor/airlango/repo/tof_main.cpp @@ -0,0 +1,370 @@ +/**************************************************************************** + * Copyright (c) 2016 Airlango, Inc. All rights reserved. + * + ****************************************************************************/ + +/** + * @file tof_main.cpp + * + * TOF device driver task + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//#define NO_ADIAG_LOG +//#define NO_ADIAG_STATS +#include + +/** driver 'main' command */ +extern "C" { __EXPORT int tof_main(int argc, char *argv[]); } + +/* + * Default parameters for tof driver + */ +#define TOF_DEFAULT_BAUDRATE 115200 +#define TOF_DEFAULT_INTERVAL 100 /* milliseconds */ + +#define MAX_LEN_DEV_PATH 32 + +static param_t algo_aes; + +namespace tof +{ + +/** device path that TOF is connected to */ +static char _device[MAX_LEN_DEV_PATH]; + +/** serial device speed (uart baudrate) */ +static int _baudrate = TOF_DEFAULT_BAUDRATE; + +/** sampling frequency in Hz */ +static int _frequency = 10; + +/** tof device model */ +static int _device_model = 0; + +/** flag indicating if TOF driver task is running */ +static bool _is_running = false; + +/** flag indicating if TOF driver task should stop */ +static bool _task_should_stop = false; + +/** handle to the task main thread */ +static px4_task_t _task_handle = -1; + +/** TOF measurement data */ +static struct TofData _data; + +/** TOF data publication */ +static orb_advert_t _tof_pub = nullptr; + +/** Print out the usage information */ +static void usage(); + +/** TOF start measurement */ +static void start(); + +/** TOF stop measurement */ +static void stop(); + +/** task main trampoline function */ +static void task_main_trampoline(int argc, char *argv[]); + +/** TOF measurement thread primary entry point */ +static void task_main(int argc, char *argv[]); + +void tof_rx_callback(const TofData* data, void* context) +{ + (void)context; + + /* copy out the TOF data */ + _data = *data; + + /* send signal to measurement thread */ + px4_task_kill(_task_handle, SIGRTMIN); +} + +void publish_reports() +{ + algo_aes = param_find("ALGO_AES"); + int aes; + + if (param_get(algo_aes, &aes) == 0) { + // AD_DEBUG("ALGO_AES %d", aes); + } + + struct distance_sensor_s report; + report.timestamp = hrt_absolute_time(); + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + report.current_distance = + static_cast(_data.distance_mm) / 1000; /* in metre */ + +// PX4_WARN("tof-avg %d, tof-raw %d", _data.distance_mm, _data.raw_distance_mm); + AD_STATS("tof_time %llu, tof-avg %d, tof-raw %d",report.timestamp, _data.distance_mm, _data.raw_distance_mm); + // TODO: subject to tune + if (report.current_distance < 0.17f) { + // NOTE(xiaoming@airlango.com): add a random noise to avoid round up error in blockstats deviation calculation + report.current_distance = 0.15f + float(rand())/100000000000000; + // AD_DEBUG("RANDOM %f", float(rand())/100000000000000); + } + + switch (aes) { + case 0: + report.min_distance = 0.1f; + break; + + case 1: + report.min_distance = 0.01f; + break; + + default: + report.min_distance = 0.17f; + break; + } + + report.max_distance = 10.0f; + report.orientation = 25;//downward facing + report.covariance = 0.002f; + // report.covariance = 0.01f; + + /* TODO: set proper ID */ + report.id = 90; + + if (_tof_pub == nullptr) { + _tof_pub = orb_advertise(ORB_ID(distance_sensor), &report); + } else { + orb_publish(ORB_ID(distance_sensor), _tof_pub, &report); + } + + // PX4_WARN("Published distance sensor data: %.3f m", report.current_distance); +} + +void task_main(int argc, char *argv[]) +{ + PX4_WARN("enter tof task_main"); + + int interval_ms = TOF_DEFAULT_INTERVAL; + if (_frequency > 0) { + interval_ms = 1000 / _frequency; + } + + /* + * initialize signal + */ + sigset_t set; + sigemptyset(&set); + sigaddset(&set, SIGRTMIN); + + /* + * start tof driver + */ + TofModel tof_model = static_cast(_device_model); + + Tof* driver = Tof::GetInstance(tof_model); + if (driver == nullptr) { + PX4_ERR("fail to instantiate tof driver"); + goto _failure; + } + + if (driver->Initialize(_device, _baudrate) < 0) { + PX4_ERR("fail to initialize tof driver"); + goto _failure; + } + + if (driver->Start(interval_ms, tof_rx_callback, nullptr) < 0) { + PX4_ERR("fail to start tof driver"); + goto _failure; + } + + /* + * enter working loop + */ + while (!_task_should_stop) { + /* wait on signal */ + int sig = 0; + int rv = sigwait(&set, &sig); + + /* check if we are waken up by the proper signal */ + if (rv != 0 || sig != SIGRTMIN) { + PX4_WARN("sigwait failed rv %d sig %d", rv, sig); + continue; + } + + /* publish distance sensor reports */ + publish_reports(); + } + +_failure: + PX4_WARN("closing tof"); + if (driver != nullptr) { + driver->Stop(); + delete driver; + } + + _is_running = false; +} + +void task_main_trampoline(int argc, char *argv[]) +{ + PX4_WARN("task_main_trampoline"); + task_main(argc, argv); +} + +void start() +{ + ASSERT(_task_handle == -1); + + _task_handle = px4_task_spawn_cmd("tof_main", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX, + 1500, + (px4_main_t)&task_main_trampoline, + nullptr); + if (_task_handle < 0) { + PX4_WARN("tof task start failed"); + return; + } + + _is_running = true; +} + +void stop() +{ + _task_should_stop = true; + + _is_running = false; + + _task_handle = -1; +} + +void usage() +{ + PX4_WARN("missing command: try 'start', 'stop', 'status'"); + PX4_WARN("options:"); + PX4_WARN(" -D device device path, e.g. /dev/tty-1"); + PX4_WARN(" -F frequency sampling frequency (Hz), default to 10"); + PX4_WARN(" -M model device hardware model (0: LANBAO_ISL)"); +} + +}; // namespace tof + + +int tof_main(int argc, char* argv[]) +{ + int ch; + int myoptind = 1; + const char* myoptarg = nullptr; + const char* device = nullptr; + const char* frequency = nullptr; + const char* device_model = nullptr; + const char* baudrate = nullptr; + + if (argc < 2) { + tof::usage(); + return -1; + } + + while ((ch = px4_getopt(argc, argv, "D:F:M:B:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'D': + device = myoptarg; + break; + + case 'F': + frequency = myoptarg; + break; + + case 'M': + device_model = myoptarg; + break; + + case 'B': + baudrate = myoptarg; + break; + + default: + tof::usage(); + return -1; + } + } + + if (device == NULL || strlen(device) == 0) { + tof::usage(); + return -1; + } + + memset(tof::_device, 0, MAX_LEN_DEV_PATH); + strncpy(tof::_device, device, MAX_LEN_DEV_PATH - 1); + + if (frequency != nullptr) { + char *endptr; + long val = strtoul(frequency, &endptr, 0); + if ((errno == ERANGE && (val == LONG_MAX || val == LONG_MIN)) + || (errno != 0 && val == 0) + || (*endptr != '\0')) { + PX4_WARN("Invalid parameter for frequency, ignore"); + } else { + tof::_frequency = val; + } + } + + if (device_model != nullptr) { + char *endptr; + long val = strtoul(device_model, &endptr, 0); + if ((errno == ERANGE && (val == LONG_MAX || val == LONG_MIN)) + || (errno != 0 && val == 0) + || (*endptr != '\0')) { + PX4_WARN("Invalid parameter for device_model, ignore"); + } else { + tof::_device_model = val; + } + } + + if (baudrate != nullptr) { + char *endptr; + long val = strtoul(baudrate, &endptr, 0); + if ((errno == ERANGE && (val == LONG_MAX || val == LONG_MIN)) + || (errno != 0 && val == 0) + || (*endptr != '\0')) { + PX4_WARN("Invalid parameter for baudrate, ignore"); + } else { + tof::_baudrate = val; + } + } + + const char* verb = argv[myoptind]; + if (!strcmp(verb, "start")) { + if (tof::_is_running) { + PX4_WARN("tof already running"); + return 1; + } + tof::start(); + + } else if (!strcmp(verb, "stop")) { + if (!tof::_is_running) { + PX4_WARN("tof is not running"); + return 1; + } + tof::stop(); + + } else if (!strcmp(verb, "status")) { + PX4_WARN("tof is %s", tof::_is_running ? "running" : "stopped"); + return 0; + + } else { + tof::usage(); + return -1; + } + + return 0; +}