forked from Archive/PX4-Autopilot
Merge branch 'fix_ringbuffer' into lidarlite_merge
This commit is contained in:
commit
d473b8bddd
|
@ -128,7 +128,7 @@ Airspeed::init()
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_reports = new RingBuffer(2, sizeof(differential_pressure_s));
|
_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));
|
||||||
if (_reports == nullptr)
|
if (_reports == nullptr)
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
|
|
|
@ -109,7 +109,7 @@ public:
|
||||||
virtual void print_info();
|
virtual void print_info();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports;
|
||||||
perf_counter_t _buffer_overflows;
|
perf_counter_t _buffer_overflows;
|
||||||
|
|
||||||
/* this class has pointer data members and should not be copied */
|
/* this class has pointer data members and should not be copied */
|
||||||
|
|
|
@ -41,7 +41,8 @@ SRCS = \
|
||||||
cdev.cpp \
|
cdev.cpp \
|
||||||
i2c_nuttx.cpp \
|
i2c_nuttx.cpp \
|
||||||
pio.cpp \
|
pio.cpp \
|
||||||
spi.cpp
|
spi.cpp \
|
||||||
|
ringbuffer.cpp
|
||||||
else
|
else
|
||||||
SRCS = \
|
SRCS = \
|
||||||
device_posix.cpp \
|
device_posix.cpp \
|
||||||
|
@ -50,5 +51,5 @@ SRCS = \
|
||||||
vdev_posix.cpp \
|
vdev_posix.cpp \
|
||||||
i2c_posix.cpp \
|
i2c_posix.cpp \
|
||||||
sim.cpp \
|
sim.cpp \
|
||||||
ringbuffer.cpp
|
ringbuffer.cpp
|
||||||
endif
|
endif
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -40,6 +40,9 @@
|
||||||
#include "ringbuffer.h"
|
#include "ringbuffer.h"
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
|
namespace ringbuffer
|
||||||
|
{
|
||||||
|
|
||||||
RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
|
RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
|
||||||
_num_items(num_items),
|
_num_items(num_items),
|
||||||
_item_size(item_size),
|
_item_size(item_size),
|
||||||
|
@ -57,25 +60,25 @@ RingBuffer::~RingBuffer()
|
||||||
unsigned
|
unsigned
|
||||||
RingBuffer::_next(unsigned index)
|
RingBuffer::_next(unsigned index)
|
||||||
{
|
{
|
||||||
return (0 == index) ? _num_items : (index - 1);
|
return (0 == index) ? _num_items : (index - 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
RingBuffer::empty()
|
RingBuffer::empty()
|
||||||
{
|
{
|
||||||
return _tail == _head;
|
return _tail == _head;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
RingBuffer::full()
|
RingBuffer::full()
|
||||||
{
|
{
|
||||||
return _next(_head) == _tail;
|
return _next(_head) == _tail;
|
||||||
}
|
}
|
||||||
|
|
||||||
unsigned
|
unsigned
|
||||||
RingBuffer::size()
|
RingBuffer::size()
|
||||||
{
|
{
|
||||||
return (_buf != nullptr) ? _num_items : 0;
|
return (_buf != nullptr) ? _num_items : 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -86,7 +89,7 @@ RingBuffer::flush()
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
RingBuffer::put(const void *val, size_t val_size)
|
RingBuffer::put(const void *val, size_t val_size)
|
||||||
{
|
{
|
||||||
unsigned next = _next(_head);
|
unsigned next = _next(_head);
|
||||||
if (next != _tail) {
|
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
|
#define __PX4_SBCAP __sync_bool_compare_and_swap
|
||||||
#endif
|
#endif
|
||||||
bool
|
bool
|
||||||
RingBuffer::get(void *val, size_t val_size)
|
RingBuffer::get(void *val, size_t val_size)
|
||||||
{
|
{
|
||||||
if (_tail != _head) {
|
if (_tail != _head) {
|
||||||
unsigned candidate;
|
unsigned candidate;
|
||||||
|
@ -340,7 +343,7 @@ RingBuffer::get(double &val)
|
||||||
}
|
}
|
||||||
|
|
||||||
unsigned
|
unsigned
|
||||||
RingBuffer::space(void)
|
RingBuffer::space(void)
|
||||||
{
|
{
|
||||||
unsigned tail, head;
|
unsigned tail, head;
|
||||||
|
|
||||||
|
@ -361,7 +364,7 @@ RingBuffer::space(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
unsigned
|
unsigned
|
||||||
RingBuffer::count(void)
|
RingBuffer::count(void)
|
||||||
{
|
{
|
||||||
/*
|
/*
|
||||||
* Note that due to the conservative nature of space(), this may
|
* Note that due to the conservative nature of space(), this may
|
||||||
|
@ -371,7 +374,7 @@ RingBuffer::count(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
bool
|
bool
|
||||||
RingBuffer::resize(unsigned new_size)
|
RingBuffer::resize(unsigned new_size)
|
||||||
{
|
{
|
||||||
char *old_buffer;
|
char *old_buffer;
|
||||||
char *new_buffer = new char [(new_size+1) * _item_size];
|
char *new_buffer = new char [(new_size+1) * _item_size];
|
||||||
|
@ -388,13 +391,15 @@ RingBuffer::resize(unsigned new_size)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
RingBuffer::print_info(const char *name)
|
RingBuffer::print_info(const char *name)
|
||||||
{
|
{
|
||||||
printf("%s %u/%lu (%u/%u @ %p)\n",
|
printf("%s %u/%lu (%u/%u @ %p)\n",
|
||||||
name,
|
name,
|
||||||
_num_items,
|
_num_items,
|
||||||
(unsigned long)_num_items * _item_size,
|
(unsigned long)_num_items * _item_size,
|
||||||
_head,
|
_head,
|
||||||
_tail,
|
_tail,
|
||||||
_buf);
|
_buf);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
} // namespace ringbuffer
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -43,6 +43,9 @@
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
|
namespace ringbuffer __EXPORT
|
||||||
|
{
|
||||||
|
|
||||||
class RingBuffer {
|
class RingBuffer {
|
||||||
public:
|
public:
|
||||||
RingBuffer(unsigned ring_size, size_t entry_size);
|
RingBuffer(unsigned ring_size, size_t entry_size);
|
||||||
|
@ -161,7 +164,7 @@ public:
|
||||||
private:
|
private:
|
||||||
unsigned _num_items;
|
unsigned _num_items;
|
||||||
const size_t _item_size;
|
const size_t _item_size;
|
||||||
char *_buf;
|
char *_buf;
|
||||||
volatile unsigned _head; /**< insertion point in _item_size units */
|
volatile unsigned _head; /**< insertion point in _item_size units */
|
||||||
volatile unsigned _tail; /**< removal point in _item_size units */
|
volatile unsigned _tail; /**< removal point in _item_size units */
|
||||||
|
|
||||||
|
@ -172,9 +175,4 @@ private:
|
||||||
RingBuffer operator=(const RingBuffer&);
|
RingBuffer operator=(const RingBuffer&);
|
||||||
};
|
};
|
||||||
|
|
||||||
#ifdef __PX4_NUTTX
|
} // namespace ringbuffer
|
||||||
// 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
|
|
||||||
|
|
|
@ -156,7 +156,7 @@ private:
|
||||||
work_s _work;
|
work_s _work;
|
||||||
unsigned _measure_ticks;
|
unsigned _measure_ticks;
|
||||||
|
|
||||||
RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports;
|
||||||
mag_scale _scale;
|
mag_scale _scale;
|
||||||
float _range_scale;
|
float _range_scale;
|
||||||
float _range_ga;
|
float _range_ga;
|
||||||
|
@ -423,7 +423,7 @@ HMC5883::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_reports = new RingBuffer(2, sizeof(mag_report));
|
_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
|
||||||
if (_reports == nullptr)
|
if (_reports == nullptr)
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
|
@ -921,10 +921,10 @@ HMC5883::collect()
|
||||||
|
|
||||||
_temperature_counter = 0;
|
_temperature_counter = 0;
|
||||||
|
|
||||||
ret = _interface->read(ADDR_TEMP_OUT_MSB,
|
ret = _interface->read(ADDR_TEMP_OUT_MSB,
|
||||||
raw_temperature, sizeof(raw_temperature));
|
raw_temperature, sizeof(raw_temperature));
|
||||||
if (ret == OK) {
|
if (ret == OK) {
|
||||||
int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) +
|
int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) +
|
||||||
raw_temperature[1];
|
raw_temperature[1];
|
||||||
new_report.temperature = 25 + (temp16 / (16*8.0f));
|
new_report.temperature = 25 + (temp16 / (16*8.0f));
|
||||||
_temperature_error_count = 0;
|
_temperature_error_count = 0;
|
||||||
|
@ -1146,8 +1146,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
||||||
ret = -EIO;
|
ret = -EIO;
|
||||||
goto out;
|
goto out;
|
||||||
}
|
}
|
||||||
float cal[3] = {fabsf(expected_cal[0] / report.x),
|
float cal[3] = {fabsf(expected_cal[0] / report.x),
|
||||||
fabsf(expected_cal[1] / report.y),
|
fabsf(expected_cal[1] / report.y),
|
||||||
fabsf(expected_cal[2] / report.z)};
|
fabsf(expected_cal[2] / report.z)};
|
||||||
|
|
||||||
if (cal[0] > 0.7f && cal[0] < 1.35f &&
|
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("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("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("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)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
|
||||||
(double)(1.0f/_range_scale), (double)_range_ga);
|
(double)(1.0f/_range_scale), (double)_range_ga);
|
||||||
printf("temperature %.2f\n", (double)_last_report.temperature);
|
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;
|
bus.dev = NULL;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
int fd = open(bus.devpath, O_RDONLY);
|
int fd = open(bus.devpath, O_RDONLY);
|
||||||
if (fd < 0) {
|
if (fd < 0) {
|
||||||
return false;
|
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) {
|
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
|
||||||
return bus_options[i];
|
return bus_options[i];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
errx(1, "bus %u not started", (unsigned)busid);
|
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.
|
* Start/load the driver.
|
||||||
|
|
|
@ -229,7 +229,7 @@ private:
|
||||||
struct hrt_call _call;
|
struct hrt_call _call;
|
||||||
unsigned _call_interval;
|
unsigned _call_interval;
|
||||||
|
|
||||||
RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports;
|
||||||
|
|
||||||
struct gyro_scale _gyro_scale;
|
struct gyro_scale _gyro_scale;
|
||||||
float _gyro_range_scale;
|
float _gyro_range_scale;
|
||||||
|
@ -472,7 +472,7 @@ L3GD20::init()
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_reports = new RingBuffer(2, sizeof(gyro_report));
|
_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
|
||||||
|
|
||||||
if (_reports == nullptr)
|
if (_reports == nullptr)
|
||||||
goto out;
|
goto out;
|
||||||
|
|
|
@ -275,8 +275,8 @@ private:
|
||||||
unsigned _call_accel_interval;
|
unsigned _call_accel_interval;
|
||||||
unsigned _call_mag_interval;
|
unsigned _call_mag_interval;
|
||||||
|
|
||||||
RingBuffer *_accel_reports;
|
ringbuffer::RingBuffer *_accel_reports;
|
||||||
RingBuffer *_mag_reports;
|
ringbuffer::RingBuffer *_mag_reports;
|
||||||
|
|
||||||
struct accel_scale _accel_scale;
|
struct accel_scale _accel_scale;
|
||||||
unsigned _accel_range_m_s2;
|
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;
|
_debug_enabled = true;
|
||||||
|
|
||||||
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D;
|
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D;
|
||||||
|
|
||||||
/* Prime _mag with parents devid. */
|
/* Prime _mag with parents devid. */
|
||||||
_mag->_device_id.devid = _device_id.devid;
|
_mag->_device_id.devid = _device_id.devid;
|
||||||
_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
|
_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
|
||||||
|
@ -646,12 +646,12 @@ LSM303D::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* 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)
|
if (_accel_reports == nullptr)
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
_mag_reports = new RingBuffer(2, sizeof(mag_report));
|
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
|
||||||
|
|
||||||
if (_mag_reports == nullptr)
|
if (_mag_reports == nullptr)
|
||||||
goto out;
|
goto out;
|
||||||
|
@ -1399,9 +1399,9 @@ LSM303D::start()
|
||||||
_mag_reports->flush();
|
_mag_reports->flush();
|
||||||
|
|
||||||
/* start polling at the specified rate */
|
/* start polling at the specified rate */
|
||||||
hrt_call_every(&_accel_call,
|
hrt_call_every(&_accel_call,
|
||||||
1000,
|
1000,
|
||||||
_call_accel_interval - LSM303D_TIMER_REDUCTION,
|
_call_accel_interval - LSM303D_TIMER_REDUCTION,
|
||||||
(hrt_callout)&LSM303D::measure_trampoline, this);
|
(hrt_callout)&LSM303D::measure_trampoline, this);
|
||||||
hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
|
hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
|
||||||
}
|
}
|
||||||
|
|
|
@ -125,7 +125,7 @@ private:
|
||||||
float _min_distance;
|
float _min_distance;
|
||||||
float _max_distance;
|
float _max_distance;
|
||||||
work_s _work;
|
work_s _work;
|
||||||
RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports;
|
||||||
bool _sensor_ok;
|
bool _sensor_ok;
|
||||||
int _measure_ticks;
|
int _measure_ticks;
|
||||||
bool _collect_phase;
|
bool _collect_phase;
|
||||||
|
@ -255,7 +255,7 @@ MB12XX::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_reports = new RingBuffer(2, sizeof(range_finder_report));
|
_reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report));
|
||||||
|
|
||||||
_index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */
|
_index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */
|
||||||
set_address(_index_counter); /* set I2c port to temp sonar i2c adress */
|
set_address(_index_counter); /* set I2c port to temp sonar i2c adress */
|
||||||
|
|
|
@ -242,7 +242,7 @@ private:
|
||||||
struct hrt_call _call;
|
struct hrt_call _call;
|
||||||
unsigned _call_interval;
|
unsigned _call_interval;
|
||||||
|
|
||||||
RingBuffer *_accel_reports;
|
ringbuffer::RingBuffer *_accel_reports;
|
||||||
|
|
||||||
struct accel_scale _accel_scale;
|
struct accel_scale _accel_scale;
|
||||||
float _accel_range_scale;
|
float _accel_range_scale;
|
||||||
|
@ -251,7 +251,7 @@ private:
|
||||||
int _accel_orb_class_instance;
|
int _accel_orb_class_instance;
|
||||||
int _accel_class_instance;
|
int _accel_class_instance;
|
||||||
|
|
||||||
RingBuffer *_gyro_reports;
|
ringbuffer::RingBuffer *_gyro_reports;
|
||||||
|
|
||||||
struct gyro_scale _gyro_scale;
|
struct gyro_scale _gyro_scale;
|
||||||
float _gyro_range_scale;
|
float _gyro_range_scale;
|
||||||
|
@ -611,11 +611,11 @@ MPU6000::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* 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)
|
if (_accel_reports == nullptr)
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
_gyro_reports = new RingBuffer(2, sizeof(gyro_report));
|
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
|
||||||
if (_gyro_reports == nullptr)
|
if (_gyro_reports == nullptr)
|
||||||
goto out;
|
goto out;
|
||||||
|
|
||||||
|
|
|
@ -127,7 +127,7 @@ protected:
|
||||||
struct work_s _work;
|
struct work_s _work;
|
||||||
unsigned _measure_ticks;
|
unsigned _measure_ticks;
|
||||||
|
|
||||||
RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports;
|
||||||
|
|
||||||
bool _collect_phase;
|
bool _collect_phase;
|
||||||
unsigned _measure_phase;
|
unsigned _measure_phase;
|
||||||
|
@ -269,7 +269,7 @@ MS5611::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_reports = new RingBuffer(2, sizeof(baro_report));
|
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
|
||||||
|
|
||||||
if (_reports == nullptr) {
|
if (_reports == nullptr) {
|
||||||
debug("can't get memory for reports");
|
debug("can't get memory for reports");
|
||||||
|
@ -896,7 +896,7 @@ start_bus(struct ms5611_bus_option &bus)
|
||||||
bus.dev = NULL;
|
bus.dev = NULL;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
int fd = open(bus.devpath, O_RDONLY);
|
int fd = open(bus.devpath, O_RDONLY);
|
||||||
|
|
||||||
/* set the poll rate to default, starts automatic data collection */
|
/* 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) {
|
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
|
||||||
return bus_options[i];
|
return bus_options[i];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
errx(1,"bus %u not started", (unsigned)busid);
|
errx(1,"bus %u not started", (unsigned)busid);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -121,7 +121,7 @@ private:
|
||||||
work_s _work; ///< work queue for scheduling reads
|
work_s _work; ///< work queue for scheduling reads
|
||||||
bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED
|
bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED
|
||||||
uint8_t _num_healthy; ///< number of healthy LEDs
|
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 _last_gencall;
|
||||||
uint64_t _start_time; ///< system time we first attempt to communicate with battery
|
uint64_t _start_time; ///< system time we first attempt to communicate with battery
|
||||||
};
|
};
|
||||||
|
@ -176,7 +176,7 @@ OREOLED::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* allocate command queue */
|
/* 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) {
|
if (_cmd_queue == nullptr) {
|
||||||
return ENOTTY;
|
return ENOTTY;
|
||||||
|
|
|
@ -120,16 +120,16 @@ protected:
|
||||||
private:
|
private:
|
||||||
|
|
||||||
work_s _work;
|
work_s _work;
|
||||||
RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports;
|
||||||
bool _sensor_ok;
|
bool _sensor_ok;
|
||||||
int _measure_ticks;
|
int _measure_ticks;
|
||||||
bool _collect_phase;
|
bool _collect_phase;
|
||||||
orb_advert_t _px4flow_topic;
|
orb_advert_t _px4flow_topic;
|
||||||
|
|
||||||
perf_counter_t _sample_perf;
|
perf_counter_t _sample_perf;
|
||||||
perf_counter_t _comms_errors;
|
perf_counter_t _comms_errors;
|
||||||
perf_counter_t _buffer_overflows;
|
perf_counter_t _buffer_overflows;
|
||||||
|
|
||||||
enum Rotation _sensor_rotation;
|
enum Rotation _sensor_rotation;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -217,7 +217,7 @@ PX4FLOW::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_reports = new RingBuffer(2, sizeof(optical_flow_s));
|
_reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s));
|
||||||
|
|
||||||
if (_reports == nullptr) {
|
if (_reports == nullptr) {
|
||||||
goto out;
|
goto out;
|
||||||
|
@ -486,10 +486,10 @@ PX4FLOW::collect()
|
||||||
report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
|
report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
|
||||||
|
|
||||||
report.sensor_id = 0;
|
report.sensor_id = 0;
|
||||||
|
|
||||||
/* rotate measurements according to parameter */
|
/* rotate measurements according to parameter */
|
||||||
float zeroval = 0.0f;
|
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) {
|
if (_px4flow_topic < 0) {
|
||||||
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
|
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
|
||||||
|
|
|
@ -118,7 +118,7 @@ private:
|
||||||
float _min_distance;
|
float _min_distance;
|
||||||
float _max_distance;
|
float _max_distance;
|
||||||
work_s _work;
|
work_s _work;
|
||||||
RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports;
|
||||||
bool _sensor_ok;
|
bool _sensor_ok;
|
||||||
int _measure_ticks;
|
int _measure_ticks;
|
||||||
bool _collect_phase;
|
bool _collect_phase;
|
||||||
|
@ -270,7 +270,7 @@ SF0X::init()
|
||||||
if (ret != OK) break;
|
if (ret != OK) break;
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_reports = new RingBuffer(2, sizeof(range_finder_report));
|
_reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report));
|
||||||
if (_reports == nullptr) {
|
if (_reports == nullptr) {
|
||||||
warnx("mem err");
|
warnx("mem err");
|
||||||
ret = -1;
|
ret = -1;
|
||||||
|
|
|
@ -82,7 +82,7 @@
|
||||||
#define TRONE_WHO_AM_I_REG 0x01 /* Who am I test register */
|
#define TRONE_WHO_AM_I_REG 0x01 /* Who am I test register */
|
||||||
#define TRONE_WHO_AM_I_REG_VAL 0xA1
|
#define TRONE_WHO_AM_I_REG_VAL 0xA1
|
||||||
|
|
||||||
|
|
||||||
/* Device limits */
|
/* Device limits */
|
||||||
#define TRONE_MIN_DISTANCE (0.20f)
|
#define TRONE_MIN_DISTANCE (0.20f)
|
||||||
#define TRONE_MAX_DISTANCE (14.00f)
|
#define TRONE_MAX_DISTANCE (14.00f)
|
||||||
|
@ -122,7 +122,7 @@ private:
|
||||||
float _min_distance;
|
float _min_distance;
|
||||||
float _max_distance;
|
float _max_distance;
|
||||||
work_s _work;
|
work_s _work;
|
||||||
RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports;
|
||||||
bool _sensor_ok;
|
bool _sensor_ok;
|
||||||
int _measure_ticks;
|
int _measure_ticks;
|
||||||
bool _collect_phase;
|
bool _collect_phase;
|
||||||
|
@ -281,7 +281,7 @@ TRONE::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_reports = new RingBuffer(2, sizeof(range_finder_report));
|
_reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report));
|
||||||
|
|
||||||
if (_reports == nullptr) {
|
if (_reports == nullptr) {
|
||||||
goto out;
|
goto out;
|
||||||
|
|
|
@ -36,7 +36,6 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "baro.hpp"
|
#include "baro.hpp"
|
||||||
#include <drivers/device/ringbuffer.h>
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
|
||||||
const char *const UavcanBarometerBridge::NAME = "baro";
|
const char *const UavcanBarometerBridge::NAME = "baro";
|
||||||
|
@ -56,7 +55,7 @@ int UavcanBarometerBridge::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_reports = new RingBuffer(2, sizeof(baro_report));
|
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
|
||||||
if (_reports == nullptr)
|
if (_reports == nullptr)
|
||||||
return -1;
|
return -1;
|
||||||
|
|
||||||
|
|
|
@ -39,11 +39,10 @@
|
||||||
|
|
||||||
#include "sensor_bridge.hpp"
|
#include "sensor_bridge.hpp"
|
||||||
#include <drivers/drv_baro.h>
|
#include <drivers/drv_baro.h>
|
||||||
|
#include <drivers/device/ringbuffer.h>
|
||||||
|
|
||||||
#include <uavcan/equipment/air_data/StaticAirData.hpp>
|
#include <uavcan/equipment/air_data/StaticAirData.hpp>
|
||||||
|
|
||||||
class RingBuffer;
|
|
||||||
|
|
||||||
class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
|
class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -68,5 +67,5 @@ private:
|
||||||
|
|
||||||
uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
|
uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
|
||||||
unsigned _msl_pressure = 101325;
|
unsigned _msl_pressure = 101325;
|
||||||
RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports;
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue