From b084558a793137e1c57d6ecd90d68303f5b128a2 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 24 Mar 2015 10:04:40 +0100 Subject: [PATCH 01/51] LidarLite: Code cleanup, prepare for PWM version of the driver --- src/drivers/ll40ls/LidarLiteI2C.cpp | 653 ++++++++++++++++++++++ src/drivers/ll40ls/LidarLiteI2C.h | 142 +++++ src/drivers/ll40ls/ll40ls.cpp | 805 +--------------------------- src/drivers/ll40ls/module.mk | 3 +- 4 files changed, 813 insertions(+), 790 deletions(-) create mode 100644 src/drivers/ll40ls/LidarLiteI2C.cpp create mode 100644 src/drivers/ll40ls/LidarLiteI2C.h diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp new file mode 100644 index 0000000000..ce674293e8 --- /dev/null +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -0,0 +1,653 @@ +/**************************************************************************** + * + * 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 LidarLiteI2C.cpp + * @author Allyson Kreft + * + * Driver for the PulsedLight Lidar-Lite range finders connected via I2C. + */ + +#include +#include "LidarLiteI2C.h" +#include +#include +#include +#include +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : + I2C("LL40LS", path, bus, address, 100000), + _min_distance(LL40LS_MIN_DISTANCE), + _max_distance(LL40LS_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _class_instance(-1), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), + _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), + _sensor_resets(perf_alloc(PC_COUNT, "ll40ls_resets")), + _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_zero_resets")), + _last_distance(0), + _zero_counter(0), + _pause_measurements(false), + _bus(bus) +{ + // up the retries since the device misses the first measure attempts + _retries = 3; + + // enable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +LidarLiteI2C::~LidarLiteI2C() +{ + /* 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); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + perf_free(_sensor_resets); + perf_free(_sensor_zero_resets); +} + +int LidarLiteI2C::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + goto out; + } + + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val) +{ + return transfer(®, 1, &val, 1); +} + +int LidarLiteI2C::probe() +{ + // cope with both old and new I2C bus address + const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; + + // more retries for detection + _retries = 10; + + for (uint8_t i=0; i 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + reset_sensor(); + return OK; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(LL40LS_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int LidarLiteI2C::measure() +{ + int ret; + + if (_pause_measurements) { + // we are in print_registers() and need to avoid + // acquisition to keep the I2C peripheral on the + // sensor active + return OK; + } + + /* + * Send the command to begin a measurement. + */ + const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; + ret = transfer(cmd, sizeof(cmd), nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + debug("i2c::transfer returned %d", ret); + // if we are getting lots of I2C transfer errors try + // resetting the sensor + if (perf_event_count(_comms_errors) % 10 == 0) { + perf_count(_sensor_resets); + reset_sensor(); + } + return ret; + } + + // remember when we sent the acquire so we can know when the + // acquisition has timed out + _acquire_time_usec = hrt_absolute_time(); + ret = OK; + + return ret; +} + +/* + reset the sensor to power on defaults + */ +int LidarLiteI2C::reset_sensor() +{ + const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET }; + int ret = transfer(cmd, sizeof(cmd), nullptr, 0); + return ret; +} + +/* + dump sensor registers for debugging + */ +void LidarLiteI2C::print_registers() +{ + _pause_measurements = true; + printf("ll40ls registers\n"); + // wait for a while to ensure the lidar is in a ready state + usleep(50000); + for (uint8_t reg=0; reg<=0x67; reg++) { + uint8_t val = 0; + int ret = transfer(®, 1, &val, 1); + if (ret != OK) { + printf("%02x:XX ",(unsigned)reg); + } else { + printf("%02x:%02x ",(unsigned)reg, (unsigned)val); + } + if (reg % 16 == 15) { + printf("\n"); + } + } + printf("\n"); + _pause_measurements = false; +} + +int LidarLiteI2C::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + // read the high and low byte distance registers + uint8_t distance_reg = LL40LS_DISTHIGH_REG; + ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); + + if (ret < 0) { + if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { + /* + NACKs from the sensor are expected when we + read before it is ready, so only consider it + an error if more than 100ms has elapsed. + */ + debug("error reading from sensor: %d", ret); + perf_count(_comms_errors); + if (perf_event_count(_comms_errors) % 10 == 0) { + perf_count(_sensor_resets); + reset_sensor(); + } + } + perf_end(_sample_perf); + // if we are getting lots of I2C transfer errors try + // resetting the sensor + return ret; + } + + uint16_t distance = (val[0] << 8) | val[1]; + float si_units = distance * 0.01f; /* cm to m */ + struct range_finder_report report; + + if (distance == 0) { + _zero_counter++; + if (_zero_counter == 20) { + /* we have had 20 zeros in a row - reset the + sensor. This is a known bad state of the + sensor where it returns 16 bits of zero for + the distance with a trailing NACK, and + keeps doing that even when the target comes + into a valid range. + */ + _zero_counter = 0; + perf_end(_sample_perf); + perf_count(_sensor_zero_resets); + return reset_sensor(); + } + } else { + _zero_counter = 0; + } + + _last_distance = distance; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); + if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { + report.valid = 1; + } + else { + report.valid = 0; + } + + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void LidarLiteI2C::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&LidarLiteI2C::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void LidarLiteI2C::stop() +{ + work_cancel(HPWORK, &_work); +} + +void LidarLiteI2C::cycle_trampoline(void *arg) +{ + LidarLiteI2C *dev = (LidarLiteI2C *)arg; + + dev->cycle(); +} + +void LidarLiteI2C::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* try a collection */ + if (OK != collect()) { + debug("collection error"); + /* if we've been waiting more than 200ms then + send a new acquire */ + if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT*2) { + _collect_phase = false; + } + } else { + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&LidarLiteI2C::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + + return; + } + } + } + + if (_collect_phase == false) { + /* measurement phase */ + if (OK != measure()) { + debug("measure error"); + } else { + /* next phase is collection. Don't switch to + collection phase until we have a successful + acquire request I2C transfer */ + _collect_phase = true; + } + } + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&LidarLiteI2C::cycle_trampoline, + this, + USEC2TICK(LL40LS_CONVERSION_INTERVAL)); +} + +void LidarLiteI2C::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + perf_print_counter(_sensor_resets); + perf_print_counter(_sensor_zero_resets); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); + printf("distance: %ucm (0x%04x)\n", + (unsigned)_last_distance, (unsigned)_last_distance); +} diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h new file mode 100644 index 0000000000..6dec1a8825 --- /dev/null +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -0,0 +1,142 @@ +#pragma once + +//Forward declaration +class RingBuffer; + +#include +#include +#include + +#include +#include + +#include +#include + +/* Configuration Constants */ +#define LL40LS_BUS PX4_I2C_BUS_EXPANSION +#define LL40LS_BASEADDR 0x62 /* 7-bit address */ +#define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */ +#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" +#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" + +/* LL40LS Registers addresses */ + +#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ +#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */ +#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ +#define LL40LS_MAX_ACQ_COUNT_REG 0x02 /* maximum acquisition count register */ +#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ +#define LL40LS_WHO_AM_I_REG 0x11 +#define LL40LS_WHO_AM_I_REG_VAL 0xCA +#define LL40LS_SIGNAL_STRENGTH_REG 0x5b + +/* Device limits */ +#define LL40LS_MIN_DISTANCE (0.00f) +#define LL40LS_MAX_DISTANCE (60.00f) + +// normal conversion wait time +#define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */ + +// maximum time to wait for a conversion to complete. +#define LL40LS_CONVERSION_TIMEOUT 100*1000UL /* 100ms */ + +class LidarLiteI2C : public device::I2C +{ +public: + LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR); + virtual ~LidarLiteI2C(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + /** + * print registers to console + */ + void print_registers(); + +protected: + virtual int probe(); + virtual int read_reg(uint8_t reg, uint8_t &val); + +private: + float _min_distance; + float _max_distance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + unsigned _measure_ticks; + bool _collect_phase; + int _class_instance; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + perf_counter_t _sensor_resets; + perf_counter_t _sensor_zero_resets; + uint16_t _last_distance; + uint16_t _zero_counter; + uint64_t _acquire_time_usec; + volatile bool _pause_measurements; + + /**< the bus the device is connected to */ + int _bus; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * 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(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE + * and LL40LS_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + int reset_sensor(); + + /** + * 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); +}; diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 8e2caf8a04..b9c453bfc9 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -34,800 +34,28 @@ /** * @file ll40ls.cpp * @author Allyson Kreft + * @author Johan Jansen * - * Driver for the PulsedLight Lidar-Lite range finders connected via I2C. + * Interface for the PulsedLight Lidar-Lite range finders. */ -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include - +#include "LidarLiteI2C.h" #include - -/* Configuration Constants */ -#define LL40LS_BUS PX4_I2C_BUS_EXPANSION -#define LL40LS_BASEADDR 0x62 /* 7-bit address */ -#define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */ -#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" -#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" - -/* LL40LS Registers addresses */ - -#define LL40LS_MEASURE_REG 0x00 /* Measure range register */ -#define LL40LS_MSRREG_RESET 0x00 /* reset to power on defaults */ -#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */ -#define LL40LS_MAX_ACQ_COUNT_REG 0x02 /* maximum acquisition count register */ -#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */ -#define LL40LS_WHO_AM_I_REG 0x11 -#define LL40LS_WHO_AM_I_REG_VAL 0xCA -#define LL40LS_SIGNAL_STRENGTH_REG 0x5b - -/* Device limits */ -#define LL40LS_MIN_DISTANCE (0.00f) -#define LL40LS_MAX_DISTANCE (60.00f) - -// normal conversion wait time -#define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */ - -// maximum time to wait for a conversion to complete. -#define LL40LS_CONVERSION_TIMEOUT 100*1000UL /* 100ms */ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +#include +#include +#include +#include +#include #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class LL40LS : public device::I2C -{ -public: - LL40LS(int bus, const char *path, int address = LL40LS_BASEADDR); - virtual ~LL40LS(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - - /** - * print registers to console - */ - void print_registers(); - -protected: - virtual int probe(); - virtual int read_reg(uint8_t reg, uint8_t &val); - -private: - float _min_distance; - float _max_distance; - work_s _work; - RingBuffer *_reports; - bool _sensor_ok; - unsigned _measure_ticks; - bool _collect_phase; - int _class_instance; - - orb_advert_t _range_finder_topic; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - perf_counter_t _sensor_resets; - perf_counter_t _sensor_zero_resets; - uint16_t _last_distance; - uint16_t _zero_counter; - uint64_t _acquire_time_usec; - volatile bool _pause_measurements; - - /**< the bus the device is connected to */ - int _bus; - - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - - /** - * 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(); - - /** - * Set the min and max distance thresholds if you want the end points of the sensors - * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE - * and LL40LS_MAX_DISTANCE - */ - void set_minimum_distance(float min); - void set_maximum_distance(float max); - float get_minimum_distance(); - float get_maximum_distance(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void cycle(); - int measure(); - int collect(); - int reset_sensor(); - /** - * 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 ll40ls_main(int argc, char *argv[]); -LL40LS::LL40LS(int bus, const char *path, int address) : - I2C("LL40LS", path, bus, address, 100000), - _min_distance(LL40LS_MIN_DISTANCE), - _max_distance(LL40LS_MAX_DISTANCE), - _reports(nullptr), - _sensor_ok(false), - _measure_ticks(0), - _collect_phase(false), - _class_instance(-1), - _range_finder_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), - _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), - _sensor_resets(perf_alloc(PC_COUNT, "ll40ls_resets")), - _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_zero_resets")), - _last_distance(0), - _zero_counter(0), - _pause_measurements(false), - _bus(bus) -{ - // up the retries since the device misses the first measure attempts - _retries = 3; - - // enable debug() calls - _debug_enabled = false; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); -} - -LL40LS::~LL40LS() -{ - /* 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); - } - - // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); - perf_free(_buffer_overflows); - perf_free(_sensor_resets); - perf_free(_sensor_zero_resets); -} - -int -LL40LS::init() -{ - int ret = ERROR; - - /* do I2C init (and probe) first */ - if (I2C::init() != OK) { - goto out; - } - - /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); - - if (_reports == nullptr) { - goto out; - } - - _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; - measure(); - _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); - - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); - } - } - - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; -out: - return ret; -} - -int -LL40LS::read_reg(uint8_t reg, uint8_t &val) -{ - return transfer(®, 1, &val, 1); -} - -int -LL40LS::probe() -{ - // cope with both old and new I2C bus address - const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; - - // more retries for detection - _retries = 10; - - for (uint8_t i=0; i 100)) { - return -EINVAL; - } - - irqstate_t flags = irqsave(); - - if (!_reports->resize(arg)) { - irqrestore(flags); - return -ENOMEM; - } - - irqrestore(flags); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _reports->size(); - - case SENSORIOCRESET: - reset_sensor(); - return OK; - - case RANGEFINDERIOCSETMINIUMDISTANCE: { - set_minimum_distance(*(float *)arg); - return 0; - } - break; - - case RANGEFINDERIOCSETMAXIUMDISTANCE: { - set_maximum_distance(*(float *)arg); - return 0; - } - break; - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} - -ssize_t -LL40LS::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(rbuf)) { - ret += sizeof(*rbuf); - rbuf++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - do { - _reports->flush(); - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - usleep(LL40LS_CONVERSION_INTERVAL); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - if (_reports->get(rbuf)) { - ret = sizeof(*rbuf); - } - - } while (0); - - return ret; -} - -int -LL40LS::measure() -{ - int ret; - - if (_pause_measurements) { - // we are in print_registers() and need to avoid - // acquisition to keep the I2C peripheral on the - // sensor active - return OK; - } - - /* - * Send the command to begin a measurement. - */ - const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; - ret = transfer(cmd, sizeof(cmd), nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - debug("i2c::transfer returned %d", ret); - // if we are getting lots of I2C transfer errors try - // resetting the sensor - if (perf_event_count(_comms_errors) % 10 == 0) { - perf_count(_sensor_resets); - reset_sensor(); - } - return ret; - } - - // remember when we sent the acquire so we can know when the - // acquisition has timed out - _acquire_time_usec = hrt_absolute_time(); - ret = OK; - - return ret; -} - -/* - reset the sensor to power on defaults - */ -int -LL40LS::reset_sensor() -{ - const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET }; - int ret = transfer(cmd, sizeof(cmd), nullptr, 0); - return ret; -} - -/* - dump sensor registers for debugging - */ -void -LL40LS::print_registers() -{ - _pause_measurements = true; - printf("ll40ls registers\n"); - // wait for a while to ensure the lidar is in a ready state - usleep(50000); - for (uint8_t reg=0; reg<=0x67; reg++) { - uint8_t val = 0; - int ret = transfer(®, 1, &val, 1); - if (ret != OK) { - printf("%02x:XX ",(unsigned)reg); - } else { - printf("%02x:%02x ",(unsigned)reg, (unsigned)val); - } - if (reg % 16 == 15) { - printf("\n"); - } - } - printf("\n"); - _pause_measurements = false; -} - -int -LL40LS::collect() -{ - int ret = -EIO; - - /* read from the sensor */ - uint8_t val[2] = {0, 0}; - - perf_begin(_sample_perf); - - // read the high and low byte distance registers - uint8_t distance_reg = LL40LS_DISTHIGH_REG; - ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); - - if (ret < 0) { - if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { - /* - NACKs from the sensor are expected when we - read before it is ready, so only consider it - an error if more than 100ms has elapsed. - */ - debug("error reading from sensor: %d", ret); - perf_count(_comms_errors); - if (perf_event_count(_comms_errors) % 10 == 0) { - perf_count(_sensor_resets); - reset_sensor(); - } - } - perf_end(_sample_perf); - // if we are getting lots of I2C transfer errors try - // resetting the sensor - return ret; - } - - uint16_t distance = (val[0] << 8) | val[1]; - float si_units = distance * 0.01f; /* cm to m */ - struct range_finder_report report; - - if (distance == 0) { - _zero_counter++; - if (_zero_counter == 20) { - /* we have had 20 zeros in a row - reset the - sensor. This is a known bad state of the - sensor where it returns 16 bits of zero for - the distance with a trailing NACK, and - keeps doing that even when the target comes - into a valid range. - */ - _zero_counter = 0; - perf_end(_sample_perf); - perf_count(_sensor_zero_resets); - return reset_sensor(); - } - } else { - _zero_counter = 0; - } - - _last_distance = distance; - - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { - report.valid = 1; - } - else { - report.valid = 0; - } - - /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); - } - - if (_reports->force(&report)) { - perf_count(_buffer_overflows); - } - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - ret = OK; - - perf_end(_sample_perf); - return ret; -} - -void -LL40LS::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - _reports->flush(); - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&LL40LS::cycle_trampoline, this, 1); - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_RANGEFINDER - }; - static orb_advert_t pub = -1; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } -} - -void -LL40LS::stop() -{ - work_cancel(HPWORK, &_work); -} - -void -LL40LS::cycle_trampoline(void *arg) -{ - LL40LS *dev = (LL40LS *)arg; - - dev->cycle(); -} - -void -LL40LS::cycle() -{ - /* collection phase? */ - if (_collect_phase) { - - /* try a collection */ - if (OK != collect()) { - debug("collection error"); - /* if we've been waiting more than 200ms then - send a new acquire */ - if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT*2) { - _collect_phase = false; - } - } else { - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&LL40LS::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); - - return; - } - } - } - - if (_collect_phase == false) { - /* measurement phase */ - if (OK != measure()) { - debug("measure error"); - } else { - /* next phase is collection. Don't switch to - collection phase until we have a successful - acquire request I2C transfer */ - _collect_phase = true; - } - } - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&LL40LS::cycle_trampoline, - this, - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); -} - -void -LL40LS::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - perf_print_counter(_sensor_resets); - perf_print_counter(_sensor_zero_resets); - printf("poll interval: %u ticks\n", _measure_ticks); - _reports->print_info("report queue"); - printf("distance: %ucm (0x%04x)\n", - (unsigned)_last_distance, (unsigned)_last_distance); -} /** * Local functions in support of the shell command. @@ -841,8 +69,8 @@ namespace ll40ls #endif const int ERROR = -1; -LL40LS *g_dev_int; -LL40LS *g_dev_ext; +LidarLiteI2C *g_dev_int; +LidarLiteI2C *g_dev_ext; void start(int bus); void stop(int bus); @@ -855,14 +83,13 @@ void usage(); /** * Start the driver. */ -void -start(int bus) +void start(int bus) { /* create the driver, attempt expansion bus first */ if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { if (g_dev_ext != nullptr) errx(0, "already started external"); - g_dev_ext = new LL40LS(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); + g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { delete g_dev_ext; g_dev_ext = nullptr; @@ -877,7 +104,7 @@ start(int bus) if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { if (g_dev_int != nullptr) errx(0, "already started internal"); - g_dev_int = new LL40LS(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); + g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); if (g_dev_int != nullptr && OK != g_dev_int->init()) { /* tear down the failing onboard instance */ delete g_dev_int; @@ -938,7 +165,7 @@ fail: */ void stop(int bus) { - LL40LS **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext); + LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext); if (*g_dev != nullptr) { delete *g_dev; *g_dev = nullptr; @@ -1048,7 +275,7 @@ reset(int bus) void info(int bus) { - LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); + LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); if (g_dev == nullptr) { errx(1, "driver not running"); } @@ -1065,7 +292,7 @@ info(int bus) void regdump(int bus) { - LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); + LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); if (g_dev == nullptr) { errx(1, "driver not running"); } diff --git a/src/drivers/ll40ls/module.mk b/src/drivers/ll40ls/module.mk index fb627afeea..d9a43940c1 100644 --- a/src/drivers/ll40ls/module.mk +++ b/src/drivers/ll40ls/module.mk @@ -37,6 +37,7 @@ MODULE_COMMAND = ll40ls -SRCS = ll40ls.cpp +SRCS = ll40ls.cpp \ + LidarLiteI2C.cpp MAXOPTIMIZATION = -Os From eeb8562c6e5dadc36e23b114dfe032635d258e6c Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 24 Mar 2015 10:10:11 +0100 Subject: [PATCH 02/51] LidarLite: Compile with weffc++ and fix all warnings --- src/drivers/ll40ls/LidarLiteI2C.cpp | 2 ++ src/drivers/ll40ls/LidarLiteI2C.h | 45 +++++++++++++++++++++++++++++ src/drivers/ll40ls/module.mk | 4 +-- 3 files changed, 49 insertions(+), 2 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index ce674293e8..244135a4a7 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -60,6 +60,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : I2C("LL40LS", path, bus, address, 100000), _min_distance(LL40LS_MIN_DISTANCE), _max_distance(LL40LS_MAX_DISTANCE), + _work(), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), @@ -73,6 +74,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_zero_resets")), _last_distance(0), _zero_counter(0), + _acquire_time_usec(0), _pause_measurements(false), _bus(bus) { diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index 6dec1a8825..c0e9d4ce4a 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -1,3 +1,44 @@ +/**************************************************************************** + * + * 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 LidarLiteI2C.h + * @author Allyson Kreft + * + * Driver for the PulsedLight Lidar-Lite range finders connected via I2C. + */ + #pragma once //Forward declaration @@ -139,4 +180,8 @@ private: * @param arg Instance pointer for the driver that is polling. */ static void cycle_trampoline(void *arg); + +private: + LidarLiteI2C(const LidarLiteI2C ©) = delete; + LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete; }; diff --git a/src/drivers/ll40ls/module.mk b/src/drivers/ll40ls/module.mk index d9a43940c1..a3a0ead55a 100644 --- a/src/drivers/ll40ls/module.mk +++ b/src/drivers/ll40ls/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-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 @@ -40,4 +40,4 @@ MODULE_COMMAND = ll40ls SRCS = ll40ls.cpp \ LidarLiteI2C.cpp -MAXOPTIMIZATION = -Os +MAXOPTIMIZATION = -Os -Weffc++ From b1da12b43fd62ecce1583dad183a90a7a84930cb Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 24 Mar 2015 11:31:11 +0100 Subject: [PATCH 03/51] LidarLite: Added PWM version of the LidarLite driver --- src/drivers/ll40ls/LidarLite.cpp | 172 ++++++++++++++++++++++++++++ src/drivers/ll40ls/LidarLite.h | 103 +++++++++++++++++ src/drivers/ll40ls/LidarLiteI2C.cpp | 152 +++++------------------- src/drivers/ll40ls/LidarLiteI2C.h | 42 ++----- src/drivers/ll40ls/LidarLitePWM.cpp | 75 ++++++++++++ src/drivers/ll40ls/LidarLitePWM.h | 69 +++++++++++ src/drivers/ll40ls/ll40ls.cpp | 3 + src/drivers/ll40ls/module.mk | 4 +- 8 files changed, 460 insertions(+), 160 deletions(-) create mode 100644 src/drivers/ll40ls/LidarLite.cpp create mode 100644 src/drivers/ll40ls/LidarLite.h create mode 100644 src/drivers/ll40ls/LidarLitePWM.cpp create mode 100644 src/drivers/ll40ls/LidarLitePWM.h diff --git a/src/drivers/ll40ls/LidarLite.cpp b/src/drivers/ll40ls/LidarLite.cpp new file mode 100644 index 0000000000..33dce6192c --- /dev/null +++ b/src/drivers/ll40ls/LidarLite.cpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * 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 LidarLite.h + * @author Johan Jansen + * + * Generic interface driver for the PulsedLight Lidar-Lite range finders. + */ +#include "LidarLite.h" +#include +#include + +LidarLite::LidarLite() : + _min_distance(LL40LS_MIN_DISTANCE), + _max_distance(LL40LS_MAX_DISTANCE), + _measure_ticks(0) +{ + //ctor +} + +LidarLite::~LidarLite() +{ + //dtor +} + +void LidarLite::set_minimum_distance(const float min) +{ + _min_distance = min; +} + +void LidarLite::set_maximum_distance(const float max) +{ + _max_distance = max; +} + +float LidarLite::get_minimum_distance() const +{ + return _min_distance; +} + +float LidarLite::get_maximum_distance() const +{ + return _max_distance; +} + +uint32_t LidarLite::getMeasureTicks() const +{ + return _measure_ticks; +} + +int LidarLite::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + 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(LL40LS_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 */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(LL40LS_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; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCRESET: + reset_sensor(); + return OK; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return OK; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return OK; + } + break; + + default: + return -EINVAL; + } +} diff --git a/src/drivers/ll40ls/LidarLite.h b/src/drivers/ll40ls/LidarLite.h new file mode 100644 index 0000000000..fa2e0e3cde --- /dev/null +++ b/src/drivers/ll40ls/LidarLite.h @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 LidarLite.h + * @author Johan Jansen + * + * Generic interface driver for the PulsedLight Lidar-Lite range finders. + */ + #pragma once + +#include +#include + +/* Device limits */ +#define LL40LS_MIN_DISTANCE (0.00f) +#define LL40LS_MAX_DISTANCE (60.00f) + +// normal conversion wait time +#define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */ + +// maximum time to wait for a conversion to complete. +#define LL40LS_CONVERSION_TIMEOUT 100*1000UL /* 100ms */ + +class LidarLite +{ +public: + LidarLite(); + + virtual ~LidarLite(); + + virtual int init() = 0; + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + virtual void start() = 0; + + virtual void stop() = 0; + + /** + * @brief + * Diagnostics - print some basic information about the driver. + */ + virtual void print_info() = 0; + + /** + * @brief + * print registers to console + */ + virtual void print_registers() = 0; + +private: + virtual int reset_sensor() = 0; + +protected: + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE + * and LL40LS_MAX_DISTANCE + */ + void set_minimum_distance(const float min); + void set_maximum_distance(const float max); + float get_minimum_distance() const; + float get_maximum_distance() const; + + uint32_t getMeasureTicks() const; + +private: + float _min_distance; + float _max_distance; + uint32_t _measure_ticks; +}; diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 244135a4a7..d7f5e86294 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -39,7 +39,6 @@ * Driver for the PulsedLight Lidar-Lite range finders connected via I2C. */ -#include #include "LidarLiteI2C.h" #include #include @@ -58,12 +57,9 @@ static const int ERROR = -1; LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : I2C("LL40LS", path, bus, address, 100000), - _min_distance(LL40LS_MIN_DISTANCE), - _max_distance(LL40LS_MAX_DISTANCE), _work(), _reports(nullptr), _sensor_ok(false), - _measure_ticks(0), _collect_phase(false), _class_instance(-1), _range_finder_topic(-1), @@ -196,136 +192,40 @@ ok: return reset_sensor(); } -void LidarLiteI2C::set_minimum_distance(float min) -{ - _min_distance = min; -} - -void LidarLiteI2C::set_maximum_distance(float max) -{ - _max_distance = max; -} - -float LidarLiteI2C::get_minimum_distance() -{ - return _min_distance; -} - -float LidarLiteI2C::get_maximum_distance() -{ - return _max_distance; -} - int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg) { - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; - - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - 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(LL40LS_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; + switch(arg) { + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; } - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + irqstate_t flags = irqsave(); - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(LL40LS_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; + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } - } - } - case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) { - return SENSOR_POLLRATE_MANUAL; - } - - return (1000 / _measure_ticks); - - case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { - return -EINVAL; - } - - irqstate_t flags = irqsave(); - - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; + + return OK; } - irqrestore(flags); + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); - return OK; + default: + { + int result = LidarLite::ioctl(filp, cmd, arg); + + if(result == -EINVAL) { + result = I2C::ioctl(filp, cmd, arg); + } + + return result; } - - case SENSORIOCGQUEUEDEPTH: - return _reports->size(); - - case SENSORIOCRESET: - reset_sensor(); - return OK; - - case RANGEFINDERIOCSETMINIUMDISTANCE: { - set_minimum_distance(*(float *)arg); - return 0; - } - break; - - case RANGEFINDERIOCSETMAXIUMDISTANCE: { - set_maximum_distance(*(float *)arg); - return 0; - } - break; - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); } } @@ -341,7 +241,7 @@ ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen) } /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { + if (getMeasureTicks() > 0) { /* * While there is space in the caller's buffer, and reports, copy them. @@ -607,14 +507,14 @@ void LidarLiteI2C::cycle() /* * Is there a collect->measure gap? */ - if (_measure_ticks > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, (worker_t)&LidarLiteI2C::cycle_trampoline, this, - _measure_ticks - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); return; } @@ -648,7 +548,7 @@ void LidarLiteI2C::print_info() perf_print_counter(_buffer_overflows); perf_print_counter(_sensor_resets); perf_print_counter(_sensor_zero_resets); - printf("poll interval: %u ticks\n", _measure_ticks); + printf("poll interval: %u ticks\n", getMeasureTicks()); _reports->print_info("report queue"); printf("distance: %ucm (0x%04x)\n", (unsigned)_last_distance, (unsigned)_last_distance); diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index c0e9d4ce4a..d96c0d8918 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -31,7 +31,6 @@ * ****************************************************************************/ - /** * @file LidarLiteI2C.h * @author Allyson Kreft @@ -44,12 +43,13 @@ //Forward declaration class RingBuffer; +#include "LidarLite.h" + #include #include #include #include -#include #include #include @@ -58,8 +58,6 @@ class RingBuffer; #define LL40LS_BUS PX4_I2C_BUS_EXPANSION #define LL40LS_BASEADDR 0x62 /* 7-bit address */ #define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */ -#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" -#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" /* LL40LS Registers addresses */ @@ -72,48 +70,35 @@ class RingBuffer; #define LL40LS_WHO_AM_I_REG_VAL 0xCA #define LL40LS_SIGNAL_STRENGTH_REG 0x5b -/* Device limits */ -#define LL40LS_MIN_DISTANCE (0.00f) -#define LL40LS_MAX_DISTANCE (60.00f) - -// normal conversion wait time -#define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */ - -// maximum time to wait for a conversion to complete. -#define LL40LS_CONVERSION_TIMEOUT 100*1000UL /* 100ms */ - -class LidarLiteI2C : public device::I2C +class LidarLiteI2C : public LidarLite, public device::I2C { public: LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR); virtual ~LidarLiteI2C(); - virtual int init(); + virtual int init() override; virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg) override; /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_info() override; /** * print registers to console */ - void print_registers(); + void print_registers() override; protected: virtual int probe(); virtual int read_reg(uint8_t reg, uint8_t &val); private: - float _min_distance; - float _max_distance; work_s _work; RingBuffer *_reports; bool _sensor_ok; - unsigned _measure_ticks; bool _collect_phase; int _class_instance; @@ -154,16 +139,6 @@ private: */ void stop(); - /** - * Set the min and max distance thresholds if you want the end points of the sensors - * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE - * and LL40LS_MAX_DISTANCE - */ - void set_minimum_distance(float min); - void set_maximum_distance(float max); - float get_minimum_distance(); - float get_maximum_distance(); - /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -171,7 +146,8 @@ private: void cycle(); int measure(); int collect(); - int reset_sensor(); + + int reset_sensor() override; /** * Static trampoline from the workq context; because we don't have a diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp new file mode 100644 index 0000000000..f225c0768f --- /dev/null +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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 LidarLitePWM.h + * @author Johan Jansen + * + * Driver for the PulsedLight Lidar-Lite range finders connected via PWM. + */ +#include "LidarLitePWM.h" +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef __cplusplus +static const int ERROR = -1; +#endif + +LidarLitePWM::LidarLitePWM() : + _pwmSub(-1), + _pwm{} +{ + +} + +int LidarLitePWM::init() +{ + _pwmSub = orb_subscribe(ORB_ID(pwm_input)); + + if(_pwmSub == -1) { + return ERROR; + } + + return OK; +} + +void LidarLitePWM::print_info() +{ + printf("poll interval: %u ticks\n", getMeasureTicks()); +} + +void LidarLitePWM::print_registers() +{ + printf("Not supported in PWM mode\n"); +} diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h new file mode 100644 index 0000000000..787469378b --- /dev/null +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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 LidarLitePWM.h + * @author Johan Jansen + * + * Driver for the PulsedLight Lidar-Lite range finders connected via PWM. + */ +#pragma once + +#include "LidarLite.h" +#include +#include + +class LidarLitePWM : public LidarLite +{ +public: + LidarLitePWM(); + + int init() override; + + /** + * @brief + * Diagnostics - print some basic information about the driver. + */ + void print_info() override; + + /** + * @brief + * print registers to console + */ + void print_registers() override; + +private: + int _pwmSub; + pwm_input_s _pwm; +}; diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index b9c453bfc9..827143a9c2 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -51,6 +51,9 @@ # error This requires CONFIG_SCHED_WORKQUEUE. #endif +#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" +#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" + /* * Driver 'main' command. */ diff --git a/src/drivers/ll40ls/module.mk b/src/drivers/ll40ls/module.mk index a3a0ead55a..99c0815793 100644 --- a/src/drivers/ll40ls/module.mk +++ b/src/drivers/ll40ls/module.mk @@ -38,6 +38,8 @@ MODULE_COMMAND = ll40ls SRCS = ll40ls.cpp \ - LidarLiteI2C.cpp + LidarLite.cpp \ + LidarLiteI2C.cpp \ + LidarLitePWM.cpp MAXOPTIMIZATION = -Os -Weffc++ From 874c31988811c3c43c5dad7515a18804dece27b6 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 24 Mar 2015 11:49:34 +0100 Subject: [PATCH 04/51] LidarLite: Code cleanup --- src/drivers/ll40ls/LidarLite.h | 7 ++++--- src/drivers/ll40ls/LidarLiteI2C.h | 6 +++--- src/drivers/ll40ls/LidarLitePWM.cpp | 15 +++++++++++++++ src/drivers/ll40ls/LidarLitePWM.h | 7 +++++++ 4 files changed, 29 insertions(+), 6 deletions(-) diff --git a/src/drivers/ll40ls/LidarLite.h b/src/drivers/ll40ls/LidarLite.h index fa2e0e3cde..a00f81740d 100644 --- a/src/drivers/ll40ls/LidarLite.h +++ b/src/drivers/ll40ls/LidarLite.h @@ -80,9 +80,6 @@ public: */ virtual void print_registers() = 0; -private: - virtual int reset_sensor() = 0; - protected: /** * Set the min and max distance thresholds if you want the end points of the sensors @@ -96,6 +93,10 @@ protected: uint32_t getMeasureTicks() const; + virtual int measure() = 0; + + virtual int reset_sensor() = 0; + private: float _min_distance; float _max_distance; diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index d96c0d8918..f889969e84 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -95,6 +95,9 @@ protected: virtual int probe(); virtual int read_reg(uint8_t reg, uint8_t &val); + int measure() override; + int reset_sensor() override; + private: work_s _work; RingBuffer *_reports; @@ -144,11 +147,8 @@ private: * and start a new one. */ void cycle(); - int measure(); int collect(); - int reset_sensor() override; - /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index f225c0768f..7396ffd121 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -73,3 +73,18 @@ void LidarLitePWM::print_registers() { printf("Not supported in PWM mode\n"); } + +void LidarLitePWM::start() +{ + +} + +void LidarLitePWM::stop() +{ + +} + +int LidarLitePWM::measure() +{ + return OK; +} \ No newline at end of file diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index 787469378b..75d8be245a 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -51,6 +51,10 @@ public: int init() override; + void start() override; + + void stop() override; + /** * @brief * Diagnostics - print some basic information about the driver. @@ -63,6 +67,9 @@ public: */ void print_registers() override; +protected: + int measure() override; + private: int _pwmSub; pwm_input_s _pwm; From d160817de3065ebf6f65d8f2f6472409b7f2259b Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 24 Mar 2015 12:06:54 +0100 Subject: [PATCH 05/51] LidarLite: Fix bug for I2C ioctl --- src/drivers/ll40ls/LidarLiteI2C.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index d7f5e86294..809b5c39bb 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -194,7 +194,7 @@ ok: int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg) { - switch(arg) { + switch(cmd) { case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { From 4e7fa5aade14c4864d254400695e44f8e0f05074 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 24 Mar 2015 12:23:15 +0100 Subject: [PATCH 06/51] LidarLite: Add uORB handling in PWM drivers --- src/drivers/ll40ls/LidarLitePWM.cpp | 33 +++++++++++++++++++++++++---- src/drivers/ll40ls/LidarLitePWM.h | 2 ++ 2 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 7396ffd121..5486bbe62c 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -40,6 +40,7 @@ */ #include "LidarLitePWM.h" #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef __cplusplus @@ -48,7 +49,9 @@ static const int ERROR = -1; LidarLitePWM::LidarLitePWM() : _pwmSub(-1), - _pwm{} + _pwm{}, + _rangePub(-1), + _range{} { } @@ -61,6 +64,10 @@ int LidarLitePWM::init() return ERROR; } + _range.type = RANGE_FINDER_TYPE_LASER; + _range.valid = false; + _rangePub = orb_advertise(ORB_ID(sensor_range_finder), &_range); + return OK; } @@ -76,15 +83,33 @@ void LidarLitePWM::print_registers() void LidarLitePWM::start() { - + //TODO: start measurement task } void LidarLitePWM::stop() { - + //TODO: stop measurement task } int LidarLitePWM::measure() { + bool update; + orb_check(_pwmSub, &update); + + if(update) { + orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm); + + _range.timestamp = hrt_absolute_time(); + _range.error_count = _pwm.error_count; + _range.valid = true; + _range.maximum_distance = get_maximum_distance(); + _range.minimum_distance = get_minimum_distance(); + _range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite + _range.distance_vector[0] = _range.distance; + _range.just_updated = 0; + + orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); + } + return OK; -} \ No newline at end of file +} diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index 75d8be245a..ca43662efa 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -73,4 +73,6 @@ protected: private: int _pwmSub; pwm_input_s _pwm; + orb_advert_t _rangePub; + range_finder_report _range; }; From c4bc9d19cb48489ec830c7ea1e1eb01670373791 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 24 Mar 2015 12:57:22 +0100 Subject: [PATCH 07/51] LidarLite: Added collect phase to PWM --- src/drivers/ll40ls/LidarLite.h | 1 + src/drivers/ll40ls/LidarLitePWM.cpp | 59 +++++++++++++++++++++-------- src/drivers/ll40ls/LidarLitePWM.h | 5 +++ 3 files changed, 49 insertions(+), 16 deletions(-) diff --git a/src/drivers/ll40ls/LidarLite.h b/src/drivers/ll40ls/LidarLite.h index a00f81740d..1ea595909d 100644 --- a/src/drivers/ll40ls/LidarLite.h +++ b/src/drivers/ll40ls/LidarLite.h @@ -94,6 +94,7 @@ protected: uint32_t getMeasureTicks() const; virtual int measure() = 0; + virtual int collect() = 0; virtual int reset_sensor() = 0; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 5486bbe62c..50ee4880b9 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -48,6 +48,7 @@ static const int ERROR = -1; #endif LidarLitePWM::LidarLitePWM() : + _terminateRequested(false), _pwmSub(-1), _pwm{}, _rangePub(-1), @@ -74,6 +75,8 @@ int LidarLitePWM::init() void LidarLitePWM::print_info() { printf("poll interval: %u ticks\n", getMeasureTicks()); + printf("distance: %ucm (0x%04x)\n", + (unsigned)_range.distance, (unsigned)_range.distance); } void LidarLitePWM::print_registers() @@ -83,33 +86,57 @@ void LidarLitePWM::print_registers() void LidarLitePWM::start() { - //TODO: start measurement task + } void LidarLitePWM::stop() { //TODO: stop measurement task + _terminateRequested = true; } int LidarLitePWM::measure() { - bool update; - orb_check(_pwmSub, &update); + int result = OK; - if(update) { - orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm); + _range.error_count = _pwm.error_count; + _range.maximum_distance = get_maximum_distance(); + _range.minimum_distance = get_minimum_distance(); + _range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite + _range.distance_vector[0] = _range.distance; + _range.just_updated = 0; + _range.valid = true; - _range.timestamp = hrt_absolute_time(); - _range.error_count = _pwm.error_count; - _range.valid = true; - _range.maximum_distance = get_maximum_distance(); - _range.minimum_distance = get_minimum_distance(); - _range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite - _range.distance_vector[0] = _range.distance; - _range.just_updated = 0; - - orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); + //TODO: due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) + if(_range.distance <= 0.0f) { + _range.valid = false; + _range.error_count++; + result = ERROR; } - return OK; + orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); + + return result; +} + +int LidarLitePWM::collect() +{ + //Check PWM + bool update; + orb_check(_pwmSub, &update); + if(update) { + orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm); + _range.timestamp = hrt_absolute_time(); + return OK; + } + + //Timeout readings after 0.2 seconds and mark as invalid + if(hrt_absolute_time() - _range.timestamp > LL40LS_CONVERSION_TIMEOUT*2) { + _range.timestamp = hrt_absolute_time(); + _range.valid = false; + orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); + return ERROR; + } + + return EAGAIN; } diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index ca43662efa..0b85d53abc 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -70,7 +70,12 @@ public: protected: int measure() override; + int collect() override; + + void task_main_trampoline(int argc, char *argv[]); + private: + bool _terminateRequested; int _pwmSub; pwm_input_s _pwm; orb_advert_t _rangePub; From dbe58d6165cfc392e81b86b2d1f7f0e7ab60706f Mon Sep 17 00:00:00 2001 From: Max Shvetsov Date: Tue, 19 May 2015 12:40:42 +0300 Subject: [PATCH 08/51] [pwm_input] reset feature added publication to range_finder topic added --- makefiles/config_px4fmu-v2_default.mk | 2 + src/drivers/boards/px4fmu-v2/board_config.h | 2 +- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 1 + src/drivers/pwm_input/module.mk | 1 + src/drivers/pwm_input/pwm_input.cpp | 98 +++++++++++++++++++-- 5 files changed, 97 insertions(+), 7 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 7884b94cb0..a0edd7710f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -45,6 +45,7 @@ MODULES += drivers/mkblctrl MODULES += drivers/px4flow MODULES += drivers/oreoled MODULES += drivers/gimbal +MODULES += drivers/pwm_input # # System commands @@ -70,6 +71,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led +MODULES += modules/gpio_tool MODULES += modules/uavcan MODULES += modules/land_detector diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 6188e29ae1..2aec6e8c14 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -161,7 +161,7 @@ __BEGIN_DECLS #define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) #define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) #define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) -#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14) /* Power supply control and monitoring GPIOs */ #define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 266642cbbf..039fb213c3 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -243,6 +243,7 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_VDD_SERVO_VALID); stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); + stm32_configgpio(GPIO_GPIO5_OUTPUT); /* configure the high-resolution time/callout interface */ hrt_init(); diff --git a/src/drivers/pwm_input/module.mk b/src/drivers/pwm_input/module.mk index 04f04d6cbc..a135ce777e 100644 --- a/src/drivers/pwm_input/module.mk +++ b/src/drivers/pwm_input/module.mk @@ -39,3 +39,4 @@ MODULE_COMMAND = pwm_input SRCS = pwm_input.cpp +EXTRACXXFLAGS = -Wno-pmf-conversions diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 2d771266c0..586fe38977 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include "chip.h" #include "up_internal.h" @@ -80,6 +81,9 @@ #include #include +// Reset pin define +#define GPIO_VDD_RANGEFINDER_EN GPIO_GPIO5_OUTPUT + #if HRT_TIMER == PWMIN_TIMER #error cannot share timer between HRT and PWMIN #endif @@ -216,6 +220,7 @@ #error PWMIN_TIMER_CHANNEL must be either 1 and 2. #endif +#define TIMEOUT 300000 /* reset after no responce over this time in microseconds [0.3 secs] */ class PWMIN : device::CDev { @@ -230,20 +235,33 @@ public: void _publish(uint16_t status, uint32_t period, uint32_t pulse_width); void _print_info(void); + void hard_reset(); private: uint32_t error_count; uint32_t pulses_captured; uint32_t last_period; uint32_t last_width; + uint64_t last_poll_time; RingBuffer *reports; bool timer_started; + range_finder_report data; + orb_advert_t range_finder_pub; + + hrt_call hard_reset_call; // HRT callout for note completion + hrt_call freeze_test_call; // HRT callout for note completion + void timer_init(void); + + void turn_on(); + void turn_off(); + void freeze_test(); + }; static int pwmin_tim_isr(int irq, void *context); -static void pwmin_start(void); +static void pwmin_start(bool full_start); static void pwmin_info(void); static void pwmin_test(void); static void pwmin_reset(void); @@ -257,8 +275,10 @@ PWMIN::PWMIN() : last_period(0), last_width(0), reports(nullptr), - timer_started(false) + timer_started(false), + range_finder_pub(-1) { + memset(&data, 0, sizeof(data)); } PWMIN::~PWMIN() @@ -280,11 +300,22 @@ PWMIN::init() // activate the timer when requested to when the device is opened CDev::init(); + data.type = RANGE_FINDER_TYPE_LASER; + data.minimum_distance = 0.20f; + data.maximum_distance = 7.0f; + + range_finder_pub = orb_advertise(ORB_ID(sensor_range_finder), &data); + fprintf(stderr, "[pwm_input] advertising %d\n" + ,range_finder_pub); + reports = new RingBuffer(2, sizeof(struct pwm_input_s)); if (reports == nullptr) { return -ENOMEM; } + // Schedule freeze check to invoke periodically + hrt_call_every(&freeze_test_call, 0, TIMEOUT, reinterpret_cast(&PWMIN::freeze_test), this); + return OK; } @@ -346,6 +377,31 @@ void PWMIN::timer_init(void) timer_started = true; } +void +PWMIN::freeze_test() +{ + /* timeout is true if least read was away back */ + bool timeout = false; + timeout = (hrt_absolute_time() - last_poll_time > TIMEOUT) ? true : false; + if (timeout) { + fprintf(stderr, "[pwm_input] Lidar is down, reseting\n"); + hard_reset(); + } +} + +void +PWMIN::turn_on() { stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1); } + +void +PWMIN::turn_off() { stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0); } + +void +PWMIN::hard_reset() +{ + turn_off(); + hrt_call_after(&hard_reset_call, 9000, reinterpret_cast(&PWMIN::turn_on), this); +} + /* hook for open of the driver. We start the timer at this point, then leave it running @@ -440,12 +496,27 @@ void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width) return; } + last_poll_time = hrt_absolute_time(); + struct pwm_input_s pwmin_report; - pwmin_report.timestamp = hrt_absolute_time(); + pwmin_report.timestamp = last_poll_time; pwmin_report.error_count = error_count; pwmin_report.period = period; pwmin_report.pulse_width = pulse_width; + data.distance = pulse_width * 1e-3f; + data.timestamp = pwmin_report.timestamp; + data.error_count = error_count; + + if (data.distance < data.minimum_distance || data.distance > data.maximum_distance) { + data.valid = false; + } else { + data.valid = true; + } + if (range_finder_pub > 0) { + orb_publish(ORB_ID(sensor_range_finder), range_finder_pub, &data); + } + reports->force(&pwmin_report); last_period = period; @@ -491,7 +562,7 @@ static int pwmin_tim_isr(int irq, void *context) /* start the driver */ -static void pwmin_start(void) +static void pwmin_start(bool full_start) { if (g_dev != nullptr) { printf("driver already started\n"); @@ -504,6 +575,13 @@ static void pwmin_start(void) if (g_dev->init() != OK) { errx(1, "driver init failed"); } + if (full_start) { + int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open device"); + } + close(fd); + } exit(0); } @@ -538,6 +616,7 @@ static void pwmin_test(void) */ static void pwmin_reset(void) { + g_dev->hard_reset(); int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); if (fd == -1) { errx(1, "Failed to open device"); @@ -569,12 +648,19 @@ static void pwmin_info(void) int pwm_input_main(int argc, char * argv[]) { const char *verb = argv[1]; + /* + * init driver and start reading + */ + bool full_start = false; + if (!strcmp(argv[2], "full")) { + full_start = true; + } /* * Start/load the driver. */ if (!strcmp(verb, "start")) { - pwmin_start(); + pwmin_start(full_start); } /* @@ -598,6 +684,6 @@ int pwm_input_main(int argc, char * argv[]) pwmin_reset(); } - errx(1, "unrecognized command, try 'start', 'info', 'reset' or 'test'"); + errx(1, "unrecognized command, try 'start', 'start full', 'info', 'reset' or 'test'"); return 0; } From 37e96e230ccb056dc4d43113214901694c00b34b Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 19 May 2015 23:25:33 +0100 Subject: [PATCH 09/51] distance_sensor: first topic commit --- src/modules/uORB/topics/distance_sensor.h | 76 +++++++++++++++++++++++ 1 file changed, 76 insertions(+) create mode 100644 src/modules/uORB/topics/distance_sensor.h diff --git a/src/modules/uORB/topics/distance_sensor.h b/src/modules/uORB/topics/distance_sensor.h new file mode 100644 index 0000000000..ea123c7faf --- /dev/null +++ b/src/modules/uORB/topics/distance_sensor.h @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: @author Nuno Marques + * + * 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 distance_sensor.h + * Definition of the distance sensor uORB topic. + */ + +#ifndef TOPIC_DISTANCE_SENSOR_H_ +#define TOPIC_DISTANCE_SENSOR_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + */ + +/** + * Distance sensor data, with range in centimeters (not in SI). + * + * @see http://en.wikipedia.org/wiki/International_System_of_Units + */ +struct distance_sensor_s { + + uint32_t time_boot_ms; /**< Time since system boot */ + uint16_t min_distance; /**< Minimum distance the sensor can measure in centimeters */ + uint16_t max_distance; /**< Maximum distance the sensor can measure in centimeters */ + uint16_t current_distance; /**< Current distance reading */ + uint8_t type; /**< Type from MAV_DISTANCE_SENSOR enum */ + uint8_t id; /**< Onboard ID of the sensor */ + uint8_t orientation; /**< Direction the sensor faces from MAV_SENSOR_ORIENTATION enum */ + uint8_t covariance; /**< Measurement covariance in centimeters, 0 for unknown / invalid readings */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(distance_sensor); + +#endif \ No newline at end of file From 364492a325f25fa5dce10af28a03afb435294846 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 19 May 2015 23:42:15 +0100 Subject: [PATCH 10/51] distance_sensor: added def to objects_common.cpp --- src/modules/uORB/objects_common.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index eb89b79585..b34f9a742d 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -256,3 +256,6 @@ ORB_DEFINE(time_offset, struct time_offset_s); #include "topics/mc_att_ctrl_status.h" ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s); + +#include "topics/distance_sensor.h" +ORB_DEFINE(distance_sensor, struct distance_sensor_s); \ No newline at end of file From 30218b0de709a0bf38f4b5c0cc104caf66f4e343 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 20 May 2015 00:15:27 +0100 Subject: [PATCH 11/51] distance_sensor: added mavlink_receiver handler for msg --- src/modules/mavlink/mavlink_receiver.cpp | 36 +++++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 3 ++ 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3c4fa76a0c..b84d381c6f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -134,7 +134,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _att_sp{}, _rates_sp{}, _time_offset_avg_alpha(0.6), - _time_offset(0) + _time_offset(0), + _distance_sensor_pub(-1) { } @@ -211,6 +212,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_timesync(msg); break; + case MAVLINK_MSG_ID_DISTANCE_SENSOR: + handle_message_distance_sensor(msg); + break; + default: break; } @@ -495,6 +500,35 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) +{ + /* distance sensor */ + mavlink_distance_sensor_t dist_sensor; + mavlink_msg_distance_sensor_decode(msg, &dist_sensor); + + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); + + d.time_boot_ms = dist_sensor.time_boot_ms; + d.min_distance = dist_sensor.min_distance; + d.max_distance = dist_sensor.max_distance; + d.current_distance = dist_sensor.current_distance; + d.type = dist_sensor.type; + d.id = dist_sensor.id; + d.orientation = dist_sensor.orientation; + d.covariance = dist_sensor.covariance; + + /// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum + + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + + } else { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); + } +} + void MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 887d2f88ed..7b592ab51a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -74,6 +74,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -135,6 +136,7 @@ private: void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); + void handle_message_distance_sensor(mavlink_message_t *msg); void *receive_thread(void *arg); @@ -191,6 +193,7 @@ private: struct vehicle_rates_setpoint_s _rates_sp; double _time_offset_avg_alpha; uint64_t _time_offset; + orb_advert_t _distance_sensor_pub; /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver &); From 796b105382c0a9962bf94047b9be03120c064515 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 20 May 2015 00:54:36 +0100 Subject: [PATCH 12/51] distance_sensor: updated distance_sensor stream sub --- src/modules/mavlink/mavlink_messages.cpp | 42 ++++++++++++++---------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e62d223e0c..c8f0f6da0b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -2179,7 +2180,6 @@ protected: } }; - class MavlinkStreamDistanceSensor : public MavlinkStream { public: @@ -2205,12 +2205,12 @@ public: unsigned get_size() { - return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _distance_sensor_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_range_sub; - uint64_t _range_time; + MavlinkOrbSubscription *_distance_sensor_sub; + uint64_t _dist_sensor_time; /* do not allow top copying this class */ MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); @@ -2218,36 +2218,44 @@ private: protected: explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), - _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))), - _range_time(0) + _distance_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(distance_sensor))), + _dist_sensor_time(0) {} void send(const hrt_abstime t) { - struct range_finder_report range; + struct distance_sensor_s dist_sensor; - if (_range_sub->update(&_range_time, &range)) { + if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) { mavlink_distance_sensor_t msg; - msg.time_boot_ms = range.timestamp / 1000; + msg.time_boot_ms = dist_sensor.time_boot_ms; - switch (range.type) { - case RANGE_FINDER_TYPE_LASER: + switch (dist_sensor.type) { + case MAV_DISTANCE_SENSOR_ULTRASOUND: msg.type = MAV_DISTANCE_SENSOR_LASER; break; + case MAV_DISTANCE_SENSOR_LASER: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + + /*case MAV_DISTANCE_SENSOR_INFRARED: + msg.type = MAV_DISTANCE_SENSOR_INFRARED; + break;*/ //TODO: update mavlink def + default: msg.type = MAV_DISTANCE_SENSOR_LASER; break; } - msg.id = 0; - msg.orientation = 0; - msg.min_distance = range.minimum_distance * 100; - msg.max_distance = range.maximum_distance * 100; - msg.current_distance = range.distance * 100; - msg.covariance = 20; + msg.id = dist_sensor.id; + msg.orientation = dist_sensor.orientation; + msg.min_distance = dist_sensor.min_distance; + msg.max_distance = dist_sensor.max_distance; + msg.current_distance = dist_sensor.current_distance; + msg.covariance = dist_sensor.covariance; _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); } From c180b5d8251b5d9259e403c15ce3bb9babf73241 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 20 May 2015 12:40:15 +0100 Subject: [PATCH 13/51] distance_sensor: added msg definition to 'msg' folder --- CMakeLists.txt | 1 + msg/distance_sensor.msg | 17 +++++++ src/modules/uORB/topics/distance_sensor.h | 58 +++++++++++++---------- 3 files changed, 50 insertions(+), 26 deletions(-) create mode 100644 msg/distance_sensor.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 457a0bfa74..bfd7b662b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,6 +80,7 @@ add_message_files( vehicle_global_velocity_setpoint.msg offboard_control_mode.msg vehicle_force_setpoint.msg + distance_sensor.msg ) ## Generate services in the 'srv' folder diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg new file mode 100644 index 0000000000..ef11c586fc --- /dev/null +++ b/msg/distance_sensor.msg @@ -0,0 +1,17 @@ +# DISTANCE_SENSOR message data + +uint32 time_boot_ms + +uint16 min_distance +uint16 max_distance +uint16 current_distance + +uint8 type +uint8 MAV_DISTANCE_SENSOR_LASER = 0 +uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 +uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 + +uint8 id + +uint8 orientation +uint8 covariance \ No newline at end of file diff --git a/src/modules/uORB/topics/distance_sensor.h b/src/modules/uORB/topics/distance_sensor.h index ea123c7faf..b3eb9e4f0f 100644 --- a/src/modules/uORB/topics/distance_sensor.h +++ b/src/modules/uORB/topics/distance_sensor.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: @author Nuno Marques + * Copyright (C) 2013-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 @@ -32,38 +31,47 @@ * ****************************************************************************/ -/** - * @file distance_sensor.h - * Definition of the distance sensor uORB topic. - */ +/* Auto-generated by genmsg_cpp from file /home/nuno/px4/Firmware/msg/distance_sensor.msg */ -#ifndef TOPIC_DISTANCE_SENSOR_H_ -#define TOPIC_DISTANCE_SENSOR_H_ + +#pragma once #include -#include -#include "../uORB.h" +#include + + +#ifndef __cplusplus +#define MAV_DISTANCE_SENSOR_LASER 0 +#define MAV_DISTANCE_SENSOR_ULTRASOUND 1 +#define MAV_DISTANCE_SENSOR_INFRARED 2 + +#endif /** * @addtogroup topics + * @{ */ -/** - * Distance sensor data, with range in centimeters (not in SI). - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - */ + +#ifdef __cplusplus +struct __EXPORT distance_sensor_s { +#else struct distance_sensor_s { +#endif + uint32_t time_boot_ms; + uint16_t min_distance; + uint16_t max_distance; + uint16_t current_distance; + uint8_t type; + uint8_t id; + uint8_t orientation; + uint8_t covariance; +#ifdef __cplusplus + static const uint8_t MAV_DISTANCE_SENSOR_LASER = 0; + static const uint8_t MAV_DISTANCE_SENSOR_ULTRASOUND = 1; + static const uint8_t MAV_DISTANCE_SENSOR_INFRARED = 2; - uint32_t time_boot_ms; /**< Time since system boot */ - uint16_t min_distance; /**< Minimum distance the sensor can measure in centimeters */ - uint16_t max_distance; /**< Maximum distance the sensor can measure in centimeters */ - uint16_t current_distance; /**< Current distance reading */ - uint8_t type; /**< Type from MAV_DISTANCE_SENSOR enum */ - uint8_t id; /**< Onboard ID of the sensor */ - uint8_t orientation; /**< Direction the sensor faces from MAV_SENSOR_ORIENTATION enum */ - uint8_t covariance; /**< Measurement covariance in centimeters, 0 for unknown / invalid readings */ - +#endif }; /** @@ -72,5 +80,3 @@ struct distance_sensor_s { /* register this as object request broker structure */ ORB_DECLARE(distance_sensor); - -#endif \ No newline at end of file From 80c7719b42307a2716b6a58848af6e8f61a4b788 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 20 May 2015 12:51:20 +0100 Subject: [PATCH 14/51] distance_sensor: typo correction on msg.type --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c8f0f6da0b..cc00fc69b8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2234,7 +2234,7 @@ protected: switch (dist_sensor.type) { case MAV_DISTANCE_SENSOR_ULTRASOUND: - msg.type = MAV_DISTANCE_SENSOR_LASER; + msg.type = MAV_DISTANCE_SENSOR_ULTRASOUND; break; case MAV_DISTANCE_SENSOR_LASER: From 1978f1bcac1d854d035f78672decd8d4ac67f6ce Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 20 May 2015 17:37:54 +0100 Subject: [PATCH 15/51] distance_sensor: remove 'distance_sensor.h' autogenerated header --- src/modules/uORB/topics/distance_sensor.h | 82 ----------------------- 1 file changed, 82 deletions(-) delete mode 100644 src/modules/uORB/topics/distance_sensor.h diff --git a/src/modules/uORB/topics/distance_sensor.h b/src/modules/uORB/topics/distance_sensor.h deleted file mode 100644 index b3eb9e4f0f..0000000000 --- a/src/modules/uORB/topics/distance_sensor.h +++ /dev/null @@ -1,82 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-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. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/nuno/px4/Firmware/msg/distance_sensor.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus -#define MAV_DISTANCE_SENSOR_LASER 0 -#define MAV_DISTANCE_SENSOR_ULTRASOUND 1 -#define MAV_DISTANCE_SENSOR_INFRARED 2 - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT distance_sensor_s { -#else -struct distance_sensor_s { -#endif - uint32_t time_boot_ms; - uint16_t min_distance; - uint16_t max_distance; - uint16_t current_distance; - uint8_t type; - uint8_t id; - uint8_t orientation; - uint8_t covariance; -#ifdef __cplusplus - static const uint8_t MAV_DISTANCE_SENSOR_LASER = 0; - static const uint8_t MAV_DISTANCE_SENSOR_ULTRASOUND = 1; - static const uint8_t MAV_DISTANCE_SENSOR_INFRARED = 2; - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(distance_sensor); From db48df15c8e141eb8c4eea5a20aec9ebdeb049d2 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 21 May 2015 12:26:55 +0100 Subject: [PATCH 16/51] Merge branch 'master' --- Makefile | 22 ++- Tools/generate_listener.py | 4 +- makefiles/firmware_posix.mk | 2 +- makefiles/firmware_qurt.mk | 2 +- makefiles/qurt/config_qurt_hello.mk | 73 ++++++++ msg/filtered_bottom_flow.msg | 8 + msg/home_position.msg | 10 + msg/optical_flow.msg | 17 ++ msg/range_finder.msg | 13 ++ msg/sensor_combined.msg | 100 ++++++++++ msg/vehicle_global_position.msg | 20 ++ msg/vehicle_gps_position.msg | 29 +++ nuttx-configs/px4fmu-v2/nsh/defconfig | 4 +- src/drivers/drv_range_finder.h | 37 +--- src/drivers/sf0x/sf0x.cpp | 41 +++-- src/include/containers/List.hpp | 14 +- .../commander/accelerometer_calibration.cpp | 26 ++- src/modules/controllib/block/Block.cpp | 5 + src/modules/controllib/block/Block.hpp | 12 +- src/modules/controllib/block/BlockParam.cpp | 2 + src/modules/controllib/block/BlockParam.hpp | 2 + src/modules/controllib/uorb/blocks.cpp | 1 + src/modules/controllib/uorb/blocks.hpp | 4 - src/modules/mavlink/mavlink_main.cpp | 4 + src/modules/mavlink/mavlink_messages.cpp | 1 - src/modules/mavlink/mavlink_receiver.h | 1 - src/modules/sensors/sensors.cpp | 46 ++--- src/modules/uORB/Publication.cpp | 8 + src/modules/uORB/Publication.hpp | 12 +- src/modules/uORB/Subscription.cpp | 2 + src/modules/uORB/Subscription.hpp | 5 + .../uORB/topics/actuator_controls_effective.h | 79 -------- src/modules/uORB/topics/optical_flow.h | 84 --------- src/modules/uORB/topics/sensor_combined.h | 172 ------------------ .../uORB/topics/vehicle_global_position.h | 87 --------- .../uORB/topics/vehicle_gps_position.h | 94 ---------- .../posix/drivers/airspeedsim/airspeedsim.cpp | 2 +- .../qurt/px4_layer/commands_default.c} | 71 ++++---- .../qurt/px4_layer/commands_hello.c} | 50 +---- src/platforms/qurt/px4_layer/main.cpp | 30 +-- src/platforms/qurt/px4_layer/module.mk | 8 +- src/systemcmds/tests/test_eigen.cpp | 24 +-- src/systemcmds/tests/tests_main.c | 2 +- 43 files changed, 481 insertions(+), 749 deletions(-) create mode 100644 makefiles/qurt/config_qurt_hello.mk create mode 100644 msg/filtered_bottom_flow.msg create mode 100644 msg/home_position.msg create mode 100644 msg/optical_flow.msg create mode 100644 msg/range_finder.msg create mode 100644 msg/sensor_combined.msg create mode 100644 msg/vehicle_global_position.msg create mode 100644 msg/vehicle_gps_position.msg delete mode 100644 src/modules/uORB/topics/actuator_controls_effective.h delete mode 100644 src/modules/uORB/topics/optical_flow.h delete mode 100644 src/modules/uORB/topics/sensor_combined.h delete mode 100644 src/modules/uORB/topics/vehicle_global_position.h delete mode 100644 src/modules/uORB/topics/vehicle_gps_position.h rename src/{modules/uORB/topics/filtered_bottom_flow.h => platforms/qurt/px4_layer/commands_default.c} (63%) rename src/{modules/uORB/topics/home_position.h => platforms/qurt/px4_layer/commands_hello.c} (62%) diff --git a/Makefile b/Makefile index 4a0f13cd19..3f78f32362 100644 --- a/Makefile +++ b/Makefile @@ -33,6 +33,12 @@ # Top-level Makefile for building PX4 firmware images. # +TARGETS := nuttx posix qurt +EXPLICIT_TARGET := $(filter $(TARGETS),$(MAKECMDGOALS)) +ifneq ($(EXPLICIT_TARGET),) + export PX4_TARGET_OS=$(EXPLICIT_TARGET) +endif + # # Get path and tool configuration # @@ -271,14 +277,14 @@ testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) -posix: - make PX4_TARGET_OS=posix +nuttx: + make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) -nuttx: - make PX4_TARGET_OS=nuttx +posix: + make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) -qurt: - make PX4_TARGET_OS=qurt +qurt: + make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) posixrun: Tools/posix_run.sh @@ -327,9 +333,11 @@ help: @$(ECHO) " Available targets:" @$(ECHO) " ------------------" @$(ECHO) "" +ifeq ($(PX4_TARGET_OS),nuttx) @$(ECHO) " archives" @$(ECHO) " Build the NuttX RTOS archives that are used by the firmware build." @$(ECHO) "" +endif @$(ECHO) " all" @$(ECHO) " Build all firmware configs: $(CONFIGS)" @$(ECHO) " A limited set of configs can be built with CONFIGS=" @@ -342,6 +350,7 @@ help: @$(ECHO) " clean" @$(ECHO) " Remove all firmware build pieces." @$(ECHO) "" +ifeq ($(PX4_TARGET_OS),nuttx) @$(ECHO) " distclean" @$(ECHO) " Remove all compilation products, including NuttX RTOS archives." @$(ECHO) "" @@ -350,6 +359,7 @@ help: @$(ECHO) " firmware to the board when the build is complete. Not supported for" @$(ECHO) " all configurations." @$(ECHO) "" +endif @$(ECHO) " testbuild" @$(ECHO) " Perform a complete clean build of the entire tree." @$(ECHO) "" diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index e52d3cd26f..5cfe4a6281 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -91,6 +91,8 @@ print """ #include #include #include +#define __STDC_FORMAT_MACROS +#include """ for m in messages: print "#include " % m @@ -135,7 +137,7 @@ for index,m in enumerate(messages[1:]): print "\t\t\t}" print "\t\t\tprintf(\"\\n\");" elif item[0] == "uint64": - print "\t\t\tprintf(\"%s: %%lu\\n \",container.%s);" % (item[1], item[1]) + print "\t\t\tprintf(\"%s: %%\" PRIu64 \"\\n \",container.%s);" % (item[1], item[1]) elif item[0] == "uint8": print "\t\t\tprintf(\"%s: %%u\\n \",container.%s);" % (item[1], item[1]) elif item[0] == "bool": diff --git a/makefiles/firmware_posix.mk b/makefiles/firmware_posix.mk index 6acec4e733..912e7bcc3f 100644 --- a/makefiles/firmware_posix.mk +++ b/makefiles/firmware_posix.mk @@ -32,7 +32,7 @@ # # Built products # -FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) +FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) all: $(FIRMWARES) diff --git a/makefiles/firmware_qurt.mk b/makefiles/firmware_qurt.mk index 41128fa60f..a367e4a327 100644 --- a/makefiles/firmware_qurt.mk +++ b/makefiles/firmware_qurt.mk @@ -32,7 +32,7 @@ # # Built products # -FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) +FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) all: $(FIRMWARES) diff --git a/makefiles/qurt/config_qurt_hello.mk b/makefiles/qurt/config_qurt_hello.mk new file mode 100644 index 0000000000..4b11a79fe3 --- /dev/null +++ b/makefiles/qurt/config_qurt_hello.mk @@ -0,0 +1,73 @@ +# +# Makefile for the Foo *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +#MODULES += drivers/blinkm +#MODULES += drivers/hil +#MODULES += drivers/led +#MODULES += drivers/rgbled +#MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +MODULES += systemcmds/param + +# +# General system control +# +#MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +#MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +#MODULES += modules/mc_att_control + +# +# Library modules +# +MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +MODULES += modules/uORB +#MODULES += modules/dataman +#MODULES += modules/sdlog2 +#MODULES += modules/simulator +#MODULES += modules/commander + +# +# Libraries +# +#MODULES += lib/mathlib +#MODULES += lib/mathlib/math/filter +#MODULES += lib/geo +#MODULES += lib/geo_lookup +#MODULES += lib/conversion + +# +# QuRT port +# +MODULES += platforms/qurt/px4_layer +#MODULES += platforms/posix/drivers/accelsim +#MODULES += platforms/posix/drivers/gyrosim +#MODULES += platforms/posix/drivers/adcsim +#MODULES += platforms/posix/drivers/barosim + +# +# Unit tests +# +MODULES += platforms/qurt/tests/hello +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + diff --git a/msg/filtered_bottom_flow.msg b/msg/filtered_bottom_flow.msg new file mode 100644 index 0000000000..815a38414d --- /dev/null +++ b/msg/filtered_bottom_flow.msg @@ -0,0 +1,8 @@ +# Filtered bottom flow in bodyframe. +uint64 timestamp # time of this estimate, in microseconds since system start + +float32 sumx # Integrated bodyframe x flow in meters +float32 sumy # Integrated bodyframe y flow in meters + +float32 vx # Flow bodyframe x speed, m/s +float32 vy # Flow bodyframe y Speed, m/s diff --git a/msg/home_position.msg b/msg/home_position.msg new file mode 100644 index 0000000000..d8aff3f634 --- /dev/null +++ b/msg/home_position.msg @@ -0,0 +1,10 @@ +# GPS home position in WGS84 coordinates. +uint64 timestamp # Timestamp (microseconds since system boot) + +float64 lat # Latitude in degrees +float64 lon # Longitude in degrees +float32 alt # Altitude in meters (AMSL) + +float32 x # X coordinate in meters +float32 y # Y coordinate in meters +float32 z # Z coordinate in meters diff --git a/msg/optical_flow.msg b/msg/optical_flow.msg new file mode 100644 index 0000000000..d34e32b8fd --- /dev/null +++ b/msg/optical_flow.msg @@ -0,0 +1,17 @@ +# Optical flow in NED body frame in SI units. +# @see http://en.wikipedia.org/wiki/International_System_of_Units + +uint64 timestamp # in microseconds since system start +uint8 sensor_id # id of the sensor emitting the flow value +float32 pixel_flow_x_integral # accumulated optical flow in radians around x axis +float32 pixel_flow_y_integral # accumulated optical flow in radians around y axis +float32 gyro_x_rate_integral # accumulated gyro value in radians around x axis +float32 gyro_y_rate_integral # accumulated gyro value in radians around y axis +float32 gyro_z_rate_integral # accumulated gyro value in radians around z axis +float32 ground_distance_m # Altitude / distance to ground in meters +uint32 integration_timespan # accumulation timespan in microseconds +uint32 time_since_last_sonar_update # time since last sonar update in microseconds +uint16 frame_count_since_last_readout # number of accumulated frames in timespan +int16 gyro_temperature # Temperature * 100 in centi-degrees Celsius +uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality + diff --git a/msg/range_finder.msg b/msg/range_finder.msg new file mode 100644 index 0000000000..d2e67543e5 --- /dev/null +++ b/msg/range_finder.msg @@ -0,0 +1,13 @@ +int16 RANGE_FINDER_TYPE_LASER = 0 + +# range finder report structure. Reads from the device must be in multiples of this +# structure. +uint64 timestamp +uint64 error_count +uint16 type # type, following RANGE_FINDER_TYPE enum +float32 distance # in meters +float32 minimum_distance # minimum distance the sensor can measure +float32 maximum_distance # maximum distance the sensor can measure +uint8 valid # 1 == within sensor range, 0 = outside sensor range +float32[12] distance_vector # in meters length should match MB12XX_MAX_RANGEFINDERS +uint8 just_updated # number of the most recent measurement sensor diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg new file mode 100644 index 0000000000..c5e427e194 --- /dev/null +++ b/msg/sensor_combined.msg @@ -0,0 +1,100 @@ +# Definition of the sensor_combined uORB topic. + +int32 MAGNETOMETER_MODE_NORMAL = 0 +int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1 +int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 + +# Sensor readings in raw and SI-unit form. +# +# These values are read from the sensors. Raw values are in sensor-specific units, +# the scaled values are in SI-units, as visible from the ending of the variable +# or the comments. The use of the SI fields is in general advised, as these fields +# are scaled and offset-compensated where possible and do not change with board +# revisions and sensor updates. +# +# Actual data, this is specific to the type of data which is stored in this struct +# A line containing L0GME will be added by the Python logging code generator to the logged dataset. +# +# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only + +uint64 timestamp # Timestamp in microseconds since boot, from gyro +# +int16[3] gyro_raw # Raw sensor values of angular velocity +float32[3] gyro_rad_s # Angular velocity in radian per seconds +uint32 gyro_errcount # Error counter for gyro 0 +float32 gyro_temp # Temperature of gyro 0 + +int16[3] accelerometer_raw # Raw acceleration in NED body frame +float32[3] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 +int16 accelerometer_mode # Accelerometer measurement mode +float32 accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 +uint64 accelerometer_timestamp # Accelerometer timestamp +uint32 accelerometer_errcount # Error counter for accel 0 +float32 accelerometer_temp # Temperature of accel 0 + +int16[3] magnetometer_raw # Raw magnetic field in NED body frame +float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss +int16 magnetometer_mode # Magnetometer measurement mode +float32 magnetometer_range_ga # measurement range in Gauss +float32 magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor +uint64 magnetometer_timestamp # Magnetometer timestamp +uint32 magnetometer_errcount # Error counter for mag 0 +float32 magnetometer_temp # Temperature of mag 0 + +int16[3] gyro1_raw # Raw sensor values of angular velocity +float32[3] gyro1_rad_s # Angular velocity in radian per seconds +uint64 gyro1_timestamp # Gyro timestamp +uint32 gyro1_errcount # Error counter for gyro 1 +float32 gyro1_temp # Temperature of gyro 1 + +int16[3] accelerometer1_raw # Raw acceleration in NED body frame +float32[3] accelerometer1_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer1_timestamp # Accelerometer timestamp +uint32 accelerometer1_errcount # Error counter for accel 1 +float32 accelerometer1_temp # Temperature of accel 1 + +int16[3] magnetometer1_raw # Raw magnetic field in NED body frame +float32[3] magnetometer1_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer1_timestamp # Magnetometer timestamp +uint32 magnetometer1_errcount # Error counter for mag 1 +float32 magnetometer1_temp # Temperature of mag 1 + +int16[3] gyro2_raw # Raw sensor values of angular velocity +float32[3] gyro2_rad_s # Angular velocity in radian per seconds +uint64 gyro2_timestamp # Gyro timestamp +uint32 gyro2_errcount # Error counter for gyro 1 +float32 gyro2_temp # Temperature of gyro 1 + +int16[3] accelerometer2_raw # Raw acceleration in NED body frame +float32[3] accelerometer2_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer2_timestamp # Accelerometer timestamp +uint32 accelerometer2_errcount # Error counter for accel 2 +float32 accelerometer2_temp # Temperature of accel 2 + +int16[3] magnetometer2_raw # Raw magnetic field in NED body frame +float32[3] magnetometer2_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer2_timestamp # Magnetometer timestamp +uint32 magnetometer2_errcount # Error counter for mag 2 +float32 magnetometer2_temp # Temperature of mag 2 + +float32 baro_pres_mbar # Barometric pressure, already temp. comp. +float32 baro_alt_meter # Altitude, already temp. comp. +float32 baro_temp_celcius # Temperature in degrees celsius +uint64 baro_timestamp # Barometer timestamp + +float32 baro1_pres_mbar # Barometric pressure, already temp. comp. +float32 baro1_alt_meter # Altitude, already temp. comp. +float32 baro1_temp_celcius # Temperature in degrees celsius +uint64 baro1_timestamp # Barometer timestamp + +float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1 +uint16[10] adc_mapping # Channel indices of each of these values +float32 mcu_temp_celcius # Internal temperature measurement of MCU + +float32 differential_pressure_pa # Airspeed sensor differential pressure +uint64 differential_pressure_timestamp # Last measurement timestamp +float32 differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading + +float32 differential_pressure1_pa # Airspeed sensor differential pressure +uint64 differential_pressure1_timestamp # Last measurement timestamp +float32 differential_pressure1_filtered_pa # Low pass filtered airspeed sensor differential pressure reading diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg new file mode 100644 index 0000000000..6a38e2c9d7 --- /dev/null +++ b/msg/vehicle_global_position.msg @@ -0,0 +1,20 @@ +# Fused global position in WGS84. +# This struct contains global position estimation. It is not the raw GPS +# measurement (@see vehicle_gps_position). This topic is usually published by the position +# estimator, which will take more sources of information into account than just GPS, +# e.g. control inputs of the vehicle in a Kalman-filter implementation. +# +uint64 timestamp # Time of this estimate, in microseconds since system start +uint64 time_utc_usec # GPS UTC timestamp in microseconds +float64 lat # Latitude in degrees +float64 lon # Longitude in degrees +float32 alt # Altitude AMSL in meters +float32 vel_n # Ground north velocity, m/s +float32 vel_e # Ground east velocity, m/s +float32 vel_d # Ground downside velocity, m/s +float32 yaw # Yaw in radians -PI..+PI. +float32 eph # Standard deviation of position estimate horizontally +float32 epv # Standard deviation of position vertically +float32 terrain_alt # Terrain altitude in m, WGS84 +bool terrain_alt_valid # Terrain altitude estimate is valid +bool dead_reckoning # True if this position is estimated through dead-reckoning diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg new file mode 100644 index 0000000000..25c532ccaa --- /dev/null +++ b/msg/vehicle_gps_position.msg @@ -0,0 +1,29 @@ +# GPS position in WGS84 coordinates. +uint64 timestamp_position # Timestamp for position information +int32 lat # Latitude in 1E-7 degrees +int32 lon # Longitude in 1E-7 degrees +int32 alt # Altitude in 1E-3 meters (millimeters) above MSL + +uint64 timestamp_variance +float32 s_variance_m_s # speed accuracy estimate m/s +float32 c_variance_rad # course accuracy estimate rad +uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + +float32 eph # GPS HDOP horizontal dilution of position in m +float32 epv # GPS VDOP horizontal dilution of position in m + +int32 noise_per_ms # GPS noise per millisecond +int32 jamming_indicator # indicates jamming is occurring + +uint64 timestamp_velocity # Timestamp for velocity informations +float32 vel_m_s # GPS ground speed (m/s) +float32 vel_n_m_s # GPS ground speed in m/s +float32 vel_e_m_s # GPS ground speed in m/s +float32 vel_d_m_s # GPS ground speed in m/s +float32 cog_rad # Course over ground (NOT heading, but direction of movement) in rad, -PI..PI +bool vel_ned_valid # Flag to indicate if NED speed is valid + +uint64 timestamp_time # Timestamp for time information +uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 + +uint8 satellites_used # Number of satellites used diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index ab8f9eef62..19e0f7c632 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -560,8 +560,8 @@ CONFIG_USART1_2STOP=0 # # USART2 Configuration # -CONFIG_USART2_RXBUFSIZE=512 -CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=2200 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index cc91569cd9..bd72971b94 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -48,39 +48,14 @@ #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" #define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected -enum RANGE_FINDER_TYPE { - RANGE_FINDER_TYPE_LASER = 0, -}; +#define range_finder_report range_finder_s +#define __orb_sensor_range_finder __orb_range_finder -/** - * @addtogroup topics - * @{ - */ +#include -/** - * range finder report structure. Reads from the device must be in multiples of this - * structure. - */ -struct range_finder_report { - uint64_t timestamp; - uint64_t error_count; - unsigned type; /**< type, following RANGE_FINDER_TYPE enum */ - float distance; /**< in meters */ - float minimum_distance; /**< minimum distance the sensor can measure */ - float maximum_distance; /**< maximum distance the sensor can measure */ - uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ - float distance_vector[MB12XX_MAX_RANGEFINDERS]; /** in meters */ - uint8_t just_updated; /** number of the most recent measurement sensor */ -}; - -/** - * @} - */ - -/* - * ObjDev tag for raw range finder data. - */ -ORB_DECLARE(sensor_range_finder); +#ifndef RANGE_FINDER_TYPE_LASER +#define RANGE_FINDER_TYPE_LASER 0 +#endif /* * ioctl() definitions diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 5c9978f1f0..c45e73fd9a 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -260,32 +260,37 @@ SF0X::~SF0X() int SF0X::init() { - /* do regular cdev init */ - if (CDev::init() != OK) { - goto out; - } + /* status */ + int ret = 0; - /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + do { /* create a scope to handle exit conditions using break */ - if (_reports == nullptr) { - warnx("mem err"); - goto out; - } + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) break; - /* get a publish handle on the range finder topic */ - struct range_finder_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + if (_reports == nullptr) { + warnx("mem err"); + ret = -1; + break; + } - if (_range_finder_topic < 0) { - warnx("advert err"); - } + /* get a publish handle on the range finder topic */ + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + + if (_range_finder_topic < 0) { + warnx("advert err"); + } + } while(0); /* close the fd */ ::close(_fd); _fd = -1; -out: + return OK; } diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 13cbda9382..1906030abb 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -45,6 +45,7 @@ class __EXPORT ListNode public: ListNode() : _sibling(nullptr) { } + virtual ~ListNode() {}; void setSibling(T sibling) { _sibling = sibling; } T getSibling() { return _sibling; } T get() { @@ -52,6 +53,11 @@ public: } protected: T _sibling; +private: + // forbid copy + ListNode(const ListNode& other); + // forbid assignment + ListNode & operator = (const ListNode &); }; template @@ -60,12 +66,18 @@ class __EXPORT List public: List() : _head() { } + virtual ~List() {}; void add(T newNode) { newNode->setSibling(getHead()); setHead(newNode); } T getHead() { return _head; } -private: +protected: void setHead(T &head) { _head = head; } T _head; +private: + // forbid copy + List(const List& other); + // forbid assignment + List& operator = (const List &); }; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 1155631ea3..213de16562 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -570,6 +570,12 @@ int do_level_calibration(int mavlink_fd) { param_t roll_offset_handler = param_find("SENS_BOARD_X_OFF"); param_t pitch_offset_handler = param_find("SENS_BOARD_Y_OFF"); + // save old values if calibration fails + float roll_offset_current; + float pitch_offset_current; + param_get(roll_offset_handler, &roll_offset_current); + param_get(pitch_offset_handler, &pitch_offset_current); + float zero = 0.0f; param_set(roll_offset_handler, &zero); param_set(pitch_offset_handler, &zero); @@ -602,28 +608,34 @@ int do_level_calibration(int mavlink_fd) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); + bool success = false; if (counter > (cal_time * cal_hz / 2 )) { roll_mean /= counter; pitch_mean /= counter; } else { mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken"); - return 1; } if (fabsf(roll_mean) > 0.8f ) { mavlink_and_console_log_critical(mavlink_fd, "excess roll angle"); - return 1; } else if (fabsf(pitch_mean) > 0.8f ) { mavlink_and_console_log_critical(mavlink_fd, "excess pitch angle"); - return 1; - } - else { + } else { roll_mean *= (float)M_RAD_TO_DEG; pitch_mean *= (float)M_RAD_TO_DEG; param_set(roll_offset_handler, &roll_mean); param_set(pitch_offset_handler, &pitch_mean); + success = true; } - mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level"); - return 0; + if (success) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level"); + return 0; + } else { + // set old parameters + param_set(roll_offset_handler, &roll_offset_current); + param_set(pitch_offset_handler, &pitch_offset_current); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level"); + return 1; + } } diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index f3cd877287..083ab9498e 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -208,4 +208,9 @@ void SuperBlock::updateChildPublications() } } + } // namespace control + +template class List; +template class List; +template class List; diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index d2f9cdf07e..d169d35c5f 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -43,12 +43,9 @@ #include #include - -// forward declaration -namespace uORB { - class SubscriptionNode; - class PublicationNode; -} +#include +#include +#include namespace control { @@ -60,8 +57,8 @@ static const uint16_t maxPublicationsPerBlock = 100; static const uint8_t blockNameLengthMax = 80; // forward declaration -class BlockParamBase; class SuperBlock; +class BlockParamBase; /** */ @@ -137,4 +134,5 @@ protected: List _children; }; + } // namespace control diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index 8f98da74fa..532a037d40 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -43,6 +43,8 @@ #include "BlockParam.hpp" +#include + namespace control { diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 437e43bfb4..cab28c65fd 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -47,6 +47,8 @@ namespace control { +class Block; + /** * A base class for block params that enables traversing linked list. */ diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 454d0db19c..7c520219f1 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -38,6 +38,7 @@ */ #include "blocks.hpp" +#include namespace control { diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 491d4681de..e3c0811116 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -61,10 +61,6 @@ #include #include -extern "C" { -#include -} - #include "../blocks.hpp" #include #include diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 30d2a64164..76f36e694f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1454,6 +1454,9 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 50.0f); + configure_stream("HIGHRES_IMU", 50.0f); + configure_stream("VFR_HUD", 5.0f); + configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); configure_stream("CAMERA_CAPTURE", 2.0f); @@ -1462,6 +1465,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("RC_CHANNELS_RAW", 20.0f); configure_stream("VFR_HUD", 10.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cc00fc69b8..aae3e8d8cb 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -60,7 +60,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 7b592ab51a..4be73bc990 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -64,7 +64,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c3ac138ad5..acf1726984 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2070,35 +2070,25 @@ Sensors::task_main() { /* start individual sensors */ - int ret; - ret = accel_init(); + int ret = 0; + do { /* create a scope to handle exit with break */ + ret = accel_init(); + if (ret) break; + ret = gyro_init(); + if (ret) break; + ret = mag_init(); + if (ret) break; + ret = baro_init(); + if (ret) break; + ret = adc_init(); + if (ret) break; + break; + } while (0); if (ret) { - goto exit_immediate; - } - - ret = gyro_init(); - - if (ret) { - goto exit_immediate; - } - - ret = mag_init(); - - if (ret) { - goto exit_immediate; - } - - ret = baro_init(); - - if (ret) { - goto exit_immediate; - } - - ret = adc_init(); - - if (ret) { - goto exit_immediate; + _sensors_task = -1; + _exit(ret); + return; } /* @@ -2241,8 +2231,6 @@ Sensors::task_main() } warnx("exiting."); - -exit_immediate: _sensors_task = -1; px4_task_exit(ret); } diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index eb2d84726f..0ea8e5db51 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -69,6 +69,14 @@ void * Publication::getDataVoidPtr() { return (void *)(T *)(this); } + +PublicationNode::PublicationNode(const struct orb_metadata *meta, + List * list) : + PublicationBase(meta) { + if (list != nullptr) list->add(this); +} + + template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 8cbe51119e..9b4159cdfd 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -94,6 +94,11 @@ protected: // attributes const struct orb_metadata *_meta; orb_advert_t _handle; +private: + // forbid copy + PublicationBase(const PublicationBase&) : _meta(), _handle() {}; + // forbid assignment + PublicationBase& operator = (const PublicationBase &); }; /** @@ -120,10 +125,7 @@ public: * that this should be appended to. */ PublicationNode(const struct orb_metadata *meta, - List * list=nullptr) : - PublicationBase(meta) { - if (list != nullptr) list->add(this); - } + List * list=nullptr); /** * This function is the callback for list traversal @@ -136,7 +138,7 @@ public: * Publication wrapper class */ template -class Publication : +class __EXPORT Publication : public T, // this must be first! public PublicationNode { diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index db47218d9d..0c9433f036 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -53,6 +53,8 @@ #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" #include "topics/rc_channels.h" +#include "topics/vehicle_control_mode.h" +#include "topics/actuator_armed.h" namespace uORB { diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 12301ce96f..31842ed715 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -106,6 +106,11 @@ protected: // attributes const struct orb_metadata *_meta; int _handle; +private: + // forbid copy + SubscriptionBase(const SubscriptionBase& other); + // forbid assignment + SubscriptionBase& operator = (const SubscriptionBase &); }; /** diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h deleted file mode 100644 index 54d84231f0..0000000000 --- a/src/modules/uORB/topics/actuator_controls_effective.h +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 actuator_controls_effective.h - * - * Actuator control topics - mixer inputs. - * - * Values published to these topics are the outputs of the vehicle control - * system and mixing process; they are the control-scale values that are - * then fed to the actual actuator driver. - * - * Each topic can be published by a single controller - */ - -#ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H -#define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H - -//#include -//#include "../uORB.h" -//#include "actuator_controls.h" -// -//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS -//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ -// -///** -// * @addtogroup topics -// * @{ -// */ -// -//struct actuator_controls_effective_s { -// uint64_t timestamp; -// float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; -//}; -// -///** -// * @} -// */ -// -///* actuator control sets; this list can be expanded as more controllers emerge */ -//ORB_DECLARE(actuator_controls_effective_0); -//ORB_DECLARE(actuator_controls_effective_1); -//ORB_DECLARE(actuator_controls_effective_2); -//ORB_DECLARE(actuator_controls_effective_3); -// -///* control sets with pre-defined applications */ -//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) - -#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */ diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h deleted file mode 100644 index d3dc46ee0b..0000000000 --- a/src/modules/uORB/topics/optical_flow.h +++ /dev/null @@ -1,84 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 optical_flow.h - * Definition of the optical flow uORB topic. - */ - -#ifndef TOPIC_OPTICAL_FLOW_H_ -#define TOPIC_OPTICAL_FLOW_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - */ - -/** - * Optical flow in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - */ -struct optical_flow_s { - - uint64_t timestamp; /**< in microseconds since system start */ - uint8_t sensor_id; /**< id of the sensor emitting the flow value */ - float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */ - float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */ - float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */ - float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */ - float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint32_t integration_timespan; /** - * @author Julian Oes - * @author Lorenz Meier - */ - -#ifndef SENSOR_COMBINED_H_ -#define SENSOR_COMBINED_H_ - -#include -#include -#include "../uORB.h" - -enum MAGNETOMETER_MODE { - MAGNETOMETER_MODE_NORMAL = 0, - MAGNETOMETER_MODE_POSITIVE_BIAS, - MAGNETOMETER_MODE_NEGATIVE_BIAS -}; - -/** - * @addtogroup topics - * @{ - */ - -/** - * Sensor readings in raw and SI-unit form. - * - * These values are read from the sensors. Raw values are in sensor-specific units, - * the scaled values are in SI-units, as visible from the ending of the variable - * or the comments. The use of the SI fields is in general advised, as these fields - * are scaled and offset-compensated where possible and do not change with board - * revisions and sensor updates. - * - */ -struct sensor_combined_s { - - /* - * Actual data, this is specific to the type of data which is stored in this struct - * A line containing L0GME will be added by the Python logging code generator to the - * logged dataset. - */ - - /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ - - uint64_t timestamp; /**< Timestamp in microseconds since boot, from gyro */ - - int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ - float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ - unsigned gyro_errcount; /**< Error counter for gyro 0 */ - float gyro_temp; /**< Temperature of gyro 0 */ - - int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ - float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ - int accelerometer_mode; /**< Accelerometer measurement mode */ - float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ - uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */ - unsigned accelerometer_errcount; /**< Error counter for accel 0 */ - float accelerometer_temp; /**< Temperature of accel 0 */ - - int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ - float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ - int magnetometer_mode; /**< Magnetometer measurement mode */ - float magnetometer_range_ga; /**< ± measurement range in Gauss */ - float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ - unsigned magnetometer_errcount; /**< Error counter for mag 0 */ - float magnetometer_temp; /**< Temperature of mag 0 */ - - int16_t gyro1_raw[3]; /**< Raw sensor values of angular velocity */ - float gyro1_rad_s[3]; /**< Angular velocity in radian per seconds */ - uint64_t gyro1_timestamp; /**< Gyro timestamp */ - unsigned gyro1_errcount; /**< Error counter for gyro 1 */ - float gyro1_temp; /**< Temperature of gyro 1 */ - - int16_t accelerometer1_raw[3]; /**< Raw acceleration in NED body frame */ - float accelerometer1_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ - uint64_t accelerometer1_timestamp; /**< Accelerometer timestamp */ - unsigned accelerometer1_errcount; /**< Error counter for accel 1 */ - float accelerometer1_temp; /**< Temperature of accel 1 */ - - int16_t magnetometer1_raw[3]; /**< Raw magnetic field in NED body frame */ - float magnetometer1_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ - uint64_t magnetometer1_timestamp; /**< Magnetometer timestamp */ - unsigned magnetometer1_errcount; /**< Error counter for mag 1 */ - float magnetometer1_temp; /**< Temperature of mag 1 */ - - int16_t gyro2_raw[3]; /**< Raw sensor values of angular velocity */ - float gyro2_rad_s[3]; /**< Angular velocity in radian per seconds */ - uint64_t gyro2_timestamp; /**< Gyro timestamp */ - unsigned gyro2_errcount; /**< Error counter for gyro 1 */ - float gyro2_temp; /**< Temperature of gyro 1 */ - - int16_t accelerometer2_raw[3]; /**< Raw acceleration in NED body frame */ - float accelerometer2_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ - uint64_t accelerometer2_timestamp; /**< Accelerometer timestamp */ - unsigned accelerometer2_errcount; /**< Error counter for accel 2 */ - float accelerometer2_temp; /**< Temperature of accel 2 */ - - int16_t magnetometer2_raw[3]; /**< Raw magnetic field in NED body frame */ - float magnetometer2_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ - uint64_t magnetometer2_timestamp; /**< Magnetometer timestamp */ - unsigned magnetometer2_errcount; /**< Error counter for mag 2 */ - float magnetometer2_temp; /**< Temperature of mag 2 */ - - float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ - float baro_alt_meter; /**< Altitude, already temp. comp. */ - float baro_temp_celcius; /**< Temperature in degrees celsius */ - uint64_t baro_timestamp; /**< Barometer timestamp */ - - float baro1_pres_mbar; /**< Barometric pressure, already temp. comp. */ - float baro1_alt_meter; /**< Altitude, already temp. comp. */ - float baro1_temp_celcius; /**< Temperature in degrees celsius */ - uint64_t baro1_timestamp; /**< Barometer timestamp */ - - float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ - unsigned adc_mapping[10]; /**< Channel indices of each of these values */ - float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ - - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ - float differential_pressure_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ - - float differential_pressure1_pa; /**< Airspeed sensor differential pressure */ - uint64_t differential_pressure1_timestamp; /**< Last measurement timestamp */ - float differential_pressure1_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(sensor_combined); - -#endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h deleted file mode 100644 index 50c9429616..0000000000 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ /dev/null @@ -1,87 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 vehicle_global_position.h - * Definition of the global fused WGS84 position uORB topic. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - */ - -#ifndef VEHICLE_GLOBAL_POSITION_T_H_ -#define VEHICLE_GLOBAL_POSITION_T_H_ - -#include -#include -#include - -/** - * @addtogroup topics - * @{ - */ - - /** - * Fused global position in WGS84. - * - * This struct contains global position estimation. It is not the raw GPS - * measurement (@see vehicle_gps_position). This topic is usually published by the position - * estimator, which will take more sources of information into account than just GPS, - * e.g. control inputs of the vehicle in a Kalman-filter implementation. - */ -struct vehicle_global_position_s { - uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - uint64_t time_utc_usec; /**< GPS UTC timestamp in microseconds */ - double lat; /**< Latitude in degrees */ - double lon; /**< Longitude in degrees */ - float alt; /**< Altitude AMSL in meters */ - float vel_n; /**< Ground north velocity, m/s */ - float vel_e; /**< Ground east velocity, m/s */ - float vel_d; /**< Ground downside velocity, m/s */ - float yaw; /**< Yaw in radians -PI..+PI. */ - float eph; /**< Standard deviation of position estimate horizontally */ - float epv; /**< Standard deviation of position vertically */ - float terrain_alt; /**< Terrain altitude in m, WGS84 */ - bool terrain_alt_valid; /**< Terrain altitude estimate is valid */ - bool dead_reckoning; /**< True if this position is estimated through dead-reckoning*/ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_position); - -#endif diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h deleted file mode 100644 index 102914bbbb..0000000000 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ /dev/null @@ -1,94 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * - * 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 vehicle_gps_position.h - * Definition of the GPS WGS84 uORB topic. - */ - -#ifndef TOPIC_VEHICLE_GPS_H_ -#define TOPIC_VEHICLE_GPS_H_ - -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * GPS position in WGS84 coordinates. - */ -struct vehicle_gps_position_s { - uint64_t timestamp_position; /**< Timestamp for position information */ - int32_t lat; /**< Latitude in 1E-7 degrees */ - int32_t lon; /**< Longitude in 1E-7 degrees */ - int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */ - - uint64_t timestamp_variance; - float s_variance_m_s; /**< speed accuracy estimate m/s */ - float c_variance_rad; /**< course accuracy estimate rad */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - - float eph; /**< GPS HDOP horizontal dilution of position in m */ - float epv; /**< GPS VDOP horizontal dilution of position in m */ - - unsigned noise_per_ms; /**< */ - unsigned jamming_indicator; /**< */ - - uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ - float vel_m_s; /**< GPS ground speed (m/s) */ - float vel_n_m_s; /**< GPS ground speed in m/s */ - float vel_e_m_s; /**< GPS ground speed in m/s */ - float vel_d_m_s; /**< GPS ground speed in m/s */ - float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ - bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ - - uint64_t timestamp_time; /**< Timestamp for time information */ - uint64_t time_utc_usec; /**< Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */ - - uint8_t satellites_used; /**< Number of satellites used */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_gps_position); - -#endif diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index ea9d92025e..5c2de4e3ce 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -211,7 +211,7 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ if (ticks < USEC2TICK(_conversion_interval)) diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/platforms/qurt/px4_layer/commands_default.c similarity index 63% rename from src/modules/uORB/topics/filtered_bottom_flow.h rename to src/platforms/qurt/px4_layer/commands_default.c index c5d5455422..1c4c50f022 100644 --- a/src/modules/uORB/topics/filtered_bottom_flow.h +++ b/src/platforms/qurt/px4_layer/commands_default.c @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier + * Copyright (C) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,42 +30,39 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - /** - * @file filtered_bottom_flow.h - * Definition of the filtered bottom flow uORB topic. + * @file commands_default.c + * Commands to run for the "qurt_default" config + * + * @author Mark Charlebois */ -#ifndef TOPIC_FILTERED_BOTTOM_FLOW_H_ -#define TOPIC_FILTERED_BOTTOM_FLOW_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Filtered bottom flow in bodyframe. - */ -struct filtered_bottom_flow_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - - float sumx; /**< Integrated bodyframe x flow in meters */ - float sumy; /**< Integrated bodyframe y flow in meters */ - - float vx; /**< Flow bodyframe x speed, m/s */ - float vy; /**< Flow bodyframe y Speed, m/s */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(filtered_bottom_flow); - +const char *get_commands() +{ + static const char *commands = + "hello start\n" + "uorb start\n" + "simulator start -s\n" + "barosim start\n" + "adcsim start\n" + "accelsim start\n" + "gyrosim start\n" + "list_devices\n" + "list_topics\n" + "list_tasks\n" + "param show *\n" + "rgbled start\n" +#if 0 + "sensors start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "hil mode_pwm" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "mavlink start -d /tmp/ttyS0\n" + "commander start\n" #endif + ; + + return commands; +} diff --git a/src/modules/uORB/topics/home_position.h b/src/platforms/qurt/px4_layer/commands_hello.c similarity index 62% rename from src/modules/uORB/topics/home_position.h rename to src/platforms/qurt/px4_layer/commands_hello.c index 02e17cdd7f..b2ef09b457 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/platforms/qurt/px4_layer/commands_hello.c @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Julian Oes + * Copyright (C) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,46 +30,16 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - /** - * @file home_position.h - * Definition of the home position uORB topic. + * @file commands_hello.c + * Commands to run for the "qurt_hello" config * - * @author Lorenz Meier - * @author Julian Oes + * @author Mark Charlebois */ -#ifndef TOPIC_HOME_POSITION_H_ -#define TOPIC_HOME_POSITION_H_ +const char *get_commands() +{ + static const char *commands = "hello start"; -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * GPS home position in WGS84 coordinates. - */ -struct home_position_s { - uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - - double lat; /**< Latitude in degrees */ - double lon; /**< Longitude in degrees */ - float alt; /**< Altitude in meters (AMSL) */ - - float x; /**< X coordinate in meters */ - float y; /**< Y coordinate in meters */ - float z; /**< Z coordinate in meters */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(home_position); - -#endif + return commands; +} diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 3f42c1c301..538b64229f 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -50,30 +50,10 @@ using namespace std; extern void init_app_map(map &apps); extern void list_builtins(map &apps); -static const char *commands = -"hello start\n" -"uorb start\n" -"simulator start -s\n" -"barosim start\n" -"adcsim start\n" -"accelsim start\n" -"gyrosim start\n" -"list_devices\n" -"list_topics\n" -"list_tasks\n" -"param show *\n" -"rgbled start\n" -#if 0 -"hil mode_pwm" -"param set CAL_GYRO0_ID 2293760\n" -"param set CAL_ACC0_ID 1310720\n" -"param set CAL_ACC1_ID 1376256\n" -"param set CAL_MAG0_ID 196608\n" -"sensors start\n" -"mavlink start -d /tmp/ttyS0\n" -"commander start\n" -#endif -; +__BEGIN_DECLS +// The commands to run are specified in a target file: commands_.c +extern const char *get_commands(void); +__END_DECLS static void run_cmd(map &apps, const vector &appargs) { // command is appargs[0] @@ -158,7 +138,7 @@ int main(int argc, char **argv) init_app_map(apps); px4::init_once(); px4::init(argc, argv, "mainapp"); - process_commands(apps, commands); + process_commands(apps, get_commands()); for (;;) {} } diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 92f1c6abfb..714263edcb 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -52,6 +52,12 @@ SRCS = \ sq_remfirst.c \ sq_addafter.c \ dq_rem.c \ - main.cpp + main.cpp +ifeq ($(CONFIG),qurt_hello) +SRCS += commands_hello.c +endif +ifeq ($(CONFIG),qurt_default) +SRCS += commands_default.c +endif MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index 068e07c388..87035059aa 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -88,7 +88,7 @@ void printEigen(const Eigen::MatrixBase &b) for (int i = 0; i < b.rows(); ++i) { printf("("); - for (int j = 0; j < b.cols(); ++i) { + for (int j = 0; j < b.cols(); ++j) { if (j > 0) { printf(","); } @@ -129,10 +129,10 @@ int test_eigen(int argc, char *argv[]) TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]); TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f); TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1)); - TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1 + v1)); - TEST_OP_VERIFY("Vector2f - Vector2f", v - v1, v.isApprox(v1)); - TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1)); - TEST_OP_VERIFY("Vector2f -= Vector2f", v -= v1, v.isApprox(v1)); + VERIFY_OP("Vector2f + Vector2f", v = v + v1, v.isApprox(v1 + v1)); + VERIFY_OP("Vector2f - Vector2f", v = v - v1, v.isApprox(v1)); + VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1)); + VERIFY_OP("Vector2f -= Vector2f", v -= v1, v.isApprox(v1)); TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON); //TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array? } @@ -183,8 +183,8 @@ int test_eigen(int argc, char *argv[]) TEST_OP("Vector<4> = Vector<4>", v = v1); TEST_OP("Vector<4> + Vector<4>", v + v1); TEST_OP("Vector<4> - Vector<4>", v - v1); - //TEST_OP("Vector<4> += Vector<4>", v += v1); - //TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> += Vector<4>", v += v1); + TEST_OP("Vector<4> -= Vector<4>", v -= v1); TEST_OP("Vector<4> dot Vector<4>", v.dot(v1)); } @@ -225,8 +225,8 @@ int test_eigen(int argc, char *argv[]) // test nonsymmetric +, -, +=, -= const Eigen::Matrix m1_orig = - (Eigen::Matrix() << 1, 3, 3, - 4, 6, 6).finished(); + (Eigen::Matrix() << 1, 2, 3, + 4, 5, 6).finished(); Eigen::Matrix m1(m1_orig); @@ -276,6 +276,7 @@ int test_eigen(int argc, char *argv[]) } + /* QUATERNION TESTS CURRENTLY FAILING { // test conversion rotation matrix to quaternion and back Eigen::Matrix3f R_orig; @@ -295,7 +296,7 @@ int test_eigen(int argc, char *argv[]) for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { - if (fabsf(R_orig(i, j) - R(i, j)) > 0.00001f) { + if (fabsf(R_orig(i, j) - R(i, j)) > tol) { warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!"); rc = 1; } @@ -426,5 +427,6 @@ int test_eigen(int argc, char *argv[]) } } } + */ return rc; -} \ No newline at end of file +} diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 04b5efe230..546f3ceb57 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,7 +112,7 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif - {"eigen", test_eigen, OPT_NOALLTEST | OPT_NOJIGTEST}, + {"eigen", test_eigen, OPT_NOJIGTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 1c12d8fc7c4e9a737c79bde0e75b70c466a38b0f Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 21 May 2015 12:41:41 +0100 Subject: [PATCH 17/51] distance_sensor: added MAV_DISTANCE_SENSOR_INFRARED to topic stream --- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index aae3e8d8cb..58f5dc2362 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2240,9 +2240,9 @@ protected: msg.type = MAV_DISTANCE_SENSOR_LASER; break; - /*case MAV_DISTANCE_SENSOR_INFRARED: + case MAV_DISTANCE_SENSOR_INFRARED: msg.type = MAV_DISTANCE_SENSOR_INFRARED; - break;*/ //TODO: update mavlink def + break; default: msg.type = MAV_DISTANCE_SENSOR_LASER; From 29e7d5de9d32a52912113c19beffbf91ffc311e1 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Fri, 22 May 2015 19:53:52 +0100 Subject: [PATCH 18/51] distance_sensor: update msg template info --- msg/distance_sensor.msg | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg index ef11c586fc..11fc298edd 100644 --- a/msg/distance_sensor.msg +++ b/msg/distance_sensor.msg @@ -1,17 +1,17 @@ # DISTANCE_SENSOR message data -uint32 time_boot_ms +uint32 time_boot_ms # Time since system boot (us) -uint16 min_distance -uint16 max_distance -uint16 current_distance +uint16 min_distance # Minimum distance the sensor can measure (cm) +uint16 max_distance # Maximum distance the sensor can measure (cm) +uint16 current_distance # Current distance reading (cm) -uint8 type +uint8 type # Type from MAV_DISTANCE_SENSOR enum uint8 MAV_DISTANCE_SENSOR_LASER = 0 uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 -uint8 id +uint8 id # Onboard ID of the sensor -uint8 orientation -uint8 covariance \ No newline at end of file +uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum +uint8 covariance # Measurement covariance (cm), 0 for unknown / invalid readings \ No newline at end of file From 93f8d7c4e8f4ae1aabf5eb3b764a286cef18a2ac Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 15:03:32 +0100 Subject: [PATCH 19/51] distance_sensor: added distance_sensor to sdlog2 --- src/modules/sdlog2/sdlog2.c | 21 +++++++++++---------- src/modules/sdlog2/sdlog2_messages.h | 10 ++++++---- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3f1047742e..5ccadde57f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -61,8 +61,6 @@ #include #include -#include - #include #include #include @@ -92,6 +90,7 @@ #include #include #include +#include #include #include #include @@ -1084,7 +1083,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; struct telemetry_status_s telemetry; - struct range_finder_report range_finder; + struct distance_sensor_s distance_sensor; struct estimator_status_report estimator_status; struct tecs_status_s tecs_status; struct system_power_s system_power; @@ -1174,7 +1173,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_vel_sp_sub; int battery_sub; int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; - int range_finder_sub; + int distance_sensor_sub; int estimator_status_sub; int tecs_status_sub; int system_power_sub; @@ -1208,7 +1207,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = -1; subs.global_vel_sp_sub = -1; subs.battery_sub = -1; - subs.range_finder_sub = -1; + subs.distance_sensor_sub = -1; subs.estimator_status_sub = -1; subs.tecs_status_sub = -1; subs.system_power_sub = -1; @@ -1820,12 +1819,14 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- BOTTOM DISTANCE --- */ - if (copy_if_updated(ORB_ID(sensor_range_finder), &subs.range_finder_sub, &buf.range_finder)) { + /* --- DISTANCE SENSOR --- */ + if (copy_if_updated(ORB_ID(distance_sensor), &subs.distance_sensor_sub, &buf.distance_sensor)) { log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.range_finder.distance; - log_msg.body.log_DIST.bottom_rate = 0.0f; - log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); + log_msg.body.log_DIST.id = buf.distance_sensor.id; + log_msg.body.log_DIST.type = buf.distance_sensor.type; + log_msg.body.log_DIST.orientation = buf.distance_sensor.orientation; + log_msg.body.log_DIST.current_distance = buf.distance_sensor.current_distance; + log_msg.body.log_DIST.covariance = buf.distance_sensor.covariance; LOGBUFFER_WRITE_AND_COUNT(DIST); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index abdf518c51..eee44c43b9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -295,12 +295,14 @@ struct log_BATT_s { float discharged; }; -/* --- DIST - DISTANCE TO SURFACE --- */ +/* --- DIST - RANGE SENSOR DISTANCE --- */ #define LOG_DIST_MSG 21 struct log_DIST_s { - float bottom; - float bottom_rate; - uint8_t flags; + uint8_t id; + uint8_t type; + uint8_t orientation; + uint16_t current_distance; + uint8_t covariance; }; /* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ From 43668cc8c8f6413861277aeab3fa026564b5c4fa Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 17:49:52 +0100 Subject: [PATCH 20/51] distance_sensor: adopt message in both range drivers and on ekf_att_pos_estimator --- src/drivers/drv_range_finder.h | 12 +-- src/drivers/ll40ls/ll40ls.cpp | 55 ++++++------ src/drivers/mb12xx/mb12xx.cpp | 83 +++++-------------- .../AttitudePositionEstimatorEKF.h | 4 +- .../ekf_att_pos_estimator_main.cpp | 12 +-- src/modules/mavlink/mavlink_messages.cpp | 1 - src/modules/mavlink/mavlink_receiver.cpp | 52 ++++++++---- src/modules/mavlink/mavlink_receiver.h | 3 +- src/modules/uORB/objects_common.cpp | 3 - 9 files changed, 97 insertions(+), 128 deletions(-) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index bd72971b94..128e8fc0d2 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -48,14 +48,10 @@ #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" #define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected -#define range_finder_report range_finder_s -#define __orb_sensor_range_finder __orb_range_finder - -#include - -#ifndef RANGE_FINDER_TYPE_LASER -#define RANGE_FINDER_TYPE_LASER 0 -#endif +/* + * ObjDev tag for px4flow data. + */ +ORB_DECLARE(distance_sensor); /* * ioctl() definitions diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index b3368406a1..6a38baf875 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -68,6 +68,7 @@ #include #include +#include #include @@ -144,7 +145,7 @@ private: bool _collect_phase; int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -224,7 +225,7 @@ LL40LS::LL40LS(int bus, const char *path, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), @@ -278,7 +279,7 @@ LL40LS::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { goto out; @@ -288,12 +289,12 @@ LL40LS::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; + struct distance_sensor_s rf_report; measure(); _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { debug("failed to create sensor_range_finder object. Did you start uOrb?"); } } @@ -497,8 +498,8 @@ LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t LL40LS::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -664,8 +665,8 @@ LL40LS::collect() } uint16_t distance = (val[0] << 8) | val[1]; - float si_units = distance * 0.01f; /* cm to m */ - struct range_finder_report report; + float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ + struct distance_sensor_s report; if (distance == 0) { _zero_counter++; @@ -688,22 +689,18 @@ LL40LS::collect() _last_distance = distance; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { - report.valid = 1; - } - else { - report.valid = 0; - } + report.time_boot_ms = hrt_absolute_time(); + report.id = 0; + report.type = 1; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -958,7 +955,7 @@ void stop(int bus) void test(int bus) { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); @@ -977,8 +974,8 @@ test(int bus) } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.2f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -1006,8 +1003,8 @@ test(int bus) } warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.3f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to default rate */ diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 1a4ea17cf1..edbb4eefe2 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -70,6 +70,7 @@ #include #include +#include #include @@ -131,7 +132,7 @@ private: bool _collect_phase; int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -208,7 +209,7 @@ MB12XX::MB12XX(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")), @@ -255,7 +256,7 @@ MB12XX::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ @@ -268,11 +269,11 @@ MB12XX::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report = {}; + struct distance_sensor_s rf_report = {}; - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { log("failed to create sensor_range_finder object. Did you start uOrb?"); } } @@ -469,8 +470,8 @@ ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -571,51 +572,20 @@ MB12XX::collect() uint16_t distance = val[0] << 8 | val[1]; float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ - struct range_finder_report report; + struct distance_sensor_s report; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - - /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ - if (addr_ind.size() == 1) { - report.distance = si_units; - - for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) { - report.distance_vector[i] = 0; - } - - report.just_updated = 0; - - } else { - /* for multiple sonars connected */ - - /* don't use the orginial single sonar variable */ - report.distance = 0; - - /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */ - _latest_sonar_measurements[_cycle_counter] = si_units; - - for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { - report.distance_vector[i] = _latest_sonar_measurements[i]; - } - - /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */ - report.just_updated = _index_counter; - - /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */ - for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) { - report.distance_vector[addr_ind.size() + i] = 0; - } - } - - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.time_boot_ms = hrt_absolute_time(); + report.id = 0; + report.type = 1; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -840,7 +810,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -858,8 +828,7 @@ test() } warnx("single read"); - warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); - warnx("time: %lld", report.timestamp); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -887,13 +856,7 @@ test() } warnx("periodic read %u", i); - - /* Print the sonar rangefinder report sonar distance vector */ - for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) { - warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1); - } - - warnx("time: %lld", report.timestamp); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to default rate */ diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 4d5e56a961..ee6fcb3ac5 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -58,13 +58,13 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include @@ -162,7 +162,7 @@ private: struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ struct wind_estimate_s _wind; /**< wind estimate */ - struct range_finder_report _distance; /**< distance estimate */ + struct distance_sensor_s _distance; /**< distance estimate */ struct vehicle_land_detected_s _landDetector; struct actuator_armed_s _armed; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 1b6e17ac37..3c3e2d2f35 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -501,7 +501,7 @@ void AttitudePositionEstimatorEKF::task_main() /* * do subscriptions */ - _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + _distance_sub = orb_subscribe(ORB_ID(distance_sensor)); _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); @@ -1471,11 +1471,11 @@ void AttitudePositionEstimatorEKF::pollData() orb_check(_distance_sub, &_newRangeData); if (_newRangeData) { - orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); - - if (_distance.valid) { - _ekf->rngMea = _distance.distance; - _distance_last_valid = _distance.timestamp; + orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance); + if ((_distance.current_distance > _distance.min_distance) + && (_distance.current_distance < _distance.max_distance)) { + _ekf->rngMea = _distance.current_distance; + _distance_last_valid = _distance.time_boot_ms; } else { _newRangeData = false; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 58f5dc2362..650e9bf81a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -71,7 +71,6 @@ #include #include #include -#include #include #include diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ac7ab99c05..8588e4047f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -56,7 +56,6 @@ #include #include #include -#include #include #include #include @@ -109,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _battery_pub(-1), _cmd_pub(-1), _flow_pub(-1), - _range_pub(-1), + _distance_sensor_pub(-1), _offboard_control_mode_pub(-1), _actuator_controls_pub(-1), _global_vel_sp_pub(-1), @@ -134,8 +133,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _att_sp{}, _rates_sp{}, _time_offset_avg_alpha(0.6), - _time_offset(0), - _distance_sensor_pub(-1) + _time_offset(0) { } @@ -417,6 +415,25 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) } else { orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } + + /* Use distance value for distance sensor topic */ + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); + + d.time_boot_ms = hrt_absolute_time(); + d.min_distance = 3.0f; + d.max_distance = 50.0f; + d.current_distance = flow.distance; + d.type = 1; + d.id = 0; + d.orientation = 8; + d.covariance = 0.0; + + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + } else { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); + } } void @@ -449,22 +466,23 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } - /* Use distance value for range finder report */ - struct range_finder_report r; - memset(&r, 0, sizeof(r)); + /* Use distance value for distance sensor topic */ + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); - r.timestamp = hrt_absolute_time(); - r.error_count = 0; - r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.distance; - r.minimum_distance = 0.0f; - r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable - r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + d.time_boot_ms = hrt_absolute_time(); + d.min_distance = 3.0f; + d.max_distance = 50.0f; + d.current_distance = flow.distance; + d.type = 1; + d.id = 0; + d.orientation = 8; + d.covariance = 0.0; - if (_range_pub < 0) { - _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); } else { - orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4be73bc990..016305e796 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -166,7 +166,7 @@ private: orb_advert_t _battery_pub; orb_advert_t _cmd_pub; orb_advert_t _flow_pub; - orb_advert_t _range_pub; + orb_advert_t _distance_sensor_pub; orb_advert_t _offboard_control_mode_pub; orb_advert_t _actuator_controls_pub; orb_advert_t _global_vel_sp_pub; @@ -192,7 +192,6 @@ private: struct vehicle_rates_setpoint_s _rates_sp; double _time_offset_avg_alpha; uint64_t _time_offset; - orb_advert_t _distance_sensor_pub; /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver &); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b34f9a742d..f900543bc2 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -57,9 +57,6 @@ ORB_DEFINE(sensor_gyro, struct gyro_report); #include ORB_DEFINE(sensor_baro, struct baro_report); -#include -ORB_DEFINE(sensor_range_finder, struct range_finder_report); - #include ORB_DEFINE(output_pwm, struct pwm_output_values); From 7e740f262b07881d4abdc56caee8d1f523f8be99 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 17:54:52 +0100 Subject: [PATCH 21/51] distance_sensor: removed max number of rangefinders --- src/drivers/drv_range_finder.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 128e8fc0d2..1ddc07c3fb 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -46,8 +46,7 @@ #define RANGE_FINDER_BASE_DEVICE_PATH "/dev/range_finder" #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" -#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected - + /* * ObjDev tag for px4flow data. */ From 36dfda4aea3079f843f33134bfeb67836eebfdb5 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 17:58:20 +0100 Subject: [PATCH 22/51] distance_sensor: mavlink_receiver: changed to SI units --- src/modules/mavlink/mavlink_receiver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8588e4047f..25d577dd00 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -421,8 +421,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) memset(&d, 0, sizeof(d)); d.time_boot_ms = hrt_absolute_time(); - d.min_distance = 3.0f; - d.max_distance = 50.0f; + d.min_distance = 0.3f; + d.max_distance = 5.0f; d.current_distance = flow.distance; d.type = 1; d.id = 0; @@ -471,8 +471,8 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) memset(&d, 0, sizeof(d)); d.time_boot_ms = hrt_absolute_time(); - d.min_distance = 3.0f; - d.max_distance = 50.0f; + d.min_distance = 0.3f; + d.max_distance = 5.0f; d.current_distance = flow.distance; d.type = 1; d.id = 0; From c2c0cbe6829206b73b4b4917c1c1e02629ae8b4a Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 18:00:42 +0100 Subject: [PATCH 23/51] distance_sensor: msg def: changed to SI units --- msg/distance_sensor.msg | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg index 11fc298edd..8e44d76f79 100644 --- a/msg/distance_sensor.msg +++ b/msg/distance_sensor.msg @@ -2,11 +2,11 @@ uint32 time_boot_ms # Time since system boot (us) -uint16 min_distance # Minimum distance the sensor can measure (cm) -uint16 max_distance # Maximum distance the sensor can measure (cm) -uint16 current_distance # Current distance reading (cm) +uint16 min_distance # Minimum distance the sensor can measure (in SI) +uint16 max_distance # Maximum distance the sensor can measure (in SI) +uint16 current_distance # Current distance reading (in SI) -uint8 type # Type from MAV_DISTANCE_SENSOR enum +uint8 type # Type from MAV_DISTANCE_SENSOR enum uint8 MAV_DISTANCE_SENSOR_LASER = 0 uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 @@ -14,4 +14,4 @@ uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 uint8 id # Onboard ID of the sensor uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum -uint8 covariance # Measurement covariance (cm), 0 for unknown / invalid readings \ No newline at end of file +uint8 covariance # Measurement covariance (in SI), 0 for unknown / invalid readings \ No newline at end of file From 169eae8cf9353b1a552ac6f70762bf9720917d0a Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 20:47:14 +0100 Subject: [PATCH 24/51] distance_sensor: removed range_finder.msg def; readded max number of sonars --- msg/range_finder.msg | 13 ------------- 1 file changed, 13 deletions(-) delete mode 100644 msg/range_finder.msg diff --git a/msg/range_finder.msg b/msg/range_finder.msg deleted file mode 100644 index d2e67543e5..0000000000 --- a/msg/range_finder.msg +++ /dev/null @@ -1,13 +0,0 @@ -int16 RANGE_FINDER_TYPE_LASER = 0 - -# range finder report structure. Reads from the device must be in multiples of this -# structure. -uint64 timestamp -uint64 error_count -uint16 type # type, following RANGE_FINDER_TYPE enum -float32 distance # in meters -float32 minimum_distance # minimum distance the sensor can measure -float32 maximum_distance # maximum distance the sensor can measure -uint8 valid # 1 == within sensor range, 0 = outside sensor range -float32[12] distance_vector # in meters length should match MB12XX_MAX_RANGEFINDERS -uint8 just_updated # number of the most recent measurement sensor From 17bc0979e4830fd3bd387b7c6a8d09c2d57433f4 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 20:47:35 +0100 Subject: [PATCH 25/51] distance_sensor: re-add max number of sonars --- src/drivers/drv_range_finder.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 1ddc07c3fb..035eb92e0c 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -46,7 +46,8 @@ #define RANGE_FINDER_BASE_DEVICE_PATH "/dev/range_finder" #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" - +#define MB12XX_MAX_RANGEFINDERS 12 // Maximum number of Maxbotix sensors on bus + /* * ObjDev tag for px4flow data. */ From 98d02040878aaef7518f60173a0011692951a90c Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 21:06:19 +0100 Subject: [PATCH 26/51] distance_sensor: update drivers --- src/drivers/mb12xx/mb12xx.cpp | 2 +- src/drivers/sf0x/sf0x.cpp | 46 +++++++++++++++-------------- src/drivers/trone/trone.cpp | 54 +++++++++++++++++------------------ 3 files changed, 52 insertions(+), 50 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index edbb4eefe2..7e4a5e015e 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -575,7 +575,7 @@ MB12XX::collect() struct distance_sensor_s report; report.time_boot_ms = hrt_absolute_time(); - report.id = 0; + report.id = 1; report.type = 1; report.orientation = 8; report.current_distance = si_units; diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index c45e73fd9a..403ac89dc1 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -69,6 +69,7 @@ #include #include +#include #include @@ -128,7 +129,7 @@ private: enum SF0X_PARSE_STATE _parse_state; hrt_abstime _last_read; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; unsigned _consecutive_fail_count; @@ -194,7 +195,7 @@ SF0X::SF0X(const char *port) : _linebuf_index(0), _parse_state(SF0X_PARSE_STATE0_UNSYNC), _last_read(0), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), @@ -270,7 +271,7 @@ SF0X::init() if (ret != OK) break; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { warnx("mem err"); ret = -1; @@ -278,11 +279,11 @@ SF0X::init() } /* get a publish handle on the range finder topic */ - struct range_finder_report zero_report; + struct distance_sensor_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &zero_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { warnx("advert err"); } } while(0); @@ -442,8 +443,8 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t SF0X::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -571,18 +572,19 @@ SF0X::collect() debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); - struct range_finder_report report; + struct distance_sensor_s report; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); + report.time_boot_ms = hrt_absolute_time(); + report.id = 2; + report.type = 0; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); if (_reports->force(&report)) { perf_count(_buffer_overflows); @@ -815,7 +817,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); @@ -832,8 +834,8 @@ test() } warnx("single read"); - warnx("val: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("val: %0.2f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2 Hz rate */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -863,8 +865,8 @@ test() } warnx("read #%u", i); - warnx("val: %0.3f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("val: %0.3f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to the default rate */ diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index a3c8633725..9fde00cafa 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -68,6 +68,7 @@ #include #include +#include #include @@ -128,7 +129,7 @@ private: bool _collect_phase; int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -209,7 +210,7 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; -static uint8_t crc8(uint8_t *p, uint8_t len) { +/* static uint8_t crc8(uint8_t *p, uint8_t len) { uint16_t i; uint16_t crc = 0x0; @@ -219,7 +220,7 @@ static uint8_t crc8(uint8_t *p, uint8_t len) { } return crc & 0xFF; -} +}*/ /* * Driver 'main' command. @@ -235,7 +236,7 @@ TRONE::TRONE(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "trone_read")), _comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows")) @@ -281,7 +282,7 @@ TRONE::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { goto out; @@ -291,12 +292,12 @@ TRONE::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; + struct distance_sensor_s rf_report; measure(); _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { debug("failed to create sensor_range_finder object. Did you start uOrb?"); } } @@ -468,8 +469,8 @@ TRONE::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t TRONE::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -568,20 +569,20 @@ TRONE::collect() uint16_t distance = (val[0] << 8) | val[1]; float si_units = distance * 0.001f; /* mm to m */ - struct range_finder_report report; - - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + struct distance_sensor_s report; + report.time_boot_ms = hrt_absolute_time(); + report.id = 3; + report.type = 0; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -787,7 +788,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -805,8 +806,8 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.2f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -834,9 +835,8 @@ test() } warnx("periodic read %u", i); - warnx("valid %u", report.valid); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.3f", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to default rate */ From 3efaeabd5b3aa7c78789221412890e51bf646fba Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 09:42:34 +0100 Subject: [PATCH 27/51] pwm_input: astyle --- src/drivers/pwm_input/pwm_input.cpp | 205 ++++++++++++++++------------ 1 file changed, 117 insertions(+), 88 deletions(-) diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index c8d3308c14..be01e848db 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file pwm_input.cpp + * @file pwm_input.cpp * * PWM input driver based on earlier driver from Evan Slatyer, * which in turn was based on drv_hrt.c @@ -192,32 +192,32 @@ * Specific registers and bits used by HRT sub-functions */ #if PWMIN_TIMER_CHANNEL == 1 - #define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */ - #define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */ - #define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */ - #define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */ - #define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */ - #define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT)) - #define CCMR2_PWMIN 0 - #define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) - #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) - #define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT) - #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */ +#define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */ +#define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */ +#define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */ +#define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */ +#define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT)) +#define CCMR2_PWMIN 0 +#define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) +#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) +#define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT) +#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) #elif PWMIN_TIMER_CHANNEL == 2 - #define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */ - #define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */ - #define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */ - #define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */ - #define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */ - #define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */ - #define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT)) - #define CCMR2_PWMIN 0 - #define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) - #define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) - #define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT) - #define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) +#define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */ +#define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */ +#define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */ +#define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */ +#define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */ +#define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */ +#define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT)) +#define CCMR2_PWMIN 0 +#define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E) +#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF) +#define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT) +#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1) #else - #error PWMIN_TIMER_CHANNEL must be either 1 and 2. +#error PWMIN_TIMER_CHANNEL must be either 1 and 2. #endif #define TIMEOUT 300000 /* reset after no responce over this time in microseconds [0.3 secs] */ @@ -235,18 +235,18 @@ public: void _publish(uint16_t status, uint32_t period, uint32_t pulse_width); void _print_info(void); - void hard_reset(); + void hard_reset(); private: uint32_t error_count; uint32_t pulses_captured; uint32_t last_period; uint32_t last_width; - uint64_t last_poll_time; + uint64_t last_poll_time; RingBuffer *reports; bool timer_started; - range_finder_report data; + range_finder_report data; orb_advert_t range_finder_pub; hrt_call hard_reset_call; // HRT callout for note completion @@ -254,9 +254,9 @@ private: void timer_init(void); - void turn_on(); - void turn_off(); - void freeze_test(); + void turn_on(); + void turn_off(); + void freeze_test(); }; @@ -276,15 +276,16 @@ PWMIN::PWMIN() : last_width(0), reports(nullptr), timer_started(false), - range_finder_pub(-1) + range_finder_pub(-1) { memset(&data, 0, sizeof(data)); } PWMIN::~PWMIN() { - if (reports != nullptr) + if (reports != nullptr) { delete reports; + } } /* @@ -300,21 +301,22 @@ PWMIN::init() // activate the timer when requested to when the device is opened CDev::init(); - data.type = RANGE_FINDER_TYPE_LASER; - data.minimum_distance = 0.20f; - data.maximum_distance = 7.0f; + data.type = RANGE_FINDER_TYPE_LASER; + data.minimum_distance = 0.20f; + data.maximum_distance = 7.0f; - range_finder_pub = orb_advertise(ORB_ID(sensor_range_finder), &data); - fprintf(stderr, "[pwm_input] advertising %d\n" - ,range_finder_pub); + range_finder_pub = orb_advertise(ORB_ID(sensor_range_finder), &data); + fprintf(stderr, "[pwm_input] advertising %d\n" + , range_finder_pub); reports = new RingBuffer(2, sizeof(struct pwm_input_s)); + if (reports == nullptr) { return -ENOMEM; } - // Schedule freeze check to invoke periodically - hrt_call_every(&freeze_test_call, 0, TIMEOUT, reinterpret_cast(&PWMIN::freeze_test), this); + // Schedule freeze check to invoke periodically + hrt_call_every(&freeze_test_call, 0, TIMEOUT, reinterpret_cast(&PWMIN::freeze_test), this); return OK; } @@ -322,7 +324,7 @@ PWMIN::init() /* * Initialise the timer we are going to use. */ -void PWMIN::timer_init(void) +void PWMIN::timer_init(void) { // run with interrupts disabled in case the timer is already // setup. We don't want it firing while we are doing the setup @@ -333,7 +335,7 @@ void PWMIN::timer_init(void) irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr); /* Clear no bits, set timer enable bit.*/ - modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); + modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT); /* disable and configure the timer */ rCR1 = 0; @@ -351,10 +353,10 @@ void PWMIN::timer_init(void) // for simplicity scale by the clock in MHz. This gives us // readings in microseconds which is typically what is needed // for a PWM input driver - uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL; + uint32_t prescaler = PWMIN_TIMER_CLOCK / 1000000UL; - /* - * define the clock speed. We want the highest possible clock + /* + * define the clock speed. We want the highest possible clock * speed that avoids overflows. */ rPSC = prescaler - 1; @@ -380,13 +382,14 @@ void PWMIN::timer_init(void) void PWMIN::freeze_test() { - /* timeout is true if least read was away back */ - bool timeout = false; - timeout = (hrt_absolute_time() - last_poll_time > TIMEOUT) ? true : false; - if (timeout) { - fprintf(stderr, "[pwm_input] Lidar is down, reseting\n"); - hard_reset(); - } + /* timeout is true if least read was away back */ + bool timeout = false; + timeout = (hrt_absolute_time() - last_poll_time > TIMEOUT) ? true : false; + + if (timeout) { + fprintf(stderr, "[pwm_input] Lidar is down, reseting\n"); + hard_reset(); + } } void @@ -398,8 +401,8 @@ PWMIN::turn_off() { stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0); } void PWMIN::hard_reset() { - turn_off(); - hrt_call_after(&hard_reset_call, 9000, reinterpret_cast(&PWMIN::turn_on), this); + turn_off(); + hrt_call_after(&hard_reset_call, 9000, reinterpret_cast(&PWMIN::turn_on), this); } /* @@ -412,10 +415,13 @@ PWMIN::open(struct file *filp) if (g_dev == nullptr) { return -EIO; } - int ret = CDev::open(filp); + + int ret = CDev::open(filp); + if (ret == OK && !timer_started) { g_dev->timer_init(); } + return ret; } @@ -429,14 +435,17 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 500)) + if ((arg < 1) || (arg > 500)) { return -EINVAL; + } irqstate_t flags = irqsave(); + if (!reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } + irqrestore(flags); return OK; @@ -471,8 +480,9 @@ PWMIN::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } while (count--) { if (reports->get(buf)) { @@ -496,7 +506,7 @@ void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width) return; } - last_poll_time = hrt_absolute_time(); + last_poll_time = hrt_absolute_time(); struct pwm_input_s pwmin_report; pwmin_report.timestamp = last_poll_time; @@ -504,18 +514,20 @@ void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width) pwmin_report.period = period; pwmin_report.pulse_width = pulse_width; - data.distance = pulse_width * 1e-3f; - data.timestamp = pwmin_report.timestamp; - data.error_count = error_count; + data.distance = pulse_width * 1e-3f; + data.timestamp = pwmin_report.timestamp; + data.error_count = error_count; - if (data.distance < data.minimum_distance || data.distance > data.maximum_distance) { - data.valid = false; - } else { - data.valid = true; - } - if (range_finder_pub > 0) { - orb_publish(ORB_ID(sensor_range_finder), range_finder_pub, &data); - } + if (data.distance < data.minimum_distance || data.distance > data.maximum_distance) { + data.valid = false; + + } else { + data.valid = true; + } + + if (range_finder_pub > 0) { + orb_publish(ORB_ID(sensor_range_finder), range_finder_pub, &data); + } reports->force(&pwmin_report); @@ -531,6 +543,7 @@ void PWMIN::_print_info(void) { if (!timer_started) { printf("timer not started - try the 'test' command\n"); + } else { printf("count=%u period=%u width=%u\n", (unsigned)pulses_captured, @@ -568,20 +581,27 @@ static void pwmin_start(bool full_start) printf("driver already started\n"); exit(1); } + g_dev = new PWMIN(); + if (g_dev == nullptr) { errx(1, "driver allocation failed"); } + if (g_dev->init() != OK) { errx(1, "driver init failed"); } - if (full_start) { - int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); - if (fd == -1) { - errx(1, "Failed to open device"); - } - close(fd); - } + + if (full_start) { + int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + + if (fd == -1) { + errx(1, "Failed to open device"); + } + + close(fd); + } + exit(0); } @@ -591,15 +611,18 @@ static void pwmin_start(bool full_start) static void pwmin_test(void) { int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { errx(1, "Failed to open device"); } + uint64_t start_time = hrt_absolute_time(); printf("Showing samples for 5 seconds\n"); - while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) { + while (hrt_absolute_time() < start_time + 5U * 1000UL * 1000UL) { struct pwm_input_s buf; + if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) { printf("period=%u width=%u error_count=%u\n", (unsigned)buf.period, @@ -607,6 +630,7 @@ static void pwmin_test(void) (unsigned)buf.error_count); } } + close(fd); exit(0); } @@ -616,14 +640,17 @@ static void pwmin_test(void) */ static void pwmin_reset(void) { - g_dev->hard_reset(); + g_dev->hard_reset(); int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); + if (fd == -1) { errx(1, "Failed to open device"); } + if (ioctl(fd, SENSORIOCRESET, 0) != OK) { errx(1, "reset failed"); } + close(fd); exit(0); } @@ -637,6 +664,7 @@ static void pwmin_info(void) printf("driver not started\n"); exit(1); } + g_dev->_print_info(); exit(0); } @@ -645,22 +673,23 @@ static void pwmin_info(void) /* driver entry point */ -int pwm_input_main(int argc, char * argv[]) +int pwm_input_main(int argc, char *argv[]) { const char *verb = argv[1]; - /* - * init driver and start reading - */ - bool full_start = false; - if (!strcmp(argv[2], "full")) { - full_start = true; - } + /* + * init driver and start reading + */ + bool full_start = false; + + if (!strcmp(argv[2], "full")) { + full_start = true; + } /* * Start/load the driver. */ if (!strcmp(verb, "start")) { - pwmin_start(full_start); + pwmin_start(full_start); } /* From e67f681935ba5bd65eb425e4e4fba7ec9dfea79b Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 09:44:10 +0100 Subject: [PATCH 28/51] ll40ls: astyle --- src/drivers/ll40ls/LidarLite.cpp | 156 +++--- src/drivers/ll40ls/LidarLite.h | 66 +-- src/drivers/ll40ls/LidarLiteI2C.cpp | 734 ++++++++++++++-------------- src/drivers/ll40ls/LidarLiteI2C.h | 138 +++--- src/drivers/ll40ls/LidarLitePWM.cpp | 103 ++-- src/drivers/ll40ls/LidarLitePWM.h | 44 +- src/drivers/ll40ls/ll40ls.cpp | 54 +- 7 files changed, 667 insertions(+), 628 deletions(-) diff --git a/src/drivers/ll40ls/LidarLite.cpp b/src/drivers/ll40ls/LidarLite.cpp index 33dce6192c..ebe054fc35 100644 --- a/src/drivers/ll40ls/LidarLite.cpp +++ b/src/drivers/ll40ls/LidarLite.cpp @@ -43,130 +43,130 @@ #include LidarLite::LidarLite() : - _min_distance(LL40LS_MIN_DISTANCE), - _max_distance(LL40LS_MAX_DISTANCE), - _measure_ticks(0) + _min_distance(LL40LS_MIN_DISTANCE), + _max_distance(LL40LS_MAX_DISTANCE), + _measure_ticks(0) { - //ctor + //ctor } LidarLite::~LidarLite() { - //dtor + //dtor } void LidarLite::set_minimum_distance(const float min) { - _min_distance = min; + _min_distance = min; } void LidarLite::set_maximum_distance(const float max) { - _max_distance = max; + _max_distance = max; } float LidarLite::get_minimum_distance() const { - return _min_distance; + return _min_distance; } float LidarLite::get_maximum_distance() const { - return _max_distance; + return _max_distance; } -uint32_t LidarLite::getMeasureTicks() const +uint32_t LidarLite::getMeasureTicks() const { - return _measure_ticks; + return _measure_ticks; } int LidarLite::ioctl(struct file *filp, int cmd, unsigned long arg) { - switch (cmd) { + switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { + case SENSORIOCSPOLLRATE: { + switch (arg) { - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ - case 0: - return -EINVAL; + /* zero would be bad */ + case 0: + return -EINVAL; - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + 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(LL40LS_CONVERSION_INTERVAL); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL); - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; - } + return OK; + } - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* 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 */ - unsigned ticks = USEC2TICK(1000000 / arg); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { - return -EINVAL; - } + /* check against maximum rate */ + if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + return -EINVAL; + } - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; - } - } - } + return OK; + } + } + } - case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) { - return SENSOR_POLLRATE_MANUAL; - } + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } - return (1000 / _measure_ticks); + return (1000 / _measure_ticks); - case SENSORIOCRESET: - reset_sensor(); - return OK; + case SENSORIOCRESET: + reset_sensor(); + return OK; - case RANGEFINDERIOCSETMINIUMDISTANCE: { - set_minimum_distance(*(float *)arg); - return OK; - } - break; + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return OK; + } + break; - case RANGEFINDERIOCSETMAXIUMDISTANCE: { - set_maximum_distance(*(float *)arg); - return OK; - } - break; + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return OK; + } + break; - default: - return -EINVAL; - } + default: + return -EINVAL; + } } diff --git a/src/drivers/ll40ls/LidarLite.h b/src/drivers/ll40ls/LidarLite.h index 1ea595909d..72bb22f263 100644 --- a/src/drivers/ll40ls/LidarLite.h +++ b/src/drivers/ll40ls/LidarLite.h @@ -38,7 +38,7 @@ * * Generic interface driver for the PulsedLight Lidar-Lite range finders. */ - #pragma once +#pragma once #include #include @@ -56,50 +56,50 @@ class LidarLite { public: - LidarLite(); + LidarLite(); - virtual ~LidarLite(); + virtual ~LidarLite(); - virtual int init() = 0; + virtual int init() = 0; - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - virtual void start() = 0; + virtual void start() = 0; - virtual void stop() = 0; + virtual void stop() = 0; - /** - * @brief - * Diagnostics - print some basic information about the driver. - */ - virtual void print_info() = 0; + /** + * @brief + * Diagnostics - print some basic information about the driver. + */ + virtual void print_info() = 0; - /** - * @brief - * print registers to console - */ - virtual void print_registers() = 0; + /** + * @brief + * print registers to console + */ + virtual void print_registers() = 0; protected: - /** - * Set the min and max distance thresholds if you want the end points of the sensors - * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE - * and LL40LS_MAX_DISTANCE - */ - void set_minimum_distance(const float min); - void set_maximum_distance(const float max); - float get_minimum_distance() const; - float get_maximum_distance() const; + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE + * and LL40LS_MAX_DISTANCE + */ + void set_minimum_distance(const float min); + void set_maximum_distance(const float max); + float get_minimum_distance() const; + float get_maximum_distance() const; - uint32_t getMeasureTicks() const; + uint32_t getMeasureTicks() const; - virtual int measure() = 0; - virtual int collect() = 0; + virtual int measure() = 0; + virtual int collect() = 0; - virtual int reset_sensor() = 0; + virtual int reset_sensor() = 0; private: - float _min_distance; - float _max_distance; - uint32_t _measure_ticks; + float _min_distance; + float _max_distance; + uint32_t _measure_ticks; }; diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 809b5c39bb..2ed07ae0b2 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -56,273 +56,274 @@ static const int ERROR = -1; LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : - I2C("LL40LS", path, bus, address, 100000), - _work(), - _reports(nullptr), - _sensor_ok(false), - _collect_phase(false), - _class_instance(-1), - _range_finder_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), - _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), - _sensor_resets(perf_alloc(PC_COUNT, "ll40ls_resets")), - _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_zero_resets")), - _last_distance(0), - _zero_counter(0), - _acquire_time_usec(0), - _pause_measurements(false), - _bus(bus) + I2C("LL40LS", path, bus, address, 100000), + _work(), + _reports(nullptr), + _sensor_ok(false), + _collect_phase(false), + _class_instance(-1), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), + _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), + _sensor_resets(perf_alloc(PC_COUNT, "ll40ls_resets")), + _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_zero_resets")), + _last_distance(0), + _zero_counter(0), + _acquire_time_usec(0), + _pause_measurements(false), + _bus(bus) { - // up the retries since the device misses the first measure attempts - _retries = 3; + // up the retries since the device misses the first measure attempts + _retries = 3; - // enable debug() calls - _debug_enabled = false; + // enable debug() calls + _debug_enabled = false; - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); } LidarLiteI2C::~LidarLiteI2C() { - /* make sure we are truly inactive */ - stop(); + /* make sure we are truly inactive */ + stop(); - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } - if (_class_instance != -1) { - unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); - } + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); + } - // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); - perf_free(_buffer_overflows); - perf_free(_sensor_resets); - perf_free(_sensor_zero_resets); + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + perf_free(_sensor_resets); + perf_free(_sensor_zero_resets); } int LidarLiteI2C::init() { - int ret = ERROR; + int ret = ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) { - goto out; - } + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } - /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); - if (_reports == nullptr) { - goto out; - } + if (_reports == nullptr) { + goto out; + } - _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; - measure(); - _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); - } - } + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; out: - return ret; + return ret; } int LidarLiteI2C::read_reg(uint8_t reg, uint8_t &val) { - return transfer(®, 1, &val, 1); + return transfer(®, 1, &val, 1); } int LidarLiteI2C::probe() { - // cope with both old and new I2C bus address - const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; + // cope with both old and new I2C bus address + const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD}; - // more retries for detection - _retries = 10; + // more retries for detection + _retries = 10; - for (uint8_t i=0; i 100)) { - return -EINVAL; - } + switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - irqstate_t flags = irqsave(); + irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { - irqrestore(flags); - return -ENOMEM; - } + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqrestore(flags); + irqrestore(flags); - return OK; - } + return OK; + } - case SENSORIOCGQUEUEDEPTH: - return _reports->size(); + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); - default: - { - int result = LidarLite::ioctl(filp, cmd, arg); + default: { + int result = LidarLite::ioctl(filp, cmd, arg); - if(result == -EINVAL) { - result = I2C::ioctl(filp, cmd, arg); - } + if (result == -EINVAL) { + result = I2C::ioctl(filp, cmd, arg); + } - return result; - } - } + return result; + } + } } ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); - int ret = 0; + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } - /* if automatic measurement is enabled */ - if (getMeasureTicks() > 0) { + /* if automatic measurement is enabled */ + if (getMeasureTicks() > 0) { - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(rbuf)) { - ret += sizeof(*rbuf); - rbuf++; - } - } + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } - /* manual measurement - run one conversion */ - do { - _reports->flush(); + /* manual measurement - run one conversion */ + do { + _reports->flush(); - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } - /* wait for it to complete */ - usleep(LL40LS_CONVERSION_INTERVAL); + /* wait for it to complete */ + usleep(LL40LS_CONVERSION_INTERVAL); - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } - /* state machine will have generated a report, copy it out */ - if (_reports->get(rbuf)) { - ret = sizeof(*rbuf); - } + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } - } while (0); + } while (0); - return ret; + return ret; } int LidarLiteI2C::measure() { - int ret; + int ret; - if (_pause_measurements) { - // we are in print_registers() and need to avoid - // acquisition to keep the I2C peripheral on the - // sensor active - return OK; - } + if (_pause_measurements) { + // we are in print_registers() and need to avoid + // acquisition to keep the I2C peripheral on the + // sensor active + return OK; + } - /* - * Send the command to begin a measurement. - */ - const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; - ret = transfer(cmd, sizeof(cmd), nullptr, 0); + /* + * Send the command to begin a measurement. + */ + const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; + ret = transfer(cmd, sizeof(cmd), nullptr, 0); - if (OK != ret) { - perf_count(_comms_errors); - debug("i2c::transfer returned %d", ret); - // if we are getting lots of I2C transfer errors try - // resetting the sensor - if (perf_event_count(_comms_errors) % 10 == 0) { - perf_count(_sensor_resets); - reset_sensor(); - } - return ret; - } + if (OK != ret) { + perf_count(_comms_errors); + debug("i2c::transfer returned %d", ret); - // remember when we sent the acquire so we can know when the - // acquisition has timed out - _acquire_time_usec = hrt_absolute_time(); - ret = OK; + // if we are getting lots of I2C transfer errors try + // resetting the sensor + if (perf_event_count(_comms_errors) % 10 == 0) { + perf_count(_sensor_resets); + reset_sensor(); + } - return ret; + return ret; + } + + // remember when we sent the acquire so we can know when the + // acquisition has timed out + _acquire_time_usec = hrt_absolute_time(); + ret = OK; + + return ret; } /* @@ -330,9 +331,9 @@ int LidarLiteI2C::measure() */ int LidarLiteI2C::reset_sensor() { - const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET }; - int ret = transfer(cmd, sizeof(cmd), nullptr, 0); - return ret; + const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET }; + int ret = transfer(cmd, sizeof(cmd), nullptr, 0); + return ret; } /* @@ -340,216 +341,229 @@ int LidarLiteI2C::reset_sensor() */ void LidarLiteI2C::print_registers() { - _pause_measurements = true; - printf("ll40ls registers\n"); - // wait for a while to ensure the lidar is in a ready state - usleep(50000); - for (uint8_t reg=0; reg<=0x67; reg++) { - uint8_t val = 0; - int ret = transfer(®, 1, &val, 1); - if (ret != OK) { - printf("%02x:XX ",(unsigned)reg); - } else { - printf("%02x:%02x ",(unsigned)reg, (unsigned)val); - } - if (reg % 16 == 15) { - printf("\n"); - } - } - printf("\n"); - _pause_measurements = false; + _pause_measurements = true; + printf("ll40ls registers\n"); + // wait for a while to ensure the lidar is in a ready state + usleep(50000); + + for (uint8_t reg = 0; reg <= 0x67; reg++) { + uint8_t val = 0; + int ret = transfer(®, 1, &val, 1); + + if (ret != OK) { + printf("%02x:XX ", (unsigned)reg); + + } else { + printf("%02x:%02x ", (unsigned)reg, (unsigned)val); + } + + if (reg % 16 == 15) { + printf("\n"); + } + } + + printf("\n"); + _pause_measurements = false; } int LidarLiteI2C::collect() { - int ret = -EIO; + int ret = -EIO; - /* read from the sensor */ - uint8_t val[2] = {0, 0}; + /* read from the sensor */ + uint8_t val[2] = {0, 0}; - perf_begin(_sample_perf); + perf_begin(_sample_perf); - // read the high and low byte distance registers - uint8_t distance_reg = LL40LS_DISTHIGH_REG; - ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); + // read the high and low byte distance registers + uint8_t distance_reg = LL40LS_DISTHIGH_REG; + ret = transfer(&distance_reg, 1, &val[0], sizeof(val)); - if (ret < 0) { - if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { - /* - NACKs from the sensor are expected when we - read before it is ready, so only consider it - an error if more than 100ms has elapsed. - */ - debug("error reading from sensor: %d", ret); - perf_count(_comms_errors); - if (perf_event_count(_comms_errors) % 10 == 0) { - perf_count(_sensor_resets); - reset_sensor(); - } - } - perf_end(_sample_perf); - // if we are getting lots of I2C transfer errors try - // resetting the sensor - return ret; - } + if (ret < 0) { + if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { + /* + NACKs from the sensor are expected when we + read before it is ready, so only consider it + an error if more than 100ms has elapsed. + */ + debug("error reading from sensor: %d", ret); + perf_count(_comms_errors); - uint16_t distance = (val[0] << 8) | val[1]; - float si_units = distance * 0.01f; /* cm to m */ - struct range_finder_report report; + if (perf_event_count(_comms_errors) % 10 == 0) { + perf_count(_sensor_resets); + reset_sensor(); + } + } - if (distance == 0) { - _zero_counter++; - if (_zero_counter == 20) { - /* we have had 20 zeros in a row - reset the - sensor. This is a known bad state of the - sensor where it returns 16 bits of zero for - the distance with a trailing NACK, and - keeps doing that even when the target comes - into a valid range. - */ - _zero_counter = 0; - perf_end(_sample_perf); - perf_count(_sensor_zero_resets); - return reset_sensor(); - } - } else { - _zero_counter = 0; - } + perf_end(_sample_perf); + // if we are getting lots of I2C transfer errors try + // resetting the sensor + return ret; + } - _last_distance = distance; + uint16_t distance = (val[0] << 8) | val[1]; + float si_units = distance * 0.01f; /* cm to m */ + struct range_finder_report report; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { - report.valid = 1; - } - else { - report.valid = 0; - } + if (distance == 0) { + _zero_counter++; - /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); - } + if (_zero_counter == 20) { + /* we have had 20 zeros in a row - reset the + sensor. This is a known bad state of the + sensor where it returns 16 bits of zero for + the distance with a trailing NACK, and + keeps doing that even when the target comes + into a valid range. + */ + _zero_counter = 0; + perf_end(_sample_perf); + perf_count(_sensor_zero_resets); + return reset_sensor(); + } - if (_reports->force(&report)) { - perf_count(_buffer_overflows); - } + } else { + _zero_counter = 0; + } - /* notify anyone waiting for data */ - poll_notify(POLLIN); + _last_distance = distance; - ret = OK; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); - perf_end(_sample_perf); - return ret; + if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { + report.valid = 1; + + } else { + report.valid = 0; + } + + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; } void LidarLiteI2C::start() { - /* reset the report ring and state machine */ - _collect_phase = false; - _reports->flush(); + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&LidarLiteI2C::cycle_trampoline, this, 1); + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&LidarLiteI2C::cycle_trampoline, this, 1); - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_RANGEFINDER - }; - static orb_advert_t pub = -1; + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } } void LidarLiteI2C::stop() { - work_cancel(HPWORK, &_work); + work_cancel(HPWORK, &_work); } void LidarLiteI2C::cycle_trampoline(void *arg) { - LidarLiteI2C *dev = (LidarLiteI2C *)arg; + LidarLiteI2C *dev = (LidarLiteI2C *)arg; - dev->cycle(); + dev->cycle(); } void LidarLiteI2C::cycle() { - /* collection phase? */ - if (_collect_phase) { + /* collection phase? */ + if (_collect_phase) { - /* try a collection */ - if (OK != collect()) { - debug("collection error"); - /* if we've been waiting more than 200ms then - send a new acquire */ - if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT*2) { - _collect_phase = false; - } - } else { - /* next phase is measurement */ - _collect_phase = false; + /* try a collection */ + if (OK != collect()) { + debug("collection error"); - /* - * Is there a collect->measure gap? - */ - if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&LidarLiteI2C::cycle_trampoline, - this, - getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); - - return; - } - } - } + /* if we've been waiting more than 200ms then + send a new acquire */ + if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT * 2) { + _collect_phase = false; + } - if (_collect_phase == false) { - /* measurement phase */ - if (OK != measure()) { - debug("measure error"); - } else { - /* next phase is collection. Don't switch to - collection phase until we have a successful - acquire request I2C transfer */ - _collect_phase = true; - } - } + } else { + /* next phase is measurement */ + _collect_phase = false; - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&LidarLiteI2C::cycle_trampoline, - this, - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + /* + * Is there a collect->measure gap? + */ + if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&LidarLiteI2C::cycle_trampoline, + this, + getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL)); + + return; + } + } + } + + if (_collect_phase == false) { + /* measurement phase */ + if (OK != measure()) { + debug("measure error"); + + } else { + /* next phase is collection. Don't switch to + collection phase until we have a successful + acquire request I2C transfer */ + _collect_phase = true; + } + } + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&LidarLiteI2C::cycle_trampoline, + this, + USEC2TICK(LL40LS_CONVERSION_INTERVAL)); } void LidarLiteI2C::print_info() { - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - perf_print_counter(_sensor_resets); - perf_print_counter(_sensor_zero_resets); - printf("poll interval: %u ticks\n", getMeasureTicks()); - _reports->print_info("report queue"); - printf("distance: %ucm (0x%04x)\n", - (unsigned)_last_distance, (unsigned)_last_distance); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + perf_print_counter(_sensor_resets); + perf_print_counter(_sensor_zero_resets); + printf("poll interval: %u ticks\n", getMeasureTicks()); + _reports->print_info("report queue"); + printf("distance: %ucm (0x%04x)\n", + (unsigned)_last_distance, (unsigned)_last_distance); } diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index f889969e84..0784b1441a 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -73,91 +73,91 @@ class RingBuffer; class LidarLiteI2C : public LidarLite, public device::I2C { public: - LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR); - virtual ~LidarLiteI2C(); + LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR); + virtual ~LidarLiteI2C(); - virtual int init() override; + virtual int init() override; - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg) override; + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg) override; - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info() override; + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info() override; - /** - * print registers to console - */ - void print_registers() override; + /** + * print registers to console + */ + void print_registers() override; protected: - virtual int probe(); - virtual int read_reg(uint8_t reg, uint8_t &val); + virtual int probe(); + virtual int read_reg(uint8_t reg, uint8_t &val); - int measure() override; - int reset_sensor() override; + int measure() override; + int reset_sensor() override; private: - work_s _work; - RingBuffer *_reports; - bool _sensor_ok; - bool _collect_phase; - int _class_instance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + bool _collect_phase; + int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _range_finder_topic; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - perf_counter_t _sensor_resets; - perf_counter_t _sensor_zero_resets; - uint16_t _last_distance; - uint16_t _zero_counter; - uint64_t _acquire_time_usec; - volatile bool _pause_measurements; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + perf_counter_t _sensor_resets; + perf_counter_t _sensor_zero_resets; + uint16_t _last_distance; + uint16_t _zero_counter; + uint64_t _acquire_time_usec; + volatile bool _pause_measurements; - /**< the bus the device is connected to */ - int _bus; + /**< the bus the device is connected to */ + int _bus; - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); - /** - * 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(); + /** + * 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(); + /** + * 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 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); + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + 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); private: - LidarLiteI2C(const LidarLiteI2C ©) = delete; - LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete; + LidarLiteI2C(const LidarLiteI2C ©) = delete; + LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete; }; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 50ee4880b9..7a52ab274a 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -48,40 +48,40 @@ static const int ERROR = -1; #endif LidarLitePWM::LidarLitePWM() : - _terminateRequested(false), - _pwmSub(-1), - _pwm{}, - _rangePub(-1), - _range{} + _terminateRequested(false), + _pwmSub(-1), + _pwm{}, + _rangePub(-1), + _range{} { } int LidarLitePWM::init() { - _pwmSub = orb_subscribe(ORB_ID(pwm_input)); + _pwmSub = orb_subscribe(ORB_ID(pwm_input)); - if(_pwmSub == -1) { - return ERROR; - } + if (_pwmSub == -1) { + return ERROR; + } - _range.type = RANGE_FINDER_TYPE_LASER; - _range.valid = false; - _rangePub = orb_advertise(ORB_ID(sensor_range_finder), &_range); + _range.type = RANGE_FINDER_TYPE_LASER; + _range.valid = false; + _rangePub = orb_advertise(ORB_ID(sensor_range_finder), &_range); - return OK; + return OK; } void LidarLitePWM::print_info() { - printf("poll interval: %u ticks\n", getMeasureTicks()); - printf("distance: %ucm (0x%04x)\n", - (unsigned)_range.distance, (unsigned)_range.distance); + printf("poll interval: %u ticks\n", getMeasureTicks()); + printf("distance: %ucm (0x%04x)\n", + (unsigned)_range.distance, (unsigned)_range.distance); } void LidarLitePWM::print_registers() { - printf("Not supported in PWM mode\n"); + printf("Not supported in PWM mode\n"); } void LidarLitePWM::start() @@ -91,52 +91,53 @@ void LidarLitePWM::start() void LidarLitePWM::stop() { - //TODO: stop measurement task - _terminateRequested = true; + //TODO: stop measurement task + _terminateRequested = true; } int LidarLitePWM::measure() { - int result = OK; + int result = OK; - _range.error_count = _pwm.error_count; - _range.maximum_distance = get_maximum_distance(); - _range.minimum_distance = get_minimum_distance(); - _range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite - _range.distance_vector[0] = _range.distance; - _range.just_updated = 0; - _range.valid = true; + _range.error_count = _pwm.error_count; + _range.maximum_distance = get_maximum_distance(); + _range.minimum_distance = get_minimum_distance(); + _range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite + _range.distance_vector[0] = _range.distance; + _range.just_updated = 0; + _range.valid = true; - //TODO: due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) - if(_range.distance <= 0.0f) { - _range.valid = false; - _range.error_count++; - result = ERROR; - } + //TODO: due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) + if (_range.distance <= 0.0f) { + _range.valid = false; + _range.error_count++; + result = ERROR; + } - orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); + orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); - return result; + return result; } int LidarLitePWM::collect() { - //Check PWM - bool update; - orb_check(_pwmSub, &update); - if(update) { - orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm); - _range.timestamp = hrt_absolute_time(); - return OK; - } + //Check PWM + bool update; + orb_check(_pwmSub, &update); - //Timeout readings after 0.2 seconds and mark as invalid - if(hrt_absolute_time() - _range.timestamp > LL40LS_CONVERSION_TIMEOUT*2) { - _range.timestamp = hrt_absolute_time(); - _range.valid = false; - orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); - return ERROR; - } + if (update) { + orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm); + _range.timestamp = hrt_absolute_time(); + return OK; + } - return EAGAIN; + //Timeout readings after 0.2 seconds and mark as invalid + if (hrt_absolute_time() - _range.timestamp > LL40LS_CONVERSION_TIMEOUT * 2) { + _range.timestamp = hrt_absolute_time(); + _range.valid = false; + orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); + return ERROR; + } + + return EAGAIN; } diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index 0b85d53abc..6204e0f9af 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -47,37 +47,37 @@ class LidarLitePWM : public LidarLite { public: - LidarLitePWM(); + LidarLitePWM(); - int init() override; + int init() override; - void start() override; + void start() override; - void stop() override; + void stop() override; - /** - * @brief - * Diagnostics - print some basic information about the driver. - */ - void print_info() override; + /** + * @brief + * Diagnostics - print some basic information about the driver. + */ + void print_info() override; - /** - * @brief - * print registers to console - */ - void print_registers() override; + /** + * @brief + * print registers to console + */ + void print_registers() override; protected: - int measure() override; + int measure() override; - int collect() override; + int collect() override; - void task_main_trampoline(int argc, char *argv[]); + void task_main_trampoline(int argc, char *argv[]); private: - bool _terminateRequested; - int _pwmSub; - pwm_input_s _pwm; - orb_advert_t _rangePub; - range_finder_report _range; + bool _terminateRequested; + int _pwmSub; + pwm_input_s _pwm; + orb_advert_t _rangePub; + range_finder_report _range; }; diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 827143a9c2..6ef2da94e7 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -90,12 +90,16 @@ void start(int bus) { /* create the driver, attempt expansion bus first */ if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { - if (g_dev_ext != nullptr) + if (g_dev_ext != nullptr) { errx(0, "already started external"); + } + g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { delete g_dev_ext; g_dev_ext = nullptr; + if (bus == PX4_I2C_BUS_EXPANSION) { goto fail; } @@ -103,11 +107,15 @@ void start(int bus) } #ifdef PX4_I2C_BUS_ONBOARD + /* if this failed, attempt onboard sensor */ if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) + if (g_dev_int != nullptr) { errx(0, "already started internal"); + } + g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); + if (g_dev_int != nullptr && OK != g_dev_int->init()) { /* tear down the failing onboard instance */ delete g_dev_int; @@ -117,44 +125,54 @@ void start(int bus) goto fail; } } + if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { goto fail; } } + #endif /* set the poll rate to default, starts automatic data collection */ if (g_dev_int != nullptr) { int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); - if (fd == -1) { - goto fail; - } + + if (fd == -1) { + goto fail; + } + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); close(fd); + if (ret < 0) { goto fail; } - } + } if (g_dev_ext != nullptr) { int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); - if (fd == -1) { - goto fail; - } + + if (fd == -1) { + goto fail; + } + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); close(fd); + if (ret < 0) { goto fail; } - } + } exit(0); fail: + if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { delete g_dev_int; g_dev_int = nullptr; } + if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { delete g_dev_ext; g_dev_ext = nullptr; @@ -168,7 +186,8 @@ fail: */ void stop(int bus) { - LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext); + LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD ? &g_dev_int : &g_dev_ext); + if (*g_dev != nullptr) { delete *g_dev; *g_dev = nullptr; @@ -191,7 +210,7 @@ test(int bus) struct range_finder_report report; ssize_t sz; int ret; - const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); + const char *path = (bus == PX4_I2C_BUS_ONBOARD ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); int fd = open(path, O_RDONLY); @@ -254,7 +273,7 @@ test(int bus) void reset(int bus) { - const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); + const char *path = (bus == PX4_I2C_BUS_ONBOARD ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); int fd = open(path, O_RDONLY); if (fd < 0) { @@ -278,7 +297,8 @@ reset(int bus) void info(int bus) { - LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); + LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + if (g_dev == nullptr) { errx(1, "driver not running"); } @@ -295,7 +315,8 @@ info(int bus) void regdump(int bus) { - LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); + LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + if (g_dev == nullptr) { errx(1, "driver not running"); } @@ -328,13 +349,16 @@ ll40ls_main(int argc, char *argv[]) while ((ch = getopt(argc, argv, "XI")) != EOF) { switch (ch) { #ifdef PX4_I2C_BUS_ONBOARD + case 'I': bus = PX4_I2C_BUS_ONBOARD; break; #endif + case 'X': bus = PX4_I2C_BUS_EXPANSION; break; + default: ll40ls::usage(); exit(0); From 9eb7409f468b1aed40b3eb56edc51189c9fa2ed1 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 09:53:21 +0100 Subject: [PATCH 29/51] makefiles: getting rid of gpio_tool again, this was a leftover from debugging --- makefiles/nuttx/config_px4fmu-v2_default.mk | 1 - 1 file changed, 1 deletion(-) diff --git a/makefiles/nuttx/config_px4fmu-v2_default.mk b/makefiles/nuttx/config_px4fmu-v2_default.mk index a0edd7710f..4c8f60e693 100644 --- a/makefiles/nuttx/config_px4fmu-v2_default.mk +++ b/makefiles/nuttx/config_px4fmu-v2_default.mk @@ -71,7 +71,6 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -MODULES += modules/gpio_tool MODULES += modules/uavcan MODULES += modules/land_detector From b9ce447cc9f9918dbd535e76dbb04d030e5e4e91 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 10:01:28 +0100 Subject: [PATCH 30/51] ringbuffer: whitespace --- src/drivers/device/ringbuffer.cpp | 30 +++++++++++++++--------------- src/drivers/device/ringbuffer.h | 2 +- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/drivers/device/ringbuffer.cpp b/src/drivers/device/ringbuffer.cpp index 1790289338..239b263ec2 100644 --- a/src/drivers/device/ringbuffer.cpp +++ b/src/drivers/device/ringbuffer.cpp @@ -57,25 +57,25 @@ RingBuffer::~RingBuffer() unsigned RingBuffer::_next(unsigned index) { - return (0 == index) ? _num_items : (index - 1); + return (0 == index) ? _num_items : (index - 1); } bool RingBuffer::empty() { - return _tail == _head; + return _tail == _head; } bool RingBuffer::full() { - return _next(_head) == _tail; + return _next(_head) == _tail; } unsigned RingBuffer::size() { - return (_buf != nullptr) ? _num_items : 0; + return (_buf != nullptr) ? _num_items : 0; } void @@ -86,7 +86,7 @@ RingBuffer::flush() } bool -RingBuffer::put(const void *val, size_t val_size) +RingBuffer::put(const void *val, size_t val_size) { unsigned next = _next(_head); if (next != _tail) { @@ -250,7 +250,7 @@ static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned #define __PX4_SBCAP __sync_bool_compare_and_swap #endif bool -RingBuffer::get(void *val, size_t val_size) +RingBuffer::get(void *val, size_t val_size) { if (_tail != _head) { unsigned candidate; @@ -340,7 +340,7 @@ RingBuffer::get(double &val) } unsigned -RingBuffer::space(void) +RingBuffer::space(void) { unsigned tail, head; @@ -361,7 +361,7 @@ RingBuffer::space(void) } unsigned -RingBuffer::count(void) +RingBuffer::count(void) { /* * Note that due to the conservative nature of space(), this may @@ -371,7 +371,7 @@ RingBuffer::count(void) } bool -RingBuffer::resize(unsigned new_size) +RingBuffer::resize(unsigned new_size) { char *old_buffer; char *new_buffer = new char [(new_size+1) * _item_size]; @@ -388,13 +388,13 @@ RingBuffer::resize(unsigned new_size) } void -RingBuffer::print_info(const char *name) +RingBuffer::print_info(const char *name) { printf("%s %u/%lu (%u/%u @ %p)\n", - name, - _num_items, - (unsigned long)_num_items * _item_size, - _head, - _tail, + name, + _num_items, + (unsigned long)_num_items * _item_size, + _head, + _tail, _buf); } diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 1c65c02135..26d0df0272 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -161,7 +161,7 @@ public: private: unsigned _num_items; const size_t _item_size; - char *_buf; + char *_buf; volatile unsigned _head; /**< insertion point in _item_size units */ volatile unsigned _tail; /**< removal point in _item_size units */ From aac276872bc7fd71f1e25f4193d013cde0cee149 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 10:02:08 +0100 Subject: [PATCH 31/51] ringbuffer: use a namespace for ringbuffer to avoid 'multiple definitions in vtable' bug --- src/drivers/device/module.mk | 5 +++-- src/drivers/device/ringbuffer.cpp | 7 ++++++- src/drivers/device/ringbuffer.h | 12 +++++------- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index a9e964ef94..c206a5037c 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -41,7 +41,8 @@ SRCS = \ cdev.cpp \ i2c_nuttx.cpp \ pio.cpp \ - spi.cpp + spi.cpp \ + ringbuffer.cpp else SRCS = \ device_posix.cpp \ @@ -50,5 +51,5 @@ SRCS = \ vdev_posix.cpp \ i2c_posix.cpp \ sim.cpp \ - ringbuffer.cpp + ringbuffer.cpp endif diff --git a/src/drivers/device/ringbuffer.cpp b/src/drivers/device/ringbuffer.cpp index 239b263ec2..c30e0a54df 100644 --- a/src/drivers/device/ringbuffer.cpp +++ b/src/drivers/device/ringbuffer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 @@ -40,6 +40,9 @@ #include "ringbuffer.h" #include +namespace ringbuffer +{ + RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : _num_items(num_items), _item_size(item_size), @@ -398,3 +401,5 @@ RingBuffer::print_info(const char *name) _tail, _buf); } + +} // namespace ringbuffer diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 26d0df0272..4fcdaf47fa 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 @@ -43,6 +43,9 @@ #include #include +namespace ringbuffer __EXPORT +{ + class RingBuffer { public: RingBuffer(unsigned ring_size, size_t entry_size); @@ -172,9 +175,4 @@ private: RingBuffer operator=(const RingBuffer&); }; -#ifdef __PX4_NUTTX -// Not sure why NuttX requires these to be defined in the header file -// but on other targets it causes a problem with multiple definitions -// at link time -#include "ringbuffer.cpp" -#endif +} // namespace ringbuffer From 21dfd0243d38beb80c401a4858f5ff742b5d42e2 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 10:04:38 +0100 Subject: [PATCH 32/51] drivers: use new ringbuffer namespace everywhere --- src/drivers/airspeed/airspeed.cpp | 2 +- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- src/drivers/l3gd20/l3gd20.cpp | 4 ++-- src/drivers/lsm303d/lsm303d.cpp | 8 ++++---- src/drivers/mb12xx/mb12xx.cpp | 4 ++-- src/drivers/mpu6000/mpu6000.cpp | 8 ++++---- src/drivers/ms5611/ms5611_nuttx.cpp | 4 ++-- src/drivers/oreoled/oreoled.cpp | 4 ++-- src/drivers/px4flow/px4flow.cpp | 4 ++-- src/drivers/sf0x/sf0x.cpp | 4 ++-- src/drivers/trone/trone.cpp | 4 ++-- src/modules/uavcan/sensors/baro.cpp | 3 +-- src/modules/uavcan/sensors/baro.hpp | 5 ++--- 14 files changed, 29 insertions(+), 31 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 7a6a58746c..b4570e4bbd 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -128,7 +128,7 @@ Airspeed::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(differential_pressure_s)); + _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); if (_reports == nullptr) goto out; diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 3fbf3f9ddf..fad04c9978 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -109,7 +109,7 @@ public: virtual void print_info(); private: - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; perf_counter_t _buffer_overflows; /* this class has pointer data members and should not be copied */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 92ffa80665..4861d55501 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -156,7 +156,7 @@ private: work_s _work; unsigned _measure_ticks; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; mag_scale _scale; float _range_scale; float _range_ga; @@ -423,7 +423,7 @@ HMC5883::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(mag_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) goto out; diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f036987d7d..f276b67f00 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -229,7 +229,7 @@ private: struct hrt_call _call; unsigned _call_interval; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -472,7 +472,7 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(gyro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) goto out; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index e0863f46cd..87fe05d8f9 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -275,8 +275,8 @@ private: unsigned _call_accel_interval; unsigned _call_mag_interval; - RingBuffer *_accel_reports; - RingBuffer *_mag_reports; + ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -646,12 +646,12 @@ LSM303D::init() } /* allocate basic report buffers */ - _accel_reports = new RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; - _mag_reports = new RingBuffer(2, sizeof(mag_report)); + _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) goto out; diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 1a4ea17cf1..6a1740cb05 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -125,7 +125,7 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -255,7 +255,7 @@ MB12XX::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 9dd7cc7048..7049bd0788 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -242,7 +242,7 @@ private: struct hrt_call _call; unsigned _call_interval; - RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_accel_reports; struct accel_scale _accel_scale; float _accel_range_scale; @@ -251,7 +251,7 @@ private: int _accel_orb_class_instance; int _accel_class_instance; - RingBuffer *_gyro_reports; + ringbuffer::RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -611,11 +611,11 @@ MPU6000::init() } /* allocate basic report buffers */ - _accel_reports = new RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; - _gyro_reports = new RingBuffer(2, sizeof(gyro_report)); + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); if (_gyro_reports == nullptr) goto out; diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index 9d0e436eeb..95f532fbd9 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -127,7 +127,7 @@ protected: struct work_s _work; unsigned _measure_ticks; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -269,7 +269,7 @@ MS5611::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(baro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { debug("can't get memory for reports"); diff --git a/src/drivers/oreoled/oreoled.cpp b/src/drivers/oreoled/oreoled.cpp index b92e950010..3918a6575e 100644 --- a/src/drivers/oreoled/oreoled.cpp +++ b/src/drivers/oreoled/oreoled.cpp @@ -121,7 +121,7 @@ private: work_s _work; ///< work queue for scheduling reads bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED uint8_t _num_healthy; ///< number of healthy LEDs - RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs + ringbuffer::RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs uint64_t _last_gencall; uint64_t _start_time; ///< system time we first attempt to communicate with battery }; @@ -176,7 +176,7 @@ OREOLED::init() } /* allocate command queue */ - _cmd_queue = new RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); + _cmd_queue = new ringbuffer::RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); if (_cmd_queue == nullptr) { return ENOTTY; diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f35957caae..420805c73f 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -120,7 +120,7 @@ protected: private: work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -217,7 +217,7 @@ PX4FLOW::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(optical_flow_s)); + _reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) { goto out; diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index c45e73fd9a..8c11e4c14d 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -118,7 +118,7 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -270,7 +270,7 @@ SF0X::init() if (ret != OK) break; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) { warnx("mem err"); ret = -1; diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index a3c8633725..d072b1b251 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -122,7 +122,7 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -281,7 +281,7 @@ TRONE::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) { goto out; diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 576e758df5..9c3a0aefb4 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -36,7 +36,6 @@ */ #include "baro.hpp" -#include #include const char *const UavcanBarometerBridge::NAME = "baro"; @@ -56,7 +55,7 @@ int UavcanBarometerBridge::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(baro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) return -1; diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 6a39eebfb6..b5da6985a7 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -39,11 +39,10 @@ #include "sensor_bridge.hpp" #include +#include #include -class RingBuffer; - class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase { public: @@ -68,5 +67,5 @@ private: uavcan::Subscriber _sub_air_data; unsigned _msl_pressure = 101325; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; }; From 362fc205b3e4819646c58570d3d43dc3afc1dc5c Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 10:05:42 +0100 Subject: [PATCH 33/51] drivers: various whitespace --- src/drivers/hmc5883/hmc5883.cpp | 16 ++++++++-------- src/drivers/lsm303d/lsm303d.cpp | 6 +++--- src/drivers/ms5611/ms5611_nuttx.cpp | 4 ++-- src/drivers/px4flow/px4flow.cpp | 8 ++++---- src/drivers/trone/trone.cpp | 2 +- 5 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 4861d55501..2d3be15833 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -921,10 +921,10 @@ HMC5883::collect() _temperature_counter = 0; - ret = _interface->read(ADDR_TEMP_OUT_MSB, + ret = _interface->read(ADDR_TEMP_OUT_MSB, raw_temperature, sizeof(raw_temperature)); if (ret == OK) { - int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + + int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + raw_temperature[1]; new_report.temperature = 25 + (temp16 / (16*8.0f)); _temperature_error_count = 0; @@ -1146,8 +1146,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = -EIO; goto out; } - float cal[3] = {fabsf(expected_cal[0] / report.x), - fabsf(expected_cal[1] / report.y), + float cal[3] = {fabsf(expected_cal[0] / report.x), + fabsf(expected_cal[1] / report.y), fabsf(expected_cal[2] / report.z)}; if (cal[0] > 0.7f && cal[0] < 1.35f && @@ -1381,7 +1381,7 @@ HMC5883::print_info() printf("poll interval: %u ticks\n", _measure_ticks); printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z); printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); - printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, (double)(1.0f/_range_scale), (double)_range_ga); printf("temperature %.2f\n", (double)_last_report.temperature); @@ -1451,7 +1451,7 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) bus.dev = NULL; return false; } - + int fd = open(bus.devpath, O_RDONLY); if (fd < 0) { return false; @@ -1505,7 +1505,7 @@ struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid) busid == bus_options[i].busid) && bus_options[i].dev != NULL) { return bus_options[i]; } - } + } errx(1, "bus %u not started", (unsigned)busid); } @@ -1750,7 +1750,7 @@ hmc5883_main(int argc, char *argv[]) } } - const char *verb = argv[optind]; + const char *verb = argv[optind]; /* * Start/load the driver. diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 87fe05d8f9..8a77eeb917 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -588,7 +588,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _debug_enabled = true; _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D; - + /* Prime _mag with parents devid. */ _mag->_device_id.devid = _device_id.devid; _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; @@ -1399,9 +1399,9 @@ LSM303D::start() _mag_reports->flush(); /* start polling at the specified rate */ - hrt_call_every(&_accel_call, + hrt_call_every(&_accel_call, 1000, - _call_accel_interval - LSM303D_TIMER_REDUCTION, + _call_accel_interval - LSM303D_TIMER_REDUCTION, (hrt_callout)&LSM303D::measure_trampoline, this); hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); } diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp index 95f532fbd9..cf96a7a95b 100644 --- a/src/drivers/ms5611/ms5611_nuttx.cpp +++ b/src/drivers/ms5611/ms5611_nuttx.cpp @@ -896,7 +896,7 @@ start_bus(struct ms5611_bus_option &bus) bus.dev = NULL; return false; } - + int fd = open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ @@ -955,7 +955,7 @@ struct ms5611_bus_option &find_bus(enum MS5611_BUS busid) busid == bus_options[i].busid) && bus_options[i].dev != NULL) { return bus_options[i]; } - } + } errx(1,"bus %u not started", (unsigned)busid); } diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 420805c73f..319c902f12 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -123,13 +123,13 @@ private: ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; - bool _collect_phase; + bool _collect_phase; orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - + enum Rotation _sensor_rotation; /** @@ -486,10 +486,10 @@ PX4FLOW::collect() report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; - + /* rotate measurements according to parameter */ float zeroval = 0.0f; - rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index d072b1b251..58799b46ac 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -82,7 +82,7 @@ #define TRONE_WHO_AM_I_REG 0x01 /* Who am I test register */ #define TRONE_WHO_AM_I_REG_VAL 0xA1 - + /* Device limits */ #define TRONE_MIN_DISTANCE (0.20f) #define TRONE_MAX_DISTANCE (14.00f) From 7cec6d3c3b5d55f476e7fe38c6b7bbafa9c4204e Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 11:52:18 +0100 Subject: [PATCH 34/51] uORB: added pwm_input to objects_common --- src/modules/uORB/objects_common.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index eb89b79585..11bfb00d79 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -66,6 +66,9 @@ ORB_DEFINE(output_pwm, struct pwm_output_values); #include ORB_DEFINE(input_rc, struct rc_input_values); +#include "topics/pwm_input.h" +ORB_DEFINE(pwm_input, struct pwm_input_s); + #include "topics/vehicle_attitude.h" ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); From 1151996c0b979d4db86df3258936ff0b75d55bbe Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 12:38:22 +0100 Subject: [PATCH 35/51] ll40ls: Weffc++ was complaining about 'class LidarLitePWM' has pointer data members, had to disable it --- src/drivers/ll40ls/module.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/ll40ls/module.mk b/src/drivers/ll40ls/module.mk index 99c0815793..c2c5e1e22a 100644 --- a/src/drivers/ll40ls/module.mk +++ b/src/drivers/ll40ls/module.mk @@ -38,8 +38,8 @@ MODULE_COMMAND = ll40ls SRCS = ll40ls.cpp \ - LidarLite.cpp \ - LidarLiteI2C.cpp \ - LidarLitePWM.cpp + LidarLite.cpp \ + LidarLiteI2C.cpp \ + LidarLitePWM.cpp -MAXOPTIMIZATION = -Os -Weffc++ +MAXOPTIMIZATION = -Os From 4e897bf197c452b7bc68043609f467dfef4c58b0 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 12:40:29 +0100 Subject: [PATCH 36/51] pwm_input: comment style --- src/drivers/pwm_input/pwm_input.cpp | 141 ++++++++++++---------------- 1 file changed, 59 insertions(+), 82 deletions(-) diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index be01e848db..02a38b27eb 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -36,6 +36,9 @@ * * PWM input driver based on earlier driver from Evan Slatyer, * which in turn was based on drv_hrt.c + * + * @author: Andrew Tridgell + * @author: Ban Siesta */ #include @@ -81,7 +84,7 @@ #include #include -// Reset pin define +/* Reset pin define */ #define GPIO_VDD_RANGEFINDER_EN GPIO_GPIO5_OUTPUT #if HRT_TIMER == PWMIN_TIMER @@ -289,16 +292,16 @@ PWMIN::~PWMIN() } /* - initialise the driver. This doesn't actually start the timer (that - is done on open). We don't start the timer to allow for this driver - to be started in init scripts when the user may be using the input - pin as PWM output + * initialise the driver. This doesn't actually start the timer (that + * is done on open). We don't start the timer to allow for this driver + * to be started in init scripts when the user may be using the input + * pin as PWM output */ int PWMIN::init() { - // we just register the device in /dev, and only actually - // activate the timer when requested to when the device is opened + /* we just register the device in /dev, and only actually + * activate the timer when requested to when the device is opened */ CDev::init(); data.type = RANGE_FINDER_TYPE_LASER; @@ -324,10 +327,10 @@ PWMIN::init() /* * Initialise the timer we are going to use. */ -void PWMIN::timer_init(void) +void PWMIN::_timer_init(void) { - // run with interrupts disabled in case the timer is already - // setup. We don't want it firing while we are doing the setup + /* run with interrupts disabled in case the timer is already + * setup. We don't want it firing while we are doing the setup */ irqstate_t flags = irqsave(); stm32_configgpio(GPIO_PWM_IN); @@ -350,9 +353,9 @@ void PWMIN::timer_init(void) rCCER = CCER_PWMIN; rDCR = 0; - // for simplicity scale by the clock in MHz. This gives us - // readings in microseconds which is typically what is needed - // for a PWM input driver + /* for simplicity scale by the clock in MHz. This gives us + * readings in microseconds which is typically what is needed + * for a PWM input driver */ uint32_t prescaler = PWMIN_TIMER_CLOCK / 1000000UL; /* @@ -376,38 +379,41 @@ void PWMIN::timer_init(void) irqrestore(flags); - timer_started = true; + _timer_started = true; } void -PWMIN::freeze_test() +PWMIN::_freeze_test() { - /* timeout is true if least read was away back */ - bool timeout = false; - timeout = (hrt_absolute_time() - last_poll_time > TIMEOUT) ? true : false; - - if (timeout) { - fprintf(stderr, "[pwm_input] Lidar is down, reseting\n"); + /* reset if last poll time was way back and a read was recently requested */ + if (hrt_elapsed_time(&_last_poll_time) > TIMEOUT_POLL && hrt_elapsed_time(&_last_read_time) < TIMEOUT_READ) { + warnx("Lidar is down, reseting"); hard_reset(); } } void -PWMIN::turn_on() { stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1); } +PWMIN::_turn_on() +{ + stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1); +} void -PWMIN::turn_off() { stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0); } +PWMIN::_turn_off() +{ + stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0); +} void PWMIN::hard_reset() { - turn_off(); - hrt_call_after(&hard_reset_call, 9000, reinterpret_cast(&PWMIN::turn_on), this); + _turn_off(); + hrt_call_after(&_hard_reset_call, 9000, reinterpret_cast(&PWMIN::_turn_on), this); } /* - hook for open of the driver. We start the timer at this point, then - leave it running + * hook for open of the driver. We start the timer at this point, then + * leave it running */ int PWMIN::open(struct file *filp) @@ -427,7 +433,7 @@ PWMIN::open(struct file *filp) /* - handle ioctl requests + * handle ioctl requests */ int PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) @@ -452,14 +458,15 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGQUEUEDEPTH: - return reports->size(); + return _reports->size(); case SENSORIOCRESET: /* user has asked for the timer to be reset. This may - be needed if the pin was used for a different - purpose (such as PWM output) - */ - timer_init(); + * be needed if the pin was used for a different + * purpose (such as PWM output) */ + _timer_init(); + /* also reset the sensor */ + hard_reset(); return OK; default: @@ -470,7 +477,7 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) /* - read some samples from the device + * read some samples from the device */ ssize_t PWMIN::read(struct file *filp, char *buffer, size_t buflen) @@ -485,76 +492,56 @@ PWMIN::read(struct file *filp, char *buffer, size_t buflen) } while (count--) { - if (reports->get(buf)) { + if (_reports->get(buf)) { ret += sizeof(struct pwm_input_s); buf++; } } - /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } /* - publish some data from the ISR in the ring buffer + * publish some data from the ISR in the ring buffer */ -void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width) +void PWMIN::publish(uint16_t status, uint32_t period, uint32_t pulse_width) { /* if we missed an edge, we have to give up */ if (status & SR_OVF_PWMIN) { - error_count++; + _error_count++; return; } - last_poll_time = hrt_absolute_time(); + _last_poll_time = hrt_absolute_time(); struct pwm_input_s pwmin_report; - pwmin_report.timestamp = last_poll_time; - pwmin_report.error_count = error_count; + pwmin_report.timestamp = _last_poll_time; + pwmin_report.error_count = _error_count; pwmin_report.period = period; pwmin_report.pulse_width = pulse_width; - data.distance = pulse_width * 1e-3f; - data.timestamp = pwmin_report.timestamp; - data.error_count = error_count; - - if (data.distance < data.minimum_distance || data.distance > data.maximum_distance) { - data.valid = false; - - } else { - data.valid = true; - } - - if (range_finder_pub > 0) { - orb_publish(ORB_ID(sensor_range_finder), range_finder_pub, &data); - } - - reports->force(&pwmin_report); - - last_period = period; - last_width = pulse_width; - pulses_captured++; + _reports->force(&pwmin_report); } /* - print information on the last captured + * print information on the last captured */ -void PWMIN::_print_info(void) +void PWMIN::print_info(void) { - if (!timer_started) { + if (!_timer_started) { printf("timer not started - try the 'test' command\n"); } else { printf("count=%u period=%u width=%u\n", - (unsigned)pulses_captured, - (unsigned)last_period, - (unsigned)last_width); + (unsigned)_pulses_captured, + (unsigned)_last_period, + (unsigned)_last_width); } } /* - Handle the interupt, gathering pulse data + * Handle the interupt, gathering pulse data */ static int pwmin_tim_isr(int irq, void *context) { @@ -592,21 +579,11 @@ static void pwmin_start(bool full_start) errx(1, "driver init failed"); } - if (full_start) { - int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY); - - if (fd == -1) { - errx(1, "Failed to open device"); - } - - close(fd); - } - exit(0); } /* - test the driver + * test the driver */ static void pwmin_test(void) { @@ -636,7 +613,7 @@ static void pwmin_test(void) } /* - reset the timer + * reset the timer */ static void pwmin_reset(void) { @@ -656,7 +633,7 @@ static void pwmin_reset(void) } /* - show some information on the driver + * show some information on the driver */ static void pwmin_info(void) { From bb3ad64aac71f5399e037b21157762945b65c408 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 12:41:19 +0100 Subject: [PATCH 37/51] pwm_input: get rid of start full and instead start whenever someone reads from the driver, as well as various other small changes --- src/drivers/pwm_input/pwm_input.cpp | 107 ++++++++++++---------------- 1 file changed, 44 insertions(+), 63 deletions(-) diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 02a38b27eb..d06d125e58 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -223,7 +223,8 @@ #error PWMIN_TIMER_CHANNEL must be either 1 and 2. #endif -#define TIMEOUT 300000 /* reset after no responce over this time in microseconds [0.3 secs] */ +#define TIMEOUT_POLL 300000 /* reset after no response over this time in microseconds [0.3s] */ +#define TIMEOUT_READ 200000 /* don't reset if the last read is back more than this time in microseconds [0.2s] */ class PWMIN : device::CDev { @@ -236,35 +237,33 @@ public: virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - void _publish(uint16_t status, uint32_t period, uint32_t pulse_width); - void _print_info(void); + void publish(uint16_t status, uint32_t period, uint32_t pulse_width); + void print_info(void); void hard_reset(); private: - uint32_t error_count; - uint32_t pulses_captured; - uint32_t last_period; - uint32_t last_width; - uint64_t last_poll_time; - RingBuffer *reports; - bool timer_started; + uint32_t _error_count; + uint32_t _pulses_captured; + uint32_t _last_period; + uint32_t _last_width; + hrt_abstime _last_poll_time; + hrt_abstime _last_read_time; + ringbuffer::RingBuffer *_reports; + bool _timer_started; - range_finder_report data; - orb_advert_t range_finder_pub; + hrt_call _hard_reset_call; /* HRT callout for note completion */ + hrt_call _freeze_test_call; /* HRT callout for note completion */ - hrt_call hard_reset_call; // HRT callout for note completion - hrt_call freeze_test_call; // HRT callout for note completion + void _timer_init(void); - void timer_init(void); - - void turn_on(); - void turn_off(); - void freeze_test(); + void _turn_on(); + void _turn_off(); + void _freeze_test(); }; static int pwmin_tim_isr(int irq, void *context); -static void pwmin_start(bool full_start); +static void pwmin_start(); static void pwmin_info(void); static void pwmin_test(void); static void pwmin_reset(void); @@ -273,21 +272,19 @@ static PWMIN *g_dev; PWMIN::PWMIN() : CDev("pwmin", PWMIN0_DEVICE_PATH), - error_count(0), - pulses_captured(0), - last_period(0), - last_width(0), - reports(nullptr), - timer_started(false), - range_finder_pub(-1) + _error_count(0), + _pulses_captured(0), + _last_period(0), + _last_width(0), + _reports(nullptr), + _timer_started(false) { - memset(&data, 0, sizeof(data)); } PWMIN::~PWMIN() { - if (reports != nullptr) { - delete reports; + if (_reports != nullptr) { + delete _reports; } } @@ -304,22 +301,13 @@ PWMIN::init() * activate the timer when requested to when the device is opened */ CDev::init(); - data.type = RANGE_FINDER_TYPE_LASER; - data.minimum_distance = 0.20f; - data.maximum_distance = 7.0f; - - range_finder_pub = orb_advertise(ORB_ID(sensor_range_finder), &data); - fprintf(stderr, "[pwm_input] advertising %d\n" - , range_finder_pub); - - reports = new RingBuffer(2, sizeof(struct pwm_input_s)); - - if (reports == nullptr) { + _reports = new ringbuffer::RingBuffer(2, sizeof(struct pwm_input_s)); + if (_reports == nullptr) { return -ENOMEM; } - // Schedule freeze check to invoke periodically - hrt_call_every(&freeze_test_call, 0, TIMEOUT, reinterpret_cast(&PWMIN::freeze_test), this); + /* Schedule freeze check to invoke periodically */ + hrt_call_every(&_freeze_test_call, 0, TIMEOUT_POLL, reinterpret_cast(&PWMIN::_freeze_test), this); return OK; } @@ -424,8 +412,8 @@ PWMIN::open(struct file *filp) int ret = CDev::open(filp); - if (ret == OK && !timer_started) { - g_dev->timer_init(); + if (ret == OK && !_timer_started) { + g_dev->_timer_init(); } return ret; @@ -447,7 +435,7 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = irqsave(); - if (!reports->resize(arg)) { + if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } @@ -482,6 +470,8 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t PWMIN::read(struct file *filp, char *buffer, size_t buflen) { + _last_read_time = hrt_absolute_time(); + unsigned count = buflen / sizeof(struct pwm_input_s); struct pwm_input_s *buf = reinterpret_cast(buffer); int ret = 0; @@ -553,20 +543,19 @@ static int pwmin_tim_isr(int irq, void *context) rSR = 0; if (g_dev != nullptr) { - g_dev->_publish(status, period, pulse_width); + g_dev->publish(status, period, pulse_width); } return OK; } /* - start the driver + * start the driver */ -static void pwmin_start(bool full_start) +static void pwmin_start() { if (g_dev != nullptr) { - printf("driver already started\n"); - exit(1); + errx(1, "driver already started"); } g_dev = new PWMIN(); @@ -642,31 +631,23 @@ static void pwmin_info(void) exit(1); } - g_dev->_print_info(); + g_dev->print_info(); exit(0); } /* - driver entry point + * driver entry point */ int pwm_input_main(int argc, char *argv[]) { const char *verb = argv[1]; - /* - * init driver and start reading - */ - bool full_start = false; - - if (!strcmp(argv[2], "full")) { - full_start = true; - } /* * Start/load the driver. */ if (!strcmp(verb, "start")) { - pwmin_start(full_start); + pwmin_start(); } /* @@ -690,6 +671,6 @@ int pwm_input_main(int argc, char *argv[]) pwmin_reset(); } - errx(1, "unrecognized command, try 'start', 'start full', 'info', 'reset' or 'test'"); + errx(1, "unrecognized command, try 'start', 'info', 'reset' or 'test'"); return 0; } From 0ab0de5805670dbceb7be497502e0b1c5c7677ca Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 12:42:14 +0100 Subject: [PATCH 38/51] ll40ls: adapt the cli interface, so that the commands work with the PWM and I2C driver --- src/drivers/ll40ls/ll40ls.cpp | 293 +++++++++++++++++++++------------- 1 file changed, 184 insertions(+), 109 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 6ef2da94e7..9c52ce6e9a 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -35,11 +35,13 @@ * @file ll40ls.cpp * @author Allyson Kreft * @author Johan Jansen + * @author Ban Siesta * * Interface for the PulsedLight Lidar-Lite range finders. */ #include "LidarLiteI2C.h" +#include "LidarLitePWM.h" #include #include #include @@ -53,6 +55,7 @@ #define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int" #define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext" +#define LL40LS_DEVICE_PATH_PWM "/dev/ll40ls_pwm" /* * Driver 'main' command. @@ -74,93 +77,122 @@ const int ERROR = -1; LidarLiteI2C *g_dev_int; LidarLiteI2C *g_dev_ext; +LidarLitePWM *g_dev_pwm; -void start(int bus); -void stop(int bus); -void test(int bus); -void reset(int bus); -void info(int bus); -void regdump(int bus); +void start(const bool use_i2c, const int bus); +void stop(const bool use_i2c, const int bus); +void test(const bool use_i2c, const int bus); +void reset(const bool use_i2c, const int bus); +void info(const bool use_i2c, const int bus); +void regdump(const bool use_i2c, const int bus); void usage(); /** * Start the driver. */ -void start(int bus) +void start(const bool use_i2c, const int bus) { - /* create the driver, attempt expansion bus first */ - if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { - if (g_dev_ext != nullptr) { - errx(0, "already started external"); - } + if (use_i2c) { + /* create the driver, attempt expansion bus first */ + if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { + if (g_dev_ext != nullptr) { + errx(0, "already started external"); + } - g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); + g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); - if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { - delete g_dev_ext; - g_dev_ext = nullptr; + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; - if (bus == PX4_I2C_BUS_EXPANSION) { - goto fail; + if (bus == PX4_I2C_BUS_EXPANSION) { + goto fail; + } } } - } #ifdef PX4_I2C_BUS_ONBOARD - /* if this failed, attempt onboard sensor */ - if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) { - errx(0, "already started internal"); - } + /* if this failed, attempt onboard sensor */ + if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { + if (g_dev_int != nullptr) { + errx(0, "already started internal"); + } - g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); + g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); - if (g_dev_int != nullptr && OK != g_dev_int->init()) { - /* tear down the failing onboard instance */ - delete g_dev_int; - g_dev_int = nullptr; + if (g_dev_int != nullptr && OK != g_dev_int->init()) { + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; - if (bus == PX4_I2C_BUS_ONBOARD) { + if (bus == PX4_I2C_BUS_ONBOARD) { + goto fail; + } + } + + if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { goto fail; } } - if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { - goto fail; - } - } - #endif - /* set the poll rate to default, starts automatic data collection */ - if (g_dev_int != nullptr) { - int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); + /* set the poll rate to default, starts automatic data collection */ + if (g_dev_int != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); - if (fd == -1) { - goto fail; + if (fd == -1) { + goto fail; + } + + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + + if (ret < 0) { + goto fail; + } } - int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); + if (g_dev_ext != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); - if (ret < 0) { - goto fail; - } - } + if (fd == -1) { + goto fail; + } - if (g_dev_ext != nullptr) { - int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); - if (fd == -1) { - goto fail; + if (ret < 0) { + goto fail; + } } - int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); - close(fd); + } else { + g_dev_pwm = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM); - if (ret < 0) { - goto fail; + if (g_dev_pwm != nullptr && OK != g_dev_pwm->init()) { + delete g_dev_pwm; + g_dev_pwm = nullptr; + warnx("failed to init PWM"); + } + + if (g_dev_pwm != nullptr) { + int fd = open(LL40LS_DEVICE_PATH_PWM, O_RDONLY); + + if (fd == -1) { + warnx("fd nothing"); + goto fail; + } + + int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + close(fd); + + if (ret < 0) { + warnx("pollrate fail"); + goto fail; + } } } @@ -184,7 +216,7 @@ fail: /** * Stop the driver */ -void stop(int bus) +void stop(const bool use_i2c, const int bus) { LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD ? &g_dev_int : &g_dev_ext); @@ -205,17 +237,25 @@ void stop(int bus) * and automatic modes. */ void -test(int bus) +test(const bool use_i2c, const int bus) { struct range_finder_report report; ssize_t sz; int ret; - const char *path = (bus == PX4_I2C_BUS_ONBOARD ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + + const char *path; + + if (use_i2c) { + path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + + } else { + path = LL40LS_DEVICE_PATH_PWM; + } int fd = open(path, O_RDONLY); if (fd < 0) { - err(1, "%s open failed (try 'll40ls start' if the driver is not running", path); + err(1, "%s open failed, is the driver running?", path); } /* do a simple demand read */ @@ -234,7 +274,7 @@ test(int bus) errx(1, "failed to set 2Hz poll rate"); } - /* read the sensor 5x and report each value */ + /* read the sensor 5 times and report each value */ for (unsigned i = 0; i < 5; i++) { struct pollfd fds; @@ -255,7 +295,7 @@ test(int bus) } warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); + warnx("measurement: %0.3f m", (double)report.distance); warnx("time: %lld", report.timestamp); } @@ -271,9 +311,18 @@ test(int bus) * Reset the driver. */ void -reset(int bus) +reset(const bool use_i2c, const int bus) { - const char *path = (bus == PX4_I2C_BUS_ONBOARD ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + + const char *path; + + if (use_i2c) { + path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT); + + } else { + path = LL40LS_DEVICE_PATH_PWM; + } + int fd = open(path, O_RDONLY); if (fd < 0) { @@ -295,12 +344,19 @@ reset(int bus) * Print a little info about the driver. */ void -info(int bus) +info(const bool use_i2c, const int bus) { - LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + LidarLite *g_dev = nullptr; - if (g_dev == nullptr) { - errx(1, "driver not running"); + if (use_i2c) { + g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + } else { + g_dev = g_dev_pwm; } printf("state @ %p\n", g_dev); @@ -313,7 +369,7 @@ info(int bus) * Dump registers */ void -regdump(int bus) +regdump(const bool use_i2c, const int bus) { LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); @@ -330,8 +386,8 @@ regdump(int bus) void usage() { - warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump'"); - warnx("options:"); + warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info' or 'regdump' [i2c|pwm]"); + warnx("options for I2C:"); warnx(" -X only external bus"); #ifdef PX4_I2C_BUS_ONBOARD warnx(" -I only internal bus"); @@ -365,49 +421,68 @@ ll40ls_main(int argc, char *argv[]) } } - const char *verb = argv[optind]; + /* default to I2C if no protocol is given */ + bool use_i2c = true; - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - ll40ls::start(bus); + /* determine protocol first because it's needed next */ + if (argc > optind + 1) { + const char *protocol = argv[optind + 1]; + + if (!strcmp(protocol, "pwm")) { + use_i2c = false; + + } else if (!strcmp(protocol, "i2c")) { + use_i2c = true; + + } else { + warnx("unknown protocol, choose pwm or i2c"); + ll40ls::usage(); + exit(0); + } } - /* - * Stop the driver - */ - if (!strcmp(verb, "stop")) { - ll40ls::stop(bus); + /* now determine action */ + if (argc > optind) { + const char *verb = argv[optind]; + + /* Start/load the driver. */ + if (!strcmp(verb, "start")) { + + ll40ls::start(use_i2c, bus); + } + + /* Stop the driver */ + if (!strcmp(verb, "stop")) { + ll40ls::stop(use_i2c, bus); + } + + /* Test the driver/device. */ + else if (!strcmp(verb, "test")) { + ll40ls::test(use_i2c, bus); + } + + /* Reset the driver. */ + else if (!strcmp(verb, "reset")) { + ll40ls::reset(use_i2c, bus); + } + + /* dump registers */ + else if (!strcmp(verb, "regdump")) { + ll40ls::regdump(use_i2c, bus); + } + + /* Print driver information. */ + else if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + ll40ls::info(use_i2c, bus); + } + + else { + ll40ls::usage(); + exit(0); + } } - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - ll40ls::test(bus); - } - - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - ll40ls::reset(bus); - } - - /* - * dump registers - */ - if (!strcmp(verb, "regdump")) { - ll40ls::regdump(bus); - } - - /* - * Print driver information. - */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - ll40ls::info(bus); - } - - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + warnx("unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + ll40ls::usage(); + exit(0); } From fbc4ca61aca54176deee6082f35ba60558571047 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 12:44:53 +0100 Subject: [PATCH 39/51] ll40ls: removed unneeded comments --- src/drivers/ll40ls/LidarLite.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/ll40ls/LidarLite.cpp b/src/drivers/ll40ls/LidarLite.cpp index ebe054fc35..6406f9590e 100644 --- a/src/drivers/ll40ls/LidarLite.cpp +++ b/src/drivers/ll40ls/LidarLite.cpp @@ -47,12 +47,10 @@ LidarLite::LidarLite() : _max_distance(LL40LS_MAX_DISTANCE), _measure_ticks(0) { - //ctor } LidarLite::~LidarLite() { - //dtor } void LidarLite::set_minimum_distance(const float min) From ef658cf926d5afbbf28231c27b57d473bd059e16 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 12:46:23 +0100 Subject: [PATCH 40/51] ll40ls: changed I2C driver to new ringbuffer namespace, and various style stuff --- src/drivers/ll40ls/LidarLiteI2C.cpp | 15 +++++++-------- src/drivers/ll40ls/LidarLiteI2C.h | 18 ++++++++---------- 2 files changed, 15 insertions(+), 18 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 2ed07ae0b2..e2195ae029 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -46,7 +46,6 @@ #include #include #include -#include #include /* oddly, ERROR is not defined for c++ */ @@ -57,17 +56,17 @@ static const int ERROR = -1; LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : I2C("LL40LS", path, bus, address, 100000), - _work(), + _work{}, _reports(nullptr), _sensor_ok(false), _collect_phase(false), _class_instance(-1), _range_finder_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), - _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), - _sensor_resets(perf_alloc(PC_COUNT, "ll40ls_resets")), - _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_zero_resets")), + _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")), + _comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_i2c_overflows")), + _sensor_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_resets")), + _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_zero_resets")), _last_distance(0), _zero_counter(0), _acquire_time_usec(0), @@ -116,7 +115,7 @@ int LidarLiteI2C::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) { goto out; diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index 0784b1441a..cd80b162cb 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -40,9 +40,6 @@ #pragma once -//Forward declaration -class RingBuffer; - #include "LidarLite.h" #include @@ -50,6 +47,7 @@ class RingBuffer; #include #include +#include #include #include @@ -76,10 +74,10 @@ public: LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR); virtual ~LidarLiteI2C(); - virtual int init() override; + int init() override; - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg) override; + ssize_t read(struct file *filp, char *buffer, size_t buflen); + int ioctl(struct file *filp, int cmd, unsigned long arg) override; /** * Diagnostics - print some basic information about the driver. @@ -92,15 +90,15 @@ public: void print_registers() override; protected: - virtual int probe(); - virtual int read_reg(uint8_t reg, uint8_t &val); + int probe(); + int read_reg(uint8_t reg, uint8_t &val); int measure() override; - int reset_sensor() override; + int reset_sensor(); private: work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; bool _collect_phase; int _class_instance; From e7f1f3ba281935e68f5b59194d72b039ae9cb934 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 12:47:04 +0100 Subject: [PATCH 41/51] ll40ls: made PWM driver version functional --- src/drivers/ll40ls/LidarLitePWM.cpp | 230 +++++++++++++++++++++++----- src/drivers/ll40ls/LidarLitePWM.h | 50 +++++- 2 files changed, 238 insertions(+), 42 deletions(-) diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 7a52ab274a..32e35d0be6 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -35,48 +35,110 @@ /** * @file LidarLitePWM.h * @author Johan Jansen + * @author Ban Siesta * * Driver for the PulsedLight Lidar-Lite range finders connected via PWM. + * + * This driver accesses the pwm_input published by the pwm_input driver. */ + #include "LidarLitePWM.h" #include +#include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef __cplusplus static const int ERROR = -1; #endif -LidarLitePWM::LidarLitePWM() : - _terminateRequested(false), +LidarLitePWM::LidarLitePWM(const char *path) : + CDev("LidarLitePWM", path), + _work{}, + _reports(nullptr), + _class_instance(-1), _pwmSub(-1), _pwm{}, - _rangePub(-1), - _range{} + _range_finder_topic(-1), + _range{}, + _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_pwm_read")), + _read_errors(perf_alloc(PC_COUNT, "ll40ls_pwm_read_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_pwm_buffer_overflows")), + _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_pwm_zero_resets")) { - } +LidarLitePWM::~LidarLitePWM() +{ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_buffer_overflows); + perf_free(_sensor_zero_resets); +} + + int LidarLitePWM::init() { - _pwmSub = orb_subscribe(ORB_ID(pwm_input)); - if (_pwmSub == -1) { + /* do regular cdev init */ + int ret = CDev::init(); + + if (ret != OK) { return ERROR; } - _range.type = RANGE_FINDER_TYPE_LASER; - _range.valid = false; - _rangePub = orb_advertise(ORB_ID(sensor_range_finder), &_range); + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + return ERROR; + } + + //struct range_finder_report empty_report; + //memset(&empty_report, 0, sizeof(empty_report)); + //_rangePub = orb_advertise(ORB_ID(sensor_range_finder), &empty_report); + + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + + //if (_rangePub < 0) { + // debug("failed to create sensor_range_finder object. Did you start uOrb?"); + //} return OK; } void LidarLitePWM::print_info() { - printf("poll interval: %u ticks\n", getMeasureTicks()); - printf("distance: %ucm (0x%04x)\n", - (unsigned)_range.distance, (unsigned)_range.distance); + perf_print_counter(_sample_perf); + perf_print_counter(_read_errors); + perf_print_counter(_buffer_overflows); + perf_print_counter(_sensor_zero_resets); + warnx("poll interval: %u ticks", getMeasureTicks()); + warnx("distance: %.3fm", (double)_range.distance); } void LidarLitePWM::print_registers() @@ -86,19 +148,47 @@ void LidarLitePWM::print_registers() void LidarLitePWM::start() { - + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&LidarLitePWM::cycle_trampoline, this, 1); } void LidarLitePWM::stop() { - //TODO: stop measurement task - _terminateRequested = true; + work_cancel(HPWORK, &_work); +} + +void LidarLitePWM::cycle_trampoline(void *arg) +{ + LidarLitePWM *dev = (LidarLitePWM *)arg; + + dev->cycle(); +} + +void LidarLitePWM::cycle() +{ + measure(); + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&LidarLitePWM::cycle_trampoline, + this, + getMeasureTicks()); + return; } int LidarLitePWM::measure() { - int result = OK; + perf_begin(_sample_perf); + if (OK != collect()) { + debug("collection error"); + perf_count(_read_errors); + perf_end(_sample_perf); + return ERROR; + } + + _range.type = RANGE_FINDER_TYPE_LASER; _range.error_count = _pwm.error_count; _range.maximum_distance = get_maximum_distance(); _range.minimum_distance = get_minimum_distance(); @@ -107,37 +197,107 @@ int LidarLitePWM::measure() _range.just_updated = 0; _range.valid = true; - //TODO: due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) + // Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) if (_range.distance <= 0.0f) { _range.valid = false; _range.error_count++; - result = ERROR; + perf_count(_sensor_zero_resets); + perf_end(_sample_perf); + return reset_sensor(); } - orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_range); + } - return result; + if (_reports->force(&_range)) { + perf_count(_buffer_overflows); + } + + poll_notify(POLLIN); + perf_end(_sample_perf); + return OK; +} + +ssize_t LidarLitePWM::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (getMeasureTicks() > 0) { + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + + } else { + + _reports->flush(); + measure(); + + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + } + + return ret; +} + +int +LidarLitePWM::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + /* no custom ioctls implemented for now */ + default: + /* give it to the superclass */ + return LidarLite::ioctl(filp, cmd, arg); + } } int LidarLitePWM::collect() { - //Check PWM - bool update; - orb_check(_pwmSub, &update); + int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY); - if (update) { - orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm); - _range.timestamp = hrt_absolute_time(); - return OK; - } - - //Timeout readings after 0.2 seconds and mark as invalid - if (hrt_absolute_time() - _range.timestamp > LL40LS_CONVERSION_TIMEOUT * 2) { - _range.timestamp = hrt_absolute_time(); - _range.valid = false; - orb_publish(ORB_ID(sensor_range_finder), _rangePub, &_range); + if (fd == -1) { return ERROR; } + if (::read(fd, &_pwm, sizeof(_pwm)) == sizeof(_pwm)) { + ::close(fd); + return OK; + } + + ::close(fd); return EAGAIN; } + +int LidarLitePWM::reset_sensor() +{ + + int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY); + + if (fd == -1) { + return ERROR; + } + + int ret = ::ioctl(fd, SENSORIOCRESET, 0); + ::close(fd); + return ret; +} diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index 6204e0f9af..d2cc7f36e4 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -35,26 +35,44 @@ /** * @file LidarLitePWM.h * @author Johan Jansen + * @author Ban Siesta * * Driver for the PulsedLight Lidar-Lite range finders connected via PWM. + * + * This driver accesses the pwm_input published by the pwm_input driver. */ #pragma once #include "LidarLite.h" + +#include +#include + +#include +#include + #include #include -class LidarLitePWM : public LidarLite + + +class LidarLitePWM : public LidarLite, public device::CDev { public: - LidarLitePWM(); + LidarLitePWM(const char *path); + virtual ~LidarLitePWM(); int init() override; + ssize_t read(struct file *filp, char *buffer, size_t buflen) override; + int ioctl(struct file *filp, int cmd, unsigned long arg); + void start() override; void stop() override; + void cycle(); + /** * @brief * Diagnostics - print some basic information about the driver. @@ -67,17 +85,35 @@ public: */ void print_registers() override; + /** + * 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); + protected: + int measure() override; int collect() override; + int reset_sensor() override; + void task_main_trampoline(int argc, char *argv[]); private: - bool _terminateRequested; - int _pwmSub; - pwm_input_s _pwm; - orb_advert_t _rangePub; - range_finder_report _range; + work_s _work; + ringbuffer::RingBuffer *_reports; + int _class_instance; + int _pwmSub; + struct pwm_input_s _pwm; + orb_advert_t _range_finder_topic; + range_finder_report _range; + + perf_counter_t _sample_perf; + perf_counter_t _read_errors; + perf_counter_t _buffer_overflows; + perf_counter_t _sensor_zero_resets; }; From 2c4c0ecb32d99a8e0309878b467fbfa1a5f2c6b3 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 13:00:45 +0100 Subject: [PATCH 42/51] ll40ls: forgot to remove commented out stuff in PWM driver --- src/drivers/ll40ls/LidarLitePWM.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 32e35d0be6..2ebdc6fd7e 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -106,10 +106,6 @@ int LidarLitePWM::init() return ERROR; } - //struct range_finder_report empty_report; - //memset(&empty_report, 0, sizeof(empty_report)); - //_rangePub = orb_advertise(ORB_ID(sensor_range_finder), &empty_report); - _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { @@ -124,10 +120,6 @@ int LidarLitePWM::init() } } - //if (_rangePub < 0) { - // debug("failed to create sensor_range_finder object. Did you start uOrb?"); - //} - return OK; } From 103d59e9bee7a147429a9e70f1ec17c89be57059 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 14:28:19 +0100 Subject: [PATCH 43/51] distance_sensor: adapted message to float for distance and adapted the drivers --- msg/distance_sensor.msg | 14 +++++++------- src/drivers/mb12xx/mb12xx.cpp | 12 ++++++------ src/drivers/sf0x/sf0x.cpp | 12 ++++++------ src/drivers/trone/trone.cpp | 10 +++++----- 4 files changed, 24 insertions(+), 24 deletions(-) diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg index 8e44d76f79..ad01ce63e0 100644 --- a/msg/distance_sensor.msg +++ b/msg/distance_sensor.msg @@ -1,17 +1,17 @@ # DISTANCE_SENSOR message data -uint32 time_boot_ms # Time since system boot (us) +uint64 timestamp # Microseconds since system boot -uint16 min_distance # Minimum distance the sensor can measure (in SI) -uint16 max_distance # Maximum distance the sensor can measure (in SI) -uint16 current_distance # Current distance reading (in SI) +float32 min_distance # Minimum distance the sensor can measure (in m) +float32 max_distance # Maximum distance the sensor can measure (in m) +float32 current_distance # Current distance reading (in m) +float32 covariance # Measurement covariance (in m), 0 for unknown / invalid readings -uint8 type # Type from MAV_DISTANCE_SENSOR enum +uint8 type # Type from MAV_DISTANCE_SENSOR enum uint8 MAV_DISTANCE_SENSOR_LASER = 0 uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 -uint8 id # Onboard ID of the sensor +uint8 id # Onboard ID of the sensor uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum -uint8 covariance # Measurement covariance (in SI), 0 for unknown / invalid readings \ No newline at end of file diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 432b579193..7ecfa21c9d 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -570,18 +570,18 @@ MB12XX::collect() return ret; } - uint16_t distance = val[0] << 8 | val[1]; - float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ - struct distance_sensor_s report; + uint16_t distance_cm = val[0] << 8 | val[1]; + float distance_m = float(distance_cm) * 1e-2f; - report.time_boot_ms = hrt_absolute_time(); + struct distance_sensor_s report; + report.timestamp = hrt_absolute_time(); report.id = 1; report.type = 1; report.orientation = 8; - report.current_distance = si_units; + report.current_distance = distance_m; report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); - report.covariance = 0.0; + report.covariance = 0.0f; /* publish it, if we are the primary */ if (_distance_sensor_topic >= 0) { diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 4c85bc8c4c..86a4967f19 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -557,11 +557,11 @@ SF0X::collect() _last_read = hrt_absolute_time(); - float si_units; + float distance_m = -1.0f; bool valid = false; for (int i = 0; i < ret; i++) { - if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) { + if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) { valid = true; } } @@ -570,18 +570,18 @@ SF0X::collect() return -EAGAIN; } - debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); + debug("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO")); struct distance_sensor_s report; - report.time_boot_ms = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); report.id = 2; report.type = 0; report.orientation = 8; - report.current_distance = si_units; + report.current_distance = distance_m; report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); - report.covariance = 0.0; + report.covariance = 0.0f; /* publish it */ orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 460f5570a8..ffc262a570 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -567,18 +567,18 @@ TRONE::collect() return ret; } - uint16_t distance = (val[0] << 8) | val[1]; - float si_units = distance * 0.001f; /* mm to m */ + uint16_t distance_mm = (val[0] << 8) | val[1]; + float distance_m = float(distance_mm) * 1e-3f; struct distance_sensor_s report; - report.time_boot_ms = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); report.id = 3; report.type = 0; report.orientation = 8; - report.current_distance = si_units; + report.current_distance = distance_m; report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); - report.covariance = 0.0; + report.covariance = 0.0f; /* publish it, if we are the primary */ if (_distance_sensor_topic >= 0) { From 0128fef8e0e7e7683a6c38b1b7ff0f20475852fa Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 15:57:42 +0100 Subject: [PATCH 44/51] drivers: more adaptations to changed message --- src/drivers/mb12xx/mb12xx.cpp | 9 +++++---- src/drivers/sf0x/sf0x.cpp | 9 +++++---- src/drivers/trone/trone.cpp | 10 ++++++---- 3 files changed, 16 insertions(+), 12 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 7ecfa21c9d..7fa38e0ab4 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -575,13 +575,14 @@ MB12XX::collect() struct distance_sensor_s report; report.timestamp = hrt_absolute_time(); - report.id = 1; - report.type = 1; + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; report.orientation = 8; report.current_distance = distance_m; report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; + /* TODO: set proper ID */ + report.id = 0; /* publish it, if we are the primary */ if (_distance_sensor_topic >= 0) { @@ -828,7 +829,7 @@ test() } warnx("single read"); - warnx("time: %d", report.time_boot_ms); + warnx("time: %llu", report.timestamp); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -856,7 +857,7 @@ test() } warnx("periodic read %u", i); - warnx("time: %d", report.time_boot_ms); + warnx("time: %llu", report.timestamp); } /* reset the sensor polling to default rate */ diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 86a4967f19..71660c390a 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -575,13 +575,14 @@ SF0X::collect() struct distance_sensor_s report; report.timestamp = hrt_absolute_time(); - report.id = 2; - report.type = 0; + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; report.orientation = 8; report.current_distance = distance_m; report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; + /* TODO: set proper ID */ + report.id = 0; /* publish it */ orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); @@ -835,7 +836,7 @@ test() warnx("single read"); warnx("val: %0.2f m", (double)report.current_distance); - warnx("time: %d", report.time_boot_ms); + warnx("time: %llu", report.timestamp); /* start the sensor polling at 2 Hz rate */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -866,7 +867,7 @@ test() warnx("read #%u", i); warnx("val: %0.3f m", (double)report.current_distance); - warnx("time: %d", report.time_boot_ms); + warnx("time: %llu", report.timestamp); } /* reset the sensor polling to the default rate */ diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index ffc262a570..aba9749717 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -572,13 +572,15 @@ TRONE::collect() struct distance_sensor_s report; report.timestamp = hrt_absolute_time(); - report.id = 3; - report.type = 0; + /* there is no enum item for a combined LASER and ULTRASOUND which it should be */ + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; report.orientation = 8; report.current_distance = distance_m; report.min_distance = get_minimum_distance(); report.max_distance = get_maximum_distance(); report.covariance = 0.0f; + /* TODO: set proper ID */ + report.id = 0; /* publish it, if we are the primary */ if (_distance_sensor_topic >= 0) { @@ -807,7 +809,7 @@ test() warnx("single read"); warnx("measurement: %0.2f m", (double)report.current_distance); - warnx("time: %d", report.time_boot_ms); + warnx("time: %llu", report.timestamp); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -836,7 +838,7 @@ test() warnx("periodic read %u", i); warnx("measurement: %0.3f", (double)report.current_distance); - warnx("time: %d", report.time_boot_ms); + warnx("time: %llu", report.timestamp); } /* reset the sensor polling to default rate */ From 13a4cdd05e59cbce249c259bf553a08be2e43475 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 15:58:15 +0100 Subject: [PATCH 45/51] ekf_att_pos_estimator: adapted to change in distance_sensor msg --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3c3e2d2f35..c04d79f4e7 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1475,7 +1475,7 @@ void AttitudePositionEstimatorEKF::pollData() if ((_distance.current_distance > _distance.min_distance) && (_distance.current_distance < _distance.max_distance)) { _ekf->rngMea = _distance.current_distance; - _distance_last_valid = _distance.time_boot_ms; + _distance_last_valid = _distance.timestamp; } else { _newRangeData = false; From cf39e8c721c7a20374300576a24bd813e341265c Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 15:58:44 +0100 Subject: [PATCH 46/51] mavlink: adapted to change in distance_sensor msg --- src/modules/mavlink/mavlink_messages.cpp | 12 +++++++----- src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++++-------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 650e9bf81a..1b2689e6bb 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2228,7 +2228,10 @@ protected: mavlink_distance_sensor_t msg; - msg.time_boot_ms = dist_sensor.time_boot_ms; + msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */ + + /* TODO: use correct ID here */ + msg.id = 0; switch (dist_sensor.type) { case MAV_DISTANCE_SENSOR_ULTRASOUND: @@ -2248,11 +2251,10 @@ protected: break; } - msg.id = dist_sensor.id; msg.orientation = dist_sensor.orientation; - msg.min_distance = dist_sensor.min_distance; - msg.max_distance = dist_sensor.max_distance; - msg.current_distance = dist_sensor.current_distance; + msg.min_distance = dist_sensor.min_distance * 100.0f; /* m to cm */ + msg.max_distance = dist_sensor.max_distance * 100.0f; /* m to cm */ + msg.current_distance = dist_sensor.current_distance * 100.0f; /* m to cm */ msg.covariance = dist_sensor.covariance; _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 25d577dd00..6fbb30a829 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -420,10 +420,10 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) struct distance_sensor_s d; memset(&d, 0, sizeof(d)); - d.time_boot_ms = hrt_absolute_time(); + d.timestamp = hrt_absolute_time(); d.min_distance = 0.3f; d.max_distance = 5.0f; - d.current_distance = flow.distance; + d.current_distance = flow.distance; /* both are in m */ d.type = 1; d.id = 0; d.orientation = 8; @@ -470,10 +470,10 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) struct distance_sensor_s d; memset(&d, 0, sizeof(d)); - d.time_boot_ms = hrt_absolute_time(); + d.timestamp = hrt_absolute_time(); d.min_distance = 0.3f; d.max_distance = 5.0f; - d.current_distance = flow.distance; + d.current_distance = flow.distance; /* both are in m */ d.type = 1; d.id = 0; d.orientation = 8; @@ -530,10 +530,10 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) struct distance_sensor_s d; memset(&d, 0, sizeof(d)); - d.time_boot_ms = dist_sensor.time_boot_ms; - d.min_distance = dist_sensor.min_distance; - d.max_distance = dist_sensor.max_distance; - d.current_distance = dist_sensor.current_distance; + d.timestamp = dist_sensor.time_boot_ms * 1000; /* ms to us */ + d.min_distance = float(dist_sensor.min_distance) * 1e-2f; /* cm to m */ + d.max_distance = float(dist_sensor.max_distance) * 1e-2f; /* cm to m */ + d.current_distance = float(dist_sensor.current_distance) * 1e-2f; /* cm to m */ d.type = dist_sensor.type; d.id = dist_sensor.id; d.orientation = dist_sensor.orientation; From a729d6f3014b5fedb2458d6e63d08d06ffd6d44d Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 15:59:29 +0100 Subject: [PATCH 47/51] ll40ls: first stab at adapting ll40ls to the new distance_sensor msg --- src/drivers/ll40ls/LidarLiteI2C.cpp | 52 ++++++++++++++--------------- src/drivers/ll40ls/LidarLiteI2C.h | 3 +- src/drivers/ll40ls/LidarLitePWM.cpp | 52 ++++++++++++++--------------- src/drivers/ll40ls/LidarLitePWM.h | 5 +-- src/drivers/ll40ls/ll40ls.cpp | 6 ++-- 5 files changed, 58 insertions(+), 60 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index e2195ae029..4a2922f7df 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -61,7 +61,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : _sensor_ok(false), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_i2c_overflows")), @@ -115,7 +115,7 @@ int LidarLiteI2C::init() } /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); if (_reports == nullptr) { goto out; @@ -125,13 +125,13 @@ int LidarLiteI2C::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; + struct distance_sensor_s ds_report; measure(); - _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _reports->get(&ds_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report); - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + if (_distance_sensor_topic < 0) { + debug("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -229,8 +229,8 @@ int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -400,11 +400,11 @@ int LidarLiteI2C::collect() return ret; } - uint16_t distance = (val[0] << 8) | val[1]; - float si_units = distance * 0.01f; /* cm to m */ - struct range_finder_report report; + uint16_t distance_cm = (val[0] << 8) | val[1]; + float distance_m = float(distance_cm) * 1e-2f; + struct distance_sensor_s report; - if (distance == 0) { + if (distance_cm == 0) { _zero_counter++; if (_zero_counter == 20) { @@ -425,25 +425,23 @@ int LidarLiteI2C::collect() _zero_counter = 0; } - _last_distance = distance; + _last_distance = distance_m; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - - if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { - report.valid = 1; - - } else { - report.valid = 0; - } + report.current_distance = distance_m; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0f; + /* the sensor is in fact a laser + sonar but there is no enum for this */ + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + report.orientation = 8; + /* TODO: set proper ID */ + report.id = 0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index cd80b162cb..9f58475b98 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -50,6 +50,7 @@ #include #include +#include #include /* Configuration Constants */ @@ -103,7 +104,7 @@ private: bool _collect_phase; int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 2ebdc6fd7e..2adb9e8ed3 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -60,7 +60,7 @@ LidarLitePWM::LidarLitePWM(const char *path) : _class_instance(-1), _pwmSub(-1), _pwm{}, - _range_finder_topic(-1), + _distance_sensor_topic(-1), _range{}, _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_pwm_read")), _read_errors(perf_alloc(PC_COUNT, "ll40ls_pwm_read_errors")), @@ -82,7 +82,7 @@ LidarLitePWM::~LidarLitePWM() unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); } - // free perf counters + /* free perf counters */ perf_free(_sample_perf); perf_free(_buffer_overflows); perf_free(_sensor_zero_resets); @@ -91,7 +91,6 @@ LidarLitePWM::~LidarLitePWM() int LidarLitePWM::init() { - /* do regular cdev init */ int ret = CDev::init(); @@ -100,7 +99,7 @@ int LidarLitePWM::init() } /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); if (_reports == nullptr) { return ERROR; @@ -109,14 +108,14 @@ int LidarLitePWM::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; + /* get a publish handle on the distance_sensor topic */ + struct distance_sensor_s ds_report; measure(); - _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _reports->get(&ds_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report); - if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + if (_distance_sensor_topic < 0) { + debug("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -130,7 +129,7 @@ void LidarLitePWM::print_info() perf_print_counter(_buffer_overflows); perf_print_counter(_sensor_zero_resets); warnx("poll interval: %u ticks", getMeasureTicks()); - warnx("distance: %.3fm", (double)_range.distance); + warnx("distance: %.3fm", (double)_range.current_distance); } void LidarLitePWM::print_registers() @@ -180,26 +179,25 @@ int LidarLitePWM::measure() return ERROR; } - _range.type = RANGE_FINDER_TYPE_LASER; - _range.error_count = _pwm.error_count; - _range.maximum_distance = get_maximum_distance(); - _range.minimum_distance = get_minimum_distance(); - _range.distance = _pwm.pulse_width / 1000.0f; //10 usec = 1 cm distance for LIDAR-Lite - _range.distance_vector[0] = _range.distance; - _range.just_updated = 0; - _range.valid = true; + _range.timestamp = hrt_absolute_time(); + _range.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + _range.max_distance = get_maximum_distance(); + _range.min_distance = get_minimum_distance(); + _range.current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */ + _range.covariance = 0.0f; + _range.orientation = 8; + /* TODO: set proper ID */ + _range.id = 0; - // Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) - if (_range.distance <= 0.0f) { - _range.valid = false; - _range.error_count++; + /* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */ + if (_range.current_distance <= 0.0f) { perf_count(_sensor_zero_resets); perf_end(_sample_perf); return reset_sensor(); } - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_range); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &_range); } if (_reports->force(&_range)) { @@ -213,8 +211,8 @@ int LidarLitePWM::measure() ssize_t LidarLitePWM::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index d2cc7f36e4..93cb80e29a 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -53,6 +53,7 @@ #include #include +#include @@ -109,8 +110,8 @@ private: int _class_instance; int _pwmSub; struct pwm_input_s _pwm; - orb_advert_t _range_finder_topic; - range_finder_report _range; + orb_advert_t _distance_sensor_topic; + struct distance_sensor_s _range; perf_counter_t _sample_perf; perf_counter_t _read_errors; diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 9c52ce6e9a..8984220625 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -239,7 +239,7 @@ void stop(const bool use_i2c, const int bus) void test(const bool use_i2c, const int bus) { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -266,7 +266,7 @@ test(const bool use_i2c, const int bus) } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); + warnx("measurement: %0.2f m", (double)report.current_distance); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ @@ -295,7 +295,7 @@ test(const bool use_i2c, const int bus) } warnx("periodic read %u", i); - warnx("measurement: %0.3f m", (double)report.distance); + warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %lld", report.timestamp); } From 72e9390985e51a7b2ff8c3ebc7b5dab064e2d808 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sun, 24 May 2015 16:03:01 +0100 Subject: [PATCH 48/51] distance_sensor: minor changes --- ROMFS/px4fmu_common/init.d/rc.usb | 1 + src/drivers/drv_range_finder.h | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/sdlog2/sdlog2_messages.h | 3 +-- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 0442637941..4c2119aa2f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -9,6 +9,7 @@ mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 +mavlink stream -d /dev/ttyACM0 -s DISTANCE_SENSOR -r 10 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30 diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 035eb92e0c..725835cc0a 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -49,7 +49,7 @@ #define MB12XX_MAX_RANGEFINDERS 12 // Maximum number of Maxbotix sensors on bus /* - * ObjDev tag for px4flow data. + * ObjDev tag for distance sensor data. */ ORB_DECLARE(distance_sensor); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 25d577dd00..4ed7ffc501 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -420,7 +420,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) struct distance_sensor_s d; memset(&d, 0, sizeof(d)); - d.time_boot_ms = hrt_absolute_time(); + d.time_boot_ms = flow.integration_time_us; d.min_distance = 0.3f; d.max_distance = 5.0f; d.current_distance = flow.distance; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index eee44c43b9..062ab88f4f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -488,7 +488,6 @@ struct log_PARM_s { }; #pragma pack(pop) - /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ @@ -516,7 +515,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), - LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), + LOG_FORMAT(DIST, "iiff", "Type,Orientation,Distance,Covariance"), LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), From 517acd4586a0bb71c7bebb6d15b6cedcb597b4d9 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 17:31:16 +0100 Subject: [PATCH 49/51] px4flow: publish sonar distance to distance_sensor topic as well --- src/drivers/px4flow/px4flow.cpp | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 319c902f12..2993037cff 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -34,6 +34,7 @@ /** * @file px4flow.cpp * @author Dominik Honegger + * @author Ban Siesta * * Driver for the PX4FLOW module connected via I2C. */ @@ -71,6 +72,7 @@ #include #include #include +#include #include @@ -83,6 +85,9 @@ #define PX4FLOW_CONVERSION_INTERVAL 100000 ///< in microseconds! 20000 = 50 Hz 100000 = 10Hz #define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed +#define PX4FLOW_MAX_DISTANCE 5.0f +#define PX4FLOW_MIN_DISTANCE 0.3f + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -125,6 +130,7 @@ private: int _measure_ticks; bool _collect_phase; orb_advert_t _px4flow_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -183,6 +189,7 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : _measure_ticks(0), _collect_phase(false), _px4flow_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")), @@ -499,6 +506,25 @@ PX4FLOW::collect() orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); } + /* publish to the distance_sensor topic as well */ + struct distance_sensor_s distance_report; + distance_report.timestamp = report.timestamp; + distance_report.min_distance = PX4FLOW_MIN_DISTANCE; + distance_report.max_distance = PX4FLOW_MAX_DISTANCE; + distance_report.current_distance = report.ground_distance_m; + distance_report.covariance = 0.0f; + distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + /* TODO: the ID needs to be properly set */ + distance_report.id = 0; + distance_report.orientation = 8; + + if (_distance_sensor_topic < 0) { + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &distance_report); + } else { + /* publish it */ + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report); + } + /* post a report to the ring */ if (_reports->force(&report)) { perf_count(_buffer_overflows); From 69cbbd9b5eae2c475549518b1785bd1f5edeb7ea Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 19:20:25 +0100 Subject: [PATCH 50/51] distance_sensor: changed all distance sensors to orb_advertise_multi --- src/drivers/ll40ls/LidarLiteI2C.cpp | 4 +++- src/drivers/ll40ls/LidarLiteI2C.h | 1 + src/drivers/ll40ls/LidarLitePWM.cpp | 4 +++- src/drivers/ll40ls/LidarLitePWM.h | 1 + src/drivers/mb12xx/mb12xx.cpp | 9 +++++--- src/drivers/px4flow/px4flow.cpp | 32 ++++++++++++++++++-------- src/drivers/sf0x/sf0x.cpp | 35 ++++++++++++++++++++++------- src/drivers/trone/trone.cpp | 16 ++++++++----- 8 files changed, 74 insertions(+), 28 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 4a2922f7df..a9626c8c45 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -61,6 +61,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : _sensor_ok(false), _collect_phase(false), _class_instance(-1), + _orb_class_instance(-1), _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")), @@ -128,7 +129,8 @@ int LidarLiteI2C::init() struct distance_sensor_s ds_report; measure(); _reports->get(&ds_report); - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic < 0) { debug("failed to create distance_sensor object. Did you start uOrb?"); diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index 9f58475b98..f03784fc4d 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -103,6 +103,7 @@ private: bool _sensor_ok; bool _collect_phase; int _class_instance; + int _orb_class_instance; orb_advert_t _distance_sensor_topic; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 2adb9e8ed3..14ce921b4d 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -58,6 +58,7 @@ LidarLitePWM::LidarLitePWM(const char *path) : _work{}, _reports(nullptr), _class_instance(-1), + _orb_class_instance(-1), _pwmSub(-1), _pwm{}, _distance_sensor_topic(-1), @@ -112,7 +113,8 @@ int LidarLitePWM::init() struct distance_sensor_s ds_report; measure(); _reports->get(&ds_report); - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic < 0) { debug("failed to create distance_sensor object. Did you start uOrb?"); diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index 93cb80e29a..8d337e48ba 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -108,6 +108,7 @@ private: work_s _work; ringbuffer::RingBuffer *_reports; int _class_instance; + int _orb_class_instance; int _pwmSub; struct pwm_input_s _pwm; orb_advert_t _distance_sensor_topic; diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 7fa38e0ab4..fcac2e1b81 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -130,7 +130,8 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - int _class_instance; + int _class_instance; + int _orb_class_instance; orb_advert_t _distance_sensor_topic; @@ -209,6 +210,7 @@ MB12XX::MB12XX(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), + _orb_class_instance(-1), _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), @@ -269,9 +271,10 @@ MB12XX::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct distance_sensor_s rf_report = {}; + struct distance_sensor_s ds_report = {}; - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic < 0) { log("failed to create sensor_range_finder object. Did you start uOrb?"); diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 2993037cff..e48b0b3b48 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -67,6 +67,7 @@ #include #include +#include #include #include @@ -125,10 +126,12 @@ protected: private: work_s _work; - ringbuffer::RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; - int _measure_ticks; + int _measure_ticks; bool _collect_phase; + int _class_instance; + int _orb_class_instance; orb_advert_t _px4flow_topic; orb_advert_t _distance_sensor_topic; @@ -188,6 +191,8 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), + _class_instance(-1), + _orb_class_instance(-1), _px4flow_topic(-1), _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), @@ -195,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")), _sensor_rotation(rotation) { - // enable debug() calls + // disable debug() calls _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... @@ -230,6 +235,20 @@ PX4FLOW::init() goto out; } + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* 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 < 0) { + log("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -518,12 +537,7 @@ PX4FLOW::collect() distance_report.id = 0; distance_report.orientation = 8; - if (_distance_sensor_topic < 0) { - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &distance_report); - } else { - /* publish it */ - orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report); - } + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report); /* post a report to the ring */ if (_reports->force(&report)) { diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 71660c390a..bace2aaa0d 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * 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 @@ -119,7 +119,7 @@ private: float _min_distance; float _max_distance; work_s _work; - ringbuffer::RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -129,6 +129,9 @@ private: enum SF0X_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; @@ -195,6 +198,8 @@ SF0X::SF0X(const char *port) : _linebuf_index(0), _parse_state(SF0X_PARSE_STATE0_UNSYNC), _last_read(0), + _class_instance(-1), + _orb_class_instance(-1), _distance_sensor_topic(-1), _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), @@ -256,6 +261,14 @@ SF0X::~SF0X() 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); + perf_free(_buffer_overflows); } int @@ -278,14 +291,20 @@ SF0X::init() break; } - /* get a publish handle on the range finder topic */ - struct distance_sensor_s zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &zero_report); + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_distance_sensor_topic < 0) { - warnx("advert err"); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* 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 < 0) { + log("failed to create sensor_range_finder object. Did you start uOrb?"); + } } + } while(0); /* close the fd */ diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index aba9749717..691c509e23 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-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 @@ -127,7 +127,8 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - int _class_instance; + int _class_instance; + int _orb_class_instance; orb_advert_t _distance_sensor_topic; @@ -236,6 +237,7 @@ TRONE::TRONE(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), + _orb_class_instance(-1), _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "trone_read")), _comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")), @@ -292,13 +294,15 @@ TRONE::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct distance_sensor_s rf_report; + struct distance_sensor_s ds_report; measure(); - _reports->get(&rf_report); - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); + _reports->get(&ds_report); + + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + log("failed to create sensor_range_finder object. Did you start uOrb?"); } } From 9f314dd1918d2fdb2b0bc5819db9424b2e707af6 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 20:56:25 +0100 Subject: [PATCH 51/51] drivers: corrected log string --- src/drivers/mb12xx/mb12xx.cpp | 2 +- src/drivers/px4flow/px4flow.cpp | 2 +- src/drivers/sf0x/sf0x.cpp | 2 +- src/drivers/trone/trone.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index fcac2e1b81..afeb8e5545 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -277,7 +277,7 @@ MB12XX::init() &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic < 0) { - log("failed to create sensor_range_finder object. Did you start uOrb?"); + log("failed to create distance_sensor object. Did you start uOrb?"); } } diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index e48b0b3b48..4d87d1cfe1 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -245,7 +245,7 @@ PX4FLOW::init() &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic < 0) { - log("failed to create sensor_range_finder object. Did you start uOrb?"); + log("failed to create distance_sensor object. Did you start uOrb?"); } } diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index bace2aaa0d..431184d046 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -301,7 +301,7 @@ SF0X::init() &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic < 0) { - log("failed to create sensor_range_finder object. Did you start uOrb?"); + log("failed to create distance_sensor object. Did you start uOrb?"); } } diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 691c509e23..23e52547a1 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -302,7 +302,7 @@ TRONE::init() &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic < 0) { - log("failed to create sensor_range_finder object. Did you start uOrb?"); + log("failed to create distance_sensor object. Did you start uOrb?"); } }