diff --git a/src/drivers/distance_sensor/CMakeLists.txt b/src/drivers/distance_sensor/CMakeLists.txt
index 11e14117fda6c6fb912dbbac0c7e633159b4b94c..093e6cf573d68d406a6410609d0e433f49ff9444 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 0000000000000000000000000000000000000000..056faae748ae485eeab3e921d2cb83d272565bc1
--- /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 0000000000000000000000000000000000000000..e2a6be9fbda60404a5c7ee25d9b6abf5216d70ab
--- /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 <claudio@auterion.com>
+ *
+ * Driver for the Lanbao ISL2950
+ */
+
+ #include <px4_config.h>
+ #include <px4_getopt.h>
+ #include <px4_workqueue.h>
+
+ #include <sys/types.h>
+ #include <stdint.h>
+ #include <stdlib.h>
+ #include <stdbool.h>
+ #include <semaphore.h>
+ #include <string.h>
+ #include <fcntl.h>
+ #include <poll.h>
+ #include <errno.h>
+ #include <stdio.h>
+ #include <math.h>
+ #include <unistd.h>
+ #include <termios.h>
+
+ #include <perf/perf_counter.h>
+
+ #include <drivers/drv_hrt.h>
+ #include <drivers/drv_range_finder.h>
+ #include <drivers/device/device.h>
+ #include <drivers/device/ringbuffer.h>
+
+ #include <uORB/uORB.h>
+ #include <uORB/topics/distance_sensor.h>
+
+ #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<ISL2950 *>(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 0000000000000000000000000000000000000000..2f50974040cba2bc3895408f95b7cc5e3ff3d931
--- /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 <string.h>
+#include <stdlib.h>
+
+#include "isl2950_parser.h"
+#include <string.h>
+#include <stdlib.h>
+
+
+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 0000000000000000000000000000000000000000..6e4b3a9f9da679f5b39f2e9ab39e441a1914bce6
--- /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 <lm@inf.ethz.ch>
+ *
+ * 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 0000000000000000000000000000000000000000..2194da350b3817bd2a41319318196538583190db
--- /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 0000000000000000000000000000000000000000..36fbbee93438f36b8cd27b8e6bf926d1f18dabe2
--- /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 <fcntl.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <sys/ioctl.h>
+#include <dev_fs_lib_serial.h>
+#include <systemlib/err.h>
+#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<LanbaoIsl*>(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 0000000000000000000000000000000000000000..9f52686454f8e71bd7a0beb4eb60162c7d8a9f9e
--- /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 <pthread.h>
+#include <stdint.h>
+#include <string.h>
+#include "tof.h"
+#include "adiag.h"
+
+// Average filter
+template <typename T>
+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<int> 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 0000000000000000000000000000000000000000..ab1d3b350e25762dd2689c46833e1bec1dece1cc
--- /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 <systemlib/err.h>
+#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 0000000000000000000000000000000000000000..4247a19b2d5598ac342a710b14dfa830bf57fd22
--- /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 0000000000000000000000000000000000000000..cdc5220535b9a897d4a210e032a5504d0bcc004b
--- /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 <errno.h>
+#include <systemlib/err.h>
+#include <pthread.h>
+#include <unistd.h>
+#include <signal.h>
+#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 0000000000000000000000000000000000000000..7564ca4f391ce02269869a41840089357c43bc97
--- /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 <pthread.h>
+
+// 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 0000000000000000000000000000000000000000..abd8d1ba72a6c3380ef5fd4685bc7e3556eced5f
--- /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 <px4_includes.h>
+#include <px4_getopt.h>
+#include <px4_tasks.h>
+#include <px4_log.h>
+#include <stdlib.h>
+#include <string.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/distance_sensor.h>
+#include <drivers/drv_hrt.h>
+#include <tof.h>
+#include <systemlib/param/param.h>
+
+//#define NO_ADIAG_LOG
+//#define NO_ADIAG_STATS
+#include <adiag.h>
+
+/** 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<float>(_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<TofModel>(_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;
+}