Merged master

src/drivers/airspeed/airspeed.cpp

@@ -79,6 +79,7 @@
 Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
 	I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
 	_reports(nullptr),
+	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
 	_max_differential_pressure_pa(0),
 	_sensor_ok(false),
 	_measure_ticks(0),
@@ -87,8 +88,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
 	_airspeed_pub(-1),
 	_conversion_interval(conversion_interval),
 	_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows"))
+	_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -122,7 +122,7 @@ Airspeed::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_reports = new RingBuffer<differential_pressure_s>(2);
+	_reports = new RingBuffer(2, sizeof(differential_pressure_s));
 	if (_reports == nullptr)
 		goto out;
 
@@ -282,7 +282,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_reports->get(*abuf)) {
+			if (_reports->get(abuf)) {
 				ret += sizeof(*abuf);
 				abuf++;
 			}
@@ -312,7 +312,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		if (_reports->get(*abuf)) {
+		if (_reports->get(abuf)) {
 			ret = sizeof(*abuf);
 		}
 
@@ -375,6 +375,6 @@ Airspeed::print_info()
 void
 Airspeed::new_report(const differential_pressure_s &report)
 {
-	if (!_reports->force(report))
+	if (!_reports->force(&report))
 		perf_count(_buffer_overflows);
 }

src/drivers/airspeed/airspeed.h

@@ -104,7 +104,7 @@ public:
 	virtual void	print_info();
 
 private:
-	RingBuffer<differential_pressure_s> *_reports;
+	RingBuffer		*_reports;
 	perf_counter_t		_buffer_overflows;
 
 protected:

src/drivers/bma180/bma180.cpp

@@ -147,14 +147,7 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
-	/*
-	  this wrapper type is needed to avoid a linker error for
-	  RingBuffer instances which appear in two drivers.
-	 */
-	struct _accel_report {
-		accel_report r;
-	};
-	RingBuffer<struct _accel_report> *_reports;
+	RingBuffer		*_reports;
 
 	struct accel_scale	_accel_scale;
 	float			_accel_range_scale;
@@ -284,7 +277,7 @@ BMA180::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_reports = new RingBuffer<_accel_report>(2);
+	_reports = new RingBuffer(2, sizeof(accel_report));
 
 	if (_reports == nullptr)
 		goto out;
@@ -349,7 +342,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);
+	struct accel_report *arp = reinterpret_cast<struct accel_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -365,7 +358,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_reports->get(*arp)) {
+			if (_reports->get(arp)) {
 				ret += sizeof(*arp);
 				arp++;
 			}
@@ -380,7 +373,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	if (_reports->get(*arp))
+	if (_reports->get(arp))
 		ret = sizeof(*arp);
 
 	return ret;
@@ -676,7 +669,7 @@ BMA180::measure()
 // 	} raw_report;
 // #pragma pack(pop)
 
-	struct _accel_report report;
+	struct accel_report report;
 
 	/* start the performance counter */
 	perf_begin(_sample_perf);
@@ -696,40 +689,40 @@ BMA180::measure()
 	 * them before.  There is no good way to synchronise with the internal
 	 * measurement flow without using the external interrupt.
 	 */
-	report.r.timestamp = hrt_absolute_time();
+	report.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.r.y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
-	report.r.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
-	report.r.x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
-	report.r.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
-	report.r.z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
-	report.r.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
+	report.y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
+	report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
+	report.x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
+	report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
+	report.z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
+	report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
 
 	/* discard two non-value bits in the 16 bit measurement */
-	report.r.x_raw = (report.r.x_raw / 4);
-	report.r.y_raw = (report.r.y_raw / 4);
-	report.r.z_raw = (report.r.z_raw / 4);
+	report.x_raw = (report.x_raw / 4);
+	report.y_raw = (report.y_raw / 4);
+	report.z_raw = (report.z_raw / 4);
 
 	/* invert y axis, due to 14 bit data no overflow can occur in the negation */
-	report.r.y_raw = -report.r.y_raw;
+	report.y_raw = -report.y_raw;
 
-	report.r.x = ((report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	report.r.y = ((report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	report.r.z = ((report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-	report.r.scaling = _accel_range_scale;
-	report.r.range_m_s2 = _accel_range_m_s2;
+	report.x = ((report.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.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	report.scaling = _accel_range_scale;
+	report.range_m_s2 = _accel_range_m_s2;
 
-	_reports->force(report);
+	_reports->force(&report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &report.r);
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
 
 	/* stop the perf counter */
 	perf_end(_sample_perf);

src/drivers/device/ringbuffer.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,15 +34,14 @@
 /**
  * @file ringbuffer.h
  *
- * A simple ringbuffer template.
+ * A flexible ringbuffer class.
  */
 
 #pragma once
 
-template<typename T>
 class RingBuffer {
 public:
-	RingBuffer(unsigned size);
+	RingBuffer(unsigned ring_size, size_t entry_size);
 	virtual ~RingBuffer();
 
 	/**
@@ -52,15 +50,18 @@ public:
 	 * @param val		Item to put
 	 * @return		true if the item was put, false if the buffer is full
 	 */
-	bool			put(T &val);
+	bool			put(const void *val, size_t val_size = 0);
 
-	/**
-	 * 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);
+	bool			put(int8_t val);
+	bool			put(uint8_t val);
+	bool			put(int16_t val);
+	bool			put(uint16_t val);
+	bool			put(int32_t val);
+	bool			put(uint32_t val);
+	bool			put(int64_t val);
+	bool			put(uint64_t val);
+	bool			put(float val);
+	bool			put(double val);
 
 	/**
 	 * Force an item into the buffer, discarding an older item if there is not space.
@@ -68,15 +69,18 @@ public:
 	 * @param val		Item to put
 	 * @return		true if an item was discarded to make space
 	 */
-	bool			force(T &val);
+	bool			force(const void *val, size_t val_size = 0);
 
-	/**
-	 * 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);
+	bool			force(int8_t val);
+	bool			force(uint8_t val);
+	bool			force(int16_t val);
+	bool			force(uint16_t val);
+	bool			force(int32_t val);
+	bool			force(uint32_t val);
+	bool			force(int64_t val);
+	bool			force(uint64_t val);
+	bool			force(float val);
+	bool			force(double val);
 
 	/**
 	 * Get an item from the buffer.
@@ -84,15 +88,18 @@ public:
 	 * @param val		Item that was gotten
 	 * @return		true if an item was got, false if the buffer was empty.
 	 */
-	bool			get(T &val);
+	bool			get(void *val, size_t val_size = 0);
 
-	/**
-	 * Get an item from the buffer (scalars only).
-	 *
-	 * @return		The value that was fetched. If the buffer is empty, 
-	 *			returns zero.
-	 */
-	T			get(void);
+	bool			get(int8_t &val);
+	bool			get(uint8_t &val);
+	bool			get(int16_t &val);
+	bool			get(uint16_t &val);
+	bool			get(int32_t &val);
+	bool			get(uint32_t &val);
+	bool			get(int64_t &val);
+	bool			get(uint64_t &val);
+	bool			get(float &val);
+	bool			get(double &val);
 
 	/*
 	 * Get the number of slots free in the buffer.
@@ -148,67 +155,68 @@ public:
 	void			print_info(const char *name);
 
 private:
-	T			*_buf;	
-	unsigned		_size;
-	volatile unsigned	_head;	/**< insertion point */
-	volatile unsigned	_tail;	/**< removal point */
+	unsigned		_num_items;
+	const size_t		_item_size;
+	char			*_buf;	
+	volatile unsigned	_head;	/**< insertion point in _item_size units */
+	volatile unsigned	_tail;	/**< removal point in _item_size units */
 
 	unsigned		_next(unsigned index);
 };
 
-template <typename T>
-RingBuffer<T>::RingBuffer(unsigned with_size) :
-	_buf(new T[with_size + 1]),
-	_size(with_size),
- 	_head(with_size),
- 	_tail(with_size)
+RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
+	_num_items(num_items),
+	_item_size(item_size),
+	_buf(new char[(_num_items+1) * item_size]),
+ 	_head(_num_items),
+ 	_tail(_num_items)
 {}
 
-template <typename T>
-RingBuffer<T>::~RingBuffer()
+RingBuffer::~RingBuffer()
 {
 	if (_buf != nullptr)
 		delete[] _buf;
 }
 
-template <typename T>
-bool RingBuffer<T>::empty()
+unsigned
+RingBuffer::_next(unsigned index)
+{
+	return (0 == index) ? _num_items : (index - 1); 
+}
+
+bool
+RingBuffer::empty()
 {
 	return _tail == _head; 
 }
 
-template <typename T>
-bool RingBuffer<T>::full()
+bool
+RingBuffer::full()
 {
 	return _next(_head) == _tail; 
 }
 
-template <typename T>
-unsigned RingBuffer<T>::size()
+unsigned
+RingBuffer::size()
 {
-	return (_buf != nullptr) ? _size : 0; 
+	return (_buf != nullptr) ? _num_items : 0; 
 }
 
-template <typename T>
-void RingBuffer<T>::flush()
+void
+RingBuffer::flush()
 {
-	T junk;
 	while (!empty())
-		get(junk); 
+		get(NULL);
 }
 
-template <typename T>
-unsigned RingBuffer<T>::_next(unsigned index)
-{
-	return (0 == index) ? _size : (index - 1); 
-}
-
-template <typename T>
-bool RingBuffer<T>::put(T &val) 
+bool
+RingBuffer::put(const void *val, size_t val_size) 
 {
 	unsigned next = _next(_head);
 	if (next != _tail) {
-		_buf[_head] = val;
+		if ((val_size == 0) || (val_size > _item_size))
+			val_size = _item_size;
+		memcpy(&_buf[_head * _item_size], val, val_size);
 		_head = next;
 		return true;
 	} else {
@@ -216,55 +224,150 @@ bool RingBuffer<T>::put(T &val)
 	}
 }
 
-template <typename T>
-bool RingBuffer<T>::put(const T &val) 
+bool
+RingBuffer::put(int8_t val)
 {
-	unsigned next = _next(_head);
-	if (next != _tail) {
-		_buf[_head] = val;
-		_head = next;
-		return true;
-	} else {
-		return false;
-	}
+	return put(&val, sizeof(val));
 }
 
-template <typename T>
-bool RingBuffer<T>::force(T &val)
+bool
+RingBuffer::put(uint8_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int16_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint16_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int32_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint32_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(int64_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(uint64_t val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(float val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::put(double val)
+{
+	return put(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(const void *val, size_t val_size)
 {
 	bool overwrote = false;
 
 	for (;;) {
-		if (put(val))
+		if (put(val, val_size))
 			break;
-		T junk;
-		get(junk);
+		get(NULL);
 		overwrote = true;
 	}
 	return overwrote;
 }
 
-template <typename T>
-bool RingBuffer<T>::force(const T &val)
+bool
+RingBuffer::force(int8_t val)
 {
-	bool overwrote = false;
-
-	for (;;) {
-		if (put(val))
-			break;
-		T junk;
-		get(junk);
-		overwrote = true;
-	}
-	return overwrote;
+	return force(&val, sizeof(val));
 }
 
-template <typename T>
-bool RingBuffer<T>::get(T &val) 
+bool
+RingBuffer::force(uint8_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int16_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint16_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int32_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint32_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(int64_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(uint64_t val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(float val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::force(double val)
+{
+	return force(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(void *val, size_t val_size) 
 {
 	if (_tail != _head) {
 		unsigned candidate;
 		unsigned next;
+
+		if ((val_size == 0) || (val_size > _item_size))
+			val_size = _item_size;
+
 		do {
 			/* decide which element we think we're going to read */
 			candidate = _tail;
@@ -273,7 +376,8 @@ bool RingBuffer<T>::get(T &val)
 			next = _next(candidate);
 
 			/* go ahead and read from this index */
-			val = _buf[candidate];
+			if (val != NULL)
+				memcpy(val, &_buf[candidate * _item_size], val_size);
 
 			/* if the tail pointer didn't change, we got our item */
 		} while (!__sync_bool_compare_and_swap(&_tail, candidate, next));
@@ -284,15 +388,68 @@ bool RingBuffer<T>::get(T &val)
 	}
 }
 
-template <typename T>
-T RingBuffer<T>::get(void) 
+bool
+RingBuffer::get(int8_t &val)
 {
-	T val;
-	return get(val) ? val : 0;
+	return get(&val, sizeof(val));
 }
 
-template <typename T>
-unsigned RingBuffer<T>::space(void) 
+bool
+RingBuffer::get(uint8_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int16_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint16_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int32_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint32_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(int64_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(uint64_t &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(float &val)
+{
+	return get(&val, sizeof(val));
+}
+
+bool
+RingBuffer::get(double &val)
+{
+	return get(&val, sizeof(val));
+}
+
+unsigned
+RingBuffer::space(void) 
 {
 	unsigned tail, head;
 
@@ -309,39 +466,44 @@ unsigned RingBuffer<T>::space(void)
 		tail = _tail;
 	} while (head != _head);
 
-	return (tail >= head) ? (_size - (tail - head)) : (head - tail - 1);
+	return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1);
 }
 
-template <typename T>
-unsigned RingBuffer<T>::count(void) 
+unsigned
+RingBuffer::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();
+	return _num_items - space();
 }
 
-template <typename T>
-bool RingBuffer<T>::resize(unsigned new_size) 
+bool
+RingBuffer::resize(unsigned new_size) 
 {
-	T *old_buffer;
-	T *new_buffer = new T[new_size + 1];
+	char *old_buffer;
+	char *new_buffer = new char [(new_size+1) * _item_size];
 	if (new_buffer == nullptr) {
 		return false;
 	}
 	old_buffer = _buf;
 	_buf = new_buffer;
-	_size = new_size;
+	_num_items = new_size;
 	_head = new_size;
 	_tail = new_size;
 	delete[] old_buffer;
 	return true;
 }
 
-template <typename T>
-void RingBuffer<T>::print_info(const char *name) 
+void
+RingBuffer::print_info(const char *name) 
 {
-	printf("%s	%u (%u/%u @ %p)\n",
-	       name, _size, _head, _tail, _buf);
+	printf("%s	%u/%u (%u/%u @ %p)\n",
+	       name, 
+	       _num_items, 
+	       _num_items * _item_size, 
+	       _head, 
+	       _tail, 
+	       _buf);
 }

src/drivers/hmc5883/hmc5883.cpp

@@ -149,7 +149,7 @@ private:
 	work_s			_work;
 	unsigned		_measure_ticks;
 
-	RingBuffer<struct mag_report> *_reports;
+	RingBuffer		*_reports;
 	mag_scale		_scale;
 	float 			_range_scale;
 	float 			_range_ga;
@@ -367,7 +367,7 @@ HMC5883::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_reports = new RingBuffer<struct mag_report>(2);
+	_reports = new RingBuffer(2, sizeof(mag_report));
 	if (_reports == nullptr)
 		goto out;
 
@@ -496,7 +496,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_reports->get(*mag_buf)) {
+			if (_reports->get(mag_buf)) {
 				ret += sizeof(struct mag_report);
 				mag_buf++;
 			}
@@ -526,7 +526,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
 			break;
 		}
 
-		if (_reports->get(*mag_buf)) {
+		if (_reports->get(mag_buf)) {
 			ret = sizeof(struct mag_report);
 		}
 	} while (0);
@@ -878,7 +878,7 @@ HMC5883::collect()
 	orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
 
 	/* post a report to the ring */
-	if (_reports->force(new_report)) {
+	if (_reports->force(&new_report)) {
 		perf_count(_buffer_overflows);
 	}
 

src/drivers/l3gd20/l3gd20.cpp

@@ -185,7 +185,7 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 	
-	RingBuffer<gyro_report> *_reports;
+	RingBuffer		*_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
@@ -347,7 +347,7 @@ L3GD20::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_reports = new RingBuffer<struct gyro_report>(2);
+	_reports = new RingBuffer(2, sizeof(gyro_report));
 
 	if (_reports == nullptr)
 		goto out;
@@ -367,9 +367,12 @@ out:
 int
 L3GD20::probe()
 {
+	irqstate_t flags = irqsave();
 	/* read dummy value to void to clear SPI statemachine on sensor */
 	(void)read_reg(ADDR_WHO_AM_I);
 
+	bool success = false;
+
 	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
 	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
 
@@ -380,15 +383,21 @@ L3GD20::probe()
 		#else
 			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
 		#endif
-		return OK;
+
+		success = true;
 	}
 
 
 	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
 		_orientation = SENSOR_BOARD_ROTATION_180_DEG;
-		return OK;
+		success = true;
 	}
 
+	irqrestore(flags);
+
+	if (success)
+		return OK;
+
 	return -EIO;
 }
 
@@ -412,7 +421,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with it.
 		 */
 		while (count--) {
-			if (_reports->get(*gbuf)) {
+			if (_reports->get(gbuf)) {
 				ret += sizeof(*gbuf);
 				gbuf++;
 			}
@@ -427,7 +436,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	if (_reports->get(*gbuf)) {
+	if (_reports->get(gbuf)) {
 		ret = sizeof(*gbuf);
 	}
 
@@ -806,7 +815,7 @@ L3GD20::measure()
 	report.scaling = _gyro_range_scale;
 	report.range_rad_s = _gyro_range_rad_s;
 
-	_reports->force(report);
+	_reports->force(&report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);

src/drivers/lsm303d/lsm303d.cpp

@@ -219,19 +219,8 @@ private:
 	unsigned		_call_accel_interval;
 	unsigned		_call_mag_interval;
 
-	/*
-	  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;
-	};
-	RingBuffer<struct _accel_report> *_accel_reports;
-
-	struct _mag_report {
-		struct mag_report r;
-	};
-	RingBuffer<struct _mag_report> *_mag_reports;
+	RingBuffer		*_accel_reports;
+	RingBuffer		*_mag_reports;
 
 	struct accel_scale	_accel_scale;
 	unsigned		_accel_range_m_s2;
@@ -409,7 +398,7 @@ public:
 	LSM303D_mag(LSM303D *parent);
 	~LSM303D_mag();
 
-	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
 	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
 
 protected:
@@ -493,11 +482,13 @@ LSM303D::init()
 	int mag_ret;
 
 	/* do SPI init (and probe) first */
-	if (SPI::init() != OK)
+	if (SPI::init() != OK) {
+		warnx("SPI init failed");
 		goto out;
+	}
 
 	/* allocate basic report buffers */
-	_accel_reports = new RingBuffer<struct _accel_report>(2);
+	_accel_reports = new RingBuffer(2, sizeof(accel_report));
 
 	if (_accel_reports == nullptr)
 		goto out;
@@ -507,7 +498,7 @@ LSM303D::init()
 	memset(&zero_report, 0, sizeof(zero_report));
 	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
 
-	_mag_reports = new RingBuffer<struct _mag_report>(2);
+	_mag_reports = new RingBuffer(2, sizeof(mag_report));
 
 	if (_mag_reports == nullptr)
 		goto out;
@@ -534,6 +525,7 @@ out:
 void
 LSM303D::reset()
 {
+	irqstate_t flags = irqsave();
 	/* enable accel*/
 	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
 
@@ -548,6 +540,7 @@ LSM303D::reset()
 
 	mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
 	mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
+	irqrestore(flags);
 
 	_accel_read = 0;
 	_mag_read = 0;
@@ -556,11 +549,16 @@ LSM303D::reset()
 int
 LSM303D::probe()
 {
+	irqstate_t flags = irqsave();
 	/* read dummy value to void to clear SPI statemachine on sensor */
 	(void)read_reg(ADDR_WHO_AM_I);
 
 	/* verify that the device is attached and functioning */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
+	bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
+	
+	irqrestore(flags);
+
+	if (success)
 		return OK;
 
 	return -EIO;
@@ -570,7 +568,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);
+	accel_report *arb = reinterpret_cast<accel_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -583,7 +581,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 		 * While there is space in the caller's buffer, and reports, copy them.
 		 */
 		while (count--) {
-			if (_accel_reports->get(*arb)) {
+			if (_accel_reports->get(arb)) {
 				ret += sizeof(*arb);
 				arb++;
 			}
@@ -597,7 +595,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	if (_accel_reports->get(*arb))
+	if (_accel_reports->get(arb))
 		ret = sizeof(*arb);
 
 	return ret;
@@ -607,7 +605,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);
+	mag_report *mrb = reinterpret_cast<mag_report *>(buffer);
 	int ret = 0;
 
 	/* buffer must be large enough */
@@ -621,7 +619,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
 		 * While there is space in the caller's buffer, and reports, copy them.
 		 */
 		while (count--) {
-			if (_mag_reports->get(*mrb)) {
+			if (_mag_reports->get(mrb)) {
 				ret += sizeof(*mrb);
 				mrb++;
 			}
@@ -636,7 +634,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
 	measure();
 
 	/* measurement will have generated a report, copy it out */
-	if (_mag_reports->get(*mrb))
+	if (_mag_reports->get(mrb))
 		ret = sizeof(*mrb);
 
 	return ret;
@@ -1228,7 +1226,7 @@ LSM303D::measure()
 	} raw_accel_report;
 #pragma pack(pop)
 
-	struct _accel_report accel_report;
+	accel_report accel_report;
 
 	/* start the performance counter */
 	perf_begin(_accel_sample_perf);
@@ -1253,30 +1251,30 @@ LSM303D::measure()
 	 */
 
 
-	accel_report.r.timestamp = hrt_absolute_time();
+	accel_report.timestamp = hrt_absolute_time();
 
-	accel_report.r.x_raw = raw_accel_report.x;
-	accel_report.r.y_raw = raw_accel_report.y;
-	accel_report.r.z_raw = raw_accel_report.z;
+	accel_report.x_raw = raw_accel_report.x;
+	accel_report.y_raw = raw_accel_report.y;
+	accel_report.z_raw = raw_accel_report.z;
 
-	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.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	float z_in_new = ((accel_report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
+	float x_in_new = ((accel_report.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 z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
 
-	accel_report.r.x = _accel_filter_x.apply(x_in_new);
-	accel_report.r.y = _accel_filter_y.apply(y_in_new);
-	accel_report.r.z = _accel_filter_z.apply(z_in_new);
+	accel_report.x = _accel_filter_x.apply(x_in_new);
+	accel_report.y = _accel_filter_y.apply(y_in_new);
+	accel_report.z = _accel_filter_z.apply(z_in_new);
 
-	accel_report.r.scaling = _accel_range_scale;
-	accel_report.r.range_m_s2 = _accel_range_m_s2;
+	accel_report.scaling = _accel_range_scale;
+	accel_report.range_m_s2 = _accel_range_m_s2;
 
-	_accel_reports->force(accel_report);
+	_accel_reports->force(&accel_report);
 
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report.r);
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 
 	_accel_read++;
 
@@ -1298,7 +1296,7 @@ LSM303D::mag_measure()
 	} raw_mag_report;
 #pragma pack(pop)
 
-	struct _mag_report mag_report;
+	mag_report mag_report;
 
 	/* start the performance counter */
 	perf_begin(_mag_sample_perf);
@@ -1323,25 +1321,25 @@ LSM303D::mag_measure()
 	 */
 
 
-	mag_report.r.timestamp = hrt_absolute_time();
+	mag_report.timestamp = hrt_absolute_time();
 
-	mag_report.r.x_raw = raw_mag_report.x;
-	mag_report.r.y_raw = raw_mag_report.y;
-	mag_report.r.z_raw = raw_mag_report.z;
-	mag_report.r.x = ((mag_report.r.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
-	mag_report.r.y = ((mag_report.r.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
-	mag_report.r.z = ((mag_report.r.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
-	mag_report.r.scaling = _mag_range_scale;
-	mag_report.r.range_ga = (float)_mag_range_ga;
+	mag_report.x_raw = raw_mag_report.x;
+	mag_report.y_raw = raw_mag_report.y;
+	mag_report.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.y = ((mag_report.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.scaling = _mag_range_scale;
+	mag_report.range_ga = (float)_mag_range_ga;
 
-	_mag_reports->force(mag_report);
+	_mag_reports->force(&mag_report);
 
 	/* 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.r);
+	orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
 
 	_mag_read++;
 
@@ -1426,8 +1424,10 @@ start()
 	/* create the driver */
 	g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
+		warnx("failed instantiating LSM303D obj");
 		goto fail;
+	}
 
 	if (OK != g_dev->init())
 		goto fail;

src/drivers/mb12xx/mb12xx.cpp

@@ -120,7 +120,7 @@ private:
 	float				_min_distance;
 	float				_max_distance;
 	work_s				_work;
-	RingBuffer<struct range_finder_report> *_reports;
+	RingBuffer		*_reports;
 	bool				_sensor_ok;
 	int					_measure_ticks;
 	bool				_collect_phase;
@@ -226,7 +226,7 @@ MB12XX::init()
 		goto out;
 
 	/* allocate basic report buffers */
-	_reports = new RingBuffer<struct range_finder_report>(2);
+	_reports = new RingBuffer(2, sizeof(range_finder_report));
 
 	if (_reports == nullptr)
 		goto out;
@@ -403,7 +403,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_reports->get(*rbuf)) {
+			if (_reports->get(rbuf)) {
 				ret += sizeof(*rbuf);
 				rbuf++;
 			}
@@ -433,7 +433,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		if (_reports->get(*rbuf)) {
+		if (_reports->get(rbuf)) {
 			ret = sizeof(*rbuf);
 		}
 
@@ -496,7 +496,7 @@ MB12XX::collect()
 	/* publish it */
 	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
 
-	if (_reports->force(report)) {
+	if (_reports->force(&report)) {
 		perf_count(_buffer_overflows);
 	}
 

src/drivers/mpu6000/mpu6000.cpp

@@ -194,26 +194,14 @@ private:
 	struct hrt_call		_call;
 	unsigned		_call_interval;
 
-	/*
-	  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;
+	RingBuffer		*_accel_reports;
 
 	struct accel_scale	_accel_scale;
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
 
-	struct _gyro_report {
-		struct gyro_report r;
-	};
-	typedef RingBuffer<_gyro_report> GyroReportBuffer;
-	GyroReportBuffer	*_gyro_reports;
+	RingBuffer		*_gyro_reports;
 
 	struct gyro_scale	_gyro_scale;
 	float			_gyro_range_scale;
@@ -441,11 +429,11 @@ MPU6000::init()
 	}
 
 	/* allocate basic report buffers */
-	_accel_reports = new AccelReportBuffer(2);
+	_accel_reports = new RingBuffer(2, sizeof(accel_report));
 	if (_accel_reports == nullptr)
 		goto out;
 
-	_gyro_reports = new GyroReportBuffer(2);
+	_gyro_reports = new RingBuffer(2, sizeof(gyro_report));
 	if (_gyro_reports == nullptr)
 		goto out;
 
@@ -475,16 +463,16 @@ MPU6000::init()
 	if (gyro_ret != OK) {
 		_gyro_topic = -1;
 	} else {
-		_gyro_report gr;
-		_gyro_reports->get(gr);
+		gyro_report gr;
+		_gyro_reports->get(&gr);
 
-		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr.r);
+		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
 	}
 
 	/* advertise accel topic */
-	_accel_report ar;
-	_accel_reports->get(ar);
-	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar.r);
+	accel_report ar;
+	_accel_reports->get(&ar);
+	_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
 
 out:
 	return ret;
@@ -665,10 +653,10 @@ 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++;
@@ -759,10 +747,10 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 		return -EAGAIN;
 
 	/* copy reports out of our buffer to the caller */
-	_gyro_report *grp = reinterpret_cast<_gyro_report *>(buffer);
+	gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
 	int transferred = 0;
 	while (count--) {
-		if (!_gyro_reports->get(*grp))
+		if (!_gyro_reports->get(grp))
 			break;
 		transferred++;
 		grp++;
@@ -1191,13 +1179,13 @@ MPU6000::measure()
 	/*
 	 * Report buffers.
 	 */
-	_accel_report		arb;
-	_gyro_report		grb;
+	accel_report		arb;
+	gyro_report		grb;
 
 	/*
 	 * Adjust and scale results to m/s^2.
 	 */
-	grb.r.timestamp = arb.r.timestamp = hrt_absolute_time();
+	grb.timestamp = arb.timestamp = hrt_absolute_time();
 
 
 	/*
@@ -1218,53 +1206,53 @@ MPU6000::measure()
 
 	/* NOTE: Axes have been swapped to match the board a few lines above. */
 
-	arb.r.x_raw = report.accel_x;
-	arb.r.y_raw = report.accel_y;
-	arb.r.z_raw = report.accel_z;
+	arb.x_raw = report.accel_x;
+	arb.y_raw = report.accel_y;
+	arb.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.r.x = _accel_filter_x.apply(x_in_new);
-	arb.r.y = _accel_filter_y.apply(y_in_new);
-	arb.r.z = _accel_filter_z.apply(z_in_new);
+	arb.x = _accel_filter_x.apply(x_in_new);
+	arb.y = _accel_filter_y.apply(y_in_new);
+	arb.z = _accel_filter_z.apply(z_in_new);
 
-	arb.r.scaling = _accel_range_scale;
-	arb.r.range_m_s2 = _accel_range_m_s2;
+	arb.scaling = _accel_range_scale;
+	arb.range_m_s2 = _accel_range_m_s2;
 
-	arb.r.temperature_raw = report.temp;
-	arb.r.temperature = (report.temp) / 361.0f + 35.0f;
+	arb.temperature_raw = report.temp;
+	arb.temperature = (report.temp) / 361.0f + 35.0f;
 
-	grb.r.x_raw = report.gyro_x;
-	grb.r.y_raw = report.gyro_y;
-	grb.r.z_raw = report.gyro_z;
+	grb.x_raw = report.gyro_x;
+	grb.y_raw = report.gyro_y;
+	grb.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.r.x = _gyro_filter_x.apply(x_gyro_in_new);
-	grb.r.y = _gyro_filter_y.apply(y_gyro_in_new);
-	grb.r.z = _gyro_filter_z.apply(z_gyro_in_new);
+	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
+	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
+	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
 
-	grb.r.scaling = _gyro_range_scale;
-	grb.r.range_rad_s = _gyro_range_rad_s;
+	grb.scaling = _gyro_range_scale;
+	grb.range_rad_s = _gyro_range_rad_s;
 
-	grb.r.temperature_raw = report.temp;
-	grb.r.temperature = (report.temp) / 361.0f + 35.0f;
+	grb.temperature_raw = report.temp;
+	grb.temperature = (report.temp) / 361.0f + 35.0f;
 
-	_accel_reports->force(arb);
-	_gyro_reports->force(grb);
+	_accel_reports->force(&arb);
+	_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.r);
+	orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	if (_gyro_topic != -1) {
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r);
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
 	}
 
 	/* stop measuring */

src/drivers/ms5611/ms5611.cpp

@@ -115,7 +115,7 @@ protected:
 	struct work_s		_work;
 	unsigned		_measure_ticks;
 
-	RingBuffer<struct baro_report> *_reports;
+	RingBuffer		*_reports;
 
 	bool			_collect_phase;
 	unsigned		_measure_phase;
@@ -241,7 +241,7 @@ MS5611::init()
 	}
 
 	/* allocate basic report buffers */
-	_reports = new RingBuffer<struct baro_report>(2);
+	_reports = new RingBuffer(2, sizeof(baro_report));
 
 	if (_reports == nullptr) {
 		debug("can't get memory for reports");
@@ -285,7 +285,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		 * we are careful to avoid racing with them.
 		 */
 		while (count--) {
-			if (_reports->get(*brp)) {
+			if (_reports->get(brp)) {
 				ret += sizeof(*brp);
 				brp++;
 			}
@@ -327,7 +327,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
 		}
 
 		/* state machine will have generated a report, copy it out */
-		if (_reports->get(*brp))
+		if (_reports->get(brp))
 			ret = sizeof(*brp);
 
 	} while (0);
@@ -664,7 +664,7 @@ MS5611::collect()
 		/* publish it */
 		orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
 
-		if (_reports->force(report)) {
+		if (_reports->force(&report)) {
 			perf_count(_buffer_overflows);
 		}
 

src/drivers/ms5611/ms5611_spi.cpp

@@ -134,6 +134,7 @@ int
 MS5611_SPI::init()
 {
 	int ret;
+	irqstate_t flags;
 
 	ret = SPI::init();
 	if (ret != OK) {
@@ -141,15 +142,23 @@ MS5611_SPI::init()
 		goto out;
 	}
 
+	/* disable interrupts, make this section atomic */
+	flags = irqsave();
 	/* send reset command */
 	ret = _reset();
+	/* re-enable interrupts */
+	irqrestore(flags);
 	if (ret != OK) {
 		debug("reset failed");
 		goto out;
 	}
 
+	/* disable interrupts, make this section atomic */
+	flags = irqsave();
 	/* read PROM */
 	ret = _read_prom();
+	/* re-enable interrupts */
+	irqrestore(flags);
 	if (ret != OK) {
 		debug("prom readout failed");
 		goto out;

src/modules/multirotor_att_control/multirotor_attitude_control.c

@@ -62,15 +62,15 @@
 #include <systemlib/param/param.h>
 #include <drivers/drv_hrt.h>
 
-PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f);
+PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f);
 PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
 //PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f);
 //PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f);
 
-PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f);
+PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f);
 PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f);
+PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f);
 //PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f);
 //PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f);
 

src/modules/multirotor_att_control/multirotor_rate_control.c

@@ -59,14 +59,14 @@
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
-PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
 //PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
 //PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
 
-PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.0f); /* 0.15 F405 Flamewheel */
-PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */
+PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
 PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
 //PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
 //PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f);	/**< roughly < 500 deg/s limit */

src/modules/multirotor_pos_control/multirotor_pos_control_params.c

@@ -54,7 +54,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f);
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
-PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
 PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)