Merge branch 'ringbuffer' of github.com:PX4/Firmware into fixedwing_l1

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 RingBuffer<differential_pressure_s>(2);
-	_reports = new struct differential_pressure_s[_num_reports];
-
-	for (unsigned i = 0; i < _num_reports; i++)
-		_reports[i].max_differential_pressure_pa = 0;
-
 	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]));
+	differential_pressure_s zero_report;
-	_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
+	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 */
+			irqstate_t flags = irqsave();
-			struct differential_pressure_s *buf = new struct differential_pressure_s[arg];
+			if (!_reports->resize(arg)) {
-
+				irqrestore(flags);
-			if (nullptr == buf)
 				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;
 		}
 
 	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) {
+			if (_reports->get(*abuf)) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(*abuf);
-				ret += sizeof(_reports[0]);
+				abuf++;
-				INCREMENT(_oldest_report, _num_reports);
 			}
 		}
 
@@ -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));
+		if (_reports->get(*abuf)) {
-		ret = sizeof(*_reports);
+			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)",
+	_reports->print_info("report queue");
-	       _num_reports, _oldest_report, _next_report, _reports);
+}
+
+void
+Airspeed::new_report(const differential_pressure_s &report)
+{
+	if (!_reports->force(report))
+		perf_count(_buffer_overflows);
 }

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;
+	uint16_t		_max_differential_pressure_pa;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	differential_pressure_s	*_reports;
 	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)
 

src/drivers/bma180/bma180.cpp

@@ -63,6 +63,7 @@
 
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>
+#include <drivers/device/ringbuffer.h>
 
 
 /* oddly, ERROR is not defined for c++ */
@@ -146,10 +147,14 @@ private:
 	struct hrt_call		_call;
 	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;
 	float			_accel_range_scale;
@@ -233,16 +238,9 @@ private:
 	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) :
 	SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
 	_call_interval(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
@@ -270,7 +268,7 @@ BMA180::~BMA180()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
@@ -286,16 +284,15 @@ BMA180::init()
 		goto out;
 
 	/* 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)
 		goto out;
 
 	/* 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) */
 	write_reg(ADDR_RESET, SOFT_RESET);
@@ -352,6 +349,7 @@ ssize_t
 BMA180::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct accel_report);
+	struct _accel_report *arp = reinterpret_cast<struct _accel_report *>(buffer);
 	int ret = 0;
 
 	/* 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.
 		 */
 		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 */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 	measure();
 
 	/* 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;
 }
@@ -449,31 +446,22 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_interval;
 
 	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:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		/* XXX implement */
@@ -654,7 +642,7 @@ BMA180::start()
 	stop();
 
 	/* reset the report ring */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* start polling at the specified rate */
 	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
@@ -688,7 +676,7 @@ BMA180::measure()
 // 	} raw_report;
 // #pragma pack(pop)
 
-	accel_report		*report = &_reports[_next_report];
+	struct _accel_report report;
 
 	/* start the performance counter */
 	perf_begin(_sample_perf);
@@ -708,45 +696,40 @@ BMA180::measure()
 	 * them before.  There is no good way to synchronise with the internal
 	 * 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
 	 * perform only the axis assignment here.
 	 * 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 */
-	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 */
-	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 */
 	poll_notify(POLLIN);
 
 	/* 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 */
 	perf_end(_sample_perf);
@@ -756,8 +739,7 @@ void
 BMA180::print_info()
 {
 	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);
 }
 
 /**

src/drivers/device/ringbuffer.h

@@ -55,13 +55,29 @@ public:
 	bool			put(T &val);
 
 	/**
-	 * Put an item into the buffer.
+	 * Put an item into the buffer if there is space.
 	 *
 	 * @param val		Item to put
 	 * @return		true if the item was put, false if the buffer is full
 	 */
 	bool			put(const T &val);
 
+	/**
+	 * Force an item into the buffer, discarding an older item if there is not space.
+	 *
+	 * @param val		Item to put
+	 * @return		true if an item was discarded to make space
+	 */
+	bool			force(T &val);
+
+	/**
+	 * Force an item into the buffer, discarding an older item if there is not space.
+	 *
+	 * @param val		Item to put
+	 * @return		true if an item was discarded to make space
+	 */
+	bool			force(const T &val);
+
 	/**
 	 * Get an item from the buffer.
 	 *
@@ -73,8 +89,8 @@ public:
 	/**
 	 * Get an item from the buffer (scalars only).
 	 *
-	 * @return		The value that was fetched, or zero if the buffer was
+	 * @return		The value that was fetched. If the buffer is empty, 
-	 *			empty.
+	 *			returns zero.
 	 */
 	T			get(void);
 
@@ -97,27 +113,43 @@ public:
 	/*
 	 * Returns true if the buffer is empty.
 	 */
-	bool			empty() { return _tail == _head; }
+	bool			empty();
 
 	/*
 	 * Returns true if the buffer is full.
 	 */
-	bool			full() { return _next(_head) == _tail; }
+	bool			full();
 
 	/*
 	 * Returns the capacity of the buffer, or zero if the buffer could
 	 * not be allocated.
 	 */
-	unsigned		size() { return (_buf != nullptr) ? _size : 0; }
+	unsigned		size();
 
 	/*
 	 * Empties the buffer.
 	 */
-	void			flush() { _head = _tail = _size; }
+	void			flush();
+
+	/*
+	 * resize the buffer. This is unsafe to be called while
+	 * a producer or consuming is running. Caller is responsible
+	 * for any locking needed
+	 *
+	 * @param new_size	new size for buffer
+	 * @return		true if the resize succeeds, false if
+	 * 			not (allocation error)
+	 */
+	bool			resize(unsigned new_size);
+
+	/*
+	 * printf() some info on the buffer
+	 */
+	void			print_info(const char *name);
 
 private:
