fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 10:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

bma180: convert to using RingBuffer

Browse Source
This commit is contained in:
Andrew Tridgell
2013-09-12 00:52:45 +02:00
committed by Lorenz Meier
parent b8ffb574ca
commit 274e3aa2ca
1 changed files with 55 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/bma180/bma180.cpp
+54 -72
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,14 @@ private:
struct hrt_call _call; struct hrt_call _call;
unsigned _call_interval; unsigned _call_interval;
unsigned _num_reports; /*
volatile unsigned _next_report; this wrapper type is needed to avoid a linker error for
volatile unsigned _oldest_report; RingBuffer instances which appear in two drivers.
struct accel_report *_reports; */
struct _accel_report {
accel_report r;
};
RingBuffer<struct _accel_report> *_reports;
struct accel_scale _accel_scale; struct accel_scale _accel_scale;
float _accel_range_scale; float _accel_range_scale;
@@ -233,16 +238,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 +268,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 +284,15 @@ BMA180::init()
goto out; goto out;
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_reports = 2; _reports = new RingBuffer<_accel_report>(2);
_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 +349,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 +365,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 +376,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 +446,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 */ irqstate_t flags = irqsave();
if ((arg < 2) || (arg > 100)) if (!_reports->resize(arg)) {
return -EINVAL; irqrestore(flags);
return -ENOMEM;
/* allocate new buffer */
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 +642,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 +676,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 +696,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.r.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.r.y_raw = read_reg(ADDR_ACC_X_LSB + 0);
report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; report.r.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
report->x_raw = read_reg(ADDR_ACC_X_LSB + 2); report.r.x_raw = read_reg(ADDR_ACC_X_LSB + 2);
report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; report.r.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
report->z_raw = read_reg(ADDR_ACC_X_LSB + 4); report.r.z_raw = read_reg(ADDR_ACC_X_LSB + 4);
report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; report.r.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.r.x_raw = (report.r.x_raw / 4);
report->y_raw = (report->y_raw / 4); report.r.y_raw = (report.r.y_raw / 4);
report->z_raw = (report->z_raw / 4); report.r.z_raw = (report.r.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.r.y_raw = -report.r.y_raw;
report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; report.r.x = ((report.r.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.r.y = ((report.r.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.r.z = ((report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
report->scaling = _accel_range_scale; report.r.scaling = _accel_range_scale;
report->range_m_s2 = _accel_range_m_s2; report.r.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.r);
/* stop the perf counter */ /* stop the perf counter */
perf_end(_sample_perf); perf_end(_sample_perf);
@@ -756,8 +739,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);
} }
/** /**