dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/drivers/airspeed/airspeed.cpp
View File

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

2
src/drivers/airspeed/airspeed.h
View File

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

55
src/drivers/bma180/bma180.cpp
View File

@ -147,14 +147,7 @@ private:
struct hrt_call _call;
unsigned _call_interval;
/*
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;
RingBuffer *_reports;
struct accel_scale _accel_scale;
float _accel_range_scale;
@ -284,7 +277,7 @@ BMA180::init()
goto out;
/* allocate basic report buffers */
_reports = new RingBuffer<_accel_report>(2);
_reports = new RingBuffer(2, sizeof(accel_report));
if (_reports == nullptr)
goto out;
@ -349,7 +342,7 @@ ssize_t
BMA180::read(struct file *filp, char *buffer, size_t buflen)
{
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;
/* 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.
*/
while (count--) {
if (_reports->get(*arp)) {
if (_reports->get(arp)) {
ret += sizeof(*arp);
arp++;
}
@ -380,7 +373,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
measure();
/* measurement will have generated a report, copy it out */
if (_reports->get(*arp))
if (_reports->get(arp))
ret = sizeof(*arp);
return ret;
@ -676,7 +669,7 @@ BMA180::measure()
// } raw_report;
// #pragma pack(pop)
struct _accel_report report;
struct accel_report report;
/* start the performance counter */
perf_begin(_sample_perf);
@ -696,40 +689,40 @@ BMA180::measure()
* them before. There is no good way to synchronise with the internal
* 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
* perform only the axis assignment here.
* Two non-value bits are discarded directly
*/
report.r.y_raw = read_reg(ADDR_ACC_X_LSB + 0);
report.r.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
report.r.x_raw = read_reg(ADDR_ACC_X_LSB + 2);
report.r.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
report.r.z_raw = read_reg(ADDR_ACC_X_LSB + 4);
report.r.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
report.y_raw = read_reg(ADDR_ACC_X_LSB + 0);
report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
report.x_raw = read_reg(ADDR_ACC_X_LSB + 2);
report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
report.z_raw = read_reg(ADDR_ACC_X_LSB + 4);
report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
/* discard two non-value bits in the 16 bit measurement */
report.r.x_raw = (report.r.x_raw / 4);
report.r.y_raw = (report.r.y_raw / 4);
report.r.z_raw = (report.r.z_raw / 4);
report.x_raw = (report.x_raw / 4);
report.y_raw = (report.y_raw / 4);
report.z_raw = (report.z_raw / 4);
/* 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.r.y = ((report.r.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.r.scaling = _accel_range_scale;
report.r.range_m_s2 = _accel_range_m_s2;
report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
report.scaling = _accel_range_scale;
report.range_m_s2 = _accel_range_m_s2;
_reports->force(report);
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* 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 */
perf_end(_sample_perf);

374
src/drivers/device/ringbuffer.h
View File

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

10
src/drivers/hmc5883/hmc5883.cpp
View File

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

10
src/drivers/l3gd20/l3gd20.cpp
View File

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

85
src/drivers/lsm303d/lsm303d.cpp
View File

@ -219,19 +219,8 @@ private:
unsigned _call_accel_interval;
unsigned _call_mag_interval;
/*
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;
};
RingBuffer<struct _accel_report> *_accel_reports;
struct _mag_report {
struct mag_report r;
};
RingBuffer<struct _mag_report> *_mag_reports;
RingBuffer *_accel_reports;
RingBuffer *_mag_reports;
struct accel_scale _accel_scale;
unsigned _accel_range_m_s2;
@ -499,7 +488,7 @@ LSM303D::init()
}
/* allocate basic report buffers */
_accel_reports = new RingBuffer<struct _accel_report>(2);
_accel_reports = new RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr)
goto out;
@ -509,7 +498,7 @@ LSM303D::init()
memset(&zero_report, 0, sizeof(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)
goto out;
@ -579,7 +568,7 @@ ssize_t
LSM303D::read(struct file *filp, char *buffer, size_t buflen)
{
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;
/* 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 (count--) {
if (_accel_reports->get(*arb)) {
if (_accel_reports->get(arb)) {
ret += sizeof(*arb);
arb++;
}
@ -606,7 +595,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
measure();
/* measurement will have generated a report, copy it out */
if (_accel_reports->get(*arb))
if (_accel_reports->get(arb))
ret = sizeof(*arb);
return ret;
@ -616,7 +605,7 @@ ssize_t
LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
{
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;
/* 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 (count--) {
if (_mag_reports->get(*mrb)) {
if (_mag_reports->get(mrb)) {
ret += sizeof(*mrb);
mrb++;
}
@ -645,7 +634,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
measure();
/* measurement will have generated a report, copy it out */
if (_mag_reports->get(*mrb))
if (_mag_reports->get(mrb))
ret = sizeof(*mrb);
return ret;
@ -1237,7 +1226,7 @@ LSM303D::measure()
} raw_accel_report;
#pragma pack(pop)
struct _accel_report accel_report;
accel_report accel_report;
/* start the performance counter */
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.r.y_raw = raw_accel_report.y;
accel_report.r.z_raw = raw_accel_report.z;
accel_report.x_raw = raw_accel_report.x;
accel_report.y_raw = raw_accel_report.y;
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 y_in_new = ((accel_report.r.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 x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_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.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.r.y = _accel_filter_y.apply(y_in_new);
accel_report.r.z = _accel_filter_z.apply(z_in_new);
accel_report.x = _accel_filter_x.apply(x_in_new);
accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.z = _accel_filter_z.apply(z_in_new);
accel_report.r.scaling = _accel_range_scale;
accel_report.r.range_m_s2 = _accel_range_m_s2;
accel_report.scaling = _accel_range_scale;
accel_report.range_m_s2 = _accel_range_m_s2;
_accel_reports->force(accel_report);
_accel_reports->force(&accel_report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* 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++;
@ -1307,7 +1296,7 @@ LSM303D::mag_measure()
} raw_mag_report;
#pragma pack(pop)
struct _mag_report mag_report;
mag_report mag_report;
/* start the performance counter */
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.r.y_raw = raw_mag_report.y;
mag_report.r.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.r.y = ((mag_report.r.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.r.scaling = _mag_range_scale;
mag_report.r.range_ga = (float)_mag_range_ga;
mag_report.x_raw = raw_mag_report.x;
mag_report.y_raw = raw_mag_report.y;
mag_report.z_raw = raw_mag_report.z;
mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
mag_report.scaling = _mag_range_scale;
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? */
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* 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++;

10
src/drivers/mb12xx/mb12xx.cpp
View File

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

94
src/drivers/mpu6000/mpu6000.cpp
View File

@ -194,26 +194,14 @@ private:
struct hrt_call _call;
unsigned _call_interval;
/*
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;
RingBuffer *_accel_reports;
struct accel_scale _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
struct _gyro_report {
struct gyro_report r;
};
typedef RingBuffer<_gyro_report> GyroReportBuffer;
GyroReportBuffer *_gyro_reports;
RingBuffer *_gyro_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
@ -441,11 +429,11 @@ MPU6000::init()
}
/* allocate basic report buffers */
_accel_reports = new AccelReportBuffer(2);
_accel_reports = new RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr)
goto out;
_gyro_reports = new GyroReportBuffer(2);
_gyro_reports = new RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr)
goto out;
@ -475,16 +463,16 @@ MPU6000::init()
if (gyro_ret != OK) {
_gyro_topic = -1;
} else {
_gyro_report gr;
_gyro_reports->get(gr);
gyro_report 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 */
_accel_report ar;
_accel_reports->get(ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar.r);
accel_report ar;
_accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
out:
return ret;
@ -665,10 +653,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
return -EAGAIN;
/* 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;
while (count--) {
if (!_accel_reports->get(*arp))
if (!_accel_reports->get(arp))
break;
transferred++;
arp++;
@ -759,10 +747,10 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
return -EAGAIN;
/* 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;
while (count--) {
if (!_gyro_reports->get(*grp))
if (!_gyro_reports->get(grp))
break;
transferred++;
grp++;
@ -1191,13 +1179,13 @@ MPU6000::measure()
/*
* Report buffers.
*/
_accel_report arb;
_gyro_report grb;
accel_report arb;
gyro_report grb;
/*
* 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. */
arb.r.x_raw = report.accel_x;
arb.r.y_raw = report.accel_y;
arb.r.z_raw = report.accel_z;
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
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 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;
arb.r.x = _accel_filter_x.apply(x_in_new);
arb.r.y = _accel_filter_y.apply(y_in_new);
arb.r.z = _accel_filter_z.apply(z_in_new);
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
arb.r.scaling = _accel_range_scale;
arb.r.range_m_s2 = _accel_range_m_s2;
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
arb.r.temperature_raw = report.temp;
arb.r.temperature = (report.temp) / 361.0f + 35.0f;
arb.temperature_raw = report.temp;
arb.temperature = (report.temp) / 361.0f + 35.0f;
grb.r.x_raw = report.gyro_x;
grb.r.y_raw = report.gyro_y;
grb.r.z_raw = report.gyro_z;
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
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 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;
grb.r.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.r.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.r.z = _gyro_filter_z.apply(z_gyro_in_new);
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
grb.r.scaling = _gyro_range_scale;
grb.r.range_rad_s = _gyro_range_rad_s;
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
grb.r.temperature_raw = report.temp;
grb.r.temperature = (report.temp) / 361.0f + 35.0f;
grb.temperature_raw = report.temp;
grb.temperature = (report.temp) / 361.0f + 35.0f;
_accel_reports->force(arb);
_gyro_reports->force(grb);
_accel_reports->force(&arb);
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
poll_notify(POLLIN);
_gyro->parent_poll_notify();
/* 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) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r);
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
}
/* stop measuring */

10
src/drivers/ms5611/ms5611.cpp
View File

@ -115,7 +115,7 @@ protected:
struct work_s _work;
unsigned _measure_ticks;
RingBuffer<struct baro_report> *_reports;
RingBuffer *_reports;
bool _collect_phase;
unsigned _measure_phase;
@ -241,7 +241,7 @@ MS5611::init()
}
/* allocate basic report buffers */
_reports = new RingBuffer<struct baro_report>(2);
_reports = new RingBuffer(2, sizeof(baro_report));
if (_reports == nullptr) {
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.
*/
while (count--) {
if (_reports->get(*brp)) {
if (_reports->get(brp)) {
ret += sizeof(*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 */
if (_reports->get(*brp))
if (_reports->get(brp))
ret = sizeof(*brp);
} while (0);
@ -664,7 +664,7 @@ MS5611::collect()
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
if (_reports->force(report)) {
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}