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

mb12xx: convert to using RingBuffer class

This commit is contained in:
Andrew Tridgell 2013-09-09 16:36:22 +10:00 committed by Lorenz Meier
parent 96b4729b37
commit 4918148aa3
1 changed files with 39 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
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<struct range_finder_report> *_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<struct range_finder_report>(2);
_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 */
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 range_finder_report *buf = new struct range_finder_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:
/* 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);
} }
/** /**