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

Merge branch 'master' of github.com:PX4/Firmware into status_monitoring

This commit is contained in:
Lorenz Meier 2013-09-12 09:26:30 +02:00
parent 7257642371 5ece19f66a
commit c9b8a019ea
12 changed files with 760 additions and 658 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -68,6 +68,7 @@
#include <drivers/drv_airspeed.h> #include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h> #include <uORB/topics/differential_pressure.h>
@ -77,10 +78,9 @@
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),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr), _reports(nullptr),
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
_max_differential_pressure_pa(0),
_sensor_ok(false), _sensor_ok(false),
_measure_ticks(0), _measure_ticks(0),
_collect_phase(false), _collect_phase(false),
@ -88,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;
@ -105,7 +104,7 @@ Airspeed::~Airspeed()
/* free any existing reports */ /* free any existing reports */
if (_reports != nullptr) if (_reports != nullptr)
delete[] _reports; delete _reports;
// free perf counters // free perf counters
perf_free(_sample_perf); perf_free(_sample_perf);
@ -123,20 +122,14 @@ Airspeed::init()
goto out; goto out;
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_reports = 2; _reports = new RingBuffer(2, sizeof(differential_pressure_s));
_reports = new struct differential_pressure_s[_num_reports];
for (unsigned i = 0; i < _num_reports; i++)
_reports[i].max_differential_pressure_pa = 0;
if (_reports == nullptr) if (_reports == nullptr)
goto out; goto out;
_oldest_report = _next_report = 0;
/* get a publish handle on the airspeed topic */ /* get a publish handle on the airspeed topic */
memset(&_reports[0], 0, sizeof(_reports[0])); differential_pressure_s zero_report;
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]); memset(&zero_report, 0, sizeof(zero_report));
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report);
if (_airspeed_pub < 0) if (_airspeed_pub < 0)
warnx("failed to create airspeed sensor object. Did you start uOrb?"); warnx("failed to create airspeed sensor object. Did you start uOrb?");
@ -229,31 +222,22 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
return (1000 / _measure_ticks); return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100)) if ((arg < 1) || (arg > 100))
return -EINVAL; return -EINVAL;
/* allocate new buffer */ irqstate_t flags = irqsave();
struct differential_pressure_s *buf = new struct differential_pressure_s[arg]; if (!_reports->resize(arg)) {
irqrestore(flags);
if (nullptr == buf)
return -ENOMEM; return -ENOMEM;
}
/* reset the measurement state machine with the new buffer, free the old */ irqrestore(flags);
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK; return OK;
} }
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1; return _reports->size();
case SENSORIOCRESET: case SENSORIOCRESET:
/* XXX implement this */ /* XXX implement this */
@ -281,7 +265,8 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t ssize_t
Airspeed::read(struct file *filp, char *buffer, size_t buflen) Airspeed::read(struct file *filp, char *buffer, size_t buflen)
{ {
unsigned count = buflen / sizeof(struct differential_pressure_s); unsigned count = buflen / sizeof(differential_pressure_s);
differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -297,10 +282,9 @@ 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 (_oldest_report != _next_report) { if (_reports->get(abuf)) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); ret += sizeof(*abuf);
ret += sizeof(_reports[0]); abuf++;
INCREMENT(_oldest_report, _num_reports);
} }
} }
@ -309,9 +293,8 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
} }
/* manual measurement - run one conversion */ /* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do { do {
_oldest_report = _next_report = 0; _reports->flush();
/* trigger a measurement */ /* trigger a measurement */
if (OK != measure()) { if (OK != measure()) {
@ -329,8 +312,9 @@ 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 */
memcpy(buffer, _reports, sizeof(*_reports)); if (_reports->get(abuf)) {
ret = sizeof(*_reports); ret = sizeof(*abuf);
}
} while (0); } while (0);
@ -342,7 +326,7 @@ Airspeed::start()
{ {
/* reset the report ring and state machine */ /* reset the report ring and state machine */
_collect_phase = false; _collect_phase = false;
_oldest_report = _next_report = 0; _reports->flush();
/* schedule a cycle to start things */ /* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
@ -385,6 +369,12 @@ Airspeed::print_info()
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows); perf_print_counter(_buffer_overflows);
warnx("poll interval: %u ticks", _measure_ticks); warnx("poll interval: %u ticks", _measure_ticks);
warnx("report queue: %u (%u/%u @ %p)", _reports->print_info("report queue");
_num_reports, _oldest_report, _next_report, _reports); }
void
Airspeed::new_report(const differential_pressure_s &report)
{
if (!_reports->force(&report))
perf_count(_buffer_overflows);
} }

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

@ -68,6 +68,7 @@
#include <drivers/drv_airspeed.h> #include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h> #include <uORB/topics/differential_pressure.h>
@ -102,6 +103,10 @@ public:
*/ */
virtual void print_info(); virtual void print_info();
private:
RingBuffer *_reports;
perf_counter_t _buffer_overflows;
protected: protected:
virtual int probe(); virtual int probe();
@ -114,10 +119,7 @@ protected:
virtual int collect() = 0; virtual int collect() = 0;
work_s _work; work_s _work;
unsigned _num_reports; uint16_t _max_differential_pressure_pa;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
differential_pressure_s *_reports;
bool _sensor_ok; bool _sensor_ok;
int _measure_ticks; int _measure_ticks;
bool _collect_phase; bool _collect_phase;
@ -129,7 +131,6 @@ protected:
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _comms_errors; perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/** /**
@ -162,8 +163,12 @@ protected:
*/ */
static void cycle_trampoline(void *arg); static void cycle_trampoline(void *arg);
/**
* add a new report to the reports queue
*
* @param report differential_pressure_s report
*/
void new_report(const differential_pressure_s &report);
}; };
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)

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

