fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 05:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ll40ls: made PWM driver version functional

Browse Source
This commit is contained in:
Ban Siesta
2015-05-24 12:47:04 +01:00
parent ef658cf926
commit e7f1f3ba28
2 changed files with 238 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ll40ls/LidarLitePWM.cpp
+195 -35
View File
@@ -35,48 +35,110 @@
/**
* @file LidarLitePWM.h
* @author Johan Jansen <jnsn.johan@gmail.com>
* @author Ban Siesta <bansiesta@gmail.com>
*
* 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 <stdio.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_input.h>
/* 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<struct range_finder_report *>(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;
}
src/drivers/ll40ls/LidarLitePWM.h
+43 -7
View File
@@ -35,26 +35,44 @@
/**
* @file LidarLitePWM.h
* @author Johan Jansen <jnsn.johan@gmail.com>
* @author Ban Siesta <bansiesta@gmail.com>
*
* 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 <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <drivers/device/ringbuffer.h>
#include <systemlib/perf_counter.h>
#include <uORB/uORB.h>
#include <uORB/topics/pwm_input.h>
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;
};