diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index e9187a8bc8..6a3a935f85 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -415,6 +415,27 @@ then fi fi +# LeddarOne +if param greater SENS_EN_LEDDAR1 0 +then + if ver hwcmp PX4FMU_V3 + then + # start the driver on serial 4/5 + leddar_one -d /dev/ttyS6 start + else + if ver hwcmp AEROFC_V1 + then + # start the driver on telemetry + leddar_one -d /dev/ttyS3 start + else + if param compare SYS_COMPANION 0 + then + # start on default mavlink companion device + leddar_one -d /dev/ttyS2 start + fi + fi +fi + # Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire) usleep 20000 sensors start diff --git a/src/drivers/distance_sensor/CMakeLists.txt b/src/drivers/distance_sensor/CMakeLists.txt index dd70156d42..11cc306f91 100644 --- a/src/drivers/distance_sensor/CMakeLists.txt +++ b/src/drivers/distance_sensor/CMakeLists.txt @@ -41,3 +41,4 @@ add_subdirectory(srf02_i2c) add_subdirectory(teraranger) add_subdirectory(tfmini) add_subdirectory(ulanding) +add_subdirectory(leddar_one) diff --git a/src/drivers/distance_sensor/leddar_one/CMakeLists.txt b/src/drivers/distance_sensor/leddar_one/CMakeLists.txt new file mode 100644 index 0000000000..16b4d0bcb7 --- /dev/null +++ b/src/drivers/distance_sensor/leddar_one/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2017 Intel Corporation. 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__leddar_one + MAIN leddar_one + STACK_MAIN 1200 + SRCS + leddar_one.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp new file mode 100644 index 0000000000..395fca19d9 --- /dev/null +++ b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp @@ -0,0 +1,568 @@ +/**************************************************************************** + * + * Copyright (C) 2017 Intel Corporation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include + +#include + +#define MIN_DISTANCE 0.01f +#define MAX_DISTANCE 40.0f + +#define NAME "leddar_one" +#define DEVICE_PATH "/dev/" NAME + +#define SENSOR_READING_FREQ 10.0f +#define READING_USEC_PERIOD (unsigned long)(1000000.0f / SENSOR_READING_FREQ) +#define OVERSAMPLE 6 +#define WORK_USEC_INTERVAL READING_USEC_PERIOD / OVERSAMPLE +#define COLLECT_USEC_TIMEOUT READING_USEC_PERIOD / (OVERSAMPLE / 2) + +/* 0.5sec */ +#define PROBE_USEC_TIMEOUT 500000 + +#define MODBUS_SLAVE_ADDRESS 0x01 +#define MODBUS_READING_FUNCTION 0x04 +#define READING_START_ADDR 0x14 +#define READING_LEN 0xA + +static const uint8_t request_reading_msg[] = { + MODBUS_SLAVE_ADDRESS, + MODBUS_READING_FUNCTION, + 0, /* starting addr high byte */ + READING_START_ADDR, + 0, /* number of bytes to read high byte */ + READING_LEN, + 0x30, /* CRC low */ + 0x09 /* CRC high */ +}; + +struct __attribute__((__packed__)) reading_msg { + uint8_t slave_addr; + uint8_t function; + uint8_t len; + uint8_t low_timestamp_high_byte; + uint8_t low_timestamp_low_byte; + uint8_t high_timestamp_high_byte; + uint8_t high_timestamp_low_byte; + uint8_t temp_high; + uint8_t temp_low; + uint8_t num_detections_high_byte; + uint8_t num_detections_low_byte; + uint8_t first_dist_high_byte; + uint8_t first_dist_low_byte; + uint8_t first_amplitude_high_byte; + uint8_t first_amplitude_low_byte; + uint8_t second_dist_high_byte; + uint8_t second_dist_low_byte; + uint8_t second_amplitude_high_byte; + uint8_t second_amplitude_low_byte; + uint8_t third_dist_high_byte; + uint8_t third_dist_low_byte; + uint8_t third_amplitude_high_byte; + uint8_t third_amplitude_low_byte; + uint16_t crc; /* little-endian */ +}; + +class leddar_one : public device::CDev +{ +public: + leddar_one(const char *device_path, const char *serial_port, uint8_t rotation); + virtual ~leddar_one(); + + virtual int init(); + void start(); + void stop(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + +private: + work_s _work; + + int _fd = -1; + const char *_serial_port; + + uint8_t _rotation; + + uint8_t _buffer[sizeof(struct reading_msg)]; + uint8_t _buffer_len = 0; + + orb_advert_t _topic = nullptr; + ringbuffer::RingBuffer *_reports = nullptr; + + enum { + state_waiting_reading = 0, + state_reading_requested + } _state = state_waiting_reading; + hrt_abstime _timeout_usec = 0; + + perf_counter_t _collect_timeout_perf; + perf_counter_t _comm_error; + perf_counter_t _sample_perf; + + int _fd_open(); + bool _request(); + int _collect(); + void _publish(uint16_t distance_cm); + int _cycle(); + + static void cycle_trampoline(void *arg); +}; + +extern "C" __EXPORT int leddar_one_main(int argc, char *argv[]); + +static void help() +{ + printf("missing command: try 'start', 'stop' or 'test'\n"); + printf("options:\n"); + printf(" -d to set the serial port were " NAME " is connected\n"); + printf(" -r rotation\n"); +} + +int leddar_one_main(int argc, char *argv[]) +{ + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + const char *serial_port = "/dev/ttyS3"; + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + static leddar_one *inst = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:r", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + serial_port = myoptarg; + break; + + case 'r': + rotation = (uint8_t)atoi(myoptarg); + break; + + default: + help(); + return PX4_ERROR; + } + } + + if (myoptind >= argc) { + help(); + return PX4_ERROR; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + inst = new leddar_one(DEVICE_PATH, serial_port, rotation); + + if (!inst) { + PX4_ERR("No memory to allocate " NAME); + return PX4_ERROR; + } + + if (inst->init() != PX4_OK) { + delete inst; + return PX4_ERROR; + } + + inst->start(); + + } else if (!strcmp(verb, "stop")) { + delete inst; + inst = nullptr; + + } else if (!strcmp(verb, "test")) { + int fd = open(DEVICE_PATH, O_RDONLY); + ssize_t sz; + struct distance_sensor_s report; + + if (fd < 0) { + PX4_ERR("Unable to open %s", DEVICE_PATH); + return PX4_ERROR; + } + + sz = read(fd, &report, sizeof(report)); + close(fd); + + if (sz != sizeof(report)) { + PX4_ERR("No sample available in %s", DEVICE_PATH); + return PX4_ERROR; + } + + bool valid = false; + + if (report.current_distance > report.min_distance + && report.current_distance < report.max_distance) { + valid = true; + } + + warnx("valid: %u\n", valid); + warnx("distance: %0.3fm\n", (double)report.current_distance); + warnx("time: %llu\n", report.timestamp); + + } else { + help(); + return PX4_ERROR; + } + + return PX4_OK; +} + +leddar_one::leddar_one(const char *device_path, const char *serial_port, uint8_t rotation): + CDev(NAME, device_path), + _rotation(rotation), + _collect_timeout_perf(perf_alloc(PC_COUNT, "leddar_one_collect_timeout")), + _comm_error(perf_alloc(PC_COUNT, "leddar_one_comm_errors")), + _sample_perf(perf_alloc(PC_ELAPSED, "leddar_one_sample")) +{ + memset(&_work, 0, sizeof(_work)); + _serial_port = strdup(serial_port); +} + +leddar_one::~leddar_one() +{ + stop(); + + free((char *)_serial_port); + + if (_fd > -1) { + ::close(_fd); + } + + if (_reports) { + delete _reports; + } + + if (_topic) { + orb_unadvertise(_topic); + } + + perf_free(_sample_perf); + perf_free(_collect_timeout_perf); + perf_free(_comm_error); +} + +int leddar_one::_fd_open() +{ + if (!_serial_port) { + return -1; + } + + _fd = ::open(_serial_port, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (_fd < 0) { + return -1; + } + + struct termios config; + + int r = tcgetattr(_fd, &config); + + if (r) { + PX4_ERR("Unable to get termios from %s.", _serial_port); + goto error; + } + + /* clear: data bit size, two stop bits, parity, hardware flow control */ + config.c_cflag &= ~(CSIZE | CSTOPB | PARENB | CRTSCTS); + /* set: 8 data bits, enable receiver, ignore modem status lines */ + config.c_cflag |= (CS8 | CREAD | CLOCAL); + /* turn off output processing */ + config.c_oflag = 0; + /* clear: echo, echo new line, canonical input and extended input */ + config.c_lflag &= (ECHO | ECHONL | ICANON | IEXTEN); + + r = cfsetispeed(&config, B115200); + r |= cfsetospeed(&config, B115200); + + if (r) { + PX4_ERR("Unable to set baudrate"); + goto error; + } + + r = tcsetattr(_fd, TCSANOW, &config); + + if (r) { + PX4_ERR("Unable to set termios to %s", _serial_port); + goto error; + } + + return PX4_OK; + +error: + ::close(_fd); + _fd = -1; + return PX4_ERROR; +} + +int leddar_one::init() +{ + if (CDev::init()) { + PX4_ERR("Unable to initialize device\n"); + return -1; + } + + _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); + + if (!_reports) { + PX4_ERR("No memory to allocate RingBuffer"); + return -1; + } + + if (_fd_open()) { + return PX4_ERROR; + } + + hrt_abstime timeout_usec, now; + + for (now = hrt_absolute_time(), timeout_usec = now + PROBE_USEC_TIMEOUT; + now < timeout_usec; + now = hrt_absolute_time()) { + + if (_cycle() > 0) { + printf(NAME " found\n"); + return PX4_OK; + } + + usleep(1000); + } + + PX4_ERR("No readings from " NAME); + + return PX4_ERROR; +} + +void +leddar_one::stop() +{ + work_cancel(HPWORK, &_work); +} + +void leddar_one::start() +{ + /* + * file descriptor can only be accessed by the process that opened it + * so closing here and it will be opened from the High priority kernel + * process + */ + ::close(_fd); + _fd = -1; + + work_queue(HPWORK, &_work, (worker_t)&leddar_one::cycle_trampoline, this, USEC2TICK(WORK_USEC_INTERVAL)); +} + +void +leddar_one::cycle_trampoline(void *arg) +{ + leddar_one *dev = reinterpret_cast(arg); + + if (dev->_fd != -1) { + dev->_cycle(); + + } else { + dev->_fd_open(); + } + + work_queue(HPWORK, &(dev->_work), (worker_t)&leddar_one::cycle_trampoline, + dev, USEC2TICK(WORK_USEC_INTERVAL)); +} + +void leddar_one::_publish(uint16_t distance_mm) +{ + struct distance_sensor_s report; + + report.timestamp = hrt_absolute_time(); + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + report.orientation = _rotation; + report.current_distance = ((float)distance_mm / 1000.0f); + report.min_distance = MIN_DISTANCE; + report.max_distance = MAX_DISTANCE; + report.covariance = 0.0f; + report.id = 0; + + _reports->force(&report); + + if (_topic == nullptr) { + _topic = orb_advertise(ORB_ID(distance_sensor), &report); + + } else { + orb_publish(ORB_ID(distance_sensor), _topic, &report); + } +} + +bool leddar_one::_request() +{ + /* flush anything in RX buffer */ + tcflush(_fd, TCIFLUSH); + + int r = ::write(_fd, request_reading_msg, sizeof(request_reading_msg)); + return r == sizeof(request_reading_msg); +} + +static uint16_t _calc_crc16(const uint8_t *buffer, uint8_t len) +{ + uint16_t crc = 0xFFFF; + + for (uint8_t i = 0; i < len; i++) { + crc ^= buffer[i]; + + for (uint8_t j = 0; j < 8; j++) { + if (crc & 1) { + crc = (crc >> 1) ^ 0xA001; + + } else { + crc >>= 1; + } + } + } + + return crc; +} + +/* + * returns 0 when still waiting for reading_msg, 1 when frame was read or + * -1 in case of error + */ +int leddar_one::_collect() +{ + struct reading_msg *msg; + + int r = ::read(_fd, _buffer + _buffer_len, sizeof(_buffer) - _buffer_len); + + if (r < 1) { + return 0; + } + + _buffer_len += r; + + if (_buffer_len < sizeof(struct reading_msg)) { + return 0; + } + + msg = (struct reading_msg *)_buffer; + + if (msg->slave_addr != MODBUS_SLAVE_ADDRESS || msg->function != MODBUS_READING_FUNCTION) { + return -1; + } + + const uint16_t crc16_calc = _calc_crc16(_buffer, _buffer_len - 2); + + if (crc16_calc != msg->crc) { + return -1; + } + + /* NOTE: little-endian support only */ + _publish(msg->first_dist_high_byte << 8 | msg->first_dist_low_byte); + return 1; +} + +int leddar_one::_cycle() +{ + int ret = 0; + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case state_waiting_reading: + if (now > _timeout_usec) { + if (_request()) { + perf_begin(_sample_perf); + _buffer_len = 0; + _state = state_reading_requested; + _timeout_usec = now + COLLECT_USEC_TIMEOUT; + } + } + + break; + + case state_reading_requested: + ret = _collect(); + + if (ret == 1) { + perf_end(_sample_perf); + _state = state_waiting_reading; + _timeout_usec = now + READING_USEC_PERIOD; + + } else { + if (ret == 0 && now < _timeout_usec) { + /* still waiting for reading */ + break; + } + + if (ret == -1) { + perf_count(_comm_error); + + } else { + perf_count(_collect_timeout_perf); + } + + perf_cancel(_sample_perf); + _state = state_waiting_reading; + _timeout_usec = 0; + return _cycle(); + } + } + + return ret; +} + +ssize_t leddar_one::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); + int ret = 0; + + if (count < 1) { + return -ENOSPC; + } + + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + return ret ? ret : -EAGAIN; +} diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index d53a79265b..27e725b64f 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -334,3 +334,13 @@ PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f); * @group Sensors */ PARAM_DEFINE_INT32(SENS_EN_TFMINI, 0); + +/** + * LeddarOne rangefinder + * + * @reboot_required true + * + * @boolean + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_EN_LEDDAR1, 0);