-	T			*const _buf;	
+	T			*_buf;	
-	const unsigned		_size;
+	unsigned		_size;
 	volatile unsigned	_head;	/**< insertion point */
 	volatile unsigned	_tail;	/**< removal point */
 
@@ -139,6 +171,38 @@ RingBuffer<T>::~RingBuffer()
 		delete[] _buf;
 }
 
+template <typename T>
+bool RingBuffer<T>::empty()
+{
+	return _tail == _head; 
+}
+
+template <typename T>
+bool RingBuffer<T>::full()
+{
+	return _next(_head) == _tail; 
+}
+
+template <typename T>
+unsigned RingBuffer<T>::size()
+{
+	return (_buf != nullptr) ? _size : 0; 
+}
+
+template <typename T>
+void RingBuffer<T>::flush()
+{
+	T junk;
+	while (!empty())
+		get(junk); 
+}
+
+template <typename T>
+unsigned RingBuffer<T>::_next(unsigned index)
+{
+	return (0 == index) ? _size : (index - 1); 
+}
+
 template <typename T>
 bool RingBuffer<T>::put(T &val) 
 {
@@ -165,12 +229,55 @@ bool RingBuffer<T>::put(const T &val)
 	}
 }
 
+template <typename T>
+bool RingBuffer<T>::force(T &val)
+{
+	bool overwrote = false;
+
+	for (;;) {
+		if (put(val))
+			break;
+		T junk;
+		get(junk);
+		overwrote = true;
+	}
+	return overwrote;
+}
+
+template <typename T>
+bool RingBuffer<T>::force(const T &val)
+{
+	bool overwrote = false;
+
+	for (;;) {
+		if (put(val))
+			break;
+		T junk;
+		get(junk);
+		overwrote = true;
+	}
+	return overwrote;
+}
+
 template <typename T>
 bool RingBuffer<T>::get(T &val) 
 {
 	if (_tail != _head) {
-		val = _buf[_tail];
+		unsigned candidate;
-		_tail = _next(_tail);
+		unsigned next;
+		do {
+			/* decide which element we think we're going to read */
+			candidate = _tail;
+
+			/* and what the corresponding next index will be */
+			next = _next(candidate);
+
+			/* go ahead and read from this index */
+			val = _buf[candidate];
+
+			/* if the tail pointer didn't change, we got our item */
+		} while (!__sync_bool_compare_and_swap(&_tail, candidate, next));
+
 		return true;
 	} else {
 		return false;
@@ -187,17 +294,54 @@ T RingBuffer<T>::get(void)
 template <typename T>
 unsigned RingBuffer<T>::space(void) 
 {
-	return (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1);
+	unsigned tail, head;
+
+	/*
+	 * Make a copy of the head/tail pointers in a fashion that
+	 * may err on the side of under-estimating the free space
+	 * in the buffer in the case that the buffer is being updated
+	 * asynchronously with our check.
+	 * If the head pointer changes (reducing space) while copying,
+	 * re-try the copy.
+	 */
+	do {
+		head = _head;
+		tail = _tail;
+	} while (head != _head);
+
+	return (tail >= head) ? (_size - (tail - head)) : (head - tail - 1);
 }
 
 template <typename T>
 unsigned RingBuffer<T>::count(void) 
 {
+	/*
+	 * Note that due to the conservative nature of space(), this may
+	 * over-estimate the number of items in the buffer.
+	 */
 	return _size - space();
 }
 
 template <typename T>
-unsigned RingBuffer<T>::_next(unsigned index) 
+bool RingBuffer<T>::resize(unsigned new_size) 
 {
-	return (0 == index) ? _size : (index - 1);
+	T *old_buffer;
+	T *new_buffer = new T[new_size + 1];
+	if (new_buffer == nullptr) {
+		return false;
+	}
+	old_buffer = _buf;
+	_buf = new_buffer;
+	_size = new_size;
+	_head = new_size;
+	_tail = new_size;
+	delete[] old_buffer;
+	return true;
+}
+
+template <typename T>
+void RingBuffer<T>::print_info(const char *name) 
+{
+	printf("%s	%u (%u/%u @ %p)\n",
+	       name, _size, _head, _tail, _buf);
 }

src/drivers/ets_airspeed/ets_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>
@@ -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) {
+	if (diff_pres_pa > _max_differential_pressure_pa) {
-		_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
+	    _max_differential_pressure_pa = diff_pres_pa;
 	}
 
+	// 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;
+
 	/* announce the airspeed if needed, just publish else */
-	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
+	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
 
-	/* post a report to the ring - note, not locked */
+	new_report(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);
-		INCREMENT(_oldest_report, _num_reports);
-	}
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);

src/drivers/hmc5883/hmc5883.cpp

@@ -65,6 +65,7 @@
 
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
@@ -148,10 +149,7 @@ private:
 	work_s			_work;
 	unsigned		_measure_ticks;
 
-	unsigned		_num_reports;
+	RingBuffer<struct mag_report> *_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	mag_report		*_reports;
 	mag_scale		_scale;
 	float 			_range_scale;
 	float 			_range_ga;
@@ -310,9 +308,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.
  */
