fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 02:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

LidarLite: Added PWM version of the LidarLite driver

Browse Source
This commit is contained in:
Johan Jansen
2015-03-24 11:31:11 +01:00
parent eeb8562c6e
commit b1da12b43f
8 changed files with 460 additions and 160 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ll40ls/LidarLite.cpp
+172
View File
@@ -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 <jnsn.johan@gmail.com>
*
* Generic interface driver for the PulsedLight Lidar-Lite range finders.
*/
#include "LidarLite.h"
#include <errno.h>
#include <nuttx/clock.h>
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;
}
}
src/drivers/ll40ls/LidarLite.h
+103
View File
@@ -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 <jnsn.johan@gmail.com>
*
* Generic interface driver for the PulsedLight Lidar-Lite range finders.
*/
#pragma once
#include <drivers/device/device.h>
#include <drivers/drv_range_finder.h>
/* 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;
};
src/drivers/ll40ls/LidarLiteI2C.cpp
+26 -126
View File
@@ -39,7 +39,6 @@
* Driver for the PulsedLight Lidar-Lite range finders connected via I2C.
*/
#include <nuttx/config.h>
#include "LidarLiteI2C.h"
#include <semaphore.h>
#include <fcntl.h>
@@ -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);
src/drivers/ll40ls/LidarLiteI2C.h
+9 -33
View File
@@ -31,7 +31,6 @@
*
****************************************************************************/
/**
* @file LidarLiteI2C.h
* @author Allyson Kreft
@@ -44,12 +43,13 @@
//Forward declaration
class RingBuffer;
#include "LidarLite.h"
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/perf_counter.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_range_finder.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
@@ -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
src/drivers/ll40ls/LidarLitePWM.cpp
+75
View File
@@ -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 <jnsn.johan@gmail.com>
*
* Driver for the PulsedLight Lidar-Lite range finders connected via PWM.
*/
#include "LidarLitePWM.h"
#include <stdio.h>
/* 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");
}
src/drivers/ll40ls/LidarLitePWM.h
+69
View File
@@ -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 <jnsn.johan@gmail.com>
*
* Driver for the PulsedLight Lidar-Lite range finders connected via PWM.
*/
#pragma once
#include "LidarLite.h"
#include <uORB/uORB.h>
#include <uORB/topics/pwm_input.h>
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;
};
src/drivers/ll40ls/ll40ls.cpp
+3
View File
@@ -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.
*/
src/drivers/ll40ls/module.mk
+3 -1
View File
@@ -38,6 +38,8 @@
MODULE_COMMAND = ll40ls
SRCS = ll40ls.cpp \
LidarLiteI2C.cpp
LidarLite.cpp \
LidarLiteI2C.cpp \
LidarLitePWM.cpp
MAXOPTIMIZATION = -Os -Weffc++