airspeed: convert to using RingBuffer class

2 files changed, 44 insertions, 49 deletions

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 <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);
 }
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 <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)