From e7f1f3ba281935e68f5b59194d72b039ae9cb934 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 12:47:04 +0100 Subject: [PATCH] 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; };