@ -63,6 +63,7 @@
#include <drivers/device/spi.h> #include <drivers/device/spi.h>
#include <drivers/drv_accel.h> #include <drivers/drv_accel.h>
#include <drivers/device/ringbuffer.h>
/* oddly, ERROR is not defined for c++ */ /* oddly, ERROR is not defined for c++ */
@ -146,10 +147,7 @@ private:
struct hrt_call _call; struct hrt_call _call;
unsigned _call_interval; unsigned _call_interval;
unsigned _num_reports; RingBuffer *_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
struct accel_report *_reports;
struct accel_scale _accel_scale; struct accel_scale _accel_scale;
float _accel_range_scale; float _accel_range_scale;
@ -233,16 +231,9 @@ private:
int set_lowpass(unsigned frequency); int set_lowpass(unsigned frequency);
}; };
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
BMA180::BMA180(int bus, spi_dev_e device) : BMA180::BMA180(int bus, spi_dev_e device) :
SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
_call_interval(0), _call_interval(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr), _reports(nullptr),
_accel_range_scale(0.0f), _accel_range_scale(0.0f),
_accel_range_m_s2(0.0f), _accel_range_m_s2(0.0f),
@ -270,7 +261,7 @@ BMA180::~BMA180()
/* free any existing reports */ /* free any existing reports */
if (_reports != nullptr) if (_reports != nullptr)
delete[] _reports; delete _reports;
/* delete the perf counter */ /* delete the perf counter */
perf_free(_sample_perf); perf_free(_sample_perf);
@ -286,16 +277,15 @@ BMA180::init()
goto out; goto out;
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_reports = 2; _reports = new RingBuffer(2, sizeof(accel_report));
_oldest_report = _next_report = 0;
_reports = new struct accel_report[_num_reports];
if (_reports == nullptr) if (_reports == nullptr)
goto out; goto out;
/* advertise sensor topic */ /* advertise sensor topic */
memset(&_reports[0], 0, sizeof(_reports[0])); struct accel_report zero_report;
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
/* perform soft reset (p48) */ /* perform soft reset (p48) */
write_reg(ADDR_RESET, SOFT_RESET); write_reg(ADDR_RESET, SOFT_RESET);
@ -352,6 +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);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -367,10 +358,9 @@ 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 (_oldest_report != _next_report) { if (_reports->get(arp)) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); ret += sizeof(*arp);
ret += sizeof(_reports[0]); arp++;
INCREMENT(_oldest_report, _num_reports);
} }
} }
@ -379,12 +369,12 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
} }
/* manual measurement */ /* manual measurement */
_oldest_report = _next_report = 0; _reports->flush();
measure(); measure();
/* measurement will have generated a report, copy it out */ /* measurement will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports)); if (_reports->get(arp))
ret = sizeof(*_reports); ret = sizeof(*arp);
return ret; return ret;
} }
@ -449,31 +439,22 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_interval; return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */ /* lower bound is mandatory, upper bound is a sanity check */
arg++; if ((arg < 2) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100)) irqstate_t flags = irqsave();
return -EINVAL; if (!_reports->resize(arg)) {
irqrestore(flags);
/* allocate new buffer */ return -ENOMEM;
struct accel_report *buf = new struct accel_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK;
} }
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1; return _reports->size();
case SENSORIOCRESET: case SENSORIOCRESET:
/* XXX implement */ /* XXX implement */
@ -654,7 +635,7 @@ BMA180::start()
stop(); stop();
/* reset the report ring */ /* reset the report ring */
_oldest_report = _next_report = 0; _reports->flush();
/* start polling at the specified rate */ /* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this); hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
@ -688,7 +669,7 @@ BMA180::measure()
// } raw_report; // } raw_report;
// #pragma pack(pop) // #pragma pack(pop)
accel_report *report = &_reports[_next_report]; struct accel_report report;
/* start the performance counter */ /* start the performance counter */
perf_begin(_sample_perf); perf_begin(_sample_perf);
@ -708,45 +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.
*/ */
_reports[_next_report].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->y_raw = read_reg(ADDR_ACC_X_LSB + 0); report.y_raw = read_reg(ADDR_ACC_X_LSB + 0);
report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; 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 + 2);
report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; 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 + 4);
report->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->x_raw = (report->x_raw / 4); report.x_raw = (report.x_raw / 4);
report->y_raw = (report->y_raw / 4); report.y_raw = (report.y_raw / 4);
report->z_raw = (report->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->y_raw = -report->y_raw; report.y_raw = -report.y_raw;
report->x = ((report->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->y = ((report->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->z = ((report->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->scaling = _accel_range_scale; report.scaling = _accel_range_scale;
report->range_m_s2 = _accel_range_m_s2; report.range_m_s2 = _accel_range_m_s2;
/* post a report to the ring - note, not locked */ _reports->force(&report);
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, fix it */
if (_next_report == _oldest_report)
INCREMENT(_oldest_report, _num_reports);
/* 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); 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);
@ -756,8 +732,7 @@ void
BMA180::print_info() BMA180::print_info()
{ {
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n", _reports->print_info("report queue");
_num_reports, _oldest_report, _next_report, _reports);
} }
/** /**

430
src/drivers/device/ringbuffer.h
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,37 @@ 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);
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);
/** /**
* Put an item into the buffer. * Force an item into the buffer, discarding an older item if there is not space.
* *
* @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 an item was discarded to make space
*/ */
bool put(const T &val); bool force(const void *val, size_t val_size = 0);
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. * Get an item from the buffer.
@ -68,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, or zero if the buffer was bool get(uint16_t &val);
* empty. 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.
@ -97,54 +120,103 @@ public:
/* /*
* Returns true if the buffer is empty. * Returns true if the buffer is empty.
*/ */
bool empty() { return _tail == _head; } bool empty();
/* /*
* Returns true if the buffer is full. * Returns true if the buffer is full.
*/ */
bool full() { return _next(_head) == _tail; } bool full();
/* /*
* Returns the capacity of the buffer, or zero if the buffer could * Returns the capacity of the buffer, or zero if the buffer could
* not be allocated. * not be allocated.
*/ */
unsigned size() { return (_buf != nullptr) ? _size : 0; } unsigned size();
/* /*
* Empties the buffer. * Empties the buffer.
*/ */
void flush() { _head = _tail = _size; } void flush();
/*
* resize the buffer. This is unsafe to be called while
* a producer or consuming is running. Caller is responsible
* for any locking needed
*
* @param new_size new size for buffer
* @return true if the resize succeeds, false if
* not (allocation error)
*/
bool resize(unsigned new_size);
/*
* printf() some info on the buffer
*/
void print_info(const char *name);
private: private:
T *const _buf; unsigned _num_items;
const unsigned _size; const size_t _item_size;
volatile unsigned _head; /**< insertion point */ char *_buf;
volatile unsigned _tail; /**< removal point */ volatile unsigned _head; /**< insertion point in _item_size units */
volatile unsigned _tail; /**< removal point in _item_size units */
unsigned _next(unsigned index); unsigned _next(unsigned index);
}; };
template <typename T> RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
RingBuffer<T>::RingBuffer(unsigned with_size) : _num_items(num_items),
_buf(new T[with_size + 1]), _item_size(item_size),
_size(with_size), _buf(new char[(_num_items+1) * item_size]),
_head(with_size), _head(_num_items),
_tail(with_size) _tail(_num_items)
{} {}
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>::put(T &val) RingBuffer::_next(unsigned index)
{
return (0 == index) ? _num_items : (index - 1);
}
bool
RingBuffer::empty()
{
return _tail == _head;
}
bool
RingBuffer::full()
{
return _next(_head) == _tail;
}
unsigned
RingBuffer::size()
{
return (_buf != nullptr) ? _num_items : 0;
}
void
RingBuffer::flush()
{
while (!empty())
get(NULL);
}
bool
RingBuffer::put(const void *val, size_t val_size)
{ {
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 * _item_size], val, val_size);
_head = next; _head = next;
return true; return true;
} else { } else {
@ -152,52 +224,286 @@ 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>::get(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;
for (;;) {
if (put(val, val_size))
break;
get(NULL);
overwrote = true;
}
return overwrote;
}
bool
RingBuffer::force(int8_t val)
{
return force(&val, sizeof(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) { if (_tail != _head) {
val = _buf[_tail]; unsigned candidate;
_tail = _next(_tail); 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;
/* and what the corresponding next index will be */
next = _next(candidate);
/* go ahead and read from this index */
if (val != NULL)
memcpy(val, &_buf[candidate * _item_size], val_size);
/* if the tail pointer didn't change, we got our item */
} while (!__sync_bool_compare_and_swap(&_tail, candidate, next));
return true; return true;
} else { } else {
return false; return false;
} }
} }
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 (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1); return get(&val, sizeof(val));
} }
template <typename T> bool
unsigned RingBuffer<T>::count(void) RingBuffer::get(int16_t &val)
{ {
return _size - space(); return get(&val, sizeof(val));
} }
template <typename T> bool
unsigned RingBuffer<T>::_next(unsigned index) RingBuffer::get(uint16_t &val)
{ {
return (0 == index) ? _size : (index - 1); 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;
/*
* Make a copy of the head/tail pointers in a fashion that
* may err on the side of under-estimating the free space
* in the buffer in the case that the buffer is being updated
* asynchronously with our check.
* If the head pointer changes (reducing space) while copying,
* re-try the copy.
*/
do {
head = _head;
tail = _tail;
} while (head != _head);
return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1);
}
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 _num_items - space();
}
bool
RingBuffer::resize(unsigned new_size)
{
char *old_buffer;
char *new_buffer = new char [(new_size+1) * _item_size];
if (new_buffer == nullptr) {
return false;
}
old_buffer = _buf;
_buf = new_buffer;
_num_items = new_size;
_head = new_size;
_tail = new_size;
delete[] old_buffer;
return true;
}
void
RingBuffer::print_info(const char *name)
{
printf("%s %u/%u (%u/%u @ %p)\n",
name,
_num_items,
_num_items * _item_size,
_head,
_tail,
_buf);
} }