@@ -322,9 +317,6 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
 HMC5883::HMC5883(int bus) :
 	I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
 	_measure_ticks(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
@@ -356,9 +348,8 @@ HMC5883::~HMC5883()
 	/* make sure we are truly inactive */
 	stop();
 
-	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	// free perf counters
 	perf_free(_sample_perf);
@@ -375,21 +366,18 @@ HMC5883::init()
 	if (I2C::init() != OK)
 		goto out;
 
-	/* reset the device configuration */
-	reset();
-
 	/* allocate basic report buffers */
-	_num_reports = 2;
+	_reports = new RingBuffer<struct mag_report>(2);
-	_reports = new struct mag_report[_num_reports];
-
 	if (_reports == nullptr)
 		goto out;
 
-	_oldest_report = _next_report = 0;
+	/* reset the device configuration */
+	reset();
 
 	/* get a publish handle on the mag topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
+	struct mag_report zero_report;
-	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
+	memset(&zero_report, 0, sizeof(zero_report));
+	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
 
 	if (_mag_topic < 0)
 		debug("failed to create sensor_mag object");
@@ -493,6 +481,7 @@ ssize_t
 HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct mag_report);
+	struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -501,17 +490,15 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 
 	/* if automatic measurement is enabled */
 	if (_measure_ticks > 0) {
-
 		/*
 		 * While there is space in the caller's buffer, and reports, copy them.
 		 * Note that we may be pre-empted by the workq thread while we are doing this;
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
+			if (_reports->get(*mag_buf)) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(struct mag_report);
-				ret += sizeof(_reports[0]);
+				mag_buf++;
-				INCREMENT(_oldest_report, _num_reports);
 			}
 		}
 
@@ -522,7 +509,7 @@ HMC5883::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()) {
@@ -539,10 +526,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 			break;
 		}
 
-		/* state machine will have generated a report, copy it out */
+		if (_reports->get(*mag_buf)) {
-		memcpy(buffer, _reports, sizeof(*_reports));
+			ret = sizeof(struct mag_report);
-		ret = sizeof(*_reports);
+		}
-
 	} while (0);
 
 	return ret;
@@ -615,31 +601,22 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000/TICK2USEC(_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 */
+			irqstate_t flags = irqsave();
-			struct mag_report *buf = new struct mag_report[arg];
+			if (!_reports->resize(arg)) {
-
+				irqrestore(flags);
-			if (nullptr == buf)
 				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;
 		}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		return reset();
@@ -701,7 +678,7 @@ HMC5883::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)&HMC5883::cycle_trampoline, this, 1);
@@ -810,9 +787,10 @@ HMC5883::collect()
 
 
 	perf_begin(_sample_perf);
+	struct mag_report new_report;
 
 	/* 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();
+	new_report.timestamp = hrt_absolute_time();
 
 	/*
 	 * @note  We could read the status register here, which could tell us that
@@ -842,8 +820,10 @@ HMC5883::collect()
 	 */
 	if ((abs(report.x) > 2048) ||
 	    (abs(report.y) > 2048) ||
-	    (abs(report.z) > 2048))
+	    (abs(report.z) > 2048)) {
+		perf_count(_comms_errors);
 		goto out;
+	}
 
 	/*
 	 * RAW outputs
@@ -851,10 +831,10 @@ HMC5883::collect()
 	 * to align the sensor axes with the board, x and y need to be flipped
 	 * and y needs to be negated
 	 */
-	_reports[_next_report].x_raw = report.y;
+	new_report.x_raw = report.y;
-	_reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x);
+	new_report.y_raw = -report.x;
 	/* z remains z */
-	_reports[_next_report].z_raw = report.z;
+	new_report.z_raw = report.z;
 
 	/* scale values for output */
 
@@ -876,34 +856,30 @@ HMC5883::collect()
 #ifdef PX4_I2C_BUS_ONBOARD
 	if (_bus == PX4_I2C_BUS_ONBOARD) {
 		/* to align the sensor axes with the board, x and y need to be flipped */
-		_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+		new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
 		/* flip axes and negate value for y */
-		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
+		new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
 		/* z remains z */
-		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+		new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 	} else {
 #endif
 		/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
 		 * therefore switch x and y and invert y */
-		_reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale;
+		new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
 		/* flip axes and negate value for y */
-		_reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
+		new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
 		/* z remains z */
-		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+		new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 #ifdef PX4_I2C_BUS_ONBOARD
 	}
 #endif
 
 	/* publish it */
-	orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
 
-	/* post a report to the ring - note, not locked */
+	/* post a report to the ring */
-	INCREMENT(_next_report, _num_reports);
+	if (_reports->force(new_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);
 	}
 
 	/* notify anyone waiting for data */
@@ -1222,8 +1198,7 @@ HMC5883::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	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);
 }
 
 /**

src/drivers/l3gd20/l3gd20.cpp

@@ -61,6 +61,7 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/device/spi.h>
 #include <drivers/drv_gyro.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -184,10 +185,7 @@ private:
 	struct hrt_call		_call;
 	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;
 	float			_gyro_range_scale;
@@ -299,16 +297,9 @@ private:
 	 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) :
 	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
 	_call_interval(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
@@ -340,7 +331,7 @@ L3GD20::~L3GD20()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	/* delete the perf counter */
 	perf_free(_sample_perf);
