From 36b7b7bc5f078f373f67cdce3f5c7855c0dcd58b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 16:35:20 +1000 Subject: airspeed: convert to using RingBuffer class --- src/drivers/airspeed/airspeed.cpp | 74 +++++++++++++---------------- src/drivers/airspeed/airspeed.h | 19 +++++--- src/drivers/ets_airspeed/ets_airspeed.cpp | 28 +++++------ src/drivers/meas_airspeed/meas_airspeed.cpp | 26 +++++----- 4 files changed, 67 insertions(+), 80 deletions(-) (limited to 'src') diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 1ec61eb60..2a6b190de 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -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(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(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); } diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index b87494b40..7850ccc7e 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -102,6 +103,10 @@ public: */ virtual void print_info(); +private: + RingBuffer *_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) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 257b41935..dd8436b10 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -68,6 +68,7 @@ #include #include +#include #include #include @@ -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; } - /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + // 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; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + /* announce the airspeed if needed, just publish else */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - /* 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); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b1cb2b3d8..03d7bbfb9 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -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; } - /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + 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; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + /* announce the airspeed if needed, just publish else */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - /* 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); -- cgit v1.2.3