From b084558a793137e1c57d6ecd90d68303f5b128a2 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 24 Mar 2015 10:04:40 +0100 Subject: [PATCH 1/7] 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 2/7] 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 3/7] 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 4/7] 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 5/7] 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 6/7] 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 7/7] 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;