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

airspeed: convert to using RingBuffer class

This commit is contained in:
Andrew Tridgell 2013-09-09 16:35:20 +10:00 committed by Lorenz Meier
parent 815ccee0e7
commit 36b7b7bc5f
4 changed files with 67 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -68,6 +68,7 @@
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
@ -77,10 +78,8 @@
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_max_differential_pressure_pa(0),
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
@ -105,7 +104,7 @@ Airspeed::~Airspeed()
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
delete _reports;
// free perf counters
perf_free(_sample_perf);
@ -123,20 +122,14 @@ Airspeed::init()
goto out;
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct differential_pressure_s[_num_reports];
for (unsigned i = 0; i < _num_reports; i++)
_reports[i].max_differential_pressure_pa = 0;
_reports = new RingBuffer<differential_pressure_s>(2);
if (_reports == nullptr)
goto out;
_oldest_report = _next_report = 0;
/* get a publish handle on the airspeed topic */
memset(&_reports[0], 0, sizeof(_reports[0]));
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
differential_pressure_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report);
if (_airspeed_pub < 0)
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);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct differential_pressure_s *buf = new struct differential_pressure_s[arg];
if (nullptr == buf)
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1;
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
@ -281,7 +265,8 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
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;
/* 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.
*/
while (count--) {
if (_oldest_report != _next_report) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
ret += sizeof(_reports[0]);
INCREMENT(_oldest_report, _num_reports);
if (_reports->get(*abuf)) {
ret += sizeof(*abuf);
abuf++;
}
}
@ -309,9 +293,8 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
}
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do {
_oldest_report = _next_report = 0;
_reports->flush();
/* trigger a measurement */
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 */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
if (_reports->get(*abuf)) {
ret = sizeof(*abuf);
}
} while (0);
@ -342,7 +326,7 @@ Airspeed::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_oldest_report = _next_report = 0;
_reports->flush();
/* schedule a cycle to start things */
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(_buffer_overflows);
warnx("poll interval: %u ticks", _measure_ticks);
warnx("report queue: %u (%u/%u @ %p)",
_num_reports, _oldest_report, _next_report, _reports);
_reports->print_info("report queue");
}
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_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
@ -102,6 +103,10 @@ public:
*/
virtual void print_info();
private:
RingBuffer<differential_pressure_s> *_reports;
perf_counter_t _buffer_overflows;
protected:
virtual int probe();
@ -114,10 +119,7 @@ protected:
virtual int collect() = 0;
work_s _work;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
differential_pressure_s *_reports;
uint16_t _max_differential_pressure_pa;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
@ -129,7 +131,6 @@ protected:
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
@ -162,8 +163,12 @@ protected:
*/
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)

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

@ -68,6 +68,7 @@
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
@ -173,27 +174,22 @@ ETSAirspeed::collect()
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).
if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
if (diff_pres_pa > _max_differential_pressure_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 */
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 */
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);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);

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

@ -199,27 +199,23 @@ MEASAirspeed::collect()
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative, enforce absolute value
uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
_reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].temperature = temperature;
_reports[_next_report].differential_pressure_pa = diff_press_pa;
struct differential_pressure_s report;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) {
_reports[_next_report].max_differential_pressure_pa = diff_press_pa;
if (diff_press_pa > _max_differential_pressure_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 */
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 */
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);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);