28
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -68,6 +68,7 @@
#include <drivers/drv_airspeed.h> #include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h> #include <uORB/topics/differential_pressure.h>
@ -173,27 +174,22 @@ ETSAirspeed::collect()
diff_pres_pa -= _diff_pres_offset; diff_pres_pa -= _diff_pres_offset;
} }
// XXX we may want to smooth out the readings to remove noise.
_reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].differential_pressure_pa = diff_pres_pa;
// Track maximum differential pressure measured (so we can work out top speed). // Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { if (diff_pres_pa > _max_differential_pressure_pa) {
_reports[_next_report].max_differential_pressure_pa = diff_pres_pa; _max_differential_pressure_pa = diff_pres_pa;
} }
// XXX we may want to smooth out the readings to remove noise.
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.differential_pressure_pa = diff_pres_pa;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
/* announce the airspeed if needed, just publish else */ /* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
/* post a report to the ring - note, not locked */ new_report(report);
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
}
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); poll_notify(POLLIN);

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

@ -65,6 +65,7 @@
#include <drivers/drv_mag.h> #include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h> #include <uORB/topics/subsystem_info.h>
@ -148,10 +149,7 @@ private:
work_s _work; work_s _work;
unsigned _measure_ticks; unsigned _measure_ticks;
unsigned _num_reports; RingBuffer *_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
mag_report *_reports;
mag_scale _scale; mag_scale _scale;
float _range_scale; float _range_scale;
float _range_ga; float _range_ga;
@ -310,9 +308,6 @@ private:
}; };
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
/* /*
* Driver 'main' command. * Driver 'main' command.
*/ */
@ -322,9 +317,6 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
HMC5883::HMC5883(int bus) : HMC5883::HMC5883(int bus) :
I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
_measure_ticks(0), _measure_ticks(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr), _reports(nullptr),
_range_scale(0), /* default range scale from counts to gauss */ _range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f), _range_ga(1.3f),
@ -356,9 +348,8 @@ HMC5883::~HMC5883()
/* make sure we are truly inactive */ /* make sure we are truly inactive */
stop(); stop();
/* free any existing reports */
if (_reports != nullptr) if (_reports != nullptr)
delete[] _reports; delete _reports;
// free perf counters // free perf counters
perf_free(_sample_perf); perf_free(_sample_perf);
@ -375,21 +366,18 @@ HMC5883::init()
if (I2C::init() != OK) if (I2C::init() != OK)
goto out; goto out;
/* reset the device configuration */
reset();
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_reports = 2; _reports = new RingBuffer(2, sizeof(mag_report));
_reports = new struct mag_report[_num_reports];
if (_reports == nullptr) if (_reports == nullptr)
goto out; goto out;
_oldest_report = _next_report = 0; /* reset the device configuration */
reset();
/* get a publish handle on the mag topic */ /* get a publish handle on the mag topic */
memset(&_reports[0], 0, sizeof(_reports[0])); struct mag_report zero_report;
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]); memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
if (_mag_topic < 0) if (_mag_topic < 0)
debug("failed to create sensor_mag object"); debug("failed to create sensor_mag object");
@ -493,6 +481,7 @@ ssize_t
HMC5883::read(struct file *filp, char *buffer, size_t buflen) HMC5883::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 *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -501,17 +490,15 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
/* if automatic measurement is enabled */ /* if automatic measurement is enabled */
if (_measure_ticks > 0) { if (_measure_ticks > 0) {
/* /*
* 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.
* Note that we may be pre-empted by the workq thread while we are doing this; * Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them. * we are careful to avoid racing with them.
*/ */
while (count--) { while (count--) {
if (_oldest_report != _next_report) { if (_reports->get(mag_buf)) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); ret += sizeof(struct mag_report);
ret += sizeof(_reports[0]); mag_buf++;
INCREMENT(_oldest_report, _num_reports);
} }
} }
@ -522,7 +509,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
/* manual measurement - run one conversion */ /* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */ /* XXX really it'd be nice to lock against other readers here */
do { do {
_oldest_report = _next_report = 0; _reports->flush();
/* trigger a measurement */ /* trigger a measurement */
if (OK != measure()) { if (OK != measure()) {
@ -539,10 +526,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
break; break;
} }
/* state machine will have generated a report, copy it out */ if (_reports->get(mag_buf)) {
memcpy(buffer, _reports, sizeof(*_reports)); ret = sizeof(struct mag_report);
ret = sizeof(*_reports); }
} while (0); } while (0);
return ret; return ret;
@ -615,31 +601,22 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000/TICK2USEC(_measure_ticks); return 1000000/TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100)) if ((arg < 1) || (arg > 100))
return -EINVAL; return -EINVAL;
/* allocate new buffer */ irqstate_t flags = irqsave();
struct mag_report *buf = new struct mag_report[arg]; if (!_reports->resize(arg)) {
irqrestore(flags);
if (nullptr == buf)
return -ENOMEM; return -ENOMEM;
}
/* reset the measurement state machine with the new buffer, free the old */ irqrestore(flags);
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK; return OK;
} }
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1; return _reports->size();
case SENSORIOCRESET: case SENSORIOCRESET:
return reset(); return reset();
@ -701,7 +678,7 @@ HMC5883::start()
{ {
/* reset the report ring and state machine */ /* reset the report ring and state machine */
_collect_phase = false; _collect_phase = false;
_oldest_report = _next_report = 0; _reports->flush();
/* schedule a cycle to start things */ /* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1); work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
@ -810,9 +787,10 @@ HMC5883::collect()
perf_begin(_sample_perf); perf_begin(_sample_perf);
struct mag_report new_report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */ /* this should be fairly close to the end of the measurement, so the best approximation of the time */
_reports[_next_report].timestamp = hrt_absolute_time(); new_report.timestamp = hrt_absolute_time();
/* /*
* @note We could read the status register here, which could tell us that * @note We could read the status register here, which could tell us that
@ -842,8 +820,10 @@ HMC5883::collect()
*/ */
if ((abs(report.x) > 2048) || if ((abs(report.x) > 2048) ||
(abs(report.y) > 2048) || (abs(report.y) > 2048) ||
(abs(report.z) > 2048)) (abs(report.z) > 2048)) {
perf_count(_comms_errors);
goto out; goto out;
}
/* /*
* RAW outputs * RAW outputs
@ -851,10 +831,10 @@ HMC5883::collect()
* to align the sensor axes with the board, x and y need to be flipped * to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated * and y needs to be negated
*/ */
_reports[_next_report].x_raw = report.y; new_report.x_raw = report.y;
_reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x); new_report.y_raw = -report.x;
/* z remains z */ /* z remains z */
_reports[_next_report].z_raw = report.z; new_report.z_raw = report.z;
/* scale values for output */ /* scale values for output */
@ -876,34 +856,30 @@ HMC5883::collect()
#ifdef PX4_I2C_BUS_ONBOARD #ifdef PX4_I2C_BUS_ONBOARD
if (_bus == PX4_I2C_BUS_ONBOARD) { if (_bus == PX4_I2C_BUS_ONBOARD) {
/* to align the sensor axes with the board, x and y need to be flipped */ /* to align the sensor axes with the board, x and y need to be flipped */
_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */ /* flip axes and negate value for y */
_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */ /* z remains z */
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
} else { } else {
#endif #endif
/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
* therefore switch x and y and invert y */ * therefore switch x and y and invert y */
_reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale; new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */ /* flip axes and negate value for y */
_reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */ /* z remains z */
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
#ifdef PX4_I2C_BUS_ONBOARD #ifdef PX4_I2C_BUS_ONBOARD
} }
#endif #endif
/* publish it */ /* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]); orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
/* post a report to the ring - note, not locked */ /* post a report to the ring */
INCREMENT(_next_report, _num_reports); if (_reports->force(&new_report)) {
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
perf_count(_buffer_overflows); perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
} }
/* notify anyone waiting for data */ /* notify anyone waiting for data */
@ -1222,8 +1198,7 @@ HMC5883::print_info()
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows); perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks); printf("poll interval: %u ticks\n", _measure_ticks);
printf("report queue: %u (%u/%u @ %p)\n", _reports->print_info("report queue");
_num_reports, _oldest_report, _next_report, _reports);
} }
/** /**

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

@ -61,6 +61,7 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/device/spi.h> #include <drivers/device/spi.h>
#include <drivers/drv_gyro.h> #include <drivers/drv_gyro.h>
#include <drivers/device/ringbuffer.h>
#include <board_config.h> #include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp> #include <mathlib/math/filter/LowPassFilter2p.hpp>
@ -183,11 +184,8 @@ private:
struct hrt_call _call; struct hrt_call _call;
unsigned _call_interval; unsigned _call_interval;
unsigned _num_reports; RingBuffer *_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
struct gyro_report *_reports;
struct gyro_scale _gyro_scale; struct gyro_scale _gyro_scale;
float _gyro_range_scale; float _gyro_range_scale;
@ -299,16 +297,9 @@ private:
int self_test(); int self_test();
}; };
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
_call_interval(0), _call_interval(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr), _reports(nullptr),
_gyro_range_scale(0.0f), _gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f), _gyro_range_rad_s(0.0f),
@ -340,7 +331,7 @@ L3GD20::~L3GD20()
/* free any existing reports */ /* free any existing reports */
if (_reports != nullptr) if (_reports != nullptr)
delete[] _reports; delete _reports;
/* delete the perf counter */ /* delete the perf counter */
perf_free(_sample_perf); perf_free(_sample_perf);
@ -356,16 +347,15 @@ L3GD20::init()
goto out; goto out;
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_reports = 2; _reports = new RingBuffer(2, sizeof(gyro_report));
_oldest_report = _next_report = 0;
_reports = new struct gyro_report[_num_reports];
if (_reports == nullptr) if (_reports == nullptr)
goto out; goto out;
/* advertise sensor topic */ /* advertise sensor topic */
memset(&_reports[0], 0, sizeof(_reports[0])); struct gyro_report zero_report;
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); memset(&zero_report, 0, sizeof(zero_report));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
reset(); reset();
@ -415,6 +405,7 @@ ssize_t
L3GD20::read(struct file *filp, char *buffer, size_t buflen) L3GD20::read(struct file *filp, char *buffer, size_t buflen)
{ {
unsigned count = buflen / sizeof(struct gyro_report); unsigned count = buflen / sizeof(struct gyro_report);
struct gyro_report *gbuf = reinterpret_cast<struct gyro_report *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -430,10 +421,9 @@ 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 (_oldest_report != _next_report) { if (_reports->get(gbuf)) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); ret += sizeof(*gbuf);
ret += sizeof(_reports[0]); gbuf++;
INCREMENT(_oldest_report, _num_reports);
} }
} }
@ -442,12 +432,13 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
} }
/* manual measurement */ /* manual measurement */
_oldest_report = _next_report = 0; _reports->flush();
measure(); measure();
/* measurement will have generated a report, copy it out */ /* measurement will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports)); if (_reports->get(gbuf)) {
ret = sizeof(*_reports); ret = sizeof(*gbuf);
}
return ret; return ret;
} }
@ -515,31 +506,22 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_interval; return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */ /* lower bound is mandatory, upper bound is a sanity check */
arg++; if ((arg < 1) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */ irqstate_t flags = irqsave();
if ((arg < 2) || (arg > 100)) if (!_reports->resize(arg)) {
return -EINVAL; irqrestore(flags);
return -ENOMEM;
/* allocate new buffer */
struct gyro_report *buf = new struct gyro_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK;
} }
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1; return _reports->size();
case SENSORIOCRESET: case SENSORIOCRESET:
reset(); reset();
@ -708,7 +690,7 @@ L3GD20::start()
stop(); stop();
/* reset the report ring */ /* reset the report ring */
_oldest_report = _next_report = 0; _reports->flush();
/* start polling at the specified rate */ /* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
@ -768,7 +750,7 @@ L3GD20::measure()
} raw_report; } raw_report;
#pragma pack(pop) #pragma pack(pop)
gyro_report *report = &_reports[_next_report]; gyro_report report;
/* start the performance counter */ /* start the performance counter */
perf_begin(_sample_perf); perf_begin(_sample_perf);
@ -791,61 +773,56 @@ L3GD20::measure()
* the offset is 74 from the origin and subtracting * the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero. * 74 from all measurements centers them around zero.
*/ */
report->timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
switch (_orientation) { switch (_orientation) {
case SENSOR_BOARD_ROTATION_000_DEG: case SENSOR_BOARD_ROTATION_000_DEG:
/* keep axes in place */ /* keep axes in place */
report->x_raw = raw_report.x; report.x_raw = raw_report.x;
report->y_raw = raw_report.y; report.y_raw = raw_report.y;
break; break;
case SENSOR_BOARD_ROTATION_090_DEG: case SENSOR_BOARD_ROTATION_090_DEG:
/* swap x and y */ /* swap x and y */
report->x_raw = raw_report.y; report.x_raw = raw_report.y;
report->y_raw = raw_report.x; report.y_raw = raw_report.x;
break; break;
case SENSOR_BOARD_ROTATION_180_DEG: case SENSOR_BOARD_ROTATION_180_DEG:
/* swap x and y and negate both */ /* swap x and y and negate both */
report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
break; break;
case SENSOR_BOARD_ROTATION_270_DEG: case SENSOR_BOARD_ROTATION_270_DEG:
/* swap x and y and negate y */ /* swap x and y and negate y */
report->x_raw = raw_report.y; report.x_raw = raw_report.y;
report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
break; break;
} }
report->z_raw = raw_report.z; report.z_raw = raw_report.z;
report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
report->x = _gyro_filter_x.apply(report->x); report.x = _gyro_filter_x.apply(report.x);
report->y = _gyro_filter_y.apply(report->y); report.y = _gyro_filter_y.apply(report.y);
report->z = _gyro_filter_z.apply(report->z); report.z = _gyro_filter_z.apply(report.z);
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;
/* post a report to the ring - note, not locked */ _reports->force(&report);
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, fix it */
if (_next_report == _oldest_report)
INCREMENT(_oldest_report, _num_reports);
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); poll_notify(POLLIN);
/* publish for subscribers */ /* publish for subscribers */
if (_gyro_topic > 0) if (_gyro_topic > 0)
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
_read++; _read++;
@ -858,8 +835,7 @@ L3GD20::print_info()
{ {
printf("gyro reads: %u\n", _read); printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n", _reports->print_info("report queue");
_num_reports, _oldest_report, _next_report, _reports);
} }
int int

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

@ -62,6 +62,7 @@
#include <drivers/device/spi.h> #include <drivers/device/spi.h>
#include <drivers/drv_accel.h> #include <drivers/drv_accel.h>
#include <drivers/drv_mag.h> #include <drivers/drv_mag.h>
#include <drivers/device/ringbuffer.h>
#include <board_config.h> #include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp> #include <mathlib/math/filter/LowPassFilter2p.hpp>
@ -218,15 +219,8 @@ private:
unsigned _call_accel_interval; unsigned _call_accel_interval;
unsigned _call_mag_interval; unsigned _call_mag_interval;
unsigned _num_accel_reports; RingBuffer *_accel_reports;
volatile unsigned _next_accel_report; RingBuffer *_mag_reports;
volatile unsigned _oldest_accel_report;
struct accel_report *_accel_reports;
unsigned _num_mag_reports;
volatile unsigned _next_mag_report;
volatile unsigned _oldest_mag_report;
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;
@ -420,22 +414,12 @@ private:
}; };
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
_mag(new LSM303D_mag(this)), _mag(new LSM303D_mag(this)),
_call_accel_interval(0), _call_accel_interval(0),
_call_mag_interval(0), _call_mag_interval(0),
_num_accel_reports(0),
_next_accel_report(0),
_oldest_accel_report(0),
_accel_reports(nullptr), _accel_reports(nullptr),
_num_mag_reports(0),
_next_mag_report(0),
_oldest_mag_report(0),
_mag_reports(nullptr), _mag_reports(nullptr),
_accel_range_m_s2(0.0f), _accel_range_m_s2(0.0f),
_accel_range_scale(0.0f), _accel_range_scale(0.0f),
@ -480,9 +464,9 @@ LSM303D::~LSM303D()
/* free any existing reports */ /* free any existing reports */
if (_accel_reports != nullptr) if (_accel_reports != nullptr)
delete[] _accel_reports; delete _accel_reports;
if (_mag_reports != nullptr) if (_mag_reports != nullptr)
delete[] _mag_reports; delete _mag_reports;
delete _mag; delete _mag;
@ -504,20 +488,17 @@ LSM303D::init()
} }
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_accel_reports = 2; _accel_reports = new RingBuffer(2, sizeof(accel_report));
_oldest_accel_report = _next_accel_report = 0;
_accel_reports = new struct accel_report[_num_accel_reports];
if (_accel_reports == nullptr) if (_accel_reports == nullptr)
goto out; goto out;
/* advertise accel topic */ /* advertise accel topic */
memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); struct accel_report zero_report;
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
_num_mag_reports = 2; _mag_reports = new RingBuffer(2, sizeof(mag_report));
_oldest_mag_report = _next_mag_report = 0;
_mag_reports = new struct mag_report[_num_mag_reports];
if (_mag_reports == nullptr) if (_mag_reports == nullptr)
goto out; goto out;
@ -525,8 +506,9 @@ LSM303D::init()
reset(); reset();
/* advertise mag topic */ /* advertise mag topic */
memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); struct mag_report zero_mag_report;
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); memset(&zero_mag_report, 0, sizeof(zero_mag_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report);
/* do CDev init for the mag device node, keep it optional */ /* do CDev init for the mag device node, keep it optional */
mag_ret = _mag->init(); mag_ret = _mag->init();
@ -586,6 +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);
accel_report *arb = reinterpret_cast<accel_report *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -594,17 +577,13 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
/* if automatic measurement is enabled */ /* if automatic measurement is enabled */
if (_call_accel_interval > 0) { if (_call_accel_interval > 0) {
/* /*
* 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.
* Note that we may be pre-empted by the measurement code while we are doing this;
* we are careful to avoid racing with it.
*/ */
while (count--) { while (count--) {
if (_oldest_accel_report != _next_accel_report) { if (_accel_reports->get(arb)) {
memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); ret += sizeof(*arb);
ret += sizeof(_accel_reports[0]); arb++;
INCREMENT(_oldest_accel_report, _num_accel_reports);
} }
} }
@ -613,12 +592,11 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
} }
/* manual measurement */ /* manual measurement */
_oldest_accel_report = _next_accel_report = 0;
measure(); measure();
/* measurement will have generated a report, copy it out */ /* measurement will have generated a report, copy it out */
memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); if (_accel_reports->get(arb))
ret = sizeof(*_accel_reports); ret = sizeof(*arb);
return ret; return ret;
} }
@ -627,6 +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);
mag_report *mrb = reinterpret_cast<mag_report *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -638,14 +617,11 @@ 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.
* Note that we may be pre-empted by the measurement code while we are doing this;
* we are careful to avoid racing with it.
*/ */
while (count--) { while (count--) {
if (_oldest_mag_report != _next_mag_report) { if (_mag_reports->get(mrb)) {
memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports)); ret += sizeof(*mrb);
ret += sizeof(_mag_reports[0]); mrb++;
INCREMENT(_oldest_mag_report, _num_mag_reports);
} }
} }
@ -654,12 +630,12 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
} }
/* manual measurement */ /* manual measurement */
_oldest_mag_report = _next_mag_report = 0; _mag_reports->flush();
measure(); measure();
/* measurement will have generated a report, copy it out */ /* measurement will have generated a report, copy it out */
memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); if (_mag_reports->get(mrb))
ret = sizeof(*_mag_reports); ret = sizeof(*mrb);
return ret; return ret;
} }
@ -727,31 +703,22 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_accel_interval; return 1000000 / _call_accel_interval;
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100)) if ((arg < 1) || (arg > 100))
return -EINVAL; return -EINVAL;
/* allocate new buffer */ irqstate_t flags = irqsave();
struct accel_report *buf = new struct accel_report[arg]; if (!_accel_reports->resize(arg)) {
irqrestore(flags);
if (nullptr == buf)
return -ENOMEM; return -ENOMEM;
}
/* reset the measurement state machine with the new buffer, free the old */ irqrestore(flags);
stop();
delete[] _accel_reports;
_num_accel_reports = arg;
_accel_reports = buf;
start();
return OK; return OK;
} }
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _num_accel_reports - 1; return _accel_reports->size();
case SENSORIOCRESET: case SENSORIOCRESET:
reset(); reset();
@ -863,31 +830,22 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_mag_interval; return 1000000 / _call_mag_interval;
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */ /* lower bound is mandatory, upper bound is a sanity check */
arg++; if ((arg < 1) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */ irqstate_t flags = irqsave();
if ((arg < 2) || (arg > 100)) if (!_mag_reports->resize(arg)) {
return -EINVAL; irqrestore(flags);
return -ENOMEM;
/* allocate new buffer */
struct mag_report *buf = new struct mag_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _mag_reports;
_num_mag_reports = arg;
_mag_reports = buf;
start();
return OK;
} }
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _num_mag_reports - 1; return _mag_reports->size();
case SENSORIOCRESET: case SENSORIOCRESET:
reset(); reset();
@ -1220,8 +1178,8 @@ LSM303D::start()
stop(); stop();
/* reset the report ring */ /* reset the report ring */
_oldest_accel_report = _next_accel_report = 0; _accel_reports->flush();
_oldest_mag_report = _next_mag_report = 0; _mag_reports->flush();
/* start polling at the specified rate */ /* start polling at the specified rate */
hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
@ -1268,7 +1226,7 @@ LSM303D::measure()
} raw_accel_report; } raw_accel_report;
#pragma pack(pop) #pragma pack(pop)
accel_report *accel_report = &_accel_reports[_next_accel_report]; accel_report accel_report;
/* start the performance counter */ /* start the performance counter */
perf_begin(_accel_sample_perf); perf_begin(_accel_sample_perf);
@ -1293,35 +1251,30 @@ LSM303D::measure()
*/ */
accel_report->timestamp = hrt_absolute_time(); accel_report.timestamp = hrt_absolute_time();
accel_report->x_raw = raw_accel_report.x; accel_report.x_raw = raw_accel_report.x;
accel_report->y_raw = raw_accel_report.y; accel_report.y_raw = raw_accel_report.y;
accel_report->z_raw = raw_accel_report.z; accel_report.z_raw = raw_accel_report.z;
float x_in_new = ((accel_report->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->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->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->x = _accel_filter_x.apply(x_in_new); accel_report.x = _accel_filter_x.apply(x_in_new);
accel_report->y = _accel_filter_y.apply(y_in_new); accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report->z = _accel_filter_z.apply(z_in_new); accel_report.z = _accel_filter_z.apply(z_in_new);
accel_report->scaling = _accel_range_scale; accel_report.scaling = _accel_range_scale;
accel_report->range_m_s2 = _accel_range_m_s2; accel_report.range_m_s2 = _accel_range_m_s2;
/* post a report to the ring - note, not locked */ _accel_reports->force(&accel_report);
INCREMENT(_next_accel_report, _num_accel_reports);
/* if we are running up against the oldest report, fix it */
if (_next_accel_report == _oldest_accel_report)
INCREMENT(_oldest_accel_report, _num_accel_reports);
/* 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); orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
_accel_read++; _accel_read++;
@ -1343,7 +1296,7 @@ LSM303D::mag_measure()
} raw_mag_report; } raw_mag_report;
#pragma pack(pop) #pragma pack(pop)
mag_report *mag_report = &_mag_reports[_next_mag_report]; mag_report mag_report;
/* start the performance counter */ /* start the performance counter */
perf_begin(_mag_sample_perf); perf_begin(_mag_sample_perf);
@ -1368,30 +1321,25 @@ LSM303D::mag_measure()
*/ */
mag_report->timestamp = hrt_absolute_time(); mag_report.timestamp = hrt_absolute_time();
mag_report->x_raw = raw_mag_report.x; mag_report.x_raw = raw_mag_report.x;
mag_report->y_raw = raw_mag_report.y; mag_report.y_raw = raw_mag_report.y;
mag_report->z_raw = raw_mag_report.z; 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.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.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.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
mag_report->scaling = _mag_range_scale; mag_report.scaling = _mag_range_scale;
mag_report->range_ga = (float)_mag_range_ga; mag_report.range_ga = (float)_mag_range_ga;
/* post a report to the ring - note, not locked */ _mag_reports->force(&mag_report);
INCREMENT(_next_mag_report, _num_mag_reports);
/* if we are running up against the oldest report, fix it */
if (_next_mag_report == _oldest_mag_report)
INCREMENT(_oldest_mag_report, _num_mag_reports);
/* 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); orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
_mag_read++; _mag_read++;
@ -1405,11 +1353,8 @@ LSM303D::print_info()
printf("accel reads: %u\n", _accel_read); printf("accel reads: %u\n", _accel_read);
printf("mag reads: %u\n", _mag_read); printf("mag reads: %u\n", _mag_read);
perf_print_counter(_accel_sample_perf); perf_print_counter(_accel_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n", _accel_reports->print_info("accel reports");
_num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); _mag_reports->print_info("mag reports");
perf_print_counter(_mag_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n",
_num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports);
} }
LSM303D_mag::LSM303D_mag(LSM303D *parent) : LSM303D_mag::LSM303D_mag(LSM303D *parent) :

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

@ -64,6 +64,7 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h> #include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h> #include <uORB/topics/subsystem_info.h>
@ -119,10 +120,7 @@ private:
float _min_distance; float _min_distance;
float _max_distance; float _max_distance;
work_s _work; work_s _work;
unsigned _num_reports; RingBuffer *_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
range_finder_report *_reports;
bool _sensor_ok; bool _sensor_ok;
int _measure_ticks; int _measure_ticks;
bool _collect_phase; bool _collect_phase;
@ -183,9 +181,6 @@ private:
}; };
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
/* /*
* Driver 'main' command. * Driver 'main' command.
*/ */
@ -195,9 +190,6 @@ MB12XX::MB12XX(int bus, int address) :
I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
_min_distance(MB12XX_MIN_DISTANCE), _min_distance(MB12XX_MIN_DISTANCE),
_max_distance(MB12XX_MAX_DISTANCE), _max_distance(MB12XX_MAX_DISTANCE),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr), _reports(nullptr),
_sensor_ok(false), _sensor_ok(false),
_measure_ticks(0), _measure_ticks(0),
@ -221,7 +213,7 @@ MB12XX::~MB12XX()
/* free any existing reports */ /* free any existing reports */
if (_reports != nullptr) if (_reports != nullptr)
delete[] _reports; delete _reports;
} }
int int
@ -234,17 +226,15 @@ MB12XX::init()
goto out; goto out;
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_reports = 2; _reports = new RingBuffer(2, sizeof(range_finder_report));
_reports = new struct range_finder_report[_num_reports];
if (_reports == nullptr) if (_reports == nullptr)
goto out; goto out;
_oldest_report = _next_report = 0;
/* get a publish handle on the range finder topic */ /* get a publish handle on the range finder topic */
memset(&_reports[0], 0, sizeof(_reports[0])); struct range_finder_report zero_report;
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); memset(&zero_report, 0, sizeof(zero_report));
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
if (_range_finder_topic < 0) if (_range_finder_topic < 0)
debug("failed to create sensor_range_finder object. Did you start uOrb?"); debug("failed to create sensor_range_finder object. Did you start uOrb?");
@ -354,31 +344,22 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
return (1000 / _measure_ticks); return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */ /* lower bound is mandatory, upper bound is a sanity check */
arg++; if ((arg < 1) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100)) irqstate_t flags = irqsave();
return -EINVAL; if (!_reports->resize(arg)) {
irqrestore(flags);
/* allocate new buffer */ return -ENOMEM;
struct range_finder_report *buf = new struct range_finder_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK;
} }
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1; return _reports->size();
case SENSORIOCRESET: case SENSORIOCRESET:
/* XXX implement this */ /* XXX implement this */
@ -406,6 +387,7 @@ ssize_t
MB12XX::read(struct file *filp, char *buffer, size_t buflen) MB12XX::read(struct file *filp, char *buffer, size_t buflen)
{ {
unsigned count = buflen / sizeof(struct range_finder_report); unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -421,10 +403,9 @@ 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 (_oldest_report != _next_report) { if (_reports->get(rbuf)) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); ret += sizeof(*rbuf);
ret += sizeof(_reports[0]); rbuf++;
INCREMENT(_oldest_report, _num_reports);
} }
} }
@ -433,9 +414,8 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
} }
/* manual measurement - run one conversion */ /* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do { do {
_oldest_report = _next_report = 0; _reports->flush();
/* trigger a measurement */ /* trigger a measurement */
if (OK != measure()) { if (OK != measure()) {
@ -453,8 +433,9 @@ 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 */
memcpy(buffer, _reports, sizeof(*_reports)); if (_reports->get(rbuf)) {
ret = sizeof(*_reports); ret = sizeof(*rbuf);
}
} while (0); } while (0);
@ -498,26 +479,25 @@ MB12XX::collect()
if (ret < 0) if (ret < 0)
{ {
log("error reading from sensor: %d", ret); log("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret; return ret;
} }
uint16_t distance = val[0] << 8 | val[1]; uint16_t distance = val[0] << 8 | val[1];
float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */ /* this should be fairly close to the end of the measurement, so the best approximation of the time */
_reports[_next_report].timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
_reports[_next_report].distance = si_units; report.distance = si_units;
_reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
/* publish it */ /* publish it */
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
/* post a report to the ring - note, not locked */ if (_reports->force(&report)) {
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
perf_count(_buffer_overflows); perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
} }
/* notify anyone waiting for data */ /* notify anyone waiting for data */
@ -525,11 +505,8 @@ MB12XX::collect()
ret = OK; ret = OK;
out:
perf_end(_sample_perf); perf_end(_sample_perf);
return ret; return ret;
return ret;
} }
void void
@ -537,7 +514,7 @@ MB12XX::start()
{ {
/* reset the report ring and state machine */ /* reset the report ring and state machine */
_collect_phase = false; _collect_phase = false;
_oldest_report = _next_report = 0; _reports->flush();
/* schedule a cycle to start things */ /* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
@ -626,8 +603,7 @@ MB12XX::print_info()
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows); perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks); printf("poll interval: %u ticks\n", _measure_ticks);
printf("report queue: %u (%u/%u @ %p)\n", _reports->print_info("report queue");
_num_reports, _oldest_report, _next_report, _reports);
} }
/** /**

26
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -199,27 +199,23 @@ MEASAirspeed::collect()
// Calculate differential pressure. As its centered around 8000 // Calculate differential pressure. As its centered around 8000
// and can go positive or negative, enforce absolute value // and can go positive or negative, enforce absolute value
uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
struct differential_pressure_s report;
_reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].temperature = temperature;
_reports[_next_report].differential_pressure_pa = diff_press_pa;
// Track maximum differential pressure measured (so we can work out top speed). // Track maximum differential pressure measured (so we can work out top speed).
if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) { if (diff_press_pa > _max_differential_pressure_pa) {
_reports[_next_report].max_differential_pressure_pa = diff_press_pa; _max_differential_pressure_pa = diff_press_pa;
} }
report.timestamp = hrt_absolute_time();
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
/* announce the airspeed if needed, just publish else */ /* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
/* post a report to the ring - note, not locked */ new_report(report);
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
}
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); poll_notify(POLLIN);

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

@ -194,16 +194,14 @@ private:
struct hrt_call _call; struct hrt_call _call;
unsigned _call_interval; unsigned _call_interval;
typedef RingBuffer<accel_report> AccelReportBuffer; RingBuffer *_accel_reports;
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;
typedef RingBuffer<gyro_report> GyroReportBuffer; RingBuffer *_gyro_reports;
GyroReportBuffer *_gyro_reports;
struct gyro_scale _gyro_scale; struct gyro_scale _gyro_scale;
float _gyro_range_scale; float _gyro_range_scale;
@ -431,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;
@ -466,14 +464,14 @@ MPU6000::init()
_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); _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); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
out: out:
@ -658,9 +656,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
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++;
} }
/* return the number of bytes transferred */ /* return the number of bytes transferred */
@ -748,12 +747,13 @@ 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 *arp = 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(*arp++)) if (!_gyro_reports->get(grp))
break; break;
transferred++; transferred++;
grp++;
} }
/* return the number of bytes transferred */ /* return the number of bytes transferred */
@ -837,28 +837,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_interval; return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) if ((arg < 1) || (arg > 100))
return -EINVAL; return -EINVAL;
/* allocate new buffer */ irqstate_t flags = irqsave();
AccelReportBuffer *buf = new AccelReportBuffer(arg); if (!_accel_reports->resize(arg)) {
irqrestore(flags);
if (nullptr == buf) return -ENOMEM;
return -ENOMEM;
if (buf->size() == 0) {
delete buf;
return -ENOMEM;
}
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _accel_reports;
_accel_reports = buf;
start();
return OK;
} }
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _accel_reports->size(); return _accel_reports->size();
@ -935,21 +926,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
if ((arg < 1) || (arg > 100)) if ((arg < 1) || (arg > 100))
return -EINVAL; return -EINVAL;
/* allocate new buffer */ irqstate_t flags = irqsave();
GyroReportBuffer *buf = new GyroReportBuffer(arg); if (!_gyro_reports->resize(arg)) {
irqrestore(flags);
if (nullptr == buf)
return -ENOMEM;
if (buf->size() == 0) {
delete buf;
return -ENOMEM; return -ENOMEM;
} }
irqrestore(flags);
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _gyro_reports;
_gyro_reports = buf;
start();
return OK; return OK;
} }
@ -1260,8 +1242,8 @@ MPU6000::measure()
grb.temperature_raw = report.temp; grb.temperature_raw = report.temp;
grb.temperature = (report.temp) / 361.0f + 35.0f; grb.temperature = (report.temp) / 361.0f + 35.0f;
_accel_reports->put(arb); _accel_reports->force(&arb);
_gyro_reports->put(grb); _gyro_reports->force(&grb);
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); poll_notify(POLLIN);
@ -1280,7 +1262,10 @@ MPU6000::measure()
void void
MPU6000::print_info() MPU6000::print_info()
{ {
perf_print_counter(_sample_perf);
printf("reads: %u\n", _reads); printf("reads: %u\n", _reads);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
} }
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :

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