@@ -356,16 +347,15 @@ L3GD20::init()
 		goto out;
 
 	/* 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)
 		goto out;
 
 	/* 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();
 
@@ -406,6 +396,7 @@ ssize_t
 L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct gyro_report);
+	struct gyro_report *gbuf = reinterpret_cast<struct gyro_report *>(buffer);
 	int ret = 0;
 
 	/* 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.
 		 */
 		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 */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 	measure();
 
 	/* 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;
 }
@@ -506,31 +497,22 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
+		/* lower bound is mandatory, upper bound is a sanity check */
-			arg++;
+		if ((arg < 1) || (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 gyro_report *buf = new struct gyro_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:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		reset();
@@ -699,7 +681,7 @@ L3GD20::start()
 	stop();
 
 	/* reset the report ring */
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* start polling at the specified rate */
 	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
@@ -759,7 +741,7 @@ L3GD20::measure()
 	} raw_report;
 #pragma pack(pop)
 
-	gyro_report		*report = &_reports[_next_report];
+	gyro_report report;
 
 	/* start the performance counter */
 	perf_begin(_sample_perf);
@@ -782,61 +764,56 @@ L3GD20::measure()
 	 *	 	  the offset is 74 from the origin and subtracting
 	 *		  74 from all measurements centers them around zero.
 	 */
-	report->timestamp = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
 	
 	switch (_orientation) {
 
 		case SENSOR_BOARD_ROTATION_000_DEG:
 			/* 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;
 
 		case SENSOR_BOARD_ROTATION_090_DEG:
 			/* 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;
 
 		case SENSOR_BOARD_ROTATION_180_DEG:
 			/* 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;
 
 		case SENSOR_BOARD_ROTATION_270_DEG:
 			/* 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;
 	}
 
-	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 */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
 	if (_gyro_topic > 0)
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
 
 	_read++;
 
@@ -849,8 +826,7 @@ L3GD20::print_info()
 {
 	printf("gyro reads:          %u\n", _read);
 	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

src/drivers/lsm303d/lsm303d.cpp

@@ -62,6 +62,7 @@
 #include <drivers/device/spi.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -218,15 +219,19 @@ private:
 	unsigned		_call_accel_interval;
 	unsigned		_call_mag_interval;
 
-	unsigned		_num_accel_reports;
+	/*
-	volatile unsigned	_next_accel_report;
+	  these wrapper types are needed to avoid a linker error for
-	volatile unsigned	_oldest_accel_report;
+	  RingBuffer instances which appear in two drivers.
-	struct accel_report	*_accel_reports;
+	 */
+	struct _accel_report {
+		struct accel_report r;
+	};
+	RingBuffer<struct _accel_report> *_accel_reports;
 
-	unsigned		_num_mag_reports;
+	struct _mag_report {
-	volatile unsigned	_next_mag_report;
+		struct mag_report r;
-	volatile unsigned	_oldest_mag_report;
+	};
-	struct mag_report	*_mag_reports;
+	RingBuffer<struct _mag_report> *_mag_reports;
 
 	struct accel_scale	_accel_scale;
 	unsigned		_accel_range_m_s2;
@@ -420,22 +425,12 @@ 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)
-
-
 LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
 	_mag(new LSM303D_mag(this)),
 	_call_accel_interval(0),
 	_call_mag_interval(0),
-	_num_accel_reports(0),
-	_next_accel_report(0),
-	_oldest_accel_report(0),
 	_accel_reports(nullptr),
-	_num_mag_reports(0),
-	_next_mag_report(0),
-	_oldest_mag_report(0),
 	_mag_reports(nullptr),
 	_accel_range_m_s2(0.0f),
 	_accel_range_scale(0.0f),
@@ -480,9 +475,9 @@ LSM303D::~LSM303D()
 
 	/* free any existing reports */
 	if (_accel_reports != nullptr)
-		delete[] _accel_reports;
+		delete _accel_reports;
 	if (_mag_reports != nullptr)
-		delete[] _mag_reports;
+		delete _mag_reports;
 
 	delete _mag;
 
@@ -502,20 +497,17 @@ LSM303D::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_num_accel_reports = 2;
+	_accel_reports = new RingBuffer<struct _accel_report>(2);
-	_oldest_accel_report = _next_accel_report = 0;
-	_accel_reports = new struct accel_report[_num_accel_reports];
 
 	if (_accel_reports == nullptr)
 		goto out;
 
 	/* advertise accel topic */
-	memset(&_accel_reports[0], 0, sizeof(_accel_reports[0]));
+	struct accel_report zero_report;
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]);
+	memset(&zero_report, 0, sizeof(zero_report));
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
 
-	_num_mag_reports = 2;
+	_mag_reports = new RingBuffer<struct _mag_report>(2);
-	_oldest_mag_report = _next_mag_report = 0;
-	_mag_reports = new struct mag_report[_num_mag_reports];
 
 	if (_mag_reports == nullptr)
 		goto out;
@@ -523,8 +515,9 @@ LSM303D::init()
 	reset();
 
 	/* advertise mag topic */
-	memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
+	struct mag_report zero_mag_report;
-	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
+	memset(&zero_mag_report, 0, sizeof(zero_mag_report));
+	_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report);
 
 	/* do CDev init for the mag device node, keep it optional */
 	mag_ret = _mag->init();
@@ -577,6 +570,7 @@ ssize_t
 LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct accel_report);
+	struct _accel_report *arb = reinterpret_cast<struct _accel_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -585,17 +579,13 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 
 	/* if automatic measurement is enabled */
 	if (_call_accel_interval > 0) {
-
 		/*
 		 * While there is space in the caller's buffer, and reports, copy them.
-		 * Note that we may be pre-empted by the measurement code while we are doing this;
-		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_oldest_accel_report != _next_accel_report) {
+			if (_accel_reports->get(*arb)) {
-				memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports));
+				ret += sizeof(*arb);
-				ret += sizeof(_accel_reports[0]);
+				arb++;
-				INCREMENT(_oldest_accel_report, _num_accel_reports);
 			}
 		}
 
@@ -604,12 +594,11 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement */
-	_oldest_accel_report = _next_accel_report = 0;
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _accel_reports, sizeof(*_accel_reports));
+	if (_accel_reports->get(*arb))
-	ret = sizeof(*_accel_reports);
+		ret = sizeof(*arb);
 
 	return ret;
 }
@@ -618,6 +607,7 @@ ssize_t
 LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct mag_report);
+	struct _mag_report *mrb = reinterpret_cast<struct _mag_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -629,14 +619,11 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
 
 		/*
 		 * While there is space in the caller's buffer, and reports, copy them.
-		 * Note that we may be pre-empted by the measurement code while we are doing this;
-		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_oldest_mag_report != _next_mag_report) {
+			if (_mag_reports->get(*mrb)) {
-				memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports));
+				ret += sizeof(*mrb);
-				ret += sizeof(_mag_reports[0]);
+				mrb++;
-				INCREMENT(_oldest_mag_report, _num_mag_reports);
 			}
 		}
 
@@ -645,12 +632,12 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* manual measurement */
-	_oldest_mag_report = _next_mag_report = 0;
+	_mag_reports->flush();
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	memcpy(buffer, _mag_reports, sizeof(*_mag_reports));
+	if (_mag_reports->get(*mrb))
-	ret = sizeof(*_mag_reports);
+		ret = sizeof(*mrb);
 
 	return ret;
 }
