forked from Archive/PX4-Autopilot
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:
parent
a5821d2928
commit
cefc7ac00e
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
|
@ -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:
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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++;
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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 */
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue