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

l3gd20: convert to using RingBuffer class

This commit is contained in:
Andrew Tridgell 2013-09-09 16:36:07 +10:00 committed by Lorenz Meier
parent 5ee40da720
commit 96b4729b37
1 changed files with 51 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

114
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>
@ -184,10 +185,7 @@ private:
struct hrt_call _call; struct hrt_call _call;
unsigned _call_interval; unsigned _call_interval;
unsigned _num_reports; RingBuffer<gyro_report> *_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<struct gyro_report>(2);
_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();
@ -406,6 +396,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 */
@ -421,10 +412,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);
} }
} }
@ -433,12 +423,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;
} }
@ -506,31 +497,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 */
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 gyro_report *buf = new struct gyro_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:
reset(); reset();
@ -699,7 +681,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);
@ -759,7 +741,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);
@ -782,61 +764,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++;
@ -849,8 +826,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