@@ -718,31 +705,22 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_accel_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-		/* account for 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 */
+		irqstate_t flags = irqsave();
-		struct accel_report *buf = new struct accel_report[arg];
+		if (!_accel_reports->resize(arg)) {
-
+			irqrestore(flags);
-		if (nullptr == buf)
 			return -ENOMEM;
-
+		}
-		/* reset the measurement state machine with the new buffer, free the old */
+		irqrestore(flags);
-		stop();
-		delete[] _accel_reports;
-		_num_accel_reports = arg;
-		_accel_reports = buf;
-		start();
 
 		return OK;
 	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_accel_reports - 1;
+		return _accel_reports->size();
 
 	case SENSORIOCRESET:
 		reset();
@@ -854,31 +832,22 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_mag_interval;
 	
 	case SENSORIOCSQUEUEDEPTH: {
-			/* account for sentinel in the ring */
+		/* lower bound is mandatory, upper bound is a sanity check */
-			arg++;
+		if ((arg < 1) || (arg > 100))
+			return -EINVAL;
 
-			/* lower bound is mandatory, upper bound is a sanity check */
+		irqstate_t flags = irqsave();
-			if ((arg < 2) || (arg > 100))
+		if (!_mag_reports->resize(arg)) {
-				return -EINVAL;
+			irqrestore(flags);
-
+			return -ENOMEM;
-			/* allocate new buffer */
-			struct mag_report *buf = new struct mag_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete[] _mag_reports;
-			_num_mag_reports = arg;
-			_mag_reports = buf;
-			start();
-
-			return OK;
 		}
+		irqrestore(flags);
+
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_mag_reports - 1;
+		return _mag_reports->size();
 
 	case SENSORIOCRESET:
 		reset();
@@ -1211,8 +1180,8 @@ LSM303D::start()
 	stop();
 
 	/* reset the report ring */
-	_oldest_accel_report = _next_accel_report = 0;
+	_accel_reports->flush();
-	_oldest_mag_report = _next_mag_report = 0;
+	_mag_reports->flush();
 
 	/* start polling at the specified rate */
 	hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
@@ -1259,7 +1228,7 @@ LSM303D::measure()
 	} raw_accel_report;
 #pragma pack(pop)
 
-	accel_report		*accel_report = &_accel_reports[_next_accel_report];
+	struct _accel_report accel_report;
 
 	/* start the performance counter */
 	perf_begin(_accel_sample_perf);
@@ -1284,35 +1253,30 @@ LSM303D::measure()
 	 */
 
 
-	accel_report->timestamp = hrt_absolute_time();
+	accel_report.r.timestamp = hrt_absolute_time();
 
-	accel_report->x_raw = raw_accel_report.x;
+	accel_report.r.x_raw = raw_accel_report.x;
-	accel_report->y_raw = raw_accel_report.y;
+	accel_report.r.y_raw = raw_accel_report.y;
-	accel_report->z_raw = raw_accel_report.z;
+	accel_report.r.z_raw = raw_accel_report.z;
 
-	float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	float x_in_new = ((accel_report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
+	float y_in_new = ((accel_report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	float z_in_new = ((accel_report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
 
-	accel_report->x = _accel_filter_x.apply(x_in_new);
+	accel_report.r.x = _accel_filter_x.apply(x_in_new);
-	accel_report->y = _accel_filter_y.apply(y_in_new);
+	accel_report.r.y = _accel_filter_y.apply(y_in_new);
-	accel_report->z = _accel_filter_z.apply(z_in_new);
+	accel_report.r.z = _accel_filter_z.apply(z_in_new);
 
-	accel_report->scaling = _accel_range_scale;
+	accel_report.r.scaling = _accel_range_scale;
-	accel_report->range_m_s2 = _accel_range_m_s2;
+	accel_report.r.range_m_s2 = _accel_range_m_s2;
 
-	/* post a report to the ring - note, not locked */
+	_accel_reports->force(accel_report);
-	INCREMENT(_next_accel_report, _num_accel_reports);
-
-	/* if we are running up against the oldest report, fix it */
-	if (_next_accel_report == _oldest_accel_report)
-		INCREMENT(_oldest_accel_report, _num_accel_reports);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report);
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report.r);
 
 	_accel_read++;
 
@@ -1334,7 +1298,7 @@ LSM303D::mag_measure()
 	} raw_mag_report;
 #pragma pack(pop)
 
-	mag_report		*mag_report = &_mag_reports[_next_mag_report];
+	struct _mag_report mag_report;
 
 	/* start the performance counter */
 	perf_begin(_mag_sample_perf);
@@ -1359,30 +1323,25 @@ LSM303D::mag_measure()
 	 */
 
 
-	mag_report->timestamp = hrt_absolute_time();
+	mag_report.r.timestamp = hrt_absolute_time();
 
-	mag_report->x_raw = raw_mag_report.x;
+	mag_report.r.x_raw = raw_mag_report.x;
-	mag_report->y_raw = raw_mag_report.y;
+	mag_report.r.y_raw = raw_mag_report.y;
-	mag_report->z_raw = raw_mag_report.z;
+	mag_report.r.z_raw = raw_mag_report.z;
-	mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
+	mag_report.r.x = ((mag_report.r.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
-	mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
+	mag_report.r.y = ((mag_report.r.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
-	mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
+	mag_report.r.z = ((mag_report.r.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
-	mag_report->scaling = _mag_range_scale;
+	mag_report.r.scaling = _mag_range_scale;
-	mag_report->range_ga = (float)_mag_range_ga;
+	mag_report.r.range_ga = (float)_mag_range_ga;
 
-	/* post a report to the ring - note, not locked */
+	_mag_reports->force(mag_report);
-	INCREMENT(_next_mag_report, _num_mag_reports);
-
-	/* if we are running up against the oldest report, fix it */
-	if (_next_mag_report == _oldest_mag_report)
-		INCREMENT(_oldest_mag_report, _num_mag_reports);
 
 	/* XXX please check this poll_notify, is it the right one? */
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report);
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report.r);
 
 	_mag_read++;
 
@@ -1396,11 +1355,8 @@ LSM303D::print_info()
 	printf("accel reads:          %u\n", _accel_read);
 	printf("mag reads:            %u\n", _mag_read);
 	perf_print_counter(_accel_sample_perf);
-	printf("report queue:   %u (%u/%u @ %p)\n",
+	_accel_reports->print_info("accel reports");
-	       _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports);
+	_mag_reports->print_info("mag reports");
-	perf_print_counter(_mag_sample_perf);
-		printf("report queue:   %u (%u/%u @ %p)\n",
-		   _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports);
 }
 
 LSM303D_mag::LSM303D_mag(LSM303D *parent) :

src/drivers/mb12xx/mb12xx.cpp

@@ -64,6 +64,7 @@
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/subsystem_info.h>
@@ -119,10 +120,7 @@ private:
 	float				_min_distance;
 	float				_max_distance;
 	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;
 	int					_measure_ticks;
 	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.
  */
@@ -195,9 +190,6 @@ MB12XX::MB12XX(int bus, int address) :
 	I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
 	_min_distance(MB12XX_MIN_DISTANCE),
 	_max_distance(MB12XX_MAX_DISTANCE),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_sensor_ok(false),
 	_measure_ticks(0),
@@ -221,7 +213,7 @@ MB12XX::~MB12XX()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 }
 
 int
@@ -234,17 +226,15 @@ MB12XX::init()
 		goto out;
 
 	/* 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)
 		goto out;
 
-	_oldest_report = _next_report = 0;
-
 	/* 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)
 		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);
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* add one to account for the sentinel in the ring */
+		/* lower bound is mandatory, upper bound is a sanity check */
-			arg++;
+		if ((arg < 1) || (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 range_finder_report *buf = new struct range_finder_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:
-		return _num_reports - 1;
+		return _reports->size();
 		
 	case SENSORIOCRESET:
 		/* XXX implement this */
@@ -406,6 +387,7 @@ ssize_t
 MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct range_finder_report);
+	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
 	int ret = 0;
 
 	/* 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.
 		 */
 		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 */
-	/* 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()) {
@@ -453,8 +433,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* 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);
 
@@ -498,26 +479,25 @@ MB12XX::collect()
 	if (ret < 0)
 	{
 		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
 		return ret;
 	}
 	
 	uint16_t distance = val[0] << 8 | val[1];
 	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 */
-	_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 */
-	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);
-		INCREMENT(_oldest_report, _num_reports);
 	}
 
 	/* notify anyone waiting for data */
@@ -525,11 +505,8 @@ MB12XX::collect()
 
 	ret = OK;
 
-out:
 	perf_end(_sample_perf);
 	return ret;
-	
-	return ret;
 }
 
 void
@@ -537,7 +514,7 @@ MB12XX::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)&MB12XX::cycle_trampoline, this, 1);
@@ -626,8 +603,7 @@ MB12XX::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	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);
 }
 
 /**

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));
-
+	struct differential_pressure_s report;
-	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].temperature = temperature;
-	_reports[_next_report].differential_pressure_pa = diff_press_pa;
 
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) {
+	if (diff_press_pa > _max_differential_pressure_pa) {
-		_reports[_next_report].max_differential_pressure_pa = diff_press_pa;
+	    _max_differential_pressure_pa = diff_press_pa;
 	}
 
+	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;
+
 	/* announce the airspeed if needed, just publish else */
-	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
+	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
 
-	/* post a report to the ring - note, not locked */
+	new_report(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);
-		INCREMENT(_oldest_report, _num_reports);
-	}
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);

src/drivers/mpu6000/mpu6000.cpp

@@ -194,7 +194,14 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
-	typedef RingBuffer<accel_report> AccelReportBuffer;
+	/*
+	  these wrapper types are needed to avoid a linker error for
+	  RingBuffer instances which appear in two drivers.
+	 */
+	struct _accel_report {
+		struct accel_report r;
+	};
+	typedef RingBuffer<_accel_report> AccelReportBuffer;
 	AccelReportBuffer	*_accel_reports;
 
 	struct accel_scale	_accel_scale;
@@ -202,7 +209,10 @@ private:
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
 
-	typedef RingBuffer<gyro_report> GyroReportBuffer;
+	struct _gyro_report {
+		struct gyro_report r;
+	};
+	typedef RingBuffer<_gyro_report> GyroReportBuffer;
 	GyroReportBuffer	*_gyro_reports;
 
 	struct gyro_scale	_gyro_scale;
@@ -465,16 +475,16 @@ MPU6000::init()
 	if (gyro_ret != OK) {
 		_gyro_topic = -1;
 	} else {
-		gyro_report gr;
+		_gyro_report gr;
 		_gyro_reports->get(gr);
 
-		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
+		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr.r);
 	}
 
 	/* advertise accel topic */
-	accel_report ar;
+	_accel_report ar;
 	_accel_reports->get(ar);
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar.r);
 
 out:
 	return ret;
@@ -655,12 +665,13 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 		return -EAGAIN;
 
 	/* copy reports out of our buffer to the caller */
-	accel_report *arp = reinterpret_cast<accel_report *>(buffer);
+	_accel_report *arp = reinterpret_cast<_accel_report *>(buffer);
 	int transferred = 0;
 	while (count--) {
-		if (!_accel_reports->get(*arp++))
+		if (!_accel_reports->get(*arp))
 			break;
 		transferred++;
+		arp++;
 	}
 
 	/* return the number of bytes transferred */
