Rework the ringbuffer class so that it's not templated, and refactor its clients so they aren't dancing around the linker anymore.

This commit is contained in:
px4dev 2013-09-09 22:23:48 -07:00 committed by Lorenz Meier
parent a5821d2928
commit cefc7ac00e
10 changed files with 396 additions and 266 deletions

View File

@ -79,6 +79,7 @@
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
_reports(nullptr), _reports(nullptr),
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
_max_differential_pressure_pa(0), _max_differential_pressure_pa(0),
_sensor_ok(false), _sensor_ok(false),
_measure_ticks(0), _measure_ticks(0),
@ -87,8 +88,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
_airspeed_pub(-1), _airspeed_pub(-1),
_conversion_interval(conversion_interval), _conversion_interval(conversion_interval),
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")), _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows"))
{ {
// enable debug() calls // enable debug() calls
_debug_enabled = true; _debug_enabled = true;
@ -122,7 +122,7 @@ Airspeed::init()
goto out; goto out;
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new RingBuffer<differential_pressure_s>(2); _reports = new RingBuffer(2, sizeof(differential_pressure_s));
if (_reports == nullptr) if (_reports == nullptr)
goto out; goto out;
@ -282,7 +282,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
* we are careful to avoid racing with them. * we are careful to avoid racing with them.
*/ */
while (count--) { while (count--) {
if (_reports->get(*abuf)) { if (_reports->get(abuf)) {
ret += sizeof(*abuf); ret += sizeof(*abuf);
abuf++; abuf++;
} }
@ -312,7 +312,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
} }
/* state machine will have generated a report, copy it out */ /* state machine will have generated a report, copy it out */
if (_reports->get(*abuf)) { if (_reports->get(abuf)) {
ret = sizeof(*abuf); ret = sizeof(*abuf);
} }
@ -375,6 +375,6 @@ Airspeed::print_info()
void void
Airspeed::new_report(const differential_pressure_s &report) Airspeed::new_report(const differential_pressure_s &report)
{ {
if (!_reports->force(report)) if (!_reports->force(&report))
perf_count(_buffer_overflows); perf_count(_buffer_overflows);
} }

View File

@ -104,7 +104,7 @@ public:
virtual void print_info(); virtual void print_info();
private: private:
RingBuffer<differential_pressure_s> *_reports; RingBuffer *_reports;
perf_counter_t _buffer_overflows; perf_counter_t _buffer_overflows;
protected: protected:

View File

@ -147,14 +147,7 @@ private:
struct hrt_call _call; struct hrt_call _call;
unsigned _call_interval; unsigned _call_interval;
/* RingBuffer *_reports;
this wrapper type is needed to avoid a linker error for
RingBuffer instances which appear in two drivers.
*/
struct _accel_report {
accel_report r;
};
RingBuffer<struct _accel_report> *_reports;
struct accel_scale _accel_scale; struct accel_scale _accel_scale;
float _accel_range_scale; float _accel_range_scale;
@ -284,7 +277,7 @@ BMA180::init()
goto out; goto out;
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new RingBuffer<_accel_report>(2); _reports = new RingBuffer(2, sizeof(accel_report));
if (_reports == nullptr) if (_reports == nullptr)
goto out; goto out;
@ -349,7 +342,7 @@ ssize_t
BMA180::read(struct file *filp, char *buffer, size_t buflen) BMA180::read(struct file *filp, char *buffer, size_t buflen)
{ {
unsigned count = buflen / sizeof(struct accel_report); unsigned count = buflen / sizeof(struct accel_report);
struct _accel_report *arp = reinterpret_cast<struct _accel_report *>(buffer); struct accel_report *arp = reinterpret_cast<struct accel_report *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -365,7 +358,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
* we are careful to avoid racing with it. * we are careful to avoid racing with it.
*/ */
while (count--) { while (count--) {
if (_reports->get(*arp)) { if (_reports->get(arp)) {
ret += sizeof(*arp); ret += sizeof(*arp);
arp++; arp++;
} }
@ -380,7 +373,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
measure(); measure();
/* measurement will have generated a report, copy it out */ /* measurement will have generated a report, copy it out */
if (_reports->get(*arp)) if (_reports->get(arp))
ret = sizeof(*arp); ret = sizeof(*arp);
return ret; return ret;
@ -676,7 +669,7 @@ BMA180::measure()
// } raw_report; // } raw_report;
// #pragma pack(pop) // #pragma pack(pop)
struct _accel_report report; struct accel_report report;
/* start the performance counter */ /* start the performance counter */
perf_begin(_sample_perf); perf_begin(_sample_perf);
@ -696,40 +689,40 @@ BMA180::measure()
* them before. There is no good way to synchronise with the internal * them before. There is no good way to synchronise with the internal
* measurement flow without using the external interrupt. * measurement flow without using the external interrupt.
*/ */
report.r.timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
/* /*
* y of board is x of sensor and x of board is -y of sensor * y of board is x of sensor and x of board is -y of sensor
* perform only the axis assignment here. * perform only the axis assignment here.
* Two non-value bits are discarded directly * Two non-value bits are discarded directly
*/ */
report.r.y_raw = read_reg(ADDR_ACC_X_LSB + 0); report.y_raw = read_reg(ADDR_ACC_X_LSB + 0);
report.r.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
report.r.x_raw = read_reg(ADDR_ACC_X_LSB + 2); report.x_raw = read_reg(ADDR_ACC_X_LSB + 2);
report.r.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
report.r.z_raw = read_reg(ADDR_ACC_X_LSB + 4); report.z_raw = read_reg(ADDR_ACC_X_LSB + 4);
report.r.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
/* discard two non-value bits in the 16 bit measurement */ /* discard two non-value bits in the 16 bit measurement */
report.r.x_raw = (report.r.x_raw / 4); report.x_raw = (report.x_raw / 4);
report.r.y_raw = (report.r.y_raw / 4); report.y_raw = (report.y_raw / 4);
report.r.z_raw = (report.r.z_raw / 4); report.z_raw = (report.z_raw / 4);
/* invert y axis, due to 14 bit data no overflow can occur in the negation */ /* invert y axis, due to 14 bit data no overflow can occur in the negation */
report.r.y_raw = -report.r.y_raw; report.y_raw = -report.y_raw;
report.r.x = ((report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
report.r.y = ((report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
report.r.z = ((report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
report.r.scaling = _accel_range_scale; report.scaling = _accel_range_scale;
report.r.range_m_s2 = _accel_range_m_s2; report.range_m_s2 = _accel_range_m_s2;
_reports->force(report); _reports->force(&report);
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); poll_notify(POLLIN);
/* publish for subscribers */ /* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report.r); orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
/* stop the perf counter */ /* stop the perf counter */
perf_end(_sample_perf); perf_end(_sample_perf);

View File

@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* *
* 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
@ -35,15 +34,14 @@
/** /**
* @file ringbuffer.h * @file ringbuffer.h
* *
* A simple ringbuffer template. * A flexible ringbuffer class.
*/ */
#pragma once #pragma once
template<typename T>
class RingBuffer { class RingBuffer {
public: public:
RingBuffer(unsigned size); RingBuffer(unsigned ring_size, size_t entry_size);
virtual ~RingBuffer(); virtual ~RingBuffer();
/** /**
@ -52,15 +50,18 @@ public:
* @param val Item to put * @param val Item to put
* @return true if the item was put, false if the buffer is full * @return true if the item was put, false if the buffer is full
*/ */
bool put(T &val); bool put(const void *val, size_t val_size = 0);
/** bool put(int8_t val);
* Put an item into the buffer if there is space. bool put(uint8_t val);
* bool put(int16_t val);
* @param val Item to put bool put(uint16_t val);
* @return true if the item was put, false if the buffer is full bool put(int32_t val);
*/ bool put(uint32_t val);
bool put(const T &val); bool put(int64_t val);
bool put(uint64_t val);
bool put(float val);
bool put(double val);
/** /**
* Force an item into the buffer, discarding an older item if there is not space. * Force an item into the buffer, discarding an older item if there is not space.
@ -68,15 +69,18 @@ public:
* @param val Item to put * @param val Item to put
* @return true if an item was discarded to make space * @return true if an item was discarded to make space
*/ */
bool force(T &val); bool force(const void *val, size_t val_size = 0);
/** bool force(int8_t val);
* Force an item into the buffer, discarding an older item if there is not space. bool force(uint8_t val);
* bool force(int16_t val);
* @param val Item to put bool force(uint16_t val);
* @return true if an item was discarded to make space bool force(int32_t val);
*/ bool force(uint32_t val);
bool force(const T &val); bool force(int64_t val);
bool force(uint64_t val);
bool force(float val);
bool force(double val);
/** /**
* Get an item from the buffer. * Get an item from the buffer.
@ -84,15 +88,18 @@ public:
* @param val Item that was gotten * @param val Item that was gotten
* @return true if an item was got, false if the buffer was empty. * @return true if an item was got, false if the buffer was empty.
*/ */
bool get(T &val); bool get(void *val, size_t val_size = 0);
/** bool get(int8_t &val);
* Get an item from the buffer (scalars only). bool get(uint8_t &val);
* bool get(int16_t &val);
* @return The value that was fetched. If the buffer is empty, bool get(uint16_t &val);
* returns zero. bool get(int32_t &val);
*/ bool get(uint32_t &val);
T get(void); bool get(int64_t &val);
bool get(uint64_t &val);
bool get(float &val);
bool get(double &val);
/* /*
* Get the number of slots free in the buffer. * Get the number of slots free in the buffer.
@ -148,67 +155,68 @@ public:
void print_info(const char *name); void print_info(const char *name);
private: private:
T *_buf; unsigned _ring_size;
unsigned _size; const size_t _item_size;
char *_buf;
volatile unsigned _head; /**< insertion point */ volatile unsigned _head; /**< insertion point */
volatile unsigned _tail; /**< removal point */ volatile unsigned _tail; /**< removal point */
unsigned _next(unsigned index); unsigned _next(unsigned index);
}; };
template <typename T> RingBuffer::RingBuffer(unsigned ring_size, size_t item_size) :
RingBuffer<T>::RingBuffer(unsigned with_size) : _ring_size((ring_size + 1) * item_size),
_buf(new T[with_size + 1]), _item_size(item_size),
_size(with_size), _buf(new char[_ring_size]),
_head(with_size), _head(ring_size),
_tail(with_size) _tail(ring_size)
{} {}
template <typename T> RingBuffer::~RingBuffer()
RingBuffer<T>::~RingBuffer()
{ {
if (_buf != nullptr) if (_buf != nullptr)
delete[] _buf; delete[] _buf;
} }
template <typename T> unsigned
bool RingBuffer<T>::empty() RingBuffer::_next(unsigned index)
{
return (0 == index) ? _ring_size : (index - _item_size);
}
bool
RingBuffer::empty()
{ {
return _tail == _head; return _tail == _head;
} }
template <typename T> bool
bool RingBuffer<T>::full() RingBuffer::full()
{ {
return _next(_head) == _tail; return _next(_head) == _tail;
} }
template <typename T> unsigned
unsigned RingBuffer<T>::size() RingBuffer::size()
{ {
return (_buf != nullptr) ? _size : 0; return (_buf != nullptr) ? _ring_size : 0;
} }
template <typename T> void
void RingBuffer<T>::flush() RingBuffer::flush()
{ {
T junk;
while (!empty()) while (!empty())
get(junk); get(NULL);
} }
template <typename T> bool
unsigned RingBuffer<T>::_next(unsigned index) RingBuffer::put(const void *val, size_t val_size)
{
return (0 == index) ? _size : (index - 1);
}
template <typename T>
bool RingBuffer<T>::put(T &val)
{ {
unsigned next = _next(_head); unsigned next = _next(_head);
if (next != _tail) { if (next != _tail) {
_buf[_head] = val; if ((val_size == 0) || (val_size > _item_size))
val_size = _item_size;
memcpy(&_buf[_head], val, val_size);
_head = next; _head = next;
return true; return true;
} else { } else {
@ -216,55 +224,150 @@ bool RingBuffer<T>::put(T &val)
} }
} }
template <typename T> bool
bool RingBuffer<T>::put(const T &val) RingBuffer::put(int8_t val)
{ {
unsigned next = _next(_head); return put(&val, sizeof(val));
if (next != _tail) {
_buf[_head] = val;
_head = next;
return true;
} else {
return false;
}
} }
template <typename T> bool
bool RingBuffer<T>::force(T &val) RingBuffer::put(uint8_t val)
{
return put(&val, sizeof(val));
}
bool
RingBuffer::put(int16_t val)
{
return put(&val, sizeof(val));
}
bool
RingBuffer::put(uint16_t val)
{
return put(&val, sizeof(val));
}
bool
RingBuffer::put(int32_t val)
{
return put(&val, sizeof(val));
}
bool
RingBuffer::put(uint32_t val)
{
return put(&val, sizeof(val));
}
bool
RingBuffer::put(int64_t val)
{
return put(&val, sizeof(val));
}
bool
RingBuffer::put(uint64_t val)
{
return put(&val, sizeof(val));
}
bool
RingBuffer::put(float val)
{
return put(&val, sizeof(val));
}
bool
RingBuffer::put(double val)
{
return put(&val, sizeof(val));
}
bool
RingBuffer::force(const void *val, size_t val_size)
{ {
bool overwrote = false; bool overwrote = false;
for (;;) { for (;;) {
if (put(val)) if (put(val, val_size))
break; break;
T junk; get(NULL);
get(junk);
overwrote = true; overwrote = true;
} }
return overwrote; return overwrote;
} }
template <typename T> bool
bool RingBuffer<T>::force(const T &val) RingBuffer::force(int8_t val)
{ {
bool overwrote = false; return force(&val, sizeof(val));
for (;;) {
if (put(val))
break;
T junk;
get(junk);
overwrote = true;
}
return overwrote;
} }
template <typename T> bool
bool RingBuffer<T>::get(T &val) RingBuffer::force(uint8_t val)
{
return force(&val, sizeof(val));
}
bool
RingBuffer::force(int16_t val)
{
return force(&val, sizeof(val));
}
bool
RingBuffer::force(uint16_t val)
{
return force(&val, sizeof(val));
}
bool
RingBuffer::force(int32_t val)
{
return force(&val, sizeof(val));
}
bool
RingBuffer::force(uint32_t val)
{
return force(&val, sizeof(val));
}
bool
RingBuffer::force(int64_t val)
{
return force(&val, sizeof(val));
}
bool
RingBuffer::force(uint64_t val)
{
return force(&val, sizeof(val));
}
bool
RingBuffer::force(float val)
{
return force(&val, sizeof(val));
}
bool
RingBuffer::force(double val)
{
return force(&val, sizeof(val));
}
bool
RingBuffer::get(void *val, size_t val_size)
{ {
if (_tail != _head) { if (_tail != _head) {
unsigned candidate; unsigned candidate;
unsigned next; unsigned next;
if ((val_size == 0) || (val_size > _item_size))
val_size = _item_size;
do { do {
/* decide which element we think we're going to read */ /* decide which element we think we're going to read */
candidate = _tail; candidate = _tail;
@ -273,7 +376,8 @@ bool RingBuffer<T>::get(T &val)
next = _next(candidate); next = _next(candidate);
/* go ahead and read from this index */ /* go ahead and read from this index */
val = _buf[candidate]; if (val != NULL)
memcpy(val, &_buf[candidate], val_size);
/* if the tail pointer didn't change, we got our item */ /* if the tail pointer didn't change, we got our item */
} while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); } while (!__sync_bool_compare_and_swap(&_tail, candidate, next));
@ -284,15 +388,68 @@ bool RingBuffer<T>::get(T &val)
} }
} }
template <typename T> bool
T RingBuffer<T>::get(void) RingBuffer::get(int8_t &val)
{ {
T val; return get(&val, sizeof(val));
return get(val) ? val : 0;
} }
template <typename T> bool
unsigned RingBuffer<T>::space(void) RingBuffer::get(uint8_t &val)
{
return get(&val, sizeof(val));
}
bool
RingBuffer::get(int16_t &val)
{
return get(&val, sizeof(val));
}
bool
RingBuffer::get(uint16_t &val)
{
return get(&val, sizeof(val));
}
bool
RingBuffer::get(int32_t &val)
{
return get(&val, sizeof(val));
}
bool
RingBuffer::get(uint32_t &val)
{
return get(&val, sizeof(val));
}
bool
RingBuffer::get(int64_t &val)
{
return get(&val, sizeof(val));
}
bool
RingBuffer::get(uint64_t &val)
{
return get(&val, sizeof(val));
}
bool
RingBuffer::get(float &val)
{
return get(&val, sizeof(val));
}
bool
RingBuffer::get(double &val)
{
return get(&val, sizeof(val));
}
unsigned
RingBuffer::space(void)
{ {
unsigned tail, head; unsigned tail, head;
@ -309,39 +466,42 @@ unsigned RingBuffer<T>::space(void)
tail = _tail; tail = _tail;
} while (head != _head); } while (head != _head);
return (tail >= head) ? (_size - (tail - head)) : (head - tail - 1); unsigned reported_space = (tail >= head) ? (_ring_size - (tail - head)) : (head - tail - 1);
return reported_space / _item_size;
} }
template <typename T> unsigned
unsigned RingBuffer<T>::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
* over-estimate the number of items in the buffer. * over-estimate the number of items in the buffer.
*/ */
return _size - space(); return (_ring_size / _item_size) - space();
} }
template <typename T> bool
bool RingBuffer<T>::resize(unsigned new_size) RingBuffer::resize(unsigned new_size)
{ {
T *old_buffer; unsigned new_ring_size = (new_size + 1) * _item_size;
T *new_buffer = new T[new_size + 1]; char *old_buffer;
char *new_buffer = new char [new_ring_size];
if (new_buffer == nullptr) { if (new_buffer == nullptr) {
return false; return false;
} }
old_buffer = _buf; old_buffer = _buf;
_buf = new_buffer; _buf = new_buffer;
_size = new_size; _ring_size = new_ring_size;
_head = new_size; _head = new_size;
_tail = new_size; _tail = new_size;
delete[] old_buffer; delete[] old_buffer;
return true; return true;
} }
template <typename T> void
void RingBuffer<T>::print_info(const char *name) RingBuffer::print_info(const char *name)
{ {
printf("%s %u (%u/%u @ %p)\n", printf("%s %u/%u (%u/%u @ %p)\n",
name, _size, _head, _tail, _buf); name, _ring_size/_item_size, _head, _tail, _buf);
} }

View File

@ -149,7 +149,7 @@ private:
work_s _work; work_s _work;
unsigned _measure_ticks; unsigned _measure_ticks;
RingBuffer<struct mag_report> *_reports; RingBuffer *_reports;
mag_scale _scale; mag_scale _scale;
float _range_scale; float _range_scale;
float _range_ga; float _range_ga;
@ -367,7 +367,7 @@ HMC5883::init()
goto out; goto out;
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new RingBuffer<struct mag_report>(2); _reports = new RingBuffer(2, sizeof(mag_report));
if (_reports == nullptr) if (_reports == nullptr)
goto out; goto out;
@ -496,7 +496,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
* we are careful to avoid racing with them. * we are careful to avoid racing with them.
*/ */
while (count--) { while (count--) {
if (_reports->get(*mag_buf)) { if (_reports->get(mag_buf)) {
ret += sizeof(struct mag_report); ret += sizeof(struct mag_report);
mag_buf++; mag_buf++;
} }
@ -526,7 +526,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
break; break;
} }
if (_reports->get(*mag_buf)) { if (_reports->get(mag_buf)) {
ret = sizeof(struct mag_report); ret = sizeof(struct mag_report);
} }
} while (0); } while (0);
@ -878,7 +878,7 @@ HMC5883::collect()
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
/* post a report to the ring */ /* post a report to the ring */
if (_reports->force(new_report)) { if (_reports->force(&new_report)) {
perf_count(_buffer_overflows); perf_count(_buffer_overflows);
} }

View File

@ -185,7 +185,7 @@ private:
struct hrt_call _call; struct hrt_call _call;
unsigned _call_interval; unsigned _call_interval;
RingBuffer<gyro_report> *_reports; RingBuffer *_reports;
struct gyro_scale _gyro_scale; struct gyro_scale _gyro_scale;
float _gyro_range_scale; float _gyro_range_scale;
@ -347,7 +347,7 @@ L3GD20::init()
goto out; goto out;
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new RingBuffer<struct gyro_report>(2); _reports = new RingBuffer(2, sizeof(gyro_report));
if (_reports == nullptr) if (_reports == nullptr)
goto out; goto out;
@ -421,7 +421,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
* we are careful to avoid racing with it. * we are careful to avoid racing with it.
*/ */
while (count--) { while (count--) {
if (_reports->get(*gbuf)) { if (_reports->get(gbuf)) {
ret += sizeof(*gbuf); ret += sizeof(*gbuf);
gbuf++; gbuf++;
} }
@ -436,7 +436,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
measure(); measure();
/* measurement will have generated a report, copy it out */ /* measurement will have generated a report, copy it out */
if (_reports->get(*gbuf)) { if (_reports->get(gbuf)) {
ret = sizeof(*gbuf); ret = sizeof(*gbuf);
} }
@ -815,7 +815,7 @@ L3GD20::measure()
report.scaling = _gyro_range_scale; report.scaling = _gyro_range_scale;
report.range_rad_s = _gyro_range_rad_s; report.range_rad_s = _gyro_range_rad_s;
_reports->force(report); _reports->force(&report);
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); poll_notify(POLLIN);

View File

@ -219,19 +219,8 @@ private:
unsigned _call_accel_interval; unsigned _call_accel_interval;
unsigned _call_mag_interval; unsigned _call_mag_interval;
/* RingBuffer *_accel_reports;
these wrapper types are needed to avoid a linker error for RingBuffer *_mag_reports;
RingBuffer instances which appear in two drivers.
*/
struct _accel_report {
struct accel_report r;
};
RingBuffer<struct _accel_report> *_accel_reports;
struct _mag_report {
struct mag_report r;
};
RingBuffer<struct _mag_report> *_mag_reports;
struct accel_scale _accel_scale; struct accel_scale _accel_scale;
unsigned _accel_range_m_s2; unsigned _accel_range_m_s2;
@ -499,7 +488,7 @@ LSM303D::init()
} }
/* allocate basic report buffers */ /* allocate basic report buffers */
_accel_reports = new RingBuffer<struct _accel_report>(2); _accel_reports = new RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) if (_accel_reports == nullptr)
goto out; goto out;
@ -509,7 +498,7 @@ LSM303D::init()
memset(&zero_report, 0, sizeof(zero_report)); memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
_mag_reports = new RingBuffer<struct _mag_report>(2); _mag_reports = new RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr) if (_mag_reports == nullptr)
goto out; goto out;
@ -579,7 +568,7 @@ ssize_t
LSM303D::read(struct file *filp, char *buffer, size_t buflen) LSM303D::read(struct file *filp, char *buffer, size_t buflen)
{ {
unsigned count = buflen / sizeof(struct accel_report); unsigned count = buflen / sizeof(struct accel_report);
struct _accel_report *arb = reinterpret_cast<struct _accel_report *>(buffer); accel_report *arb = reinterpret_cast<accel_report *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -592,7 +581,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
* While there is space in the caller's buffer, and reports, copy them. * While there is space in the caller's buffer, and reports, copy them.
*/ */
while (count--) { while (count--) {
if (_accel_reports->get(*arb)) { if (_accel_reports->get(arb)) {
ret += sizeof(*arb); ret += sizeof(*arb);
arb++; arb++;
} }
@ -606,7 +595,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
measure(); measure();
/* measurement will have generated a report, copy it out */ /* measurement will have generated a report, copy it out */
if (_accel_reports->get(*arb)) if (_accel_reports->get(arb))
ret = sizeof(*arb); ret = sizeof(*arb);
return ret; return ret;
@ -616,7 +605,7 @@ ssize_t
LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
{ {
unsigned count = buflen / sizeof(struct mag_report); unsigned count = buflen / sizeof(struct mag_report);
struct _mag_report *mrb = reinterpret_cast<struct _mag_report *>(buffer); mag_report *mrb = reinterpret_cast<mag_report *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -630,7 +619,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
* While there is space in the caller's buffer, and reports, copy them. * While there is space in the caller's buffer, and reports, copy them.
*/ */
while (count--) { while (count--) {
if (_mag_reports->get(*mrb)) { if (_mag_reports->get(mrb)) {
ret += sizeof(*mrb); ret += sizeof(*mrb);
mrb++; mrb++;
} }
@ -645,7 +634,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
measure(); measure();
/* measurement will have generated a report, copy it out */ /* measurement will have generated a report, copy it out */
if (_mag_reports->get(*mrb)) if (_mag_reports->get(mrb))
ret = sizeof(*mrb); ret = sizeof(*mrb);
return ret; return ret;
@ -1237,7 +1226,7 @@ LSM303D::measure()
} raw_accel_report; } raw_accel_report;
#pragma pack(pop) #pragma pack(pop)
struct _accel_report accel_report; accel_report accel_report;
/* start the performance counter */ /* start the performance counter */
perf_begin(_accel_sample_perf); perf_begin(_accel_sample_perf);
@ -1262,30 +1251,30 @@ LSM303D::measure()
*/ */
accel_report.r.timestamp = hrt_absolute_time(); accel_report.timestamp = hrt_absolute_time();
accel_report.r.x_raw = raw_accel_report.x; accel_report.x_raw = raw_accel_report.x;
accel_report.r.y_raw = raw_accel_report.y; accel_report.y_raw = raw_accel_report.y;
accel_report.r.z_raw = raw_accel_report.z; accel_report.z_raw = raw_accel_report.z;
float x_in_new = ((accel_report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((accel_report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((accel_report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
accel_report.r.x = _accel_filter_x.apply(x_in_new); accel_report.x = _accel_filter_x.apply(x_in_new);
accel_report.r.y = _accel_filter_y.apply(y_in_new); accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.r.z = _accel_filter_z.apply(z_in_new); accel_report.z = _accel_filter_z.apply(z_in_new);
accel_report.r.scaling = _accel_range_scale; accel_report.scaling = _accel_range_scale;
accel_report.r.range_m_s2 = _accel_range_m_s2; accel_report.range_m_s2 = _accel_range_m_s2;
_accel_reports->force(accel_report); _accel_reports->force(&accel_report);
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); poll_notify(POLLIN);
/* publish for subscribers */ /* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report.r); orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
_accel_read++; _accel_read++;
@ -1307,7 +1296,7 @@ LSM303D::mag_measure()
} raw_mag_report; } raw_mag_report;
#pragma pack(pop) #pragma pack(pop)
struct _mag_report mag_report; mag_report mag_report;
/* start the performance counter */ /* start the performance counter */
perf_begin(_mag_sample_perf); perf_begin(_mag_sample_perf);
@ -1332,25 +1321,25 @@ LSM303D::mag_measure()
*/ */
mag_report.r.timestamp = hrt_absolute_time(); mag_report.timestamp = hrt_absolute_time();
mag_report.r.x_raw = raw_mag_report.x; mag_report.x_raw = raw_mag_report.x;
mag_report.r.y_raw = raw_mag_report.y; mag_report.y_raw = raw_mag_report.y;
mag_report.r.z_raw = raw_mag_report.z; mag_report.z_raw = raw_mag_report.z;
mag_report.r.x = ((mag_report.r.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
mag_report.r.y = ((mag_report.r.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
mag_report.r.z = ((mag_report.r.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
mag_report.r.scaling = _mag_range_scale; mag_report.scaling = _mag_range_scale;
mag_report.r.range_ga = (float)_mag_range_ga; mag_report.range_ga = (float)_mag_range_ga;
_mag_reports->force(mag_report); _mag_reports->force(&mag_report);
/* XXX please check this poll_notify, is it the right one? */ /* XXX please check this poll_notify, is it the right one? */
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); poll_notify(POLLIN);
/* publish for subscribers */ /* publish for subscribers */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report.r); orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
_mag_read++; _mag_read++;

View File

@ -120,7 +120,7 @@ private:
float _min_distance; float _min_distance;
float _max_distance; float _max_distance;
work_s _work; work_s _work;
RingBuffer<struct range_finder_report> *_reports; RingBuffer *_reports;
bool _sensor_ok; bool _sensor_ok;
int _measure_ticks; int _measure_ticks;
bool _collect_phase; bool _collect_phase;
@ -226,7 +226,7 @@ MB12XX::init()
goto out; goto out;
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new RingBuffer<struct range_finder_report>(2); _reports = new RingBuffer(2, sizeof(range_finder_report));
if (_reports == nullptr) if (_reports == nullptr)
goto out; goto out;
@ -403,7 +403,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
* we are careful to avoid racing with them. * we are careful to avoid racing with them.
*/ */
while (count--) { while (count--) {
if (_reports->get(*rbuf)) { if (_reports->get(rbuf)) {
ret += sizeof(*rbuf); ret += sizeof(*rbuf);
rbuf++; rbuf++;
} }
@ -433,7 +433,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
} }
/* state machine will have generated a report, copy it out */ /* state machine will have generated a report, copy it out */
if (_reports->get(*rbuf)) { if (_reports->get(rbuf)) {
ret = sizeof(*rbuf); ret = sizeof(*rbuf);
} }
@ -496,7 +496,7 @@ MB12XX::collect()
/* publish it */ /* publish it */
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
if (_reports->force(report)) { if (_reports->force(&report)) {
perf_count(_buffer_overflows); perf_count(_buffer_overflows);
} }

View File

@ -194,26 +194,14 @@ private:
struct hrt_call _call; struct hrt_call _call;
unsigned _call_interval; unsigned _call_interval;
/* RingBuffer *_accel_reports;
these wrapper types are needed to avoid a linker error for
RingBuffer instances which appear in two drivers.
*/
struct _accel_report {
struct accel_report r;
};
typedef RingBuffer<_accel_report> AccelReportBuffer;
AccelReportBuffer *_accel_reports;
struct accel_scale _accel_scale; struct accel_scale _accel_scale;
float _accel_range_scale; float _accel_range_scale;
float _accel_range_m_s2; float _accel_range_m_s2;
orb_advert_t _accel_topic; orb_advert_t _accel_topic;
struct _gyro_report { RingBuffer *_gyro_reports;
struct gyro_report r;
};
typedef RingBuffer<_gyro_report> GyroReportBuffer;
GyroReportBuffer *_gyro_reports;
struct gyro_scale _gyro_scale; struct gyro_scale _gyro_scale;
float _gyro_range_scale; float _gyro_range_scale;
@ -441,11 +429,11 @@ MPU6000::init()
} }
/* allocate basic report buffers */ /* allocate basic report buffers */
_accel_reports = new AccelReportBuffer(2); _accel_reports = new RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) if (_accel_reports == nullptr)
goto out; goto out;
_gyro_reports = new GyroReportBuffer(2); _gyro_reports = new RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) if (_gyro_reports == nullptr)
goto out; goto out;
@ -475,16 +463,16 @@ MPU6000::init()
if (gyro_ret != OK) { if (gyro_ret != OK) {
_gyro_topic = -1; _gyro_topic = -1;
} else { } else {
_gyro_report gr; gyro_report gr;
_gyro_reports->get(gr); _gyro_reports->get(&gr);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr.r); _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
} }
/* advertise accel topic */ /* advertise accel topic */
_accel_report ar; accel_report ar;
_accel_reports->get(ar); _accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar.r); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
out: out:
return ret; return ret;
@ -665,10 +653,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
return -EAGAIN; return -EAGAIN;
/* copy reports out of our buffer to the caller */ /* copy reports out of our buffer to the caller */
_accel_report *arp = reinterpret_cast<_accel_report *>(buffer); accel_report *arp = reinterpret_cast<accel_report *>(buffer);
int transferred = 0; int transferred = 0;
while (count--) { while (count--) {
if (!_accel_reports->get(*arp)) if (!_accel_reports->get(arp))
break; break;
transferred++; transferred++;
arp++; arp++;
@ -759,10 +747,10 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
return -EAGAIN; return -EAGAIN;
/* copy reports out of our buffer to the caller */ /* copy reports out of our buffer to the caller */
_gyro_report *grp = reinterpret_cast<_gyro_report *>(buffer); gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
int transferred = 0; int transferred = 0;
while (count--) { while (count--) {
if (!_gyro_reports->get(*grp)) if (!_gyro_reports->get(grp))
break; break;
transferred++; transferred++;
grp++; grp++;
@ -1191,13 +1179,13 @@ MPU6000::measure()
/* /*
* Report buffers. * Report buffers.
*/ */
_accel_report arb; accel_report arb;
_gyro_report grb; gyro_report grb;
/* /*
* Adjust and scale results to m/s^2. * Adjust and scale results to m/s^2.
*/ */
grb.r.timestamp = arb.r.timestamp = hrt_absolute_time(); grb.timestamp = arb.timestamp = hrt_absolute_time();
/* /*
@ -1218,53 +1206,53 @@ MPU6000::measure()
/* NOTE: Axes have been swapped to match the board a few lines above. */ /* NOTE: Axes have been swapped to match the board a few lines above. */
arb.r.x_raw = report.accel_x; arb.x_raw = report.accel_x;
arb.r.y_raw = report.accel_y; arb.y_raw = report.accel_y;
arb.r.z_raw = report.accel_z; arb.z_raw = report.accel_z;
float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.r.x = _accel_filter_x.apply(x_in_new); arb.x = _accel_filter_x.apply(x_in_new);
arb.r.y = _accel_filter_y.apply(y_in_new); arb.y = _accel_filter_y.apply(y_in_new);
arb.r.z = _accel_filter_z.apply(z_in_new); arb.z = _accel_filter_z.apply(z_in_new);
arb.r.scaling = _accel_range_scale; arb.scaling = _accel_range_scale;
arb.r.range_m_s2 = _accel_range_m_s2; arb.range_m_s2 = _accel_range_m_s2;
arb.r.temperature_raw = report.temp; arb.temperature_raw = report.temp;
arb.r.temperature = (report.temp) / 361.0f + 35.0f; arb.temperature = (report.temp) / 361.0f + 35.0f;
grb.r.x_raw = report.gyro_x; grb.x_raw = report.gyro_x;
grb.r.y_raw = report.gyro_y; grb.y_raw = report.gyro_y;
grb.r.z_raw = report.gyro_z; grb.z_raw = report.gyro_z;
float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.r.x = _gyro_filter_x.apply(x_gyro_in_new); grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.r.y = _gyro_filter_y.apply(y_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.r.z = _gyro_filter_z.apply(z_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new);
grb.r.scaling = _gyro_range_scale; grb.scaling = _gyro_range_scale;
grb.r.range_rad_s = _gyro_range_rad_s; grb.range_rad_s = _gyro_range_rad_s;
grb.r.temperature_raw = report.temp; grb.temperature_raw = report.temp;
grb.r.temperature = (report.temp) / 361.0f + 35.0f; grb.temperature = (report.temp) / 361.0f + 35.0f;
_accel_reports->force(arb); _accel_reports->force(&arb);
_gyro_reports->force(grb); _gyro_reports->force(&grb);
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); poll_notify(POLLIN);
_gyro->parent_poll_notify(); _gyro->parent_poll_notify();
/* and publish for subscribers */ /* and publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb.r); orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
if (_gyro_topic != -1) { if (_gyro_topic != -1) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r); orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
} }
/* stop measuring */ /* stop measuring */

View File

@ -115,7 +115,7 @@ protected:
struct work_s _work; struct work_s _work;
unsigned _measure_ticks; unsigned _measure_ticks;
RingBuffer<struct baro_report> *_reports; RingBuffer *_reports;
bool _collect_phase; bool _collect_phase;
unsigned _measure_phase; unsigned _measure_phase;
@ -241,7 +241,7 @@ MS5611::init()
} }
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new RingBuffer<struct baro_report>(2); _reports = new 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");
@ -285,7 +285,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
* we are careful to avoid racing with them. * we are careful to avoid racing with them.
*/ */
while (count--) { while (count--) {
if (_reports->get(*brp)) { if (_reports->get(brp)) {
ret += sizeof(*brp); ret += sizeof(*brp);
brp++; brp++;
} }
@ -327,7 +327,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
} }
/* state machine will have generated a report, copy it out */ /* state machine will have generated a report, copy it out */
if (_reports->get(*brp)) if (_reports->get(brp))
ret = sizeof(*brp); ret = sizeof(*brp);
} while (0); } while (0);
@ -664,7 +664,7 @@ MS5611::collect()
/* publish it */ /* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
if (_reports->force(report)) { if (_reports->force(&report)) {
perf_count(_buffer_overflows); perf_count(_buffer_overflows);
} }