@ -60,6 +60,7 @@
#include <drivers/device/device.h> #include <drivers/device/device.h>
#include <drivers/drv_baro.h> #include <drivers/drv_baro.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <systemlib/err.h> #include <systemlib/err.h>
@ -114,10 +115,7 @@ protected:
struct work_s _work; struct work_s _work;
unsigned _measure_ticks; unsigned _measure_ticks;
unsigned _num_reports; RingBuffer *_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
struct baro_report *_reports;
bool _collect_phase; bool _collect_phase;
unsigned _measure_phase; unsigned _measure_phase;
@ -196,9 +194,6 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
_interface(interface), _interface(interface),
_prom(prom_buf.s), _prom(prom_buf.s),
_measure_ticks(0), _measure_ticks(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr), _reports(nullptr),
_collect_phase(false), _collect_phase(false),
_measure_phase(0), _measure_phase(0),
@ -223,7 +218,7 @@ MS5611::~MS5611()
/* free any existing reports */ /* free any existing reports */
if (_reports != nullptr) if (_reports != nullptr)
delete[] _reports; delete _reports;
// free perf counters // free perf counters
perf_free(_sample_perf); perf_free(_sample_perf);
@ -246,8 +241,7 @@ MS5611::init()
} }
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_reports = 2; _reports = new RingBuffer(2, sizeof(baro_report));
_reports = new struct baro_report[_num_reports];
if (_reports == nullptr) { if (_reports == nullptr) {
debug("can't get memory for reports"); debug("can't get memory for reports");
@ -255,11 +249,10 @@ MS5611::init()
goto out; goto out;
} }
_oldest_report = _next_report = 0;
/* get a publish handle on the baro topic */ /* get a publish handle on the baro topic */
memset(&_reports[0], 0, sizeof(_reports[0])); struct baro_report zero_report;
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); memset(&zero_report, 0, sizeof(zero_report));
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report);
if (_baro_topic < 0) { if (_baro_topic < 0) {
debug("failed to create sensor_baro object"); debug("failed to create sensor_baro object");
@ -276,6 +269,7 @@ ssize_t
MS5611::read(struct file *filp, char *buffer, size_t buflen) MS5611::read(struct file *filp, char *buffer, size_t buflen)
{ {
unsigned count = buflen / sizeof(struct baro_report); unsigned count = buflen / sizeof(struct baro_report);
struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
@ -291,10 +285,9 @@ 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 (_oldest_report != _next_report) { if (_reports->get(brp)) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); ret += sizeof(*brp);
ret += sizeof(_reports[0]); brp++;
INCREMENT(_oldest_report, _num_reports);
} }
} }
@ -303,10 +296,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
} }
/* manual measurement - run one conversion */ /* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do { do {
_measure_phase = 0; _measure_phase = 0;
_oldest_report = _next_report = 0; _reports->flush();
/* do temperature first */ /* do temperature first */
if (OK != measure()) { if (OK != measure()) {
@ -335,8 +327,8 @@ 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 */
memcpy(buffer, _reports, sizeof(*_reports)); if (_reports->get(brp))
ret = sizeof(*_reports); ret = sizeof(*brp);
} while (0); } while (0);
@ -411,31 +403,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
return (1000 / _measure_ticks); return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */ /* lower bound is mandatory, upper bound is a sanity check */
arg++; if ((arg < 1) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */ irqstate_t flags = irqsave();
if ((arg < 2) || (arg > 100)) if (!_reports->resize(arg)) {
return -EINVAL; irqrestore(flags);
return -ENOMEM;
/* allocate new buffer */
struct baro_report *buf = new struct baro_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop_cycle();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start_cycle();
return OK;
} }
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1; return _reports->size();
case SENSORIOCRESET: case SENSORIOCRESET:
/* XXX implement this */ /* XXX implement this */
@ -469,7 +451,7 @@ MS5611::start_cycle()
/* reset the report ring and state machine */ /* reset the report ring and state machine */
_collect_phase = false; _collect_phase = false;
_measure_phase = 0; _measure_phase = 0;
_oldest_report = _next_report = 0; _reports->flush();
/* schedule a cycle to start things */ /* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
@ -588,8 +570,9 @@ MS5611::collect()
perf_begin(_sample_perf); perf_begin(_sample_perf);
struct baro_report report;
/* this should be fairly close to the end of the conversion, so the best approximation of the time */ /* this should be fairly close to the end of the conversion, so the best approximation of the time */
_reports[_next_report].timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
/* read the most recent measurement - read offset/size are hardcoded in the interface */ /* read the most recent measurement - read offset/size are hardcoded in the interface */
ret = _interface->read(0, (void *)&raw, 0); ret = _interface->read(0, (void *)&raw, 0);
@ -638,8 +621,8 @@ MS5611::collect()
int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
/* generate a new report */ /* generate a new report */
_reports[_next_report].temperature = _TEMP / 100.0f; report.temperature = _TEMP / 100.0f;
_reports[_next_report].pressure = P / 100.0f; /* convert to millibar */ report.pressure = P / 100.0f; /* convert to millibar */
/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
@ -676,18 +659,13 @@ MS5611::collect()
* h = ------------------------------- + h1 * h = ------------------------------- + h1
* a * a
*/ */
_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
/* publish it */ /* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
/* post a report to the ring - note, not locked */ if (_reports->force(&report)) {
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
perf_count(_buffer_overflows); perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
} }
/* notify anyone waiting for data */ /* notify anyone waiting for data */
@ -709,8 +687,7 @@ MS5611::print_info()
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows); perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks); printf("poll interval: %u ticks\n", _measure_ticks);
printf("report queue: %u (%u/%u @ %p)\n", _reports->print_info("report queue");
_num_reports, _oldest_report, _next_report, _reports);
printf("TEMP: %d\n", _TEMP); printf("TEMP: %d\n", _TEMP);
printf("SENS: %lld\n", _SENS); printf("SENS: %lld\n", _SENS);
printf("OFF: %lld\n", _OFF); printf("OFF: %lld\n", _OFF);