@@ -748,12 +759,13 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 		return -EAGAIN;
 
 	/* copy reports out of our buffer to the caller */
-	gyro_report *arp = reinterpret_cast<gyro_report *>(buffer);
+	_gyro_report *grp = reinterpret_cast<_gyro_report *>(buffer);
 	int transferred = 0;
 	while (count--) {
-		if (!_gyro_reports->get(*arp++))
+		if (!_gyro_reports->get(*grp))
 			break;
 		transferred++;
+		grp++;
 	}
 
 	/* return the number of bytes transferred */
@@ -837,28 +849,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 1000000 / _call_interval;
 
 	case SENSORIOCSQUEUEDEPTH: {
-			/* lower bound is mandatory, upper bound is a sanity check */
+		/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 1) || (arg > 100))
+		if ((arg < 1) || (arg > 100))
-				return -EINVAL;
+			return -EINVAL;
 		
-			/* allocate new buffer */
+		irqstate_t flags = irqsave();
-			AccelReportBuffer *buf = new AccelReportBuffer(arg);
+		if (!_accel_reports->resize(arg)) {
-
+			irqrestore(flags);
-			if (nullptr == buf)
+			return -ENOMEM;
-				return -ENOMEM;
-			if (buf->size() == 0) {
-				delete buf;
-				return -ENOMEM;
-			}
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop();
-			delete _accel_reports;
-			_accel_reports = buf;
-			start();
-
-			return OK;
 		}
+		irqrestore(flags);
+		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _accel_reports->size();
@@ -935,21 +938,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		if ((arg < 1) || (arg > 100))
 			return -EINVAL;
 
-		/* allocate new buffer */
+		irqstate_t flags = irqsave();
-		GyroReportBuffer *buf = new GyroReportBuffer(arg);
+		if (!_gyro_reports->resize(arg)) {
-
+			irqrestore(flags);
-		if (nullptr == buf)
-			return -ENOMEM;
-		if (buf->size() == 0) {
-			delete buf;
 			return -ENOMEM;
 		}
-
+		irqrestore(flags);
-		/* reset the measurement state machine with the new buffer, free the old */
-		stop();
-		delete _gyro_reports;
-		_gyro_reports = buf;
-		start();
 
 		return OK;
 	}
@@ -1197,13 +1191,13 @@ MPU6000::measure()
 	/*
 	 * Report buffers.
 	 */
-	accel_report		arb;
+	_accel_report		arb;
-	gyro_report		grb;
+	_gyro_report		grb;
 
 	/*
 	 * Adjust and scale results to m/s^2.
 	 */
-	grb.timestamp = arb.timestamp = hrt_absolute_time();
+	grb.r.timestamp = arb.r.timestamp = hrt_absolute_time();
 
 
 	/*
@@ -1224,53 +1218,53 @@ MPU6000::measure()
 
 	/* NOTE: Axes have been swapped to match the board a few lines above. */
 
-	arb.x_raw = report.accel_x;
+	arb.r.x_raw = report.accel_x;
-	arb.y_raw = report.accel_y;
+	arb.r.y_raw = report.accel_y;
-	arb.z_raw = report.accel_z;
+	arb.r.z_raw = report.accel_z;
 
 	float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
 	float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
 	float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
 	
-	arb.x = _accel_filter_x.apply(x_in_new);
+	arb.r.x = _accel_filter_x.apply(x_in_new);
-	arb.y = _accel_filter_y.apply(y_in_new);
+	arb.r.y = _accel_filter_y.apply(y_in_new);
-	arb.z = _accel_filter_z.apply(z_in_new);
+	arb.r.z = _accel_filter_z.apply(z_in_new);
 
-	arb.scaling = _accel_range_scale;
+	arb.r.scaling = _accel_range_scale;
-	arb.range_m_s2 = _accel_range_m_s2;
+	arb.r.range_m_s2 = _accel_range_m_s2;
 
-	arb.temperature_raw = report.temp;
+	arb.r.temperature_raw = report.temp;
-	arb.temperature = (report.temp) / 361.0f + 35.0f;
+	arb.r.temperature = (report.temp) / 361.0f + 35.0f;
 
-	grb.x_raw = report.gyro_x;
+	grb.r.x_raw = report.gyro_x;
-	grb.y_raw = report.gyro_y;
+	grb.r.y_raw = report.gyro_y;
-	grb.z_raw = report.gyro_z;
+	grb.r.z_raw = report.gyro_z;
 
 	float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
 	float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
 	float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
 	
-	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
+	grb.r.x = _gyro_filter_x.apply(x_gyro_in_new);
-	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
+	grb.r.y = _gyro_filter_y.apply(y_gyro_in_new);
-	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
+	grb.r.z = _gyro_filter_z.apply(z_gyro_in_new);
 
-	grb.scaling = _gyro_range_scale;
+	grb.r.scaling = _gyro_range_scale;
-	grb.range_rad_s = _gyro_range_rad_s;
+	grb.r.range_rad_s = _gyro_range_rad_s;
 
-	grb.temperature_raw = report.temp;
+	grb.r.temperature_raw = report.temp;
-	grb.temperature = (report.temp) / 361.0f + 35.0f;
+	grb.r.temperature = (report.temp) / 361.0f + 35.0f;
 
-	_accel_reports->put(arb);
+	_accel_reports->force(arb);
-	_gyro_reports->put(grb);
+	_gyro_reports->force(grb);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
 	/* and publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb.r);
 	if (_gyro_topic != -1) {
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r);
 	}
 
 	/* stop measuring */
@@ -1280,7 +1274,10 @@ MPU6000::measure()
 void
 MPU6000::print_info()
 {
+	perf_print_counter(_sample_perf);
 	printf("reads:          %u\n", _reads);
+	_accel_reports->print_info("accel queue");
+	_gyro_reports->print_info("gyro queue");
 }
 
 MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :

