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

Merge pull request #2224 from bansiesta/lidarlite_merge

Browse Source 
Lidarlite merge and bringup
This commit is contained in:
Lorenz Meier
2015-05-24 23:18:52 +02:00
parent 5dfa3d29bd 9f314dd191
commit c5f14ef785
43 changed files with 2222 additions and 1285 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1
View File
@@ -80,6 +80,7 @@ add_message_files(
vehicle_global_velocity_setpoint.msg
offboard_control_mode.msg
vehicle_force_setpoint.msg
distance_sensor.msg
)
## Generate services in the 'srv' folder
ROMFS/px4fmu_common/init.d/rc.usb
+1
View File
@@ -9,6 +9,7 @@ mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300
mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10
mavlink stream -d /dev/ttyACM0 -s DISTANCE_SENSOR -r 10
mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30
makefiles/nuttx/config_px4fmu-v2_default.mk
+1
View File
@@ -45,6 +45,7 @@ MODULES += drivers/mkblctrl
MODULES += drivers/px4flow
MODULES += drivers/oreoled
MODULES += drivers/gimbal
MODULES += drivers/pwm_input
#
# System commands
msg/distance_sensor.msg
+17
View File
@@ -0,0 +1,17 @@
# DISTANCE_SENSOR message data
uint64 timestamp # Microseconds since system boot
float32 min_distance # Minimum distance the sensor can measure (in m)
float32 max_distance # Maximum distance the sensor can measure (in m)
float32 current_distance # Current distance reading (in m)
float32 covariance # Measurement covariance (in m), 0 for unknown / invalid readings
uint8 type # Type from MAV_DISTANCE_SENSOR enum
uint8 MAV_DISTANCE_SENSOR_LASER = 0
uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1
uint8 MAV_DISTANCE_SENSOR_INFRARED = 2
uint8 id # Onboard ID of the sensor
uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
msg/range_finder.msg
-13
View File
@@ -1,13 +0,0 @@
int16 RANGE_FINDER_TYPE_LASER = 0
# range finder report structure. Reads from the device must be in multiples of this
# structure.
uint64 timestamp
uint64 error_count
uint16 type # type, following RANGE_FINDER_TYPE enum
float32 distance # in meters
float32 minimum_distance # minimum distance the sensor can measure
float32 maximum_distance # maximum distance the sensor can measure
uint8 valid # 1 == within sensor range, 0 = outside sensor range
float32[12] distance_vector # in meters length should match MB12XX_MAX_RANGEFINDERS
uint8 just_updated # number of the most recent measurement sensor
src/drivers/airspeed/airspeed.cpp
+1 -1
View File
@@ -128,7 +128,7 @@ Airspeed::init()
goto out;
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(differential_pressure_s));
_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));
if (_reports == nullptr)
goto out;
src/drivers/airspeed/airspeed.h
+1 -1
View File
@@ -109,7 +109,7 @@ public:
virtual void print_info();
private:
RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
perf_counter_t _buffer_overflows;
/* this class has pointer data members and should not be copied */
src/drivers/boards/px4fmu-v2/board_config.h
+1 -1
View File
@@ -161,7 +161,7 @@ __BEGIN_DECLS
#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14)
/* Power supply control and monitoring GPIOs */
#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+1
View File
@@ -243,6 +243,7 @@ __EXPORT int nsh_archinitialize(void)
stm32_configgpio(GPIO_VDD_SERVO_VALID);
stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
stm32_configgpio(GPIO_GPIO5_OUTPUT);
/* configure the high-resolution time/callout interface */
hrt_init();
src/drivers/device/module.mk
+3 -2
View File
@@ -41,7 +41,8 @@ SRCS = \
cdev.cpp \
i2c_nuttx.cpp \
pio.cpp \
spi.cpp
spi.cpp \
ringbuffer.cpp
else
SRCS = \
device_posix.cpp \
@@ -50,5 +51,5 @@ SRCS = \
vdev_posix.cpp \
i2c_posix.cpp \
sim.cpp \
ringbuffer.cpp
ringbuffer.cpp
endif
src/drivers/device/ringbuffer.cpp
+21 -16
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -40,6 +40,9 @@
#include "ringbuffer.h"
#include <string.h>
namespace ringbuffer
{
RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
_num_items(num_items),
_item_size(item_size),
@@ -57,25 +60,25 @@ RingBuffer::~RingBuffer()
unsigned
RingBuffer::_next(unsigned index)
{
return (0 == index) ? _num_items : (index - 1);
return (0 == index) ? _num_items : (index - 1);
}
bool
RingBuffer::empty()
{
return _tail == _head;
return _tail == _head;
}
bool
RingBuffer::full()
{
return _next(_head) == _tail;
return _next(_head) == _tail;
}
unsigned
RingBuffer::size()
{
return (_buf != nullptr) ? _num_items : 0;
return (_buf != nullptr) ? _num_items : 0;
}
void
@@ -86,7 +89,7 @@ RingBuffer::flush()
}
bool
RingBuffer::put(const void *val, size_t val_size)
RingBuffer::put(const void *val, size_t val_size)
{
unsigned next = _next(_head);
if (next != _tail) {
@@ -250,7 +253,7 @@ static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned
#define __PX4_SBCAP __sync_bool_compare_and_swap
#endif
bool
RingBuffer::get(void *val, size_t val_size)
RingBuffer::get(void *val, size_t val_size)
{
if (_tail != _head) {
unsigned candidate;
@@ -340,7 +343,7 @@ RingBuffer::get(double &val)
}
unsigned
RingBuffer::space(void)
RingBuffer::space(void)
{
unsigned tail, head;
@@ -361,7 +364,7 @@ RingBuffer::space(void)
}
unsigned
RingBuffer::count(void)
RingBuffer::count(void)
{
/*
* Note that due to the conservative nature of space(), this may
@@ -371,7 +374,7 @@ RingBuffer::count(void)
}
bool
RingBuffer::resize(unsigned new_size)
RingBuffer::resize(unsigned new_size)
{
char *old_buffer;
char *new_buffer = new char [(new_size+1) * _item_size];
@@ -388,13 +391,15 @@ RingBuffer::resize(unsigned new_size)
}
void
RingBuffer::print_info(const char *name)
RingBuffer::print_info(const char *name)
{
printf("%s %u/%lu (%u/%u @ %p)\n",
name,
_num_items,
(unsigned long)_num_items * _item_size,
_head,
_tail,
name,
_num_items,
(unsigned long)_num_items * _item_size,
_head,
_tail,
_buf);
}
} // namespace ringbuffer
src/drivers/device/ringbuffer.h
+6 -8
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -43,6 +43,9 @@
#include <stdint.h>
#include <stdio.h>
namespace ringbuffer __EXPORT
{
class RingBuffer {
public:
RingBuffer(unsigned ring_size, size_t entry_size);
@@ -161,7 +164,7 @@ public:
private:
unsigned _num_items;
const size_t _item_size;
char *_buf;
char *_buf;
volatile unsigned _head; /**< insertion point in _item_size units */
volatile unsigned _tail; /**< removal point in _item_size units */
@@ -172,9 +175,4 @@ private:
RingBuffer operator=(const RingBuffer&);
};
#ifdef __PX4_NUTTX
// Not sure why NuttX requires these to be defined in the header file
// but on other targets it causes a problem with multiple definitions
// at link time
#include "ringbuffer.cpp"
#endif
} // namespace ringbuffer
src/drivers/drv_range_finder.h
+5 -9
View File
@@ -46,16 +46,12 @@
#define RANGE_FINDER_BASE_DEVICE_PATH "/dev/range_finder"
#define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0"
#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected
#define MB12XX_MAX_RANGEFINDERS 12 // Maximum number of Maxbotix sensors on bus
#define range_finder_report range_finder_s
#define __orb_sensor_range_finder __orb_range_finder
#include <uORB/topics/range_finder.h>
#ifndef RANGE_FINDER_TYPE_LASER
#define RANGE_FINDER_TYPE_LASER 0
#endif
/*
* ObjDev tag for distance sensor data.
*/
ORB_DECLARE(distance_sensor);
/*
* ioctl() definitions
src/drivers/hmc5883/hmc5883.cpp
+10 -10
View File
@@ -156,7 +156,7 @@ private:
work_s _work;
unsigned _measure_ticks;
RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
mag_scale _scale;
float _range_scale;
float _range_ga;
@@ -423,7 +423,7 @@ HMC5883::init()
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(mag_report));
_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_reports == nullptr)
goto out;
@@ -921,10 +921,10 @@ HMC5883::collect()
_temperature_counter = 0;
ret = _interface->read(ADDR_TEMP_OUT_MSB,
ret = _interface->read(ADDR_TEMP_OUT_MSB,
raw_temperature, sizeof(raw_temperature));
if (ret == OK) {
int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) +
int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) +
raw_temperature[1];
new_report.temperature = 25 + (temp16 / (16*8.0f));
_temperature_error_count = 0;
@@ -1146,8 +1146,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = -EIO;
goto out;
}
float cal[3] = {fabsf(expected_cal[0] / report.x),
fabsf(expected_cal[1] / report.y),
float cal[3] = {fabsf(expected_cal[0] / report.x),
fabsf(expected_cal[1] / report.y),
fabsf(expected_cal[2] / report.z)};
if (cal[0] > 0.7f && cal[0] < 1.35f &&
@@ -1381,7 +1381,7 @@ HMC5883::print_info()
printf("poll interval: %u ticks\n", _measure_ticks);
printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z);
printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
(double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
(double)(1.0f/_range_scale), (double)_range_ga);
printf("temperature %.2f\n", (double)_last_report.temperature);
@@ -1451,7 +1451,7 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
bus.dev = NULL;
return false;
}
int fd = open(bus.devpath, O_RDONLY);
if (fd < 0) {
return false;
@@ -1505,7 +1505,7 @@ struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid)
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
return bus_options[i];
}
}
}
errx(1, "bus %u not started", (unsigned)busid);
}
@@ -1750,7 +1750,7 @@ hmc5883_main(int argc, char *argv[])
}
}
const char *verb = argv[optind];
const char *verb = argv[optind];
/*
* Start/load the driver.
src/drivers/l3gd20/l3gd20.cpp
+2 -2
View File
@@ -229,7 +229,7 @@ private:
struct hrt_call _call;
unsigned _call_interval;
RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
@@ -472,7 +472,7 @@ L3GD20::init()
goto out;
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(gyro_report));
_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_reports == nullptr)
goto out;
src/drivers/ll40ls/LidarLite.cpp
+170
View File
@@ -0,0 +1,170 @@
/****************************************************************************
*
* 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)
{
}
LidarLite::~LidarLite()
{
}
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
+105
View File
@@ -0,0 +1,105 @@
/****************************************************************************
*
* 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;
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;
virtual int measure() = 0;
virtual int collect() = 0;
virtual int reset_sensor() = 0;
private:
float _min_distance;
float _max_distance;
uint32_t _measure_ticks;
};
src/drivers/ll40ls/LidarLiteI2C.cpp
+568
View File
@@ -0,0 +1,568 @@
/****************************************************************************
*
* 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 "LidarLiteI2C.h"
#include <semaphore.h>
#include <fcntl.h>
#include <errno.h>
#include <poll.h>
#include <string.h>
#include <stdio.h>
#include <drivers/drv_hrt.h>
/* 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),
_work{},
_reports(nullptr),
_sensor_ok(false),
_collect_phase(false),
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")),
_comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_i2c_overflows")),
_sensor_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_resets")),
_sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls_i2c_zero_resets")),
_last_distance(0),
_zero_counter(0),
_acquire_time_usec(0),
_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::RingBuffer(2, sizeof(struct distance_sensor_s));
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 distance_sensor_s ds_report;
measure();
_reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic < 0) {
debug("failed to create distance_sensor 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(&reg, 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 < sizeof(addresses); i++) {
uint8_t who_am_i = 0, max_acq_count = 0;
// set the I2C bus address
set_address(addresses[i]);
/* register 2 defaults to 0x80. If this matches it is
almost certainly a ll40ls */
if (read_reg(LL40LS_MAX_ACQ_COUNT_REG, max_acq_count) == OK && max_acq_count == 0x80) {
// very likely to be a ll40ls. This is the
// default max acquisition counter
goto ok;
}
if (read_reg(LL40LS_WHO_AM_I_REG, who_am_i) == OK && who_am_i == LL40LS_WHO_AM_I_REG_VAL) {
// it is responding correctly to a
// WHO_AM_I. This works with older sensors (pre-production)
goto ok;
}
debug("probe failed reg11=0x%02x reg2=0x%02x\n",
(unsigned)who_am_i,
(unsigned)max_acq_count);
}
// not found on any address
return -EIO;
ok:
_retries = 3;
// reset the sensor to ensure it is in a known state with
// correct settings
return reset_sensor();
}
int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
default: {
int result = LidarLite::ioctl(filp, cmd, arg);
if (result == -EINVAL) {
result = I2C::ioctl(filp, cmd, arg);
}
return result;
}
}
}
ssize_t LidarLiteI2C::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(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;
}
/* 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(&reg, 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_cm = (val[0] << 8) | val[1];
float distance_m = float(distance_cm) * 1e-2f;
struct distance_sensor_s report;
if (distance_cm == 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_m;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
/* the sensor is in fact a laser + sonar but there is no enum for this */
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = 8;
/* TODO: set proper ID */
report.id = 0;
/* publish it, if we are the primary */
if (_distance_sensor_topic >= 0) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_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 (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&LidarLiteI2C::cycle_trampoline,
this,
getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL));
return;
}
}
}
if (_collect_phase == false) {
/* measurement phase */
if (OK != measure()) {
debug("measure error");
} else {
/* next phase is collection. Don't switch to
collection phase until we have a successful
acquire request I2C transfer */
_collect_phase = true;
}
}
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&LidarLiteI2C::cycle_trampoline,
this,
USEC2TICK(LL40LS_CONVERSION_INTERVAL));
}
void LidarLiteI2C::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
perf_print_counter(_sensor_resets);
perf_print_counter(_sensor_zero_resets);
printf("poll interval: %u ticks\n", getMeasureTicks());
_reports->print_info("report queue");
printf("distance: %ucm (0x%04x)\n",
(unsigned)_last_distance, (unsigned)_last_distance);
}
src/drivers/ll40ls/LidarLiteI2C.h
+163
View File
@@ -0,0 +1,163 @@
/****************************************************************************
*
* 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
#include "LidarLite.h"
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/perf_counter.h>
#include <drivers/device/i2c.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/subsystem_info.h>
/* 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 */
/* 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
class LidarLiteI2C : public LidarLite, public device::I2C
{
public:
LidarLiteI2C(int bus, const char *path, int address = LL40LS_BASEADDR);
virtual ~LidarLiteI2C();
int init() override;
ssize_t read(struct file *filp, char *buffer, size_t buflen);
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info() override;
/**
* print registers to console
*/
void print_registers() override;
protected:
int probe();
int read_reg(uint8_t reg, uint8_t &val);
int measure() override;
int reset_sensor();
private:
work_s _work;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
bool _collect_phase;
int _class_instance;
int _orb_class_instance;
orb_advert_t _distance_sensor_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();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
private:
LidarLiteI2C(const LidarLiteI2C &copy) = delete;
LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete;
};
src/drivers/ll40ls/LidarLitePWM.cpp
+295
View File
@@ -0,0 +1,295 @@
/****************************************************************************
*
* 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>
* @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(const char *path) :
CDev("LidarLitePWM", path),
_work{},
_reports(nullptr),
_class_instance(-1),
_orb_class_instance(-1),
_pwmSub(-1),
_pwm{},
_distance_sensor_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()
{
/* do regular cdev init */
int ret = CDev::init();
if (ret != OK) {
return ERROR;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
if (_reports == nullptr) {
return ERROR;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the distance_sensor topic */
struct distance_sensor_s ds_report;
measure();
_reports->get(&ds_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic < 0) {
debug("failed to create distance_sensor object. Did you start uOrb?");
}
}
return OK;
}
void LidarLitePWM::print_info()
{
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.current_distance);
}
void LidarLitePWM::print_registers()
{
printf("Not supported in PWM mode\n");
}
void LidarLitePWM::start()
{
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&LidarLitePWM::cycle_trampoline, this, 1);
}
void LidarLitePWM::stop()
{
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()
{
perf_begin(_sample_perf);
if (OK != collect()) {
debug("collection error");
perf_count(_read_errors);
perf_end(_sample_perf);
return ERROR;
}
_range.timestamp = hrt_absolute_time();
_range.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
_range.max_distance = get_maximum_distance();
_range.min_distance = get_minimum_distance();
_range.current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */
_range.covariance = 0.0f;
_range.orientation = 8;
/* TODO: set proper ID */
_range.id = 0;
/* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */
if (_range.current_distance <= 0.0f) {
perf_count(_sensor_zero_resets);
perf_end(_sample_perf);
return reset_sensor();
}
if (_distance_sensor_topic >= 0) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &_range);
}
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 distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(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()
{
int fd = ::open(PWMIN0_DEVICE_PATH, O_RDONLY);
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
+121
View File
@@ -0,0 +1,121 @@
/****************************************************************************
*
* 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>
* @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>
#include <uORB/topics/distance_sensor.h>
class LidarLitePWM : public LidarLite, public device::CDev
{
public:
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.
*/
void print_info() override;
/**
* @brief
* print registers to console
*/
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:
work_s _work;
ringbuffer::RingBuffer *_reports;
int _class_instance;
int _orb_class_instance;
int _pwmSub;
struct pwm_input_s _pwm;
orb_advert_t _distance_sensor_topic;
struct distance_sensor_s _range;
perf_counter_t _sample_perf;
perf_counter_t _read_errors;
perf_counter_t _buffer_overflows;
perf_counter_t _sensor_zero_resets;
};
src/drivers/ll40ls/ll40ls.cpp
+224 -895
View File
File diff suppressed because it is too large Load Diff
src/drivers/ll40ls/module.mk
+5 -2
View File
@@ -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
@@ -37,6 +37,9 @@
MODULE_COMMAND = ll40ls
SRCS = ll40ls.cpp
SRCS = ll40ls.cpp \
LidarLite.cpp \
LidarLiteI2C.cpp \
LidarLitePWM.cpp
MAXOPTIMIZATION = -Os
src/drivers/lsm303d/lsm303d.cpp
+7 -7
View File
@@ -275,8 +275,8 @@ private:
unsigned _call_accel_interval;
unsigned _call_mag_interval;
RingBuffer *_accel_reports;
RingBuffer *_mag_reports;
ringbuffer::RingBuffer *_accel_reports;
ringbuffer::RingBuffer *_mag_reports;
struct accel_scale _accel_scale;
unsigned _accel_range_m_s2;
@@ -588,7 +588,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
_debug_enabled = true;
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D;
/* Prime _mag with parents devid. */
_mag->_device_id.devid = _device_id.devid;
_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
@@ -646,12 +646,12 @@ LSM303D::init()
}
/* allocate basic report buffers */
_accel_reports = new RingBuffer(2, sizeof(accel_report));
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr)
goto out;
_mag_reports = new RingBuffer(2, sizeof(mag_report));
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr)
goto out;
@@ -1399,9 +1399,9 @@ LSM303D::start()
_mag_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_accel_call,
hrt_call_every(&_accel_call,
1000,
_call_accel_interval - LSM303D_TIMER_REDUCTION,
_call_accel_interval - LSM303D_TIMER_REDUCTION,
(hrt_callout)&LSM303D::measure_trampoline, this);
hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
}
src/drivers/mb12xx/mb12xx.cpp
+31 -64
View File
@@ -70,6 +70,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@@ -125,13 +126,14 @@ private:
float _min_distance;
float _max_distance;
work_s _work;
RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
int _class_instance;
int _orb_class_instance;
orb_advert_t _range_finder_topic;
orb_advert_t _distance_sensor_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
@@ -208,7 +210,8 @@ MB12XX::MB12XX(int bus, int address) :
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_range_finder_topic(-1),
_orb_class_instance(-1),
_distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")),
@@ -255,7 +258,7 @@ MB12XX::init()
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(range_finder_report));
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
_index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */
set_address(_index_counter); /* set I2c port to temp sonar i2c adress */
@@ -268,12 +271,13 @@ MB12XX::init()
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct range_finder_report rf_report = {};
struct distance_sensor_s ds_report = {};
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_range_finder_topic < 0) {
log("failed to create sensor_range_finder object. Did you start uOrb?");
if (_distance_sensor_topic < 0) {
log("failed to create distance_sensor object. Did you start uOrb?");
}
}
@@ -469,8 +473,8 @@ ssize_t
MB12XX::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
@@ -569,53 +573,23 @@ MB12XX::collect()
return ret;
}
uint16_t distance = val[0] << 8 | val[1];
float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
struct range_finder_report report;
uint16_t distance_cm = val[0] << 8 | val[1];
float distance_m = float(distance_cm) * 1e-2f;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
/* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */
if (addr_ind.size() == 1) {
report.distance = si_units;
for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) {
report.distance_vector[i] = 0;
}
report.just_updated = 0;
} else {
/* for multiple sonars connected */
/* don't use the orginial single sonar variable */
report.distance = 0;
/* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */
_latest_sonar_measurements[_cycle_counter] = si_units;
for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) {
report.distance_vector[i] = _latest_sonar_measurements[i];
}
/* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */
report.just_updated = _index_counter;
/* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */
for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) {
report.distance_vector[addr_ind.size() + i] = 0;
}
}
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
report.orientation = 8;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
/* TODO: set proper ID */
report.id = 0;
/* publish it, if we are the primary */
if (_range_finder_topic >= 0) {
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
if (_distance_sensor_topic >= 0) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
}
if (_reports->force(&report)) {
@@ -840,7 +814,7 @@ void stop()
void
test()
{
struct range_finder_report report;
struct distance_sensor_s report;
ssize_t sz;
int ret;
@@ -858,8 +832,7 @@ test()
}
warnx("single read");
warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated);
warnx("time: %lld", report.timestamp);
warnx("time: %llu", report.timestamp);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -887,13 +860,7 @@ test()
}
warnx("periodic read %u", i);
/* Print the sonar rangefinder report sonar distance vector */
for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) {
warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1);
}
warnx("time: %lld", report.timestamp);
warnx("time: %llu", report.timestamp);
}
/* reset the sensor polling to default rate */
src/drivers/mpu6000/mpu6000.cpp
+4 -4
View File
@@ -242,7 +242,7 @@ private:
struct hrt_call _call;
unsigned _call_interval;
RingBuffer *_accel_reports;
ringbuffer::RingBuffer *_accel_reports;
struct accel_scale _accel_scale;
float _accel_range_scale;
@@ -251,7 +251,7 @@ private:
int _accel_orb_class_instance;
int _accel_class_instance;
RingBuffer *_gyro_reports;
ringbuffer::RingBuffer *_gyro_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
@@ -611,11 +611,11 @@ MPU6000::init()
}
/* allocate basic report buffers */
_accel_reports = new RingBuffer(2, sizeof(accel_report));
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr)
goto out;
_gyro_reports = new RingBuffer(2, sizeof(gyro_report));
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr)
goto out;
src/drivers/ms5611/ms5611_nuttx.cpp
+4 -4
View File
@@ -127,7 +127,7 @@ protected:
struct work_s _work;
unsigned _measure_ticks;
RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
bool _collect_phase;
unsigned _measure_phase;
@@ -269,7 +269,7 @@ MS5611::init()
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(baro_report));
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
if (_reports == nullptr) {
debug("can't get memory for reports");
@@ -896,7 +896,7 @@ start_bus(struct ms5611_bus_option &bus)
bus.dev = NULL;
return false;
}
int fd = open(bus.devpath, O_RDONLY);
/* set the poll rate to default, starts automatic data collection */
@@ -955,7 +955,7 @@ struct ms5611_bus_option &find_bus(enum MS5611_BUS busid)
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
return bus_options[i];
}
}
}
errx(1,"bus %u not started", (unsigned)busid);
}
src/drivers/oreoled/oreoled.cpp
+2 -2
View File
@@ -121,7 +121,7 @@ private:
work_s _work; ///< work queue for scheduling reads
bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED
uint8_t _num_healthy; ///< number of healthy LEDs
RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs
ringbuffer::RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs
uint64_t _last_gencall;
uint64_t _start_time; ///< system time we first attempt to communicate with battery
};
@@ -176,7 +176,7 @@ OREOLED::init()
}
/* allocate command queue */
_cmd_queue = new RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t));
_cmd_queue = new ringbuffer::RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t));
if (_cmd_queue == nullptr) {
return ENOTTY;
src/drivers/pwm_input/module.mk
+1
View File
@@ -39,3 +39,4 @@ MODULE_COMMAND = pwm_input
SRCS = pwm_input.cpp
EXTRACXXFLAGS = -Wno-pmf-conversions
src/drivers/pwm_input/pwm_input.cpp
+181 -108
View File
@@ -32,10 +32,13 @@
****************************************************************************/
/**
* @file pwm_input.cpp
* @file pwm_input.cpp
*
* PWM input driver based on earlier driver from Evan Slatyer,
* which in turn was based on drv_hrt.c
*
* @author: Andrew Tridgell
* @author: Ban Siesta <bansiesta@gmail.com>
*/
#include <px4_config.h>
@@ -58,6 +61,7 @@
#include <board_config.h>
#include <drivers/drv_pwm_input.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include "chip.h"
#include "up_internal.h"
@@ -80,6 +84,9 @@
#include <sys/stat.h>
#include <fcntl.h>
/* Reset pin define */
#define GPIO_VDD_RANGEFINDER_EN GPIO_GPIO5_OUTPUT
#if HRT_TIMER == PWMIN_TIMER
#error cannot share timer between HRT and PWMIN
#endif
@@ -188,34 +195,36 @@
* Specific registers and bits used by HRT sub-functions
*/
#if PWMIN_TIMER_CHANNEL == 1
#define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */
#define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */
#define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */
#define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */
#define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT))
#define CCMR2_PWMIN 0
#define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF)
#define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT)
#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
#define rCCR_PWMIN_A rCCR1 /* compare register for PWMIN */
#define DIER_PWMIN_A (GTIM_DIER_CC1IE) /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_A GTIM_SR_CC1IF /* interrupt status for PWMIN */
#define rCCR_PWMIN_B rCCR2 /* compare register for PWMIN */
#define SR_INT_PWMIN_B GTIM_SR_CC2IF /* interrupt status for PWMIN */
#define CCMR1_PWMIN ((0x02 << GTIM_CCMR1_CC2S_SHIFT) | (0x01 << GTIM_CCMR1_CC1S_SHIFT))
#define CCMR2_PWMIN 0
#define CCER_PWMIN (GTIM_CCER_CC2P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF)
#define SMCR_PWMIN_1 (0x05 << GTIM_SMCR_TS_SHIFT)
#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
#elif PWMIN_TIMER_CHANNEL == 2
#define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */
#define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */
#define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */
#define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */
#define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT))
#define CCMR2_PWMIN 0
#define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF)
#define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT)
#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
#define rCCR_PWMIN_A rCCR2 /* compare register for PWMIN */
#define DIER_PWMIN_A (GTIM_DIER_CC2IE) /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_A GTIM_SR_CC2IF /* interrupt status for PWMIN */
#define rCCR_PWMIN_B rCCR1 /* compare register for PWMIN */
#define DIER_PWMIN_B GTIM_DIER_CC1IE /* interrupt enable for PWMIN */
#define SR_INT_PWMIN_B GTIM_SR_CC1IF /* interrupt status for PWMIN */
#define CCMR1_PWMIN ((0x01 << GTIM_CCMR1_CC2S_SHIFT) | (0x02 << GTIM_CCMR1_CC1S_SHIFT))
#define CCMR2_PWMIN 0
#define CCER_PWMIN (GTIM_CCER_CC1P | GTIM_CCER_CC1E | GTIM_CCER_CC2E)
#define SR_OVF_PWMIN (GTIM_SR_CC1OF | GTIM_SR_CC2OF)
#define SMCR_PWMIN_1 (0x06 << GTIM_SMCR_TS_SHIFT)
#define SMCR_PWMIN_2 ((0x04 << GTIM_SMCR_SMS_SHIFT) | SMCR_PWMIN_1)
#else
#error PWMIN_TIMER_CHANNEL must be either 1 and 2.
#error PWMIN_TIMER_CHANNEL must be either 1 and 2.
#endif
#define TIMEOUT_POLL 300000 /* reset after no response over this time in microseconds [0.3s] */
#define TIMEOUT_READ 200000 /* don't reset if the last read is back more than this time in microseconds [0.2s] */
class PWMIN : device::CDev
{
@@ -228,22 +237,33 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
void _publish(uint16_t status, uint32_t period, uint32_t pulse_width);
void _print_info(void);
void publish(uint16_t status, uint32_t period, uint32_t pulse_width);
void print_info(void);
void hard_reset();
private:
uint32_t error_count;
uint32_t pulses_captured;
uint32_t last_period;
uint32_t last_width;
RingBuffer *reports;
bool timer_started;
uint32_t _error_count;
uint32_t _pulses_captured;
uint32_t _last_period;
uint32_t _last_width;
hrt_abstime _last_poll_time;
hrt_abstime _last_read_time;
ringbuffer::RingBuffer *_reports;
bool _timer_started;
hrt_call _hard_reset_call; /* HRT callout for note completion */
hrt_call _freeze_test_call; /* HRT callout for note completion */
void _timer_init(void);
void _turn_on();
void _turn_off();
void _freeze_test();
void timer_init(void);
};
static int pwmin_tim_isr(int irq, void *context);
static void pwmin_start(void);
static void pwmin_start();
static void pwmin_info(void);
static void pwmin_test(void);
static void pwmin_reset(void);
@@ -252,49 +272,53 @@ static PWMIN *g_dev;
PWMIN::PWMIN() :
CDev("pwmin", PWMIN0_DEVICE_PATH),
error_count(0),
pulses_captured(0),
last_period(0),
last_width(0),
reports(nullptr),
timer_started(false)
_error_count(0),
_pulses_captured(0),
_last_period(0),
_last_width(0),
_reports(nullptr),
_timer_started(false)
{
}
PWMIN::~PWMIN()
{
if (reports != nullptr)
delete reports;
if (_reports != nullptr) {
delete _reports;
}
}
/*
initialise the driver. This doesn't actually start the timer (that
is done on open). We don't start the timer to allow for this driver
to be started in init scripts when the user may be using the input
pin as PWM output
* initialise the driver. This doesn't actually start the timer (that
* is done on open). We don't start the timer to allow for this driver
* to be started in init scripts when the user may be using the input
* pin as PWM output
*/
int
PWMIN::init()
{
// we just register the device in /dev, and only actually
// activate the timer when requested to when the device is opened
/* we just register the device in /dev, and only actually
* activate the timer when requested to when the device is opened */
CDev::init();
reports = new RingBuffer(2, sizeof(struct pwm_input_s));
if (reports == nullptr) {
_reports = new ringbuffer::RingBuffer(2, sizeof(struct pwm_input_s));
if (_reports == nullptr) {
return -ENOMEM;
}
/* Schedule freeze check to invoke periodically */
hrt_call_every(&_freeze_test_call, 0, TIMEOUT_POLL, reinterpret_cast<hrt_callout>(&PWMIN::_freeze_test), this);
return OK;
}
/*
* Initialise the timer we are going to use.
*/
void PWMIN::timer_init(void)
void PWMIN::_timer_init(void)
{
// run with interrupts disabled in case the timer is already
// setup. We don't want it firing while we are doing the setup
/* run with interrupts disabled in case the timer is already
* setup. We don't want it firing while we are doing the setup */
irqstate_t flags = irqsave();
stm32_configgpio(GPIO_PWM_IN);
@@ -302,7 +326,7 @@ void PWMIN::timer_init(void)
irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr);
/* Clear no bits, set timer enable bit.*/
modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT);
modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT);
/* disable and configure the timer */
rCR1 = 0;
@@ -317,13 +341,13 @@ void PWMIN::timer_init(void)
rCCER = CCER_PWMIN;
rDCR = 0;
// for simplicity scale by the clock in MHz. This gives us
// readings in microseconds which is typically what is needed
// for a PWM input driver
uint32_t prescaler = PWMIN_TIMER_CLOCK/1000000UL;
/* for simplicity scale by the clock in MHz. This gives us
* readings in microseconds which is typically what is needed
* for a PWM input driver */
uint32_t prescaler = PWMIN_TIMER_CLOCK / 1000000UL;
/*
* define the clock speed. We want the highest possible clock
/*
* define the clock speed. We want the highest possible clock
* speed that avoids overflows.
*/
rPSC = prescaler - 1;
@@ -343,12 +367,41 @@ void PWMIN::timer_init(void)
irqrestore(flags);
timer_started = true;
_timer_started = true;
}
void
PWMIN::_freeze_test()
{
/* reset if last poll time was way back and a read was recently requested */
if (hrt_elapsed_time(&_last_poll_time) > TIMEOUT_POLL && hrt_elapsed_time(&_last_read_time) < TIMEOUT_READ) {
warnx("Lidar is down, reseting");
hard_reset();
}
}
void
PWMIN::_turn_on()
{
stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 1);
}
void
PWMIN::_turn_off()
{
stm32_gpiowrite(GPIO_VDD_RANGEFINDER_EN, 0);
}
void
PWMIN::hard_reset()
{
_turn_off();
hrt_call_after(&_hard_reset_call, 9000, reinterpret_cast<hrt_callout>(&PWMIN::_turn_on), this);
}
/*
hook for open of the driver. We start the timer at this point, then
leave it running
* hook for open of the driver. We start the timer at this point, then
* leave it running
*/
int
PWMIN::open(struct file *filp)
@@ -356,16 +409,19 @@ PWMIN::open(struct file *filp)
if (g_dev == nullptr) {
return -EIO;
}
int ret = CDev::open(filp);
if (ret == OK && !timer_started) {
g_dev->timer_init();
int ret = CDev::open(filp);
if (ret == OK && !_timer_started) {
g_dev->_timer_init();
}
return ret;
}
/*
handle ioctl requests
* handle ioctl requests
*/
int
PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg)
@@ -373,28 +429,32 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 500))
if ((arg < 1) || (arg > 500)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!reports->resize(arg)) {
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return reports->size();
return _reports->size();
case SENSORIOCRESET:
/* user has asked for the timer to be reset. This may
be needed if the pin was used for a different
purpose (such as PWM output)
*/
timer_init();
* be needed if the pin was used for a different
* purpose (such as PWM output) */
_timer_init();
/* also reset the sensor */
hard_reset();
return OK;
default:
@@ -405,72 +465,73 @@ PWMIN::ioctl(struct file *filp, int cmd, unsigned long arg)
/*
read some samples from the device
* read some samples from the device
*/
ssize_t
PWMIN::read(struct file *filp, char *buffer, size_t buflen)
{
_last_read_time = hrt_absolute_time();
unsigned count = buflen / sizeof(struct pwm_input_s);
struct pwm_input_s *buf = reinterpret_cast<struct pwm_input_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1)
if (count < 1) {
return -ENOSPC;
}
while (count--) {
if (reports->get(buf)) {
if (_reports->get(buf)) {
ret += sizeof(struct pwm_input_s);
buf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/*
publish some data from the ISR in the ring buffer
* publish some data from the ISR in the ring buffer
*/
void PWMIN::_publish(uint16_t status, uint32_t period, uint32_t pulse_width)
void PWMIN::publish(uint16_t status, uint32_t period, uint32_t pulse_width)
{
/* if we missed an edge, we have to give up */
if (status & SR_OVF_PWMIN) {
error_count++;
_error_count++;
return;
}
_last_poll_time = hrt_absolute_time();
struct pwm_input_s pwmin_report;
pwmin_report.timestamp = hrt_absolute_time();
pwmin_report.error_count = error_count;
pwmin_report.timestamp = _last_poll_time;
pwmin_report.error_count = _error_count;
pwmin_report.period = period;
pwmin_report.pulse_width = pulse_width;
reports->force(&pwmin_report);
last_period = period;
last_width = pulse_width;
pulses_captured++;
_reports->force(&pwmin_report);
}
/*
print information on the last captured
* print information on the last captured
*/
void PWMIN::_print_info(void)
void PWMIN::print_info(void)
{
if (!timer_started) {
if (!_timer_started) {
printf("timer not started - try the 'test' command\n");
} else {
printf("count=%u period=%u width=%u\n",
(unsigned)pulses_captured,
(unsigned)last_period,
(unsigned)last_width);
(unsigned)_pulses_captured,
(unsigned)_last_period,
(unsigned)_last_width);
}
}
/*
Handle the interupt, gathering pulse data
* Handle the interupt, gathering pulse data
*/
static int pwmin_tim_isr(int irq, void *context)
{
@@ -482,46 +543,52 @@ static int pwmin_tim_isr(int irq, void *context)
rSR = 0;
if (g_dev != nullptr) {
g_dev->_publish(status, period, pulse_width);
g_dev->publish(status, period, pulse_width);
}
return OK;
}
/*
start the driver
* start the driver
*/
static void pwmin_start(void)
static void pwmin_start()
{
if (g_dev != nullptr) {
printf("driver already started\n");
exit(1);
errx(1, "driver already started");
}
g_dev = new PWMIN();
if (g_dev == nullptr) {
errx(1, "driver allocation failed");
}
if (g_dev->init() != OK) {
errx(1, "driver init failed");
}
exit(0);
}
/*
test the driver
* test the driver
*/
static void pwmin_test(void)
{
int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
if (fd == -1) {
errx(1, "Failed to open device");
}
uint64_t start_time = hrt_absolute_time();
printf("Showing samples for 5 seconds\n");
while (hrt_absolute_time() < start_time+5U*1000UL*1000UL) {
while (hrt_absolute_time() < start_time + 5U * 1000UL * 1000UL) {
struct pwm_input_s buf;
if (::read(fd, &buf, sizeof(buf)) == sizeof(buf)) {
printf("period=%u width=%u error_count=%u\n",
(unsigned)buf.period,
@@ -529,28 +596,33 @@ static void pwmin_test(void)
(unsigned)buf.error_count);
}
}
close(fd);
exit(0);
}
/*
reset the timer
* reset the timer
*/
static void pwmin_reset(void)
{
g_dev->hard_reset();
int fd = open(PWMIN0_DEVICE_PATH, O_RDONLY);
if (fd == -1) {
errx(1, "Failed to open device");
}
if (ioctl(fd, SENSORIOCRESET, 0) != OK) {
errx(1, "reset failed");
}
close(fd);
exit(0);
}
/*
show some information on the driver
* show some information on the driver
*/
static void pwmin_info(void)
{
@@ -558,15 +630,16 @@ static void pwmin_info(void)
printf("driver not started\n");
exit(1);
}
g_dev->_print_info();
g_dev->print_info();
exit(0);
}
/*
driver entry point
* driver entry point
*/
int pwm_input_main(int argc, char * argv[])
int pwm_input_main(int argc, char *argv[])
{
const char *verb = argv[1];
src/drivers/px4flow/px4flow.cpp
+48 -8
View File
@@ -34,6 +34,7 @@
/**
* @file px4flow.cpp
* @author Dominik Honegger
* @author Ban Siesta <bansiesta@gmail.com>
*
* Driver for the PX4FLOW module connected via I2C.
*/
@@ -66,11 +67,13 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_px4flow.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@@ -83,6 +86,9 @@
#define PX4FLOW_CONVERSION_INTERVAL 100000 ///< in microseconds! 20000 = 50 Hz 100000 = 10Hz
#define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed
#define PX4FLOW_MAX_DISTANCE 5.0f
#define PX4FLOW_MIN_DISTANCE 0.3f
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@@ -120,16 +126,19 @@ protected:
private:
work_s _work;
RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
int _orb_class_instance;
orb_advert_t _px4flow_topic;
orb_advert_t _distance_sensor_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
enum Rotation _sensor_rotation;
/**
@@ -182,13 +191,16 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) :
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_orb_class_instance(-1),
_px4flow_topic(-1),
_distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
_comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")),
_sensor_rotation(rotation)
{
// enable debug() calls
// disable debug() calls
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
@@ -217,12 +229,26 @@ PX4FLOW::init()
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(optical_flow_s));
_reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s));
if (_reports == nullptr) {
goto out;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic < 0) {
log("failed to create distance_sensor object. Did you start uOrb?");
}
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
@@ -486,10 +512,10 @@ PX4FLOW::collect()
report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
/* rotate measurements according to parameter */
float zeroval = 0.0f;
rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
if (_px4flow_topic < 0) {
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
@@ -499,6 +525,20 @@ PX4FLOW::collect()
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
}
/* publish to the distance_sensor topic as well */
struct distance_sensor_s distance_report;
distance_report.timestamp = report.timestamp;
distance_report.min_distance = PX4FLOW_MIN_DISTANCE;
distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
distance_report.current_distance = report.ground_distance_m;
distance_report.covariance = 0.0f;
distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
/* TODO: the ID needs to be properly set */
distance_report.id = 0;
distance_report.orientation = 8;
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report);
/* post a report to the ring */
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
src/drivers/sf0x/sf0x.cpp
+51 -29
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -69,6 +69,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@@ -118,7 +119,7 @@ private:
float _min_distance;
float _max_distance;
work_s _work;
RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
@@ -128,7 +129,10 @@ private:
enum SF0X_PARSE_STATE _parse_state;
hrt_abstime _last_read;
orb_advert_t _range_finder_topic;
int _class_instance;
int _orb_class_instance;
orb_advert_t _distance_sensor_topic;
unsigned _consecutive_fail_count;
@@ -194,7 +198,9 @@ SF0X::SF0X(const char *port) :
_linebuf_index(0),
_parse_state(SF0X_PARSE_STATE0_UNSYNC),
_last_read(0),
_range_finder_topic(-1),
_class_instance(-1),
_orb_class_instance(-1),
_distance_sensor_topic(-1),
_consecutive_fail_count(0),
_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
@@ -255,6 +261,14 @@ SF0X::~SF0X()
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
}
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
}
int
@@ -270,21 +284,27 @@ SF0X::init()
if (ret != OK) break;
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(range_finder_report));
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
warnx("mem err");
ret = -1;
break;
}
/* get a publish handle on the range finder topic */
struct range_finder_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
if (_range_finder_topic < 0) {
warnx("advert err");
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic < 0) {
log("failed to create distance_sensor object. Did you start uOrb?");
}
}
} while(0);
/* close the fd */
@@ -442,8 +462,8 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
SF0X::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
@@ -556,11 +576,11 @@ SF0X::collect()
_last_read = hrt_absolute_time();
float si_units;
float distance_m = -1.0f;
bool valid = false;
for (int i = 0; i < ret; i++) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
valid = true;
}
}
@@ -569,20 +589,22 @@ SF0X::collect()
return -EAGAIN;
}
debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
debug("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO"));
struct range_finder_report report;
struct distance_sensor_s report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = 8;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
/* TODO: set proper ID */
report.id = 0;
/* publish it */
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
@@ -815,7 +837,7 @@ void stop()
void
test()
{
struct range_finder_report report;
struct distance_sensor_s report;
ssize_t sz;
int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
@@ -832,8 +854,8 @@ test()
}
warnx("single read");
warnx("val: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("val: %0.2f m", (double)report.current_distance);
warnx("time: %llu", report.timestamp);
/* start the sensor polling at 2 Hz rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -863,8 +885,8 @@ test()
}
warnx("read #%u", i);
warnx("val: %0.3f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("val: %0.3f m", (double)report.current_distance);
warnx("time: %llu", report.timestamp);
}
/* reset the sensor polling to the default rate */
src/drivers/trone/trone.cpp
+40 -34
View File
@@ -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
@@ -68,6 +68,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@@ -82,7 +83,7 @@
#define TRONE_WHO_AM_I_REG 0x01 /* Who am I test register */
#define TRONE_WHO_AM_I_REG_VAL 0xA1
/* Device limits */
#define TRONE_MIN_DISTANCE (0.20f)
#define TRONE_MAX_DISTANCE (14.00f)
@@ -122,13 +123,14 @@ private:
float _min_distance;
float _max_distance;
work_s _work;
RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
int _class_instance;
int _orb_class_instance;
orb_advert_t _range_finder_topic;
orb_advert_t _distance_sensor_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
@@ -209,7 +211,7 @@ static const uint8_t crc_table[] = {
0xfa, 0xfd, 0xf4, 0xf3
};
static uint8_t crc8(uint8_t *p, uint8_t len) {
/* static uint8_t crc8(uint8_t *p, uint8_t len) {
uint16_t i;
uint16_t crc = 0x0;
@@ -219,7 +221,7 @@ static uint8_t crc8(uint8_t *p, uint8_t len) {
}
return crc & 0xFF;
}
}*/
/*
* Driver 'main' command.
@@ -235,7 +237,8 @@ TRONE::TRONE(int bus, int address) :
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_range_finder_topic(-1),
_orb_class_instance(-1),
_distance_sensor_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows"))
@@ -281,7 +284,7 @@ TRONE::init()
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(range_finder_report));
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
goto out;
@@ -291,13 +294,15 @@ TRONE::init()
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct range_finder_report rf_report;
struct distance_sensor_s ds_report;
measure();
_reports->get(&rf_report);
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
_reports->get(&ds_report);
if (_range_finder_topic < 0) {
debug("failed to create sensor_range_finder object. Did you start uOrb?");
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_LOW);
if (_distance_sensor_topic < 0) {
log("failed to create distance_sensor object. Did you start uOrb?");
}
}
@@ -468,8 +473,8 @@ TRONE::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
TRONE::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
unsigned count = buflen / sizeof(struct distance_sensor_s);
struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
@@ -566,22 +571,24 @@ TRONE::collect()
return ret;
}
uint16_t distance = (val[0] << 8) | val[1];
float si_units = distance * 0.001f; /* mm to m */
struct range_finder_report report;
uint16_t distance_mm = (val[0] << 8) | val[1];
float distance_m = float(distance_mm) * 1e-3f;
struct distance_sensor_s report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance();
report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
/* there is no enum item for a combined LASER and ULTRASOUND which it should be */
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = 8;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
/* TODO: set proper ID */
report.id = 0;
/* publish it, if we are the primary */
if (_range_finder_topic >= 0) {
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
if (_distance_sensor_topic >= 0) {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
}
if (_reports->force(&report)) {
@@ -787,7 +794,7 @@ void stop()
void
test()
{
struct range_finder_report report;
struct distance_sensor_s report;
ssize_t sz;
int ret;
@@ -805,8 +812,8 @@ test()
}
warnx("single read");
warnx("measurement: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("measurement: %0.2f m", (double)report.current_distance);
warnx("time: %llu", report.timestamp);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -834,9 +841,8 @@ test()
}
warnx("periodic read %u", i);
warnx("valid %u", report.valid);
warnx("measurement: %0.3f", (double)report.distance);
warnx("time: %lld", report.timestamp);
warnx("measurement: %0.3f", (double)report.current_distance);
warnx("time: %llu", report.timestamp);
}
/* reset the sensor polling to default rate */
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+2 -2
View File
@@ -58,13 +58,13 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/distance_sensor.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_range_finder.h>
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
@@ -162,7 +162,7 @@ private:
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */
struct wind_estimate_s _wind; /**< wind estimate */
struct range_finder_report _distance; /**< distance estimate */
struct distance_sensor_s _distance; /**< distance estimate */
struct vehicle_land_detected_s _landDetector;
struct actuator_armed_s _armed;
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+5 -5
View File
@@ -501,7 +501,7 @@ void AttitudePositionEstimatorEKF::task_main()
/*
* do subscriptions
*/
_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
_distance_sub = orb_subscribe(ORB_ID(distance_sensor));
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -1471,10 +1471,10 @@ void AttitudePositionEstimatorEKF::pollData()
orb_check(_distance_sub, &_newRangeData);
if (_newRangeData) {
orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
if (_distance.valid) {
_ekf->rngMea = _distance.distance;
orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance);
if ((_distance.current_distance > _distance.min_distance)
&& (_distance.current_distance < _distance.max_distance)) {
_ekf->rngMea = _distance.current_distance;
_distance_last_valid = _distance.timestamp;
} else {
src/modules/mavlink/mavlink_messages.cpp
+27 -18
View File
@@ -68,9 +68,9 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/distance_sensor.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_range_finder.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
@@ -2178,7 +2178,6 @@ protected:
}
};
class MavlinkStreamDistanceSensor : public MavlinkStream
{
public:
@@ -2204,12 +2203,12 @@ public:
unsigned get_size()
{
return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
return _distance_sensor_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
MavlinkOrbSubscription *_range_sub;
uint64_t _range_time;
MavlinkOrbSubscription *_distance_sensor_sub;
uint64_t _dist_sensor_time;
/* do not allow top copying this class */
MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &);
@@ -2217,36 +2216,46 @@ private:
protected:
explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink),
_range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))),
_range_time(0)
_distance_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(distance_sensor))),
_dist_sensor_time(0)
{}
void send(const hrt_abstime t)
{
struct range_finder_report range;
struct distance_sensor_s dist_sensor;
if (_range_sub->update(&_range_time, &range)) {
if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) {
mavlink_distance_sensor_t msg;
msg.time_boot_ms = range.timestamp / 1000;
msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */
switch (range.type) {
case RANGE_FINDER_TYPE_LASER:
/* TODO: use correct ID here */
msg.id = 0;
switch (dist_sensor.type) {
case MAV_DISTANCE_SENSOR_ULTRASOUND:
msg.type = MAV_DISTANCE_SENSOR_ULTRASOUND;
break;
case MAV_DISTANCE_SENSOR_LASER:
msg.type = MAV_DISTANCE_SENSOR_LASER;
break;
case MAV_DISTANCE_SENSOR_INFRARED:
msg.type = MAV_DISTANCE_SENSOR_INFRARED;
break;
default:
msg.type = MAV_DISTANCE_SENSOR_LASER;
break;
}
msg.id = 0;
msg.orientation = 0;
msg.min_distance = range.minimum_distance * 100;
msg.max_distance = range.maximum_distance * 100;
msg.current_distance = range.distance * 100;
msg.covariance = 20;
msg.orientation = dist_sensor.orientation;
msg.min_distance = dist_sensor.min_distance * 100.0f; /* m to cm */
msg.max_distance = dist_sensor.max_distance * 100.0f; /* m to cm */
msg.current_distance = dist_sensor.current_distance * 100.0f; /* m to cm */
msg.covariance = dist_sensor.covariance;
_mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg);
}
src/modules/mavlink/mavlink_receiver.cpp
+67 -15
View File
@@ -56,7 +56,6 @@
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_range_finder.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
@@ -109,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_battery_pub(-1),
_cmd_pub(-1),
_flow_pub(-1),
_range_pub(-1),
_distance_sensor_pub(-1),
_offboard_control_mode_pub(-1),
_actuator_controls_pub(-1),
_global_vel_sp_pub(-1),
@@ -211,6 +210,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_timesync(msg);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
handle_message_distance_sensor(msg);
break;
default:
break;
}
@@ -412,6 +415,25 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
}
/* Use distance value for distance sensor topic */
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
d.timestamp = flow.integration_time_us * 1000; /* ms to us */
d.min_distance = 0.3f;
d.max_distance = 5.0f;
d.current_distance = flow.distance; /* both are in m */
d.type = 1;
d.id = 0;
d.orientation = 8;
d.covariance = 0.0;
if (_distance_sensor_pub < 0) {
_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
} else {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
}
}
void
@@ -444,22 +466,23 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
orb_publish(ORB_ID(optical_flow), _flow_pub, &f);
}
/* Use distance value for range finder report */
struct range_finder_report r;
memset(&r, 0, sizeof(r));
/* Use distance value for distance sensor topic */
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
r.timestamp = hrt_absolute_time();
r.error_count = 0;
r.type = RANGE_FINDER_TYPE_LASER;
r.distance = flow.distance;
r.minimum_distance = 0.0f;
r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable
r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance);
d.timestamp = hrt_absolute_time();
d.min_distance = 0.3f;
d.max_distance = 5.0f;
d.current_distance = flow.distance; /* both are in m */
d.type = 1;
d.id = 0;
d.orientation = 8;
d.covariance = 0.0;
if (_range_pub < 0) {
_range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r);
if (_distance_sensor_pub < 0) {
_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
} else {
orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r);
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
}
}
@@ -497,6 +520,35 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
{
/* distance sensor */
mavlink_distance_sensor_t dist_sensor;
mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
struct distance_sensor_s d;
memset(&d, 0, sizeof(d));
d.timestamp = dist_sensor.time_boot_ms * 1000; /* ms to us */
d.min_distance = float(dist_sensor.min_distance) * 1e-2f; /* cm to m */
d.max_distance = float(dist_sensor.max_distance) * 1e-2f; /* cm to m */
d.current_distance = float(dist_sensor.current_distance) * 1e-2f; /* cm to m */
d.type = dist_sensor.type;
d.id = dist_sensor.id;
d.orientation = dist_sensor.orientation;
d.covariance = dist_sensor.covariance;
/// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum
if (_distance_sensor_pub < 0) {
_distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d);
} else {
orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d);
}
}
void
MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
{
src/modules/mavlink/mavlink_receiver.h
+3 -1
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_force_setpoint.h>
#include <uORB/topics/time_offset.h>
#include <uORB/topics/distance_sensor.h>
#include "mavlink_ftp.h"
@@ -134,6 +135,7 @@ private:
void handle_message_hil_sensor(mavlink_message_t *msg);
void handle_message_hil_gps(mavlink_message_t *msg);
void handle_message_hil_state_quaternion(mavlink_message_t *msg);
void handle_message_distance_sensor(mavlink_message_t *msg);
void *receive_thread(void *arg);
@@ -164,7 +166,7 @@ private:
orb_advert_t _battery_pub;
orb_advert_t _cmd_pub;
orb_advert_t _flow_pub;
orb_advert_t _range_pub;
orb_advert_t _distance_sensor_pub;
orb_advert_t _offboard_control_mode_pub;
orb_advert_t _actuator_controls_pub;
orb_advert_t _global_vel_sp_pub;
src/modules/sdlog2/sdlog2.c
+11 -10
View File
@@ -61,8 +61,6 @@
#include <math.h>
#include <time.h>
#include <drivers/drv_range_finder.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/sensor_combined.h>
@@ -92,6 +90,7 @@
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/system_power.h>
@@ -1084,7 +1083,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_global_velocity_setpoint_s global_vel_sp;
struct battery_status_s battery;
struct telemetry_status_s telemetry;
struct range_finder_report range_finder;
struct distance_sensor_s distance_sensor;
struct estimator_status_report estimator_status;
struct tecs_status_s tecs_status;
struct system_power_s system_power;
@@ -1174,7 +1173,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int global_vel_sp_sub;
int battery_sub;
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
int range_finder_sub;
int distance_sensor_sub;
int estimator_status_sub;
int tecs_status_sub;
int system_power_sub;
@@ -1208,7 +1207,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.esc_sub = -1;
subs.global_vel_sp_sub = -1;
subs.battery_sub = -1;
subs.range_finder_sub = -1;
subs.distance_sensor_sub = -1;
subs.estimator_status_sub = -1;
subs.tecs_status_sub = -1;
subs.system_power_sub = -1;
@@ -1820,12 +1819,14 @@ int sdlog2_thread_main(int argc, char *argv[])
}
}
/* --- BOTTOM DISTANCE --- */
if (copy_if_updated(ORB_ID(sensor_range_finder), &subs.range_finder_sub, &buf.range_finder)) {
/* --- DISTANCE SENSOR --- */
if (copy_if_updated(ORB_ID(distance_sensor), &subs.distance_sensor_sub, &buf.distance_sensor)) {
log_msg.msg_type = LOG_DIST_MSG;
log_msg.body.log_DIST.bottom = buf.range_finder.distance;
log_msg.body.log_DIST.bottom_rate = 0.0f;
log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0);
log_msg.body.log_DIST.id = buf.distance_sensor.id;
log_msg.body.log_DIST.type = buf.distance_sensor.type;
log_msg.body.log_DIST.orientation = buf.distance_sensor.orientation;
log_msg.body.log_DIST.current_distance = buf.distance_sensor.current_distance;
log_msg.body.log_DIST.covariance = buf.distance_sensor.covariance;
LOGBUFFER_WRITE_AND_COUNT(DIST);
}
src/modules/sdlog2/sdlog2_messages.h
+7 -6
View File
@@ -295,12 +295,14 @@ struct log_BATT_s {
float discharged;
};
/* --- DIST - DISTANCE TO SURFACE --- */
/* --- DIST - RANGE SENSOR DISTANCE --- */
#define LOG_DIST_MSG 21
struct log_DIST_s {
float bottom;
float bottom_rate;
uint8_t flags;
uint8_t id;
uint8_t type;
uint8_t orientation;
uint16_t current_distance;
uint8_t covariance;
};
/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */
@@ -486,7 +488,6 @@ struct log_PARM_s {
};
#pragma pack(pop)
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
/* business-level messages, ID < 0x80 */
@@ -514,7 +515,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
LOG_FORMAT(DIST, "iiff", "Type,Orientation,Distance,Covariance"),
LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
src/modules/uORB/objects_common.cpp
+6 -3
View File
@@ -57,15 +57,15 @@ ORB_DEFINE(sensor_gyro, struct gyro_report);
#include <drivers/drv_baro.h>
ORB_DEFINE(sensor_baro, struct baro_report);
#include <drivers/drv_range_finder.h>
ORB_DEFINE(sensor_range_finder, struct range_finder_report);
#include <drivers/drv_pwm_output.h>
ORB_DEFINE(output_pwm, struct pwm_output_values);
#include <drivers/drv_rc_input.h>
ORB_DEFINE(input_rc, struct rc_input_values);
#include "topics/pwm_input.h"
ORB_DEFINE(pwm_input, struct pwm_input_s);
#include "topics/vehicle_attitude.h"
ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s);
@@ -256,3 +256,6 @@ ORB_DEFINE(time_offset, struct time_offset_s);
#include "topics/mc_att_ctrl_status.h"
ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s);
#include "topics/distance_sensor.h"
ORB_DEFINE(distance_sensor, struct distance_sensor_s);
src/modules/uavcan/sensors/baro.cpp
+1 -2
View File
@@ -36,7 +36,6 @@
*/
#include "baro.hpp"
#include <drivers/device/ringbuffer.h>
#include <cmath>
const char *const UavcanBarometerBridge::NAME = "baro";
@@ -56,7 +55,7 @@ int UavcanBarometerBridge::init()
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(baro_report));
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
if (_reports == nullptr)
return -1;
src/modules/uavcan/sensors/baro.hpp
+2 -3
View File
@@ -39,11 +39,10 @@
#include "sensor_bridge.hpp"
#include <drivers/drv_baro.h>
#include <drivers/device/ringbuffer.h>
#include <uavcan/equipment/air_data/StaticAirData.hpp>
class RingBuffer;
class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
{
public:
@@ -68,5 +67,5 @@ private:
uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
unsigned _msl_pressure = 101325;
RingBuffer *_reports;
ringbuffer::RingBuffer *_reports;
};