src/drivers/ms5611/ms5611.cpp

@@ -60,6 +60,7 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_baro.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/ringbuffer.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -114,10 +115,7 @@ protected:
 	struct work_s		_work;
 	unsigned		_measure_ticks;
 
-	unsigned		_num_reports;
+	RingBuffer<struct baro_report> *_reports;
-	volatile unsigned	_next_report;
-	volatile unsigned	_oldest_report;
-	struct baro_report	*_reports;
 
 	bool			_collect_phase;
 	unsigned		_measure_phase;
@@ -196,9 +194,6 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
 	_interface(interface),
 	_prom(prom_buf.s),
 	_measure_ticks(0),
-	_num_reports(0),
-	_next_report(0),
-	_oldest_report(0),
 	_reports(nullptr),
 	_collect_phase(false),
 	_measure_phase(0),
@@ -223,7 +218,7 @@ MS5611::~MS5611()
 
 	/* free any existing reports */
 	if (_reports != nullptr)
-		delete[] _reports;
+		delete _reports;
 
 	// free perf counters
 	perf_free(_sample_perf);
@@ -246,8 +241,7 @@ MS5611::init()
 	}
 
 	/* allocate basic report buffers */
-	_num_reports = 2;
+	_reports = new RingBuffer<struct baro_report>(2);
-	_reports = new struct baro_report[_num_reports];
 
 	if (_reports == nullptr) {
 		debug("can't get memory for reports");
@@ -255,11 +249,10 @@ MS5611::init()
 		goto out;
 	}
 
-	_oldest_report = _next_report = 0;
-
 	/* get a publish handle on the baro topic */
-	memset(&_reports[0], 0, sizeof(_reports[0]));
+	struct baro_report zero_report;
-	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
+	memset(&zero_report, 0, sizeof(zero_report));
+	_baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report);
 
 	if (_baro_topic < 0) {
 		debug("failed to create sensor_baro object");
@@ -276,6 +269,7 @@ ssize_t
 MS5611::read(struct file *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(struct baro_report);
+	struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -291,10 +285,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_oldest_report != _next_report) {
+			if (_reports->get(*brp)) {
-				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(*brp);
-				ret += sizeof(_reports[0]);
+				brp++;
-				INCREMENT(_oldest_report, _num_reports);
 			}
 		}
 
@@ -303,10 +296,9 @@ MS5611::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 {
 		_measure_phase = 0;
-		_oldest_report = _next_report = 0;
+		_reports->flush();
 
 		/* do temperature first */
 		if (OK != measure()) {
@@ -335,8 +327,8 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		memcpy(buffer, _reports, sizeof(*_reports));
+		if (_reports->get(*brp))
-		ret = sizeof(*_reports);
+			ret = sizeof(*brp);
 
 	} while (0);
 
@@ -411,31 +403,21 @@ MS5611::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 */
+		/* lower bound is mandatory, upper bound is a sanity check */
-			arg++;
+		if ((arg < 1) || (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 baro_report *buf = new struct baro_report[arg];
-
-			if (nullptr == buf)
-				return -ENOMEM;
-
-			/* reset the measurement state machine with the new buffer, free the old */
-			stop_cycle();
-			delete[] _reports;
-			_num_reports = arg;
-			_reports = buf;
-			start_cycle();
-
-			return OK;
 		}
+		irqrestore(flags);		
+		return OK;
+	}
 
 	case SENSORIOCGQUEUEDEPTH:
-		return _num_reports - 1;
+		return _reports->size();
 
 	case SENSORIOCRESET:
 		/* XXX implement this */
@@ -469,7 +451,7 @@ MS5611::start_cycle()
 	/* reset the report ring and state machine */
 	_collect_phase = false;
 	_measure_phase = 0;
-	_oldest_report = _next_report = 0;
+	_reports->flush();
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
@@ -588,8 +570,9 @@ MS5611::collect()
 
 	perf_begin(_sample_perf);
 
+	struct baro_report report;
 	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
-	_reports[_next_report].timestamp = hrt_absolute_time();
+	report.timestamp = hrt_absolute_time();
 
 	/* read the most recent measurement - read offset/size are hardcoded in the interface */
 	ret = _interface->read(0, (void *)&raw, 0);
@@ -638,8 +621,8 @@ MS5611::collect()
 		int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
 
 		/* generate a new report */
-		_reports[_next_report].temperature = _TEMP / 100.0f;
+		report.temperature = _TEMP / 100.0f;
-		_reports[_next_report].pressure = P / 100.0f;		/* convert to millibar */
+		report.pressure = P / 100.0f;		/* convert to millibar */
 
 		/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
 
@@ -676,18 +659,13 @@ MS5611::collect()
 		 * h = -------------------------------  + h1
 		 *                   a
 		 */
-		_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
+		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
 		/* publish it */
-		orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
+		orb_publish(ORB_ID(sensor_baro), _baro_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);
-			INCREMENT(_oldest_report, _num_reports);
 		}
 
 		/* notify anyone waiting for data */
@@ -709,8 +687,7 @@ MS5611::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	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);
 	printf("TEMP:           %d\n", _TEMP);
 	printf("SENS:           %lld\n", _SENS);
 	printf("OFF:            %lld\n", _OFF);

src/drivers/rgbled/rgbled.cpp

@@ -590,9 +590,9 @@ rgbled_main(int argc, char *argv[])
 			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
 		}
 		rgbled_rgbset_t v;
-		v.red   = strtol(argv[1], NULL, 0);
+		v.red   = strtol(argv[2], NULL, 0);
-		v.green = strtol(argv[2], NULL, 0);
+		v.green = strtol(argv[3], NULL, 0);
-		v.blue  = strtol(argv[3], NULL, 0);
+		v.blue  = strtol(argv[4], NULL, 0);
 		ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
 		close(fd);
 		exit(ret);