From 62a4f91ed8b050f3b13657484fb4b88c0706f1be Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 22 Mar 2015 21:20:11 -0700
Subject: [PATCH 01/62] sensor drivers: Add integrator stub for delta angles

---
 src/drivers/device/integrator.h | 139 ++++++++++++++++++++++++++++++++
 1 file changed, 139 insertions(+)
 create mode 100644 src/drivers/device/integrator.h

diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h
new file mode 100644
index 0000000000..fc17663e1e
--- /dev/null
+++ b/src/drivers/device/integrator.h
@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file integrator.h
+ *
+ * A resettable integrator
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+ #pragma once
+
+class Integrator {
+public:
+	Integrator(uint64_t auto_reset_interval = 0);
+	virtual ~Integrator();
+
+	/**
+	 * Put an item into the integral.
+	 *
+	 * @param val		Item to put
+	 * @return		true if putting the item triggered an integral reset
+	 *			and the integral should be published
+	 */
+	bool			put(uint64_t timestamp, float val);
+
+	/**
+	 * Get the current integral value
+	 *
+	 * @return		the integral since the last auto-reset
+	 */
+	float			get() { return _integral_auto; }
+
+	/**
+	 * Read from the integral
+	 *
+	 * @param auto_reset	Reset the integral to zero on read
+	 * @return		the integral since the last read-reset
+	 */
+	float			read(bool auto_reset);
+
+private:
+	uint64_t _auto_reset_interval;		/**< the interval after which the content will be published and the integrator reset */
+	uint64_t _last_integration;		/**< timestamp of the last integration step */
+	uint64_t _last_auto;			/**< last auto-announcement of integral value */
+	float _integral_auto;			/**< the integrated value which auto-resets after _auto_reset_interval */
+	float _integral_last_read;		/**< the integrated value since the last read */
+	& _auto_callback;			/**< the function callback for auto-reset */
+
+	/* we don't want this class to be copied */
+	Integrator(const Integrator&);
+	Integrator operator=(const Integrator&);
+};
+
+Integrator(auto_callback = nullptr, uint64_t auto_reset_interval = 0) :
+	_auto_reset_interval(4000 /* 250 Hz */),
+	_last_integration(0),
+	_integral_auto(0.0f),
+	_integral_last_read(0.0f),
+	_auto_callback(auto_callback)
+{
+
+}
+
+~Integrator()
+{
+
+}
+
+bool
+Integrator::put(uint64_t timestamp, float val)
+{
+	if (_last_integration == 0) {
+		/* this is the first item in the integrator */
+		_last_integration = timestamp;
+		_last_auto = timestamp;
+		return false;
+	}
+
+	float dt = (_last_integration - timestamp) / 1000000.0f;
+
+	float i = dt * val;
+
+	_integral_auto += i;
+	_integral_last_read += i;
+
+	_last_integration = timestamp;
+
+	if (_auto_callback &&
+		((_last_integration - _last_auto) > _auto_reset_interval)) {
+		/* call the callback */
+		_auto_callback(timestamp, _integral_auto);
+		_last_auto = timestamp;
+		_integral_auto = 0.0f;
+	}
+}
+
+// XXX this should interpolate
+float
+Integrator::read(bool auto_reset)
+{
+	float val = _integral_read;
+	if (auto_reset) {
+		_integral_read = 0.0f;
+	}
+
+	return val;
+}

From 857fced227574ea9c3e0f94bf95a7936355a2d24 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 21 Jun 2015 18:49:48 +0200
Subject: [PATCH 02/62] Integrator: Improve to 3D case, add coning correction

---
 src/drivers/device/integrator.h | 100 +++++++++++++++++++++++---------
 1 file changed, 71 insertions(+), 29 deletions(-)

diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h
index fc17663e1e..7ebd18760c 100644
--- a/src/drivers/device/integrator.h
+++ b/src/drivers/device/integrator.h
@@ -41,26 +41,30 @@
 
  #pragma once
 
+#include <mathlib/mathlib.h>
+
 class Integrator {
 public:
-	Integrator(uint64_t auto_reset_interval = 0);
+	Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false);
 	virtual ~Integrator();
 
 	/**
 	 * Put an item into the integral.
 	 *
+	 * @param timestamp	Timestamp of the current value
 	 * @param val		Item to put
+	 * @param integral	Current integral in case the integrator did reset, else the value will not be modified
 	 * @return		true if putting the item triggered an integral reset
 	 *			and the integral should be published
 	 */
-	bool			put(uint64_t timestamp, float val);
+	bool			put(hrt_abstime timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt);
 
 	/**
 	 * Get the current integral value
 	 *
 	 * @return		the integral since the last auto-reset
 	 */
-	float			get() { return _integral_auto; }
+	math::Vector<3>		get() { return _integral_auto; }
 
 	/**
 	 * Read from the integral
@@ -68,71 +72,109 @@ public:
 	 * @param auto_reset	Reset the integral to zero on read
 	 * @return		the integral since the last read-reset
 	 */
-	float			read(bool auto_reset);
+	math::Vector<3>		read(bool auto_reset);
+
+	/**
+	 * Get current integral start time
+	 */
+	hrt_abstime		current_integral_start() { return _last_auto; }
 
 private:
-	uint64_t _auto_reset_interval;		/**< the interval after which the content will be published and the integrator reset */
-	uint64_t _last_integration;		/**< timestamp of the last integration step */
-	uint64_t _last_auto;			/**< last auto-announcement of integral value */
-	float _integral_auto;			/**< the integrated value which auto-resets after _auto_reset_interval */
-	float _integral_last_read;		/**< the integrated value since the last read */
-	& _auto_callback;			/**< the function callback for auto-reset */
+	hrt_abstime _auto_reset_interval;		/**< the interval after which the content will be published and the integrator reset */
+	hrt_abstime _last_integration;			/**< timestamp of the last integration step */
+	hrt_abstime _last_auto;				/**< last auto-announcement of integral value */
+	math::Vector<3> _integral_auto;			/**< the integrated value which auto-resets after _auto_reset_interval */
+	math::Vector<3> _integral_read;			/**< the integrated value since the last read */
+	math::Vector<3> _last_val;			/**< previously integrated last value */
+	math::Vector<3> _last_delta;			/**< last local delta */
+	void (*_auto_callback)(hrt_abstime, math::Vector<3>);	/**< the function callback for auto-reset */
+	bool _coning_comp_on;				/**< coning compensation */
 
 	/* we don't want this class to be copied */
 	Integrator(const Integrator&);
 	Integrator operator=(const Integrator&);
 };
 
-Integrator(auto_callback = nullptr, uint64_t auto_reset_interval = 0) :
-	_auto_reset_interval(4000 /* 250 Hz */),
+Integrator::Integrator(hrt_abstime auto_reset_interval, bool coning_compensation) :
+	_auto_reset_interval(auto_reset_interval),
 	_last_integration(0),
-	_integral_auto(0.0f),
-	_integral_last_read(0.0f),
-	_auto_callback(auto_callback)
+	_last_auto(0),
+	_integral_auto(0.0f, 0.0f, 0.0f),
+	_integral_read(0.0f, 0.0f, 0.0f),
+	_last_val(0.0f, 0.0f, 0.0f),
+	_last_delta(0.0f, 0.0f, 0.0f),
+	_auto_callback(nullptr),
+	_coning_comp_on(coning_compensation)
 {
 
 }
 
-~Integrator()
+Integrator::~Integrator()
 {
 
 }
 
 bool
-Integrator::put(uint64_t timestamp, float val)
+Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integral, uint64_t &integral_dt)
 {
+	bool auto_reset = false;
+
 	if (_last_integration == 0) {
 		/* this is the first item in the integrator */
 		_last_integration = timestamp;
 		_last_auto = timestamp;
+		_last_val = val;
 		return false;
 	}
 
-	float dt = (_last_integration - timestamp) / 1000000.0f;
+	// Integrate
+	double dt = (double)(timestamp - _last_integration) / 1000000.0;
+	math::Vector<3> i = (val + _last_val) * dt * 0.5f;
 
-	float i = dt * val;
+	// Apply coning compensation if required
+	if (_coning_comp_on) {
+		// Coning compensation derived by Paul Riseborough and Jonathan Challinger,
+		// following:
+		// Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
+     		// Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
+
+		i += ((_integral_auto + _last_delta * (1.0f / 6.0f)) % i) * 0.5f;
+	}
 
 	_integral_auto += i;
-	_integral_last_read += i;
+	_integral_read += i;
 
 	_last_integration = timestamp;
+	_last_val = val;
+	_last_delta = i;
 
-	if (_auto_callback &&
-		((_last_integration - _last_auto) > _auto_reset_interval)) {
-		/* call the callback */
-		_auto_callback(timestamp, _integral_auto);
+	if ((timestamp - _last_auto) > _auto_reset_interval) {
+		if (_auto_callback) {
+			/* call the callback */
+			_auto_callback(timestamp, _integral_auto);
+		}
+
+		integral = _integral_auto;
+		integral_dt = (timestamp - _last_auto);
+
+		auto_reset = true;
 		_last_auto = timestamp;
-		_integral_auto = 0.0f;
+		_integral_auto(0) = 0.0f;
+		_integral_auto(1) = 0.0f;
+		_integral_auto(2) = 0.0f;
 	}
+
+	return auto_reset;
 }
 
-// XXX this should interpolate
-float
+math::Vector<3>
 Integrator::read(bool auto_reset)
 {
-	float val = _integral_read;
+	math::Vector<3> val = _integral_read;
 	if (auto_reset) {
-		_integral_read = 0.0f;
+		_integral_read(0) = 0.0f;
+		_integral_read(1) = 0.0f;
+		_integral_read(2) = 0.0f;
 	}
 
 	return val;

From d779aa04023e80fcc675e504c4bbf24bcf9da7bc Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 21 Jun 2015 18:50:05 +0200
Subject: [PATCH 03/62] sensor combined message: Add delta angles and
 velocities

---
 msg/sensor_combined.msg | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg
index 876ccd7c4a..0845918f3b 100644
--- a/msg/sensor_combined.msg
+++ b/msg/sensor_combined.msg
@@ -30,11 +30,15 @@ uint64 timestamp			# Timestamp in microseconds since boot, from gyro
 int16[3] gyro_raw			# Raw sensor values of angular velocity
 float32[3] gyro_rad_s			# Angular velocity in radian per seconds
 uint32 gyro_priority			# Sensor priority
+float32[3] gyro_integral_rad		# delta angle in radians
+uint64 gyro_integral_dt			# delta time for gyro integral in us
 uint32 gyro_errcount			# Error counter for gyro 0
 float32 gyro_temp			# Temperature of gyro 0
 
 int16[3] accelerometer_raw		# Raw acceleration in NED body frame
 float32[3] accelerometer_m_s2		# Acceleration in NED body frame, in m/s^2
+float32[3] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
+uint64 accelerometer_integral_dt		# delta time for accel integral in us
 int16 accelerometer_mode			# Accelerometer measurement mode
 float32 accelerometer_range_m_s2		# Accelerometer measurement range in m/s^2
 uint64 accelerometer_timestamp	# Accelerometer timestamp

From 62f37204be6956adc1a26fe84b9dda3b2245802a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 21 Jun 2015 18:50:24 +0200
Subject: [PATCH 04/62] Accel and gyro drivers: Add integrals

---
 src/drivers/drv_accel.h | 4 ++++
 src/drivers/drv_gyro.h  | 4 ++++
 2 files changed, 8 insertions(+)

diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 738b28a6e1..845050edf5 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -57,10 +57,14 @@
  */
 struct accel_report {
 	uint64_t timestamp;
+	uint64_t integral_dt;	/**< integration time */
 	uint64_t error_count;
 	float x;		/**< acceleration in the NED X board axis in m/s^2 */
 	float y;		/**< acceleration in the NED Y board axis in m/s^2 */
 	float z;		/**< acceleration in the NED Z board axis in m/s^2 */
+	float x_integral;	/**< velocity in the NED X board axis in m/s over the integration time frame */
+	float y_integral;	/**< velocity in the NED Y board axis in m/s over the integration time frame */
+	float z_integral;	/**< velocity in the NED Z board axis in m/s over the integration time frame */
 	float temperature;	/**< temperature in degrees celsius */
 	float range_m_s2;	/**< range in m/s^2 (+- this value) */
 	float scaling;
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 8e80413ac9..60426caf90 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -57,10 +57,14 @@
  */
 struct gyro_report {
 	uint64_t timestamp;
+	uint64_t integral_dt;	/**< integration time */
 	uint64_t error_count;
 	float x;		/**< angular velocity in the NED X board axis in rad/s */
 	float y;		/**< angular velocity in the NED Y board axis in rad/s */
 	float z;		/**< angular velocity in the NED Z board axis in rad/s */
+	float x_integral;	/**< delta angle in the NED X board axis in rad/s in the integration time frame */
+	float y_integral;	/**< delta angle in the NED Y board axis in rad/s in the integration time frame */
+	float z_integral;	/**< delta angle in the NED Z board axis in rad/s in the integration time frame */
 	float temperature;	/**< temperature in degrees celcius */
 	float range_rad_s;
 	float scaling;

From c34a5055764d95976cef01f15645e3d053649db2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 21 Jun 2015 18:50:53 +0200
Subject: [PATCH 05/62] MPU6K driver: Add delta angles / velocities including
 coning correction

---
 src/drivers/mpu6000/mpu6000.cpp | 38 +++++++++++++++++++++++++++++----
 1 file changed, 34 insertions(+), 4 deletions(-)

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 7c7bc43d3e..194fdf1669 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -69,6 +69,7 @@
 
 #include <drivers/device/spi.h>
 #include <drivers/device/ringbuffer.h>
+#include <drivers/device/integrator.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -167,10 +168,13 @@
 
 #define MPU6000_ACCEL_DEFAULT_RANGE_G			8
 #define MPU6000_ACCEL_DEFAULT_RATE			1000
+#define MPU6000_ACCEL_MAX_OUTPUT_RATE			280
 #define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
 
 #define MPU6000_GYRO_DEFAULT_RANGE_G			8
 #define MPU6000_GYRO_DEFAULT_RATE			1000
+/* rates need to be the same between accel and gyro */
+#define MPU6000_GYRO_MAX_OUTPUT_RATE			MPU6000_ACCEL_MAX_OUTPUT_RATE
 #define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ		30
 
 #define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ		42
@@ -281,6 +285,9 @@ private:
 	math::LowPassFilter2p	_gyro_filter_y;
 	math::LowPassFilter2p	_gyro_filter_z;
 
+	Integrator		_accel_int;
+	Integrator		_gyro_int;
+
 	enum Rotation		_rotation;
 
 	// this is used to support runtime checking of key
@@ -535,6 +542,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
 	_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_int(1000000 / MPU6000_ACCEL_MAX_OUTPUT_RATE),
+	_gyro_int(1000000 / MPU6000_GYRO_MAX_OUTPUT_RATE, true),
 	_rotation(rotation),
 	_checked_next(0),
 	_in_factory_test(false),
@@ -1759,6 +1768,14 @@ MPU6000::measure()
 	arb.y = _accel_filter_y.apply(y_in_new);
 	arb.z = _accel_filter_z.apply(z_in_new);
 
+	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
+	math::Vector<3> aval_integrated;
+
+	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
+	arb.x_integral = aval_integrated(0);
+	arb.y_integral = aval_integrated(1);
+	arb.z_integral = aval_integrated(2);
+
 	arb.scaling = _accel_range_scale;
 	arb.range_m_s2 = _accel_range_m_s2;
 
@@ -1786,6 +1803,14 @@ MPU6000::measure()
 	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
 	grb.z = _gyro_filter_z.apply(z_gyro_in_new);
 
+	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
+	math::Vector<3> gval_integrated;
+
+	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
+	grb.x_integral = gval_integrated(0);
+	grb.y_integral = gval_integrated(1);
+	grb.z_integral = gval_integrated(2);
+
 	grb.scaling = _gyro_range_scale;
 	grb.range_rad_s = _gyro_range_rad_s;
 
@@ -1796,10 +1821,15 @@ MPU6000::measure()
 	_gyro_reports->force(&grb);
 
 	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
-	_gyro->parent_poll_notify();
+	if (accel_notify) {
+		poll_notify(POLLIN);
+	}
 
-	if (!(_pub_blocked)) {
+	if (gyro_notify) {
+		_gyro->parent_poll_notify();
+	}
+
+	if (accel_notify && !(_pub_blocked)) {
 		/* log the time of this report */
 		perf_begin(_controller_latency_perf);
 		perf_begin(_system_latency_perf);
@@ -1807,7 +1837,7 @@ MPU6000::measure()
 		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
 	}
 
-	if (!(_pub_blocked)) {
+	if (gyro_notify && !(_pub_blocked)) {
 		/* publish it */
 		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
 	}

From f0ff10e40f18b2541bb252b3eca3ea112d685ee7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 21 Jun 2015 18:51:34 +0200
Subject: [PATCH 06/62] EKF: Use driver-level provided and coning-corrected
 delta angles

---
 .../AttitudePositionEstimatorEKF.h            |  6 ++
 .../ekf_att_pos_estimator_main.cpp            | 62 ++++++++++++++-----
 2 files changed, 53 insertions(+), 15 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index 28b23f2069..28abb0818b 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -70,6 +70,7 @@
 
 #include <geo/geo.h>
 #include <systemlib/perf_counter.h>
+#include "estimator_22states.h"
 
 //Forward declaration
 class AttPosEKF;
@@ -168,6 +169,11 @@ private:
     struct vehicle_land_detected_s      _landDetector;
     struct actuator_armed_s             _armed;
 
+    Vector3f lastAngRate;
+    Vector3f lastAccel;
+    hrt_abstime last_accel;
+    hrt_abstime last_mag;
+
     struct gyro_scale               _gyro_offsets[3];
     struct accel_scale              _accel_offsets[3];
     struct mag_scale                _mag_offsets[3];
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index b756e8a234..ba33d72b19 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -155,6 +155,11 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
     _landDetector {},
     _armed {},
 
+    lastAngRate{},
+    lastAccel{},
+    last_accel(0),
+    last_mag(0),
+
     _gyro_offsets({}),
     _accel_offsets({}),
     _mag_offsets({}),
@@ -541,8 +546,6 @@ void AttitudePositionEstimatorEKF::task_main()
 	orb_set_interval(_vstatus_sub, 200);
 
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
-	/* XXX remove this!, BUT increase the data buffer size! */
-	orb_set_interval(_sensor_combined_sub, 9);
 
 	/* sets also parameters in the EKF object */
 	parameters_update();
@@ -836,9 +839,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	_att.pitch = euler(1);
 	_att.yaw = euler(2);
 
-	_att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt;
-	_att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt;
-	_att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt;
+	_att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
+	_att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
+	_att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
 
 	// gyro offsets
 	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
@@ -1126,7 +1129,7 @@ int AttitudePositionEstimatorEKF::start()
 	/* start the task */
 	_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 40,
+					 SCHED_PRIORITY_MAX - 20,
 					 7500,
 					 (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
 					 nullptr);
@@ -1218,15 +1221,10 @@ void AttitudePositionEstimatorEKF::pollData()
 	}
 
 	//Update Gyro and Accelerometer
-	static Vector3f lastAngRate;
-	static Vector3f lastAccel;
 	bool accel_updated = false;
 
 	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
-	static hrt_abstime last_accel = 0;
-	static hrt_abstime last_mag = 0;
-
 	if (last_accel != _sensor_combined.accelerometer_timestamp) {
 		accel_updated = true;
 
@@ -1244,6 +1242,38 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
 
+	// XXX this is for assessing the filter performance
+	// leave this in as long as larger improvements are still being made.
+	#if 0
+
+	float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f;
+	float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f;
+
+	static unsigned dtoverflow5 = 0;
+	static unsigned dtoverflow10 = 0;
+	static hrt_abstime lastprint = 0;
+
+	if (hrt_elapsed_time(&lastprint) > 1000000) {
+		warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
+			(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
+			dtoverflow5, dtoverflow10);
+
+		warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f",
+		(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
+		(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z);
+
+		lastprint = hrt_absolute_time();
+	}
+
+	if (deltaT > 0.005f) {
+		dtoverflow5++;
+	}
+
+	if (deltaT > 0.01f) {
+		dtoverflow10++;
+	}
+	#endif
+
 	/* guard against too large deltaT's */
 	if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
 		deltaT = 0.01f;
@@ -1323,10 +1353,12 @@ void AttitudePositionEstimatorEKF::pollData()
 		_accel_valid = true;
 	}
 
-	_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
-	lastAngRate = _ekf->angRate;
-	_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
-	lastAccel = _ekf->accel;
+	_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
+	_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
+	_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[2];
+	_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[0];
+	_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[1];
+	_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[2];
 
 	if (last_mag != _sensor_combined.magnetometer_timestamp) {
 		_newDataMag = true;

From fa62841ff743fef7d35d8e00727276bd933c44ba Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 21 Jun 2015 18:52:03 +0200
Subject: [PATCH 07/62] sensors app: Copy sensor data from 1st accel / gyro
 also into integral fields

---
 src/modules/sensors/sensors.cpp | 21 ++++++++++++++++++---
 1 file changed, 18 insertions(+), 3 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 274f10c3e9..d8ca75e806 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1048,6 +1048,15 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		raw.accelerometer_m_s2[1] = vect(1);
 		raw.accelerometer_m_s2[2] = vect(2);
 
+		math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
+		vect_int = _board_rotation * vect_int;
+
+		raw.accelerometer_integral_m_s[0] = vect_int(0);
+		raw.accelerometer_integral_m_s[1] = vect_int(1);
+		raw.accelerometer_integral_m_s[2] = vect_int(2);
+
+		raw.accelerometer_integral_dt = accel_report.integral_dt;
+
 		raw.accelerometer_raw[0] = accel_report.x_raw;
 		raw.accelerometer_raw[1] = accel_report.y_raw;
 		raw.accelerometer_raw[2] = accel_report.z_raw;
@@ -1125,6 +1134,15 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		raw.gyro_rad_s[1] = vect(1);
 		raw.gyro_rad_s[2] = vect(2);
 
+		math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
+		vect_int = _board_rotation * vect_int;
+
+		raw.gyro_integral_rad[0] = vect_int(0);
+		raw.gyro_integral_rad[1] = vect_int(1);
+		raw.gyro_integral_rad[2] = vect_int(2);
+
+		raw.gyro_integral_dt = gyro_report.integral_dt;
+
 		raw.gyro_raw[0] = gyro_report.x_raw;
 		raw.gyro_raw[1] = gyro_report.y_raw;
 		raw.gyro_raw[2] = gyro_report.z_raw;
@@ -2169,9 +2187,6 @@ Sensors::task_main()
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vcontrol_mode_sub, 200);
 
-	/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
-	orb_set_interval(_gyro_sub, 4);
-
 	/*
 	 * do advertisements
 	 */

From 808d8520b52464de7fdbaba2edb0d60f3ac8ebe0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 27 Jun 2015 15:38:23 +0200
Subject: [PATCH 08/62] L3GD20 driver: Add delta angle support

---
 src/drivers/l3gd20/l3gd20.cpp | 39 ++++++++++++++++++++++++-----------
 1 file changed, 27 insertions(+), 12 deletions(-)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 7dcd5a0004..43a24359eb 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -66,6 +66,7 @@
 #include <drivers/device/spi.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/device/ringbuffer.h>
+#include <drivers/device/integrator.h>
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -175,6 +176,7 @@ static const int ERROR = -1;
 
 #define L3GD20_DEFAULT_RATE			760
 #define L3G4200D_DEFAULT_RATE			800
+#define L3GD20_MAX_OUTPUT_RATE			280
 #define L3GD20_DEFAULT_RANGE_DPS		2000
 #define L3GD20_DEFAULT_FILTER_FREQ		30
 #define L3GD20_TEMP_OFFSET_CELSIUS		40
@@ -256,6 +258,8 @@ private:
 	math::LowPassFilter2p	_gyro_filter_y;
 	math::LowPassFilter2p	_gyro_filter_z;
 
+	Integrator		_gyro_int;
+
 	/* true if an L3G4200D is detected */
 	bool	_is_l3g4200d;
 
@@ -427,6 +431,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_gyro_int(1000000 / L3GD20_MAX_OUTPUT_RATE, true),
 	_is_l3g4200d(false),
 	_rotation(rotation),
 	_checked_next(0)
@@ -1029,13 +1034,21 @@ L3GD20::measure()
 	// apply user specified rotation
 	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
 
-	report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
+	float xin = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
+	float yin = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
+	float zin = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
 
-	report.x = _gyro_filter_x.apply(report.x);
-	report.y = _gyro_filter_y.apply(report.y);
-	report.z = _gyro_filter_z.apply(report.z);
+	report.x = _gyro_filter_x.apply(xin);
+	report.y = _gyro_filter_y.apply(yin);
+	report.z = _gyro_filter_z.apply(zin);
+
+	math::Vector<3> gval(xin, yin, zin);
+	math::Vector<3> gval_integrated;
+
+	bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt);
+	report.x_integral = gval_integrated(0);
+	report.y_integral = gval_integrated(1);
+	report.z_integral = gval_integrated(2);
 
 	report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
 
@@ -1044,13 +1057,15 @@ L3GD20::measure()
 
 	_reports->force(&report);
 
-	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
+	if (gyro_notify) {
+		/* notify anyone waiting for data */
+		poll_notify(POLLIN);
 
-	/* publish for subscribers */
-	if (!(_pub_blocked)) {
-		/* publish it */
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+		/* publish for subscribers */
+		if (!(_pub_blocked)) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+		}
 	}
 
 	_read++;

From c66517bcd6d5b4f77a212f240b27c78ec4ab676c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 27 Jun 2015 15:38:55 +0200
Subject: [PATCH 09/62] LSM303D driver: Add delta angle support

---
 src/drivers/lsm303d/lsm303d.cpp | 23 +++++++++++++++++++----
 1 file changed, 19 insertions(+), 4 deletions(-)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 52e514a3e1..6b8c7bc60a 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -65,6 +65,7 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
 #include <drivers/device/ringbuffer.h>
+#include <drivers/device/integrator.h>
 #include <drivers/drv_tone_alarm.h>
 
 #include <board_config.h>
@@ -206,6 +207,7 @@ static const int ERROR = -1;
 #define LSM303D_ACCEL_DEFAULT_RATE			800
 #define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ	50
 #define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ	30
+#define LSM303D_ACCEL_MAX_OUTPUT_RATE			280
 
 #define LSM303D_MAG_DEFAULT_RANGE_GA			2
 #define LSM303D_MAG_DEFAULT_RATE			100
@@ -308,6 +310,8 @@ private:
 	math::LowPassFilter2p	_accel_filter_y;
 	math::LowPassFilter2p	_accel_filter_z;
 
+	Integrator		_accel_int;
+
 	enum Rotation		_rotation;
 
 	// values used to
@@ -577,6 +581,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_accel_int(1000000 / LSM303D_ACCEL_MAX_OUTPUT_RATE, true),
 	_rotation(rotation),
 	_constant_accel_count(0),
 	_last_temperature(0),
@@ -1575,17 +1580,27 @@ LSM303D::measure()
 	accel_report.y = _accel_filter_y.apply(y_in_new);
 	accel_report.z = _accel_filter_z.apply(z_in_new);
 
+	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
+	math::Vector<3> aval_integrated;
+
+	bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
+	accel_report.x_integral = aval_integrated(0);
+	accel_report.y_integral = aval_integrated(1);
+	accel_report.z_integral = aval_integrated(2);
+
 	accel_report.scaling = _accel_range_scale;
 	accel_report.range_m_s2 = _accel_range_m_s2;
 
 	_accel_reports->force(&accel_report);
 
 	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
+	if (accel_notify) {
+		poll_notify(POLLIN);
 
-	if (!(_pub_blocked)) {
-		/* publish it */
-		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		if (!(_pub_blocked)) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		}
 	}
 
 	_accel_read++;

From 0275d770aeb2ae35cf2eb7e4e38d8cc5bdd744e2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 22 Mar 2015 21:20:11 -0700
Subject: [PATCH 10/62] sensor drivers: Add integrator stub for delta angles

---
 src/drivers/device/integrator.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h
index 7ebd18760c..712bc241f6 100644
--- a/src/drivers/device/integrator.h
+++ b/src/drivers/device/integrator.h
@@ -39,7 +39,7 @@
  * @author Lorenz Meier <lorenz@px4.io>
  */
 
- #pragma once
+#pragma once
 
 #include <mathlib/mathlib.h>
 

From 99146ea6c3ec3460d90dc179452b26ddd0e8f76e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 21 Jun 2015 18:49:48 +0200
Subject: [PATCH 11/62] Integrator: Improve to 3D case, add coning correction

---
 src/drivers/device/integrator.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h
index 712bc241f6..6dc886de96 100644
--- a/src/drivers/device/integrator.h
+++ b/src/drivers/device/integrator.h
@@ -43,6 +43,8 @@
 
 #include <mathlib/mathlib.h>
 
+#include <mathlib/mathlib.h>
+
 class Integrator {
 public:
 	Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false);

From 1198a79a71286098aa1f7ed6f0b1617086ab128d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 21 Apr 2015 08:32:35 +0200
Subject: [PATCH 12/62] Data validation: Initial concept of RMS + timeout based
 data validators and validation groups of N sensors

---
 src/lib/ecl/validation/data_validator.h       | 175 ++++++++++++++++++
 src/lib/ecl/validation/data_validator_group.h | 149 +++++++++++++++
 2 files changed, 324 insertions(+)
 create mode 100644 src/lib/ecl/validation/data_validator.h
 create mode 100644 src/lib/ecl/validation/data_validator_group.h

diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h
new file mode 100644
index 0000000000..12e3606f0e
--- /dev/null
+++ b/src/lib/ecl/validation/data_validator.h
@@ -0,0 +1,175 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file data_validator.h
+ *
+ * A data validation class to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#pragma once
+
+class DataValidator {
+public:
+	DataValidator(DataValidator *prev_sibling = nullptr);
+	virtual ~DataValidator();
+
+	/**
+	 * Put an item into the validator.
+	 *
+	 * @param val		Item to put
+	 */
+	void			put(uint64_t timestamp, float val[3], uint64_t error_count);
+
+	/**
+	 * Get the next sibling in the group
+	 *
+	 * @return		the next sibling
+	 */
+	DataValidator*		sibling() { return _sibling; }
+
+	/**
+	 * Get the confidence of this validator
+	 * @return		the confidence between 0 and 1
+	 */
+	float			confidence(uint64_t timestamp);
+
+	/**
+	 * Get the value of this validator
+	 * @return		the stored value
+	 */
+	float*			value() { return _value; }
+
+	/**
+	 * Print the validator value
+	 *
+	 */
+	void			print();
+
+private:
+	static const unsigned _dimensions = 3;
+	uint64_t _time_last;			/**< last timestamp */
+	uint64_t _timeout_interval;		/**< interval in which the datastream times out in us */
+	uint64_t _event_count;			/**< total data counter */
+	uint64_t _error_count;			/**< error count */
+	float _mean[_dimensions];				/**< mean of value */
+	float _lp[3];				/**< low pass value */
+	float _M2[3];				/**< RMS component value */
+	float _rms[3];				/**< root mean square error */
+	float _value[3];			/**< last value */
+	DataValidator *_sibling;		/**< sibling in the group */
+	const unsigned _noreturn_errcount = 1000;	/**< if the error count reaches this value, return sensor as invalid */
+
+	/* we don't want this class to be copied */
+	DataValidator(const DataValidator&);
+	DataValidator operator=(const DataValidator&);
+};
+
+DataValidator::DataValidator(DataValidator *prev_sibling) :
+	_time_last(0),
+	_timeout_interval(50000),
+	_event_count(0),
+	_error_count(0),
+	_mean{0.0f},
+	_lp{0.0f},
+	_M2{0.0f},
+	_rms{0.0f},
+	_value{0.0f},
+	_sibling(prev_sibling)
+{
+
+}
+
+DataValidator::~DataValidator()
+{
+
+}
+
+void
+DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count)
+{
+	_event_count++;
+	_error_count = error_count;
+
+	for (unsigned i = 0; i < _dimensions; i++) {
+		if (_time_last == 0) {
+			_mean[i] = val[i];
+			_lp[i] = val[i];
+			_M2[i] = 0;
+		} else {
+			float lp_val = val[i] - _lp[i];
+
+			float delta_val = lp_val - _mean[i];
+			_mean[i] += delta_val / _event_count;
+			_M2[i] += delta_val * (lp_val - _mean[i]);
+			_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
+		}
+
+		// XXX replace with better filter, make it auto-tune to update rate
+		_lp[i] = _lp[i] * 0.95f + val[i] * 0.05f;
+
+		_value[i] = val[i];
+	}
+
+	_time_last = timestamp;
+}
+
+float
+DataValidator::confidence(uint64_t timestamp)
+{
+	/* check error count limit */
+	if (_error_count > _noreturn_errcount) {
+		return 0.0f;
+	}
+
+	/* timed out - that's it */
+	if (timestamp - _time_last > _timeout_interval) {
+		return 0.0f;
+	}
+
+	// XXX work out scaling between confidence and RMS
+	//return _rms / _mean;
+
+	return 1.0f;
+}
+
+void
+DataValidator::print()
+{
+	for (unsigned i = 0; i < _dimensions; i++) {
+		printf("\tmean: %8.4f lp: %8.4f RMS: %8.4f\n",
+			(double)(_lp[i] + _mean[i]), (double)_lp[i], (double)_rms[i]);
+	}
+}
diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h
new file mode 100644
index 0000000000..561768836c
--- /dev/null
+++ b/src/lib/ecl/validation/data_validator_group.h
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file data_validator_group.h
+ *
+ * A data validation group to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#pragma once
+
+#include "data_validator.h"
+
+class DataValidatorGroup {
+public:
+	DataValidatorGroup(unsigned siblings);
+	virtual ~DataValidatorGroup();
+
+	/**
+	 * Put an item into the validator group.
+	 *
+	 * @param x		X Item to put
+	 * @param y		Y Item to put
+	 * @param z		Z Item to put
+	 */
+	void			put(unsigned index, uint64_t timestamp,
+					float val[3], uint64_t error_count);
+
+	/**
+	 * Get the best data triplet of the group
+	 *
+	 * @return		pointer to the array of best values
+	 */
+	float*			get_best(uint64_t timestamp, int *index);
+
+	/**
+	 * Print the validator value
+	 *
+	 */
+	void			print();
+
+private:
+	DataValidator *_first;		/**< sibling in the group */
+
+	/* we don't want this class to be copied */
+	DataValidatorGroup(const DataValidatorGroup&);
+	DataValidatorGroup operator=(const DataValidatorGroup&);
+};
+
+DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
+	_first(nullptr)
+{
+	DataValidator *next = _first;
+
+	for (unsigned i = 0; i < siblings; i++) {
+		next = new DataValidator(next);
+	}
+}
+
+DataValidatorGroup::~DataValidatorGroup()
+{
+
+}
+
+void
+DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count)
+{
+	DataValidator *next = _first;
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		if (i == index) {
+			next->put(timestamp, val, error_count);
+			break;
+		}
+		next = next->sibling();
+		i++;
+	}
+}
+
+float*
+DataValidatorGroup::get_best(uint64_t timestamp, int *index)
+{
+	DataValidator *next = _first;
+
+	// XXX This should eventually also include voting
+	float max_confidence = 0.0f;
+	int max_index = -1;
+	DataValidator *best = nullptr;
+
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		float confidence = next->confidence(timestamp);
+		if (confidence > max_confidence) {
+			max_index = i;
+			max_confidence = confidence;
+			best = next;
+		}
+		next = next->sibling();
+		i++;
+	}
+
+	*index = max_index;
+	return (best) ? best->value() : nullptr;
+}
+
+void
+DataValidatorGroup::print()
+{
+	DataValidator *next = _first;
+
+	while (next != nullptr) {
+		next->print();
+		next = next->sibling();
+	}
+}

From c9d0b54a6f102a08fbb77cfa4955cc9317a85fad Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 21 Jun 2015 22:58:18 +0200
Subject: [PATCH 13/62] Attitude estimator Q: Integrate data validation lib

---
 .../attitude_estimator_q_main.cpp             | 41 ++++++++++++++-----
 1 file changed, 31 insertions(+), 10 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 138a8c7e5c..afb4984367 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -61,6 +61,7 @@
 
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
+#include <lib/ecl/validation/data_validator_group.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -142,6 +143,11 @@ private:
 	vehicle_global_position_s _gpos = {};
 	Vector<3>	_vel_prev;
 	Vector<3>	_pos_acc;
+
+	DataValidatorGroup _voter_gyro;
+	DataValidatorGroup _voter_accel;
+	DataValidatorGroup _voter_mag;
+
 	hrt_abstime _vel_prev_t = 0;
 
 	bool		_inited = false;
@@ -160,7 +166,11 @@ private:
 };
 
 
-AttitudeEstimatorQ::AttitudeEstimatorQ() {
+AttitudeEstimatorQ::AttitudeEstimatorQ() :
+	_voter_gyro(3),
+	_voter_accel(3),
+	_voter_mag(3)
+{
 	_params_handles.w_acc		= param_find("ATT_W_ACC");
 	_params_handles.w_mag		= param_find("ATT_W_MAG");
 	_params_handles.w_gyro_bias	= param_find("ATT_W_GYRO_BIAS");
@@ -173,7 +183,8 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() {
 /**
  * Destructor, also kills task.
  */
-AttitudeEstimatorQ::~AttitudeEstimatorQ() {
+AttitudeEstimatorQ::~AttitudeEstimatorQ()
+{
 	if (_control_task != -1) {
 		/* task wakes up every 100ms or so at the longest */
 		_task_should_exit = true;
@@ -196,7 +207,8 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() {
 	attitude_estimator_q::instance = nullptr;
 }
 
-int AttitudeEstimatorQ::start() {
+int AttitudeEstimatorQ::start()
+{
 	ASSERT(_control_task == -1);
 
 	/* start the task */
@@ -215,12 +227,13 @@ int AttitudeEstimatorQ::start() {
 	return OK;
 }
 
-void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) {
+void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
+{
 	attitude_estimator_q::instance->task_main();
 }
 
-void AttitudeEstimatorQ::task_main() {
-
+void AttitudeEstimatorQ::task_main()
+{
 	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -250,11 +263,19 @@ void AttitudeEstimatorQ::task_main() {
 		// Update sensors
 		sensor_combined_s sensors;
 		if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
-			_gyro.set(sensors.gyro_rad_s);
-			_accel.set(sensors.accelerometer_m_s2);
-			_mag.set(sensors.magnetometer_ga);
+			// Feed validator with recent sensor data
+			_voter_gyro.put(0, sensors.timestamp, sensors.gyro_rad_s, sensors.gyro_errcount);
+			_voter_gyro.put(1, sensors.timestamp, sensors.gyro1_rad_s, sensors.gyro1_errcount);
+			_voter_accel.put(0, sensors.timestamp, sensors.accelerometer_m_s2, sensors.accelerometer_errcount);
+			_voter_accel.put(1, sensors.timestamp, sensors.accelerometer1_m_s2, sensors.accelerometer_errcount);
+			_voter_mag.put(0, sensors.timestamp, sensors.magnetometer_ga, sensors.magnetometer_errcount);
+			_voter_mag.put(1, sensors.timestamp, sensors.magnetometer1_ga, sensors.magnetometer1_errcount);
 
-			_data_good = true;
+			// Get best measurement values
+			int best_gyro, best_accel, best_mag;
+			_gyro.set(_voter_gyro.get_best(sensors.timestamp, &best_gyro));
+			_accel.set(_voter_accel.get_best(sensors.timestamp, &best_accel));
+			_mag.set(_voter_mag.get_best(sensors.timestamp, &best_mag));
 		}
 
 		bool gpos_updated;

From 1148ba4a77f11a8f669ee543a1586ef34d95f046 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 21 Jun 2015 23:05:08 +0200
Subject: [PATCH 14/62] ECL: Factor in error counts from sensor

---
 src/lib/ecl/validation/data_validator.h       | 10 ++++++++--
 src/lib/ecl/validation/data_validator_group.h |  5 ++++-
 2 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h
index 12e3606f0e..4bf9878c52 100644
--- a/src/lib/ecl/validation/data_validator.h
+++ b/src/lib/ecl/validation/data_validator.h
@@ -66,6 +66,12 @@ public:
 	 */
 	float			confidence(uint64_t timestamp);
 
+	/**
+	 * Get the error count of this validator
+	 * @return		the error count
+	 */
+	uint64_t		error_count() { return _error_count; }
+
 	/**
 	 * Get the value of this validator
 	 * @return		the stored value
@@ -118,10 +124,10 @@ DataValidator::~DataValidator()
 }
 
 void
-DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count)
+DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in)
 {
 	_event_count++;
-	_error_count = error_count;
+	_error_count = error_count_in;
 
 	for (unsigned i = 0; i < _dimensions; i++) {
 		if (_time_last == 0) {
diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h
index 561768836c..f68a891532 100644
--- a/src/lib/ecl/validation/data_validator_group.h
+++ b/src/lib/ecl/validation/data_validator_group.h
@@ -118,15 +118,18 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 	// XXX This should eventually also include voting
 	float max_confidence = 0.0f;
 	int max_index = -1;
+	uint64_t min_error_count = 30000;
 	DataValidator *best = nullptr;
 
 	unsigned i = 0;
 
 	while (next != nullptr) {
 		float confidence = next->confidence(timestamp);
-		if (confidence > max_confidence) {
+		if (confidence > max_confidence &&
+			next->error_count() < min_error_count) {
 			max_index = i;
 			max_confidence = confidence;
+			min_error_count = next->error_count();
 			best = next;
 		}
 		next = next->sibling();

From 7feb25bf58a907560697a7ca4aec4a629f6889b5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 27 Jun 2015 16:02:03 +0200
Subject: [PATCH 15/62] Validator: Reject data with no timestamp

---
 src/lib/ecl/validation/data_validator.h | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h
index 4bf9878c52..c006d7c50c 100644
--- a/src/lib/ecl/validation/data_validator.h
+++ b/src/lib/ecl/validation/data_validator.h
@@ -155,6 +155,11 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in)
 float
 DataValidator::confidence(uint64_t timestamp)
 {
+	/* check if we have any data */
+	if (_time_last == 0) {
+		return 0.0f;
+	}
+
 	/* check error count limit */
 	if (_error_count > _noreturn_errcount) {
 		return 0.0f;

From 822b818c3ccee4faa5808b920bf512d07db5b900 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 27 Jun 2015 16:05:13 +0200
Subject: [PATCH 16/62] Q estimator: Feed validator with right timestamp

---
 .../attitude_estimator_q/attitude_estimator_q_main.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index afb4984367..714ca6cdf1 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -265,11 +265,11 @@ void AttitudeEstimatorQ::task_main()
 		if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
 			// Feed validator with recent sensor data
 			_voter_gyro.put(0, sensors.timestamp, sensors.gyro_rad_s, sensors.gyro_errcount);
-			_voter_gyro.put(1, sensors.timestamp, sensors.gyro1_rad_s, sensors.gyro1_errcount);
-			_voter_accel.put(0, sensors.timestamp, sensors.accelerometer_m_s2, sensors.accelerometer_errcount);
-			_voter_accel.put(1, sensors.timestamp, sensors.accelerometer1_m_s2, sensors.accelerometer_errcount);
-			_voter_mag.put(0, sensors.timestamp, sensors.magnetometer_ga, sensors.magnetometer_errcount);
-			_voter_mag.put(1, sensors.timestamp, sensors.magnetometer1_ga, sensors.magnetometer1_errcount);
+			_voter_gyro.put(1, sensors.gyro1_timestamp, sensors.gyro1_rad_s, sensors.gyro1_errcount);
+			_voter_accel.put(0, sensors.accelerometer_timestamp, sensors.accelerometer_m_s2, sensors.accelerometer_errcount);
+			_voter_accel.put(1, sensors.accelerometer1_timestamp, sensors.accelerometer1_m_s2, sensors.accelerometer1_errcount);
+			_voter_mag.put(0, sensors.magnetometer_timestamp, sensors.magnetometer_ga, sensors.magnetometer_errcount);
+			_voter_mag.put(1, sensors.magnetometer1_timestamp, sensors.magnetometer1_ga, sensors.magnetometer1_errcount);
 
 			// Get best measurement values
 			int best_gyro, best_accel, best_mag;

From eea2f61f02bb7ea092770a956f64443f1f190496 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 27 Jun 2015 16:05:38 +0200
Subject: [PATCH 17/62] Retire attitude-only EKF due to performance and memory
 consumption considerations

---
 makefiles/nuttx/config_px4fmu-v2_default.mk | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/makefiles/nuttx/config_px4fmu-v2_default.mk b/makefiles/nuttx/config_px4fmu-v2_default.mk
index d1d19927fd..f22589076f 100644
--- a/makefiles/nuttx/config_px4fmu-v2_default.mk
+++ b/makefiles/nuttx/config_px4fmu-v2_default.mk
@@ -79,7 +79,7 @@ MODULES 	+= modules/land_detector
 #
 # Estimation modules (EKF/ SO3 / other filters)
 #
-# Too high RAM usage due to static allocations
+# Removed from build due to large static allocations
 #MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/attitude_estimator_q
 MODULES		+= modules/ekf_att_pos_estimator

From b09630af03aa70d048c46b0a5f4c961bd4a81243 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 28 Jun 2015 13:25:24 +0200
Subject: [PATCH 18/62] Add vibration fields to vehicle attitude message

---
 msg/vehicle_attitude.msg | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg
index 63879149ad..0ee90cd61d 100644
--- a/msg/vehicle_attitude.msg
+++ b/msg/vehicle_attitude.msg
@@ -10,6 +10,9 @@ float32 yawspeed	# Yaw body angular rate (rad/s, x forward/y right/z down)
 float32 rollacc		# Roll angular accelration (rad/s^2, x forward/y right/z down)
 float32 pitchacc	# Pitch angular acceleration (rad/s^2, x forward/y right/z down)
 float32 yawacc		# Yaw angular acceleration (rad/s^2, x forward/y right/z down)
+float32 rate_vibration	# Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels.
+float32 accel_vibration	# Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels.
+float32 mag_vibration	# Value between 0 and 1 indicating vibration. A value of 0 means no vibration, a value of 1 indicates unbearable vibration levels.
 float32[3] rate_offsets	# Offsets of the body angular rates from zero
 float32[9] R		# Rotation matrix, body to world, (Tait-Bryan, NED)
 float32[4] q		# Quaternion (NED)

From e8ce2234f3759c994e920a07c5c89927c969b14b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 28 Jun 2015 13:33:40 +0200
Subject: [PATCH 19/62] Data validator: Better vibration level estimation,
 better failover

---
 src/lib/ecl/validation/data_validator.h       | 42 +++++++--
 src/lib/ecl/validation/data_validator_group.h | 93 ++++++++++++++++++-
 2 files changed, 122 insertions(+), 13 deletions(-)

diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h
index c006d7c50c..26d55af11e 100644
--- a/src/lib/ecl/validation/data_validator.h
+++ b/src/lib/ecl/validation/data_validator.h
@@ -73,11 +73,17 @@ public:
 	uint64_t		error_count() { return _error_count; }
 
 	/**
-	 * Get the value of this validator
+	 * Get the values of this validator
 	 * @return		the stored value
 	 */
 	float*			value() { return _value; }
 
+	/**
+	 * Get the RMS values of this validator
+	 * @return		the stored RMS
+	 */
+	float*			rms() { return _rms; }
+
 	/**
 	 * Print the validator value
 	 *
@@ -95,8 +101,10 @@ private:
 	float _M2[3];				/**< RMS component value */
 	float _rms[3];				/**< root mean square error */
 	float _value[3];			/**< last value */
+	float _value_equal_count;		/**< equal values in a row */
 	DataValidator *_sibling;		/**< sibling in the group */
-	const unsigned _noreturn_errcount = 1000;	/**< if the error count reaches this value, return sensor as invalid */
+	const unsigned NORETURN_ERRCOUNT = 1000;	/**< if the error count reaches this value, return sensor as invalid */
+	const unsigned VALUE_EQUAL_COUNT_MAX = 100;	/**< if the sensor value is the same (accumulated also between axes) this many times, flag it */
 
 	/* we don't want this class to be copied */
 	DataValidator(const DataValidator&);
@@ -113,6 +121,7 @@ DataValidator::DataValidator(DataValidator *prev_sibling) :
 	_M2{0.0f},
 	_rms{0.0f},
 	_value{0.0f},
+	_value_equal_count(0),
 	_sibling(prev_sibling)
 {
 
@@ -131,7 +140,7 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in)
 
 	for (unsigned i = 0; i < _dimensions; i++) {
 		if (_time_last == 0) {
-			_mean[i] = val[i];
+			_mean[i] = 0;
 			_lp[i] = val[i];
 			_M2[i] = 0;
 		} else {
@@ -141,10 +150,16 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in)
 			_mean[i] += delta_val / _event_count;
 			_M2[i] += delta_val * (lp_val - _mean[i]);
 			_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
+
+			if (fabsf(_value[i] - val[i]) < 0.000001f) {
+				_value_equal_count++;
+			} else {
+				_value_equal_count = 0;
+			}
 		}
 
 		// XXX replace with better filter, make it auto-tune to update rate
-		_lp[i] = _lp[i] * 0.95f + val[i] * 0.05f;
+		_lp[i] = _lp[i] * 0.5f + val[i] * 0.5f;
 
 		_value[i] = val[i];
 	}
@@ -161,7 +176,12 @@ DataValidator::confidence(uint64_t timestamp)
 	}
 
 	/* check error count limit */
-	if (_error_count > _noreturn_errcount) {
+	if (_error_count > NORETURN_ERRCOUNT) {
+		return 0.0f;
+	}
+
+	/* we got the exact same sensor value N times in a row */
+	if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
 		return 0.0f;
 	}
 
@@ -170,17 +190,19 @@ DataValidator::confidence(uint64_t timestamp)
 		return 0.0f;
 	}
 
-	// XXX work out scaling between confidence and RMS
-	//return _rms / _mean;
-
 	return 1.0f;
 }
 
 void
 DataValidator::print()
 {
+	if (_time_last == 0) {
+		printf("\tno data\n");
+		return;
+	}
+
 	for (unsigned i = 0; i < _dimensions; i++) {
-		printf("\tmean: %8.4f lp: %8.4f RMS: %8.4f\n",
-			(double)(_lp[i] + _mean[i]), (double)_lp[i], (double)_rms[i]);
+		printf("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n",
+			(double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]);
 	}
 }
diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h
index f68a891532..6b420b3f27 100644
--- a/src/lib/ecl/validation/data_validator_group.h
+++ b/src/lib/ecl/validation/data_validator_group.h
@@ -65,6 +65,20 @@ public:
 	 */
 	float*			get_best(uint64_t timestamp, int *index);
 
+	/**
+	 * Get the RMS / vibration factor
+	 *
+	 * @return		float value representing the RMS, which a valid indicator for vibration
+	 */
+	float			get_vibration_factor(uint64_t timestamp);
+
+	/**
+	 * Get the number of failover events
+	 *
+	 * @return		the number of failovers
+	 */
+	unsigned		failover_count();
+
 	/**
 	 * Print the validator value
 	 *
@@ -73,6 +87,10 @@ public:
 
 private:
 	DataValidator *_first;		/**< sibling in the group */
+	int _curr_best;		/**< currently best index */
+	int _prev_best;		/**< the previous best index */
+	uint64_t _first_failover_time;	/**< timestamp where the first failover occured or zero if none occured */
+	unsigned _toggle_count;		/**< number of back and forth switches between two sensors */
 
 	/* we don't want this class to be copied */
 	DataValidatorGroup(const DataValidatorGroup&);
@@ -80,13 +98,19 @@ private:
 };
 
 DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
-	_first(nullptr)
+	_first(nullptr),
+	_curr_best(-1),
+	_prev_best(-1),
+	_first_failover_time(0),
+	_toggle_count(0)
 {
 	DataValidator *next = _first;
 
 	for (unsigned i = 0; i < siblings; i++) {
 		next = new DataValidator(next);
 	}
+
+	_first = next;
 }
 
 DataValidatorGroup::~DataValidatorGroup()
@@ -116,6 +140,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 	DataValidator *next = _first;
 
 	// XXX This should eventually also include voting
+	int pre_check_best = _curr_best;
 	float max_confidence = 0.0f;
 	int max_index = -1;
 	uint64_t min_error_count = 30000;
@@ -125,28 +150,90 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 
 	while (next != nullptr) {
 		float confidence = next->confidence(timestamp);
-		if (confidence > max_confidence &&
-			next->error_count() < min_error_count) {
+		if (confidence > max_confidence ||
+			(fabsf(confidence - max_confidence) < 0.01f && next->error_count() < min_error_count)) {
 			max_index = i;
 			max_confidence = confidence;
 			min_error_count = next->error_count();
 			best = next;
 		}
+
 		next = next->sibling();
 		i++;
 	}
 
+	/* the current best sensor is not matching the previous best sensor */
+	if (max_index != _curr_best) {
+
+		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
+		if (_curr_best < 0) {
+			_prev_best = max_index;
+		} else {
+			/* we were initialized before, this is a real failsafe */
+			_prev_best = pre_check_best;
+			_toggle_count++;
+
+			/* if this is the first time, log when we failed */
+			if (_first_failover_time == 0) {
+				_first_failover_time = timestamp;
+			}
+		}
+
+		/* for all cases we want to keep a record of the best index */
+		_curr_best = max_index;
+	}
 	*index = max_index;
 	return (best) ? best->value() : nullptr;
 }
 
+float
+DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
+{
+	DataValidator *next = _first;
+
+	float vibe = 0.0f;
+
+	/* find the best RMS value of a non-timed out sensor */
+	while (next != nullptr) {
+
+		if (next->confidence(timestamp) > 0.5f) {
+			float* rms = next->rms();
+
+			for (unsigned j = 0; j < 3; j++) {
+				if (rms[j] > vibe) {
+					vibe = rms[j];
+				}
+			}
+		}
+
+		next = next->sibling();
+	}
+
+	return vibe;
+}
+
 void
 DataValidatorGroup::print()
 {
+	/* print the group's state */
+	warnx("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
+		_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
+		_toggle_count);
+
+
 	DataValidator *next = _first;
+	unsigned i = 0;
 
 	while (next != nullptr) {
+		printf("sensor #%u:\n", i);
 		next->print();
 		next = next->sibling();
+		i++;
 	}
 }
+
+unsigned
+DataValidatorGroup::failover_count()
+{
+	return _toggle_count;
+}

From 0b6ea40b7b14c305346c1c16c330d7ec3e27ba89 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 28 Jun 2015 13:34:15 +0200
Subject: [PATCH 20/62] Attitude estimator Q: Operational failover, warn user
 about excessive vibration levels

---
 .../attitude_estimator_q_main.cpp             | 69 +++++++++++++++++--
 .../attitude_estimator_q_params.c             |  9 +++
 2 files changed, 73 insertions(+), 5 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 714ca6cdf1..a127f9d057 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -51,7 +51,6 @@
 #include <limits.h>
 #include <math.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -62,6 +61,7 @@
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <lib/ecl/validation/data_validator_group.h>
+#include <mavlink/mavlink_log.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -104,6 +104,8 @@ public:
 
 	void		task_main();
 
+	void		print();
+
 private:
 	static constexpr float _dt_max = 0.02;
 	bool		_task_should_exit = false;		/**< if true, task should exit */
@@ -122,6 +124,7 @@ private:
 		param_t	mag_decl_auto;
 		param_t	acc_comp;
 		param_t	bias_max;
+		param_t vibe_thresh;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	float		_w_accel = 0.0f;
@@ -131,6 +134,8 @@ private:
 	bool		_mag_decl_auto = false;
 	bool		_acc_comp = false;
 	float		_bias_max = 0.0f;
+	float		_vibration_warning_threshold = 1.0f;
+	hrt_abstime	_vibration_warning_timestamp = 0;
 
 	Vector<3>	_gyro;
 	Vector<3>	_accel;
@@ -152,6 +157,10 @@ private:
 
 	bool		_inited = false;
 	bool		_data_good = false;
+	bool		_failsafe = false;
+	bool		_vibration_warning = false;
+
+	int		_mavlink_fd = -1;
 
 	perf_counter_t _update_perf;
 	perf_counter_t _loop_perf;
@@ -178,6 +187,7 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_params_handles.mag_decl_auto	= param_find("ATT_MAG_DECL_A");
 	_params_handles.acc_comp	= param_find("ATT_ACC_COMP");
 	_params_handles.bias_max	= param_find("ATT_BIAS_MAX");
+	_params_handles.vibe_thresh	= param_find("ATT_VIBE_THRESH");
 }
 
 /**
@@ -227,6 +237,16 @@ int AttitudeEstimatorQ::start()
 	return OK;
 }
 
+void AttitudeEstimatorQ::print()
+{
+	warnx("gyro status:");
+	_voter_gyro.print();
+	warnx("accel status:");
+	_voter_accel.print();
+	warnx("mag status:");
+	_voter_mag.print();
+}
+
 void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
 {
 	attitude_estimator_q::instance->task_main();
@@ -249,6 +269,10 @@ void AttitudeEstimatorQ::task_main()
 	while (!_task_should_exit) {
 		int ret = px4_poll(fds, 1, 1000);
 
+		if (_mavlink_fd < 0) {
+			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+		}
+
 		if (ret < 0) {
 			// Poll error, sleep and try again
 			usleep(10000);
@@ -271,11 +295,40 @@ void AttitudeEstimatorQ::task_main()
 			_voter_mag.put(0, sensors.magnetometer_timestamp, sensors.magnetometer_ga, sensors.magnetometer_errcount);
 			_voter_mag.put(1, sensors.magnetometer1_timestamp, sensors.magnetometer1_ga, sensors.magnetometer1_errcount);
 
-			// Get best measurement values
 			int best_gyro, best_accel, best_mag;
-			_gyro.set(_voter_gyro.get_best(sensors.timestamp, &best_gyro));
-			_accel.set(_voter_accel.get_best(sensors.timestamp, &best_accel));
-			_mag.set(_voter_mag.get_best(sensors.timestamp, &best_mag));
+
+			// Get best measurement values
+			hrt_abstime curr_time = hrt_absolute_time();
+			_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
+			_accel.set(_voter_accel.get_best(curr_time, &best_accel));
+			_mag.set(_voter_mag.get_best(curr_time, &best_mag));
+
+			_data_good = true;
+
+			if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
+				_voter_accel.failover_count() > 0 ||
+				_voter_mag.failover_count() > 0)) {
+
+				_failsafe = true;
+				mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
+			}
+
+			if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
+				_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
+				_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
+
+				if (_vibration_warning_timestamp == 0) {
+					_vibration_warning_timestamp = curr_time;
+				} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
+					_vibration_warning = true;
+					mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
+						(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
+						(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
+						(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
+				}
+			} else {
+				_vibration_warning_timestamp = 0;
+			}
 		}
 
 		bool gpos_updated;
@@ -349,6 +402,10 @@ void AttitudeEstimatorQ::task_main()
 		memcpy(&att.R[0], R.data, sizeof(att.R));
 		att.R_valid = true;
 
+		att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
+		att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
+		att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());
+
 		if (_att_pub == nullptr) {
 			_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
 		} else {
@@ -379,6 +436,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) {
 		param_get(_params_handles.acc_comp, &acc_comp_int);
 		_acc_comp = acc_comp_int != 0;
 		param_get(_params_handles.bias_max, &_bias_max);
+		param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
 	}
 }
 
@@ -522,6 +580,7 @@ int attitude_estimator_q_main(int argc, char *argv[]) {
 
 	if (!strcmp(argv[1], "status")) {
 		if (attitude_estimator_q::instance) {
+			attitude_estimator_q::instance->print();
 			warnx("running");
 			return 0;
 
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
index 730d7a4e8b..d4d9d10eb4 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
@@ -108,3 +108,12 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
  * @unit rad/s
  */
 PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);
+
+/**
+ * Threshold (of RMS) to warn about high vibration levels
+ *
+ * @group Attitude Q estimator
+ * @min 0.001
+ * @max 100
+ */
+PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.1f);

From e5834460e216c336293887b56537d04cdbc6757f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 29 Jun 2015 09:54:09 +0200
Subject: [PATCH 21/62] MPU6K: Add commandline option to set range, fix test
 command by resetting guard logic properly

---
 src/drivers/mpu6000/mpu6000.cpp | 29 +++++++++++++++++++++++++----
 1 file changed, 25 insertions(+), 4 deletions(-)

diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 194fdf1669..4af1bec586 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1534,6 +1534,13 @@ void
 MPU6000::stop()
 {
 	hrt_cancel(&_call);
+
+	/* reset internal states */
+	memset(_last_accel, 0, sizeof(_last_accel));
+
+	/* discard unread data in the buffers */
+	_accel_reports->flush();
+	_gyro_reports->flush();
 }
 
 void
@@ -1955,7 +1962,7 @@ namespace mpu6000
 MPU6000	*g_dev_int; // on internal bus
 MPU6000	*g_dev_ext; // on external bus
 
-void	start(bool, enum Rotation);
+void	start(bool, enum Rotation, int range);
 void	stop(bool);
 void	test(bool);
 void	reset(bool);
@@ -1972,7 +1979,7 @@ void	usage();
  * or failed to detect the sensor.
  */
 void
-start(bool external_bus, enum Rotation rotation)
+start(bool external_bus, enum Rotation rotation, int range)
 {
 	int fd;
         MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
@@ -2009,6 +2016,9 @@ start(bool external_bus, enum Rotation rotation)
 	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
 		goto fail;
 
+	if (ioctl(fd, ACCELIOCSRANGE, range) < 0)
+		goto fail;
+
         close(fd);
 
 	exit(0);
@@ -2106,6 +2116,12 @@ test(bool external_bus)
 	warnx("temp:  \t%8.4f\tdeg celsius", (double)a_report.temperature);
 	warnx("temp:  \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
 
+	/* reset to default polling */
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "reset to default polling");
+
+	close(fd);
+	close(fd_gyro);
 
 	/* XXX add poll-rate tests here too */
 
@@ -2205,6 +2221,7 @@ usage()
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
+	warnx("    -a accel range (in g)");
 }
 
 } // namespace
@@ -2215,9 +2232,10 @@ mpu6000_main(int argc, char *argv[])
 	bool external_bus = false;
 	int ch;
 	enum Rotation rotation = ROTATION_NONE;
+	int accel_range = 8;
 
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+	while ((ch = getopt(argc, argv, "XR:a:")) != EOF) {
 		switch (ch) {
 		case 'X':
 			external_bus = true;
@@ -2225,6 +2243,9 @@ mpu6000_main(int argc, char *argv[])
 		case 'R':
 			rotation = (enum Rotation)atoi(optarg);
 			break;
+		case 'a':
+			accel_range = atoi(optarg);
+			break;
 		default:
 			mpu6000::usage();
 			exit(0);
@@ -2238,7 +2259,7 @@ mpu6000_main(int argc, char *argv[])
 
 	 */
 	if (!strcmp(verb, "start")) {
-		mpu6000::start(external_bus, rotation);
+		mpu6000::start(external_bus, rotation, accel_range);
 	}
 
 	if (!strcmp(verb, "stop")) {

From 0693e186d9f04e0681c2b3e2e4109bd0e8b32bbf Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 29 Jun 2015 21:30:30 +0200
Subject: [PATCH 22/62] LSM303D: Add option for 16g range

---
 src/drivers/lsm303d/lsm303d.cpp | 37 ++++++++++++++++++++++++++-------
 1 file changed, 29 insertions(+), 8 deletions(-)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 6b8c7bc60a..4235ecc6f5 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1416,6 +1416,13 @@ LSM303D::stop()
 {
 	hrt_cancel(&_accel_call);
 	hrt_cancel(&_mag_call);
+
+	/* reset internal states */
+	memset(_last_accel, 0, sizeof(_last_accel));
+
+	/* discard unread data in the buffers */
+	_accel_reports->flush();
+	_mag_reports->flush();
 }
 
 void
@@ -1856,7 +1863,7 @@ namespace lsm303d
 
 LSM303D	*g_dev;
 
-void	start(bool external_bus, enum Rotation rotation);
+void	start(bool external_bus, enum Rotation rotation, unsigned range);
 void	test();
 void	reset();
 void	info();
@@ -1871,11 +1878,12 @@ void	test_error();
  * up and running or failed to detect the sensor.
  */
 void
-start(bool external_bus, enum Rotation rotation)
+start(bool external_bus, enum Rotation rotation, unsigned range)
 {
 	int fd, fd_mag;
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(0, "already started");
+	}
 
 	/* create the driver */
         if (external_bus) {
@@ -1899,11 +1907,17 @@ start(bool external_bus, enum Rotation rotation)
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		goto fail;
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		goto fail;
+	}
+
+	if (ioctl(fd, ACCELIOCSRANGE, range) < 0) {
+		goto fail;
+	}
 
 	fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
 
@@ -1995,7 +2009,10 @@ test()
 	warnx("mag z: \t%d\traw", (int)m_report.z_raw);
 	warnx("mag range: %8.4f ga", (double)m_report.range_ga);
 
-	/* XXX add poll-rate tests here too */
+	/* reset to default polling */
+	if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		err(1, "reset to default polling");
+	}
 
         close(fd_accel);
         close(fd_mag);
@@ -2099,9 +2116,10 @@ lsm303d_main(int argc, char *argv[])
 	bool external_bus = false;
 	int ch;
 	enum Rotation rotation = ROTATION_NONE;
+	int accel_range = 8;
 
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+	while ((ch = getopt(argc, argv, "XR:a:")) != EOF) {
 		switch (ch) {
 		case 'X':
 			external_bus = true;
@@ -2109,6 +2127,9 @@ lsm303d_main(int argc, char *argv[])
 		case 'R':
 			rotation = (enum Rotation)atoi(optarg);
 			break;
+		case 'a':
+			accel_range = atoi(optarg);
+			break;
 		default:
 			lsm303d::usage();
 			exit(0);
@@ -2122,7 +2143,7 @@ lsm303d_main(int argc, char *argv[])
 
 	 */
 	if (!strcmp(verb, "start"))
-		lsm303d::start(external_bus, rotation);
+		lsm303d::start(external_bus, rotation, accel_range);
 
 	/*
 	 * Test the driver/device.

From c0dc9f99a6252bd7daebd723944b68a0914f5834 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 29 Jun 2015 21:31:28 +0200
Subject: [PATCH 23/62] LSM303D: Start with 16g as default range

---
 ROMFS/px4fmu_common/init.d/rc.sensors | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index ff1a8092be..f547d02cb2 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -39,7 +39,7 @@ then
 		fi
 
 		# external LSM303D is rotated 270 degrees yaw
-		if lsm303d -X -R 6 start
+		if lsm303d -X -R 6 -a 16 start
 		then
 		fi
 
@@ -62,7 +62,7 @@ then
 		then
 		fi
 
-		if lsm303d start
+		if lsm303d -a 16 start
 		then
 		fi
 	fi

From 7ddd173c44ad6ee0d36266f443bdfb94020f258b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 29 Aug 2015 15:45:24 +0200
Subject: [PATCH 24/62] Gyro message: Add integral fields

---
 msg/sensor_gyro.msg | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg
index 4a252269e8..65aa4a8706 100644
--- a/msg/sensor_gyro.msg
+++ b/msg/sensor_gyro.msg
@@ -1,8 +1,12 @@
 uint64 timestamp
+uint64 integral_dt	# integration time
 uint64 error_count
 float32 x		# angular velocity in the NED X board axis in rad/s
 float32 y		# angular velocity in the NED Y board axis in rad/s
 float32 z		# angular velocity in the NED Z board axis in rad/s
+float32 x_integral	# delta angle in the NED X board axis in rad/s in the integration time frame
+float32 y_integral	# delta angle in the NED Y board axis in rad/s in the integration time frame
+float32 z_integral	# delta angle in the NED Z board axis in rad/s in the integration time frame
 float32 temperature	# temperature in degrees celcius
 float32 range_rad_s
 float32 scaling

From 341266a49b2558044064c3cb2e69c66ccac16e89 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 29 Aug 2015 15:45:47 +0200
Subject: [PATCH 25/62] accel message: Add integral fields

---
 msg/sensor_accel.msg | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg
index fdae381aba..2eca557465 100644
--- a/msg/sensor_accel.msg
+++ b/msg/sensor_accel.msg
@@ -1,8 +1,12 @@
 uint64 timestamp
+uint64 integral_dt	# integration time
 uint64 error_count
 float32 x		# acceleration in the NED X board axis in m/s^2
 float32 y		# acceleration in the NED Y board axis in m/s^2
 float32 z		# acceleration in the NED Z board axis in m/s^2
+float32 x_integral	# velocity in the NED X board axis in m/s over the integration time frame
+float32 y_integral	# velocity in the NED Y board axis in m/s over the integration time frame
+float32 z_integral	# velocity in the NED Z board axis in m/s over the integration time frame
 float32 temperature	# temperature in degrees celsius
 float32 range_m_s2	# range in m/s^2 (+- this value)
 float32 scaling

From 8d5fb368861207062fca882cbed8bf0b063821b1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 29 Aug 2015 16:16:46 +0200
Subject: [PATCH 26/62] Improve EKF status output printing

---
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp    | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 2cf33d9b0b..58692e92ea 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1263,10 +1263,14 @@ void AttitudePositionEstimatorEKF::pollData()
 			(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
 			dtoverflow5, dtoverflow10);
 
-		warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f",
+		warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
 		(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
 		(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z);
 
+		warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
+		(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
+		(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
+
 		lastprint = hrt_absolute_time();
 	}
 

From ee90a5d9c73d6442e42d1760d1e0ff1f8f70b81c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 29 Aug 2015 16:33:27 +0200
Subject: [PATCH 27/62] MPU6K: Switch to 16g range to deal with vibration

---
 ROMFS/px4fmu_common/init.d/rc.sensors | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index f547d02cb2..ef9e086f1e 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -24,7 +24,7 @@ then
 	fi
 
 	# external MPU6K is rotated 180 degrees yaw
-	if mpu6000 -X -R 4 start
+	if mpu6000 -X -R 4 -a 16 start
 	then
 		set BOARD_FMUV3 true
 	else
@@ -44,7 +44,7 @@ then
 		fi
 
 		# internal MPU6000 is rotated 180 deg roll, 270 deg yaw
-		if mpu6000 -R 14 start
+		if mpu6000 -R 14 -a 16 start
 		then
 		fi
 
@@ -54,7 +54,7 @@ then
 
 	else
 		# FMUv2
-		if mpu6000 start
+		if mpu6000 -a 16 start
 		then
 		fi
 

From 3dbc8d82054b1131e33742093888386e8560367d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:07:19 +0200
Subject: [PATCH 28/62] sensors combined message: Clean up field names

---
 msg/sensor_combined.msg | 127 ++++++++++++----------------------------
 1 file changed, 38 insertions(+), 89 deletions(-)

diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg
index 0845918f3b..ec58e6f923 100644
--- a/msg/sensor_combined.msg
+++ b/msg/sensor_combined.msg
@@ -26,100 +26,49 @@ uint32 SENSOR_PRIO_MAX = 255
 # NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only
 
 uint64 timestamp			# Timestamp in microseconds since boot, from gyro
-#
-int16[3] gyro_raw			# Raw sensor values of angular velocity
-float32[3] gyro_rad_s			# Angular velocity in radian per seconds
-uint32 gyro_priority			# Sensor priority
-float32[3] gyro_integral_rad		# delta angle in radians
-uint64 gyro_integral_dt			# delta time for gyro integral in us
-uint32 gyro_errcount			# Error counter for gyro 0
-float32 gyro_temp			# Temperature of gyro 0
+uint64[3] gyro_timestamp			# Gyro timestamps
+int16[9] gyro_raw			# Raw sensor values of angular velocity
+float32[9] gyro_rad_s			# Angular velocity in radian per seconds
+uint32[3] gyro_priority			# Sensor priority
+float32[9] gyro_integral_rad		# delta angle in radians
+uint64[3] gyro_integral_dt			# delta time for gyro integral in us
+uint32[3] gyro_errcount			# Error counter for gyro 0
+float32[3] gyro_temp			# Temperature of gyro 0
 
-int16[3] accelerometer_raw		# Raw acceleration in NED body frame
-float32[3] accelerometer_m_s2		# Acceleration in NED body frame, in m/s^2
-float32[3] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
-uint64 accelerometer_integral_dt		# delta time for accel integral in us
-int16 accelerometer_mode			# Accelerometer measurement mode
-float32 accelerometer_range_m_s2		# Accelerometer measurement range in m/s^2
-uint64 accelerometer_timestamp	# Accelerometer timestamp
-uint32 accelerometer_priority	# Sensor priority
-uint32 accelerometer_errcount	# Error counter for accel 0
-float32 accelerometer_temp		# Temperature of accel 0
+int16[9] accelerometer_raw		# Raw acceleration in NED body frame
+float32[9] accelerometer_m_s2		# Acceleration in NED body frame, in m/s^2
+float32[9] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
+uint64[3] accelerometer_integral_dt		# delta time for accel integral in us
+int16[3] accelerometer_mode			# Accelerometer measurement mode
+float32[3] accelerometer_range_m_s2		# Accelerometer measurement range in m/s^2
+uint64[3] accelerometer_timestamp	# Accelerometer timestamp
+uint32[3] accelerometer_priority	# Sensor priority
+uint32[3] accelerometer_errcount	# Error counter for accel 0
+float32[3] accelerometer_temp		# Temperature of accel 0
 
-int16[3] magnetometer_raw		# Raw magnetic field in NED body frame
-float32[3] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
-int16 magnetometer_mode			# Magnetometer measurement mode
-float32 magnetometer_range_ga		# measurement range in Gauss
-float32 magnetometer_cuttoff_freq_hz	# Internal analog low pass frequency of sensor
-uint64 magnetometer_timestamp	# Magnetometer timestamp
-uint32 magnetometer_priority			# Sensor priority
-uint32 magnetometer_errcount		# Error counter for mag 0
-float32 magnetometer_temp		# Temperature of mag 0
+int16[9] magnetometer_raw		# Raw magnetic field in NED body frame
+float32[9] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
+int16[3] magnetometer_mode			# Magnetometer measurement mode
+float32[3] magnetometer_range_ga		# measurement range in Gauss
+float32[3] magnetometer_cuttoff_freq_hz	# Internal analog low pass frequency of sensor
+uint64[3] magnetometer_timestamp	# Magnetometer timestamp
+uint32[3] magnetometer_priority			# Sensor priority
+uint32[3] magnetometer_errcount		# Error counter for mag 0
+float32[3] magnetometer_temp		# Temperature of mag 0
 
-int16[3] gyro1_raw			# Raw sensor values of angular velocity
-float32[3] gyro1_rad_s			# Angular velocity in radian per seconds
-uint64 gyro1_timestamp		# Gyro timestamp
-uint32 gyro1_priority			# Sensor priority
-uint32 gyro1_errcount		# Error counter for gyro 1
-float32 gyro1_temp			# Temperature of gyro 1
-
-int16[3] accelerometer1_raw		# Raw acceleration in NED body frame
-float32[3] accelerometer1_m_s2		# Acceleration in NED body frame, in m/s^2
-uint64 accelerometer1_timestamp	# Accelerometer timestamp
-uint32 accelerometer1_priority	# Sensor priority
-uint32 accelerometer1_errcount	# Error counter for accel 1
-float32 accelerometer1_temp		# Temperature of accel 1
-
-int16[3] magnetometer1_raw		# Raw magnetic field in NED body frame
-float32[3] magnetometer1_ga		# Magnetic field in NED body frame, in Gauss
-uint64 magnetometer1_timestamp	# Magnetometer timestamp
-uint32 magnetometer1_priority			# Sensor priority
-uint32 magnetometer1_errcount	# Error counter for mag 1
-float32 magnetometer1_temp		# Temperature of mag 1
-
-int16[3] gyro2_raw			# Raw sensor values of angular velocity
-float32[3] gyro2_rad_s			# Angular velocity in radian per seconds
-uint64 gyro2_timestamp		# Gyro timestamp
-uint32 gyro2_priority			# Sensor priority
-uint32 gyro2_errcount		# Error counter for gyro 1
-float32 gyro2_temp			# Temperature of gyro 1
-
-int16[3] accelerometer2_raw		# Raw acceleration in NED body frame
-float32[3] accelerometer2_m_s2		# Acceleration in NED body frame, in m/s^2
-uint64 accelerometer2_timestamp	# Accelerometer timestamp
-uint32 accelerometer2_priority	# Sensor priority
-uint32 accelerometer2_errcount	# Error counter for accel 2
-float32 accelerometer2_temp		# Temperature of accel 2
-
-int16[3] magnetometer2_raw		# Raw magnetic field in NED body frame
-float32[3] magnetometer2_ga		# Magnetic field in NED body frame, in Gauss
-uint64 magnetometer2_timestamp	# Magnetometer timestamp
-uint32 magnetometer2_priority			# Sensor priority
-uint32 magnetometer2_errcount	# Error counter for mag 2
-float32 magnetometer2_temp		# Temperature of mag 2
-
-float32 baro_pres_mbar			# Barometric pressure, already temp. comp.
-float32 baro_alt_meter			# Altitude, already temp. comp.
-float32 baro_temp_celcius		# Temperature in degrees celsius
-uint64 baro_timestamp		# Barometer timestamp
-uint32 baro_priority			# Sensor priority
-
-float32 baro1_pres_mbar			# Barometric pressure, already temp. comp.
-float32 baro1_alt_meter			# Altitude, already temp. comp.
-float32 baro1_temp_celcius		# Temperature in degrees celsius
-uint64 baro1_timestamp		# Barometer timestamp
-uint32 baro1_priority			# Sensor priority
+float32[3] baro_pres_mbar			# Barometric pressure, already temp. comp.
+float32[3] baro_alt_meter			# Altitude, already temp. comp.
+float32[3] baro_temp_celcius		# Temperature in degrees celsius
+uint64[3] baro_timestamp		# Barometer timestamp
+uint32[3] baro_priority			# Sensor priority
+uint32[3] baro_errcount			# Error count in communication
 
 float32[10] adc_voltage_v		# ADC voltages of ADC Chan 10/11/12/13 or -1
 uint16[10] adc_mapping		# Channel indices of each of these values
 float32 mcu_temp_celcius			# Internal temperature measurement of MCU
 
-float32 differential_pressure_pa			# Airspeed sensor differential pressure
-uint64 differential_pressure_timestamp	# Last measurement timestamp
-float32 differential_pressure_filtered_pa	# Low pass filtered airspeed sensor differential pressure reading
-uint32 differential_pressure_priority			# Sensor priority
-
-float32 differential_pressure1_pa			# Airspeed sensor differential pressure
-uint64 differential_pressure1_timestamp	# Last measurement timestamp
-float32 differential_pressure1_filtered_pa	# Low pass filtered airspeed sensor differential pressure reading
-uint32 differential_pressure1_priority			# Sensor priority
+float32[3] differential_pressure_pa			# Airspeed sensor differential pressure
+uint64[3] differential_pressure_timestamp	# Last measurement timestamp
+float32[3] differential_pressure_filtered_pa	# Low pass filtered airspeed sensor differential pressure reading
+uint32[3] differential_pressure_priority			# Sensor priority
+uint32[3] differential_pressure_errcount			# Error count in communication

From 374abf3b73c081c4d814ce52f4aedc3fda26b6f4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:07:44 +0200
Subject: [PATCH 29/62] Sensors app: remove redundant code

---
 src/modules/sensors/sensors.cpp | 478 ++++++++++----------------------
 1 file changed, 139 insertions(+), 339 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index bf30b6bd05..edbcce6bb6 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -201,19 +201,16 @@ private:
 	bool		_hil_enabled;			/**< if true, HIL is active */
 	bool		_publishing;			/**< if true, we are publishing sensor data */
 
-	int		_gyro_sub;			/**< raw gyro0 data subscription */
-	int		_accel_sub;			/**< raw accel0 data subscription */
-	int		_mag_sub;			/**< raw mag0 data subscription */
-	int		_gyro1_sub;			/**< raw gyro1 data subscription */
-	int		_accel1_sub;			/**< raw accel1 data subscription */
-	int		_mag1_sub;			/**< raw mag1 data subscription */
-	int		_gyro2_sub;			/**< raw gyro2 data subscription */
-	int		_accel2_sub;			/**< raw accel2 data subscription */
-	int		_mag2_sub;			/**< raw mag2 data subscription */
+	int		_gyro_sub[ORB_MULTI_MAX_INSTANCES - 1];			/**< raw gyro0 data subscription */
+	int		_accel_sub[ORB_MULTI_MAX_INSTANCES - 1];			/**< raw accel0 data subscription */
+	int		_mag_sub[ORB_MULTI_MAX_INSTANCES - 1];			/**< raw mag0 data subscription */
+	int		_baro_sub[ORB_MULTI_MAX_INSTANCES - 1];			/**< raw baro0 data subscription */
+	unsigned	_gyro_count;			/**< raw gyro0 data subscription */
+	unsigned	_accel_count;			/**< raw accel0 data subscription */
+	unsigned	_mag_count;			/**< raw mag0 data subscription */
+	unsigned	_baro_count;			/**< raw baro0 data subscription */
+	
 	int 		_rc_sub;			/**< raw rc channels data subscription */
-	int		_baro_sub;			/**< raw baro0 data subscription */
-	int		_baro1_sub;			/**< raw baro1 data subscription */
-	//int		_airspeed_sub;			/**< airspeed subscription */
 	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
@@ -228,18 +225,6 @@ private:
 	orb_advert_t	_airspeed_pub;			/**< airspeed */
 	orb_advert_t	_diff_pres_pub;			/**< differential_pressure */
 	
-	int32_t _gyro_prio;			/**< gyro0 sensor priority */
-	int32_t	_accel_prio;			/**< accel0 sensor priority */
-	int32_t	_mag_prio;			/**< mag0 sensor priority */
-	int32_t	_gyro1_prio;			/**< gyro1 sensor priority */
-	int32_t	_accel1_prio;			/**<accel1 sensor priority */
-	int32_t	_mag1_prio;			/**< mag1 sensor priority */
-	int32_t	_gyro2_prio;			/**< gyro2 sensor priority */
-	int32_t	_accel2_prio;			/**< accel2 sensor priority */
-	int32_t	_mag2_prio;			/**< mag2 sensor priority */
-	int32_t	_baro_prio;			/**< baro0 sensor priority */
-	int32_t	_baro1_prio;			/**< baro1 sensor priority */
-
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	struct rc_channels_s _rc;			/**< r/c channel data */
@@ -377,6 +362,9 @@ private:
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
+	void		init_sensor_class(const struct orb_metadata *meta, int *subs,
+		unsigned *priorities, unsigned *errcount);
+
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -497,18 +485,15 @@ Sensors::Sensors() :
 	_publishing(true),
 
 	/* subscriptions */
-	_gyro_sub(-1),
-	_accel_sub(-1),
-	_mag_sub(-1),
-	_gyro1_sub(-1),
-	_accel1_sub(-1),
-	_mag1_sub(-1),
-	_gyro2_sub(-1),
-	_accel2_sub(-1),
-	_mag2_sub(-1),
+	_gyro_sub{-1, -1, -1},
+	_accel_sub{-1, -1, -1},
+	_mag_sub{-1, -1, -1},
+	_baro_sub{-1, -1, -1},
+	_gyro_count(0),
+	_accel_count(0),
+	_mag_count(0),
+	_baro_count(0),
 	_rc_sub(-1),
-	_baro_sub(-1),
-	_baro1_sub(-1),
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_rc_parameter_map_sub(-1),
@@ -523,19 +508,6 @@ Sensors::Sensors() :
 	_airspeed_pub(nullptr),
 	_diff_pres_pub(nullptr),
 
-	/* sensor priorities */
-	_gyro_prio(-1),
-	_accel_prio(-1),
-	_mag_prio(-1),
-	_gyro1_prio(-1),
-	_accel1_prio(-1),
-	_mag1_prio(-1),
-	_gyro2_prio(-1),
-	_accel2_prio(-1),
-	_mag2_prio(-1),
-	_baro_prio(-1),
-	_baro1_prio(-1),
-
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
 
@@ -1032,287 +1004,132 @@ Sensors::adc_init()
 void
 Sensors::accel_poll(struct sensor_combined_s &raw)
 {
-	bool accel_updated;
-	orb_check(_accel_sub, &accel_updated);
+	for (unsigned i = 0; i < _accel_count; i++) {
+		bool accel_updated;
+		orb_check(_accel_sub[i], &accel_updated);
 
-	if (accel_updated) {
-		struct accel_report	accel_report;
+		if (accel_updated) {
+			struct accel_report	accel_report;
 
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
+			orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
 
-		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
-		vect = _board_rotation * vect;
+			math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
+			vect = _board_rotation * vect;
 
-		raw.accelerometer_m_s2[0] = vect(0);
-		raw.accelerometer_m_s2[1] = vect(1);
-		raw.accelerometer_m_s2[2] = vect(2);
+			raw.accelerometer_m_s2[i * 3 + 0] = vect(0);
+			raw.accelerometer_m_s2[i * 3 + 1] = vect(1);
+			raw.accelerometer_m_s2[i * 3 + 2] = vect(2);
 
-		math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
-		vect_int = _board_rotation * vect_int;
+			math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
+			vect_int = _board_rotation * vect_int;
 
-		raw.accelerometer_integral_m_s[0] = vect_int(0);
-		raw.accelerometer_integral_m_s[1] = vect_int(1);
-		raw.accelerometer_integral_m_s[2] = vect_int(2);
+			raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0);
+			raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1);
+			raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2);
 
-		raw.accelerometer_integral_dt = accel_report.integral_dt;
+			raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
 
-		raw.accelerometer_raw[0] = accel_report.x_raw;
-		raw.accelerometer_raw[1] = accel_report.y_raw;
-		raw.accelerometer_raw[2] = accel_report.z_raw;
+			raw.accelerometer_raw[i * 3 + 0] = accel_report.x_raw;
+			raw.accelerometer_raw[i * 3 + 1] = accel_report.y_raw;
+			raw.accelerometer_raw[i * 3 + 2] = accel_report.z_raw;
 
-		raw.accelerometer_timestamp = accel_report.timestamp;
-		raw.accelerometer_priority = _accel_prio;
-		raw.accelerometer_errcount = accel_report.error_count;
-		raw.accelerometer_temp = accel_report.temperature;
-	}
-
-	orb_check(_accel1_sub, &accel_updated);
-
-	if (accel_updated) {
-		struct accel_report	accel_report;
-
-		orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report);
-
-		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
-		vect = _board_rotation * vect;
-
-		raw.accelerometer1_m_s2[0] = vect(0);
-		raw.accelerometer1_m_s2[1] = vect(1);
-		raw.accelerometer1_m_s2[2] = vect(2);
-
-		raw.accelerometer1_raw[0] = accel_report.x_raw;
-		raw.accelerometer1_raw[1] = accel_report.y_raw;
-		raw.accelerometer1_raw[2] = accel_report.z_raw;
-
-		raw.accelerometer1_timestamp = accel_report.timestamp;
-		raw.accelerometer1_priority = _accel1_prio;
-		raw.accelerometer1_errcount = accel_report.error_count;
-		raw.accelerometer1_temp = accel_report.temperature;
-	}
-
-	orb_check(_accel2_sub, &accel_updated);
-
-	if (accel_updated) {
-		struct accel_report	accel_report;
-
-		orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report);
-
-		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
-		vect = _board_rotation * vect;
-
-		raw.accelerometer2_m_s2[0] = vect(0);
-		raw.accelerometer2_m_s2[1] = vect(1);
-		raw.accelerometer2_m_s2[2] = vect(2);
-
-		raw.accelerometer2_raw[0] = accel_report.x_raw;
-		raw.accelerometer2_raw[1] = accel_report.y_raw;
-		raw.accelerometer2_raw[2] = accel_report.z_raw;
-
-		raw.accelerometer2_timestamp = accel_report.timestamp;
-		raw.accelerometer2_priority = _accel2_prio;
-		raw.accelerometer2_errcount = accel_report.error_count;
-		raw.accelerometer2_temp = accel_report.temperature;
+			raw.accelerometer_timestamp[i] = accel_report.timestamp;
+			raw.accelerometer_errcount[i] = accel_report.error_count;
+			raw.accelerometer_temp[i] = accel_report.temperature;
+		}
 	}
 }
 
 void
 Sensors::gyro_poll(struct sensor_combined_s &raw)
 {
-	bool gyro_updated;
-	orb_check(_gyro_sub, &gyro_updated);
+	for (unsigned i = 0; i < _gyro_count; i++) {
+		bool gyro_updated;
+		orb_check(_gyro_sub[i], &gyro_updated);
 
-	if (gyro_updated) {
-		struct gyro_report	gyro_report;
+		if (gyro_updated) {
+			struct gyro_report	gyro_report;
 
-		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
+			orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
 
-		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
-		vect = _board_rotation * vect;
+			math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
+			vect = _board_rotation * vect;
 
-		raw.gyro_rad_s[0] = vect(0);
-		raw.gyro_rad_s[1] = vect(1);
-		raw.gyro_rad_s[2] = vect(2);
+			raw.gyro_rad_s[i * 3 + 0] = vect(0);
+			raw.gyro_rad_s[i * 3 + 1] = vect(1);
+			raw.gyro_rad_s[i * 3 + 2] = vect(2);
 
-		math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
-		vect_int = _board_rotation * vect_int;
+			math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
+			vect_int = _board_rotation * vect_int;
 
-		raw.gyro_integral_rad[0] = vect_int(0);
-		raw.gyro_integral_rad[1] = vect_int(1);
-		raw.gyro_integral_rad[2] = vect_int(2);
+			raw.gyro_integral_rad[i * 3 + 0] = vect_int(0);
+			raw.gyro_integral_rad[i * 3 + 1] = vect_int(1);
+			raw.gyro_integral_rad[i * 3 + 2] = vect_int(2);
 
-		raw.gyro_integral_dt = gyro_report.integral_dt;
+			raw.gyro_integral_dt[i] = gyro_report.integral_dt;
 
-		raw.gyro_raw[0] = gyro_report.x_raw;
-		raw.gyro_raw[1] = gyro_report.y_raw;
-		raw.gyro_raw[2] = gyro_report.z_raw;
+			raw.gyro_raw[i * 3 + 0] = gyro_report.x_raw;
+			raw.gyro_raw[i * 3 + 1] = gyro_report.y_raw;
+			raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw;
 
-		raw.timestamp = gyro_report.timestamp;
-		raw.gyro_priority = _gyro_prio;
-		raw.gyro_errcount = gyro_report.error_count;
-		raw.gyro_temp = gyro_report.temperature;
-	}
-
-	orb_check(_gyro1_sub, &gyro_updated);
-
-	if (gyro_updated) {
-		struct gyro_report	gyro_report;
-
-		orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report);
-
-		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
-		vect = _board_rotation * vect;
-
-		raw.gyro1_rad_s[0] = vect(0);
-		raw.gyro1_rad_s[1] = vect(1);
-		raw.gyro1_rad_s[2] = vect(2);
-
-		raw.gyro1_raw[0] = gyro_report.x_raw;
-		raw.gyro1_raw[1] = gyro_report.y_raw;
-		raw.gyro1_raw[2] = gyro_report.z_raw;
-
-		raw.gyro1_timestamp = gyro_report.timestamp;
-		raw.gyro1_priority = _gyro1_prio;
-		raw.gyro1_errcount = gyro_report.error_count;
-		raw.gyro1_temp = gyro_report.temperature;
-	}
-
-	orb_check(_gyro2_sub, &gyro_updated);
-
-	if (gyro_updated) {
-		struct gyro_report	gyro_report;
-
-		orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report);
-
-		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
-		vect = _board_rotation * vect;
-
-		raw.gyro2_rad_s[0] = vect(0);
-		raw.gyro2_rad_s[1] = vect(1);
-		raw.gyro2_rad_s[2] = vect(2);
-
-		raw.gyro2_raw[0] = gyro_report.x_raw;
-		raw.gyro2_raw[1] = gyro_report.y_raw;
-		raw.gyro2_raw[2] = gyro_report.z_raw;
-
-		raw.gyro2_timestamp = gyro_report.timestamp;
-		raw.gyro2_priority = _gyro2_prio;
-		raw.gyro2_errcount = gyro_report.error_count;
-		raw.gyro2_temp = gyro_report.temperature;
+			raw.gyro_timestamp[i] = gyro_report.timestamp;
+			if (i == 0) {
+				raw.timestamp = gyro_report.timestamp;
+			}
+			raw.gyro_errcount[i] = gyro_report.error_count;
+			raw.gyro_temp[i] = gyro_report.temperature;
+		}
 	}
 }
 
 void
 Sensors::mag_poll(struct sensor_combined_s &raw)
 {
-	bool mag_updated;
-	orb_check(_mag_sub, &mag_updated);
+	for (unsigned i = 0; i < _mag_count; i++) {
+		bool mag_updated;
+		orb_check(_mag_sub[i], &mag_updated);
 
-	if (mag_updated) {
-		struct mag_report	mag_report;
+		if (mag_updated) {
+			struct mag_report	mag_report;
 
-		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
+			orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report);
 
-		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
+			math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
-		vect = _mag_rotation[0] * vect;
+			vect = _mag_rotation[i] * vect;
 
-		raw.magnetometer_ga[0] = vect(0);
-		raw.magnetometer_ga[1] = vect(1);
-		raw.magnetometer_ga[2] = vect(2);
+			raw.magnetometer_ga[i * 3 + 0] = vect(0);
+			raw.magnetometer_ga[i * 3 + 1] = vect(1);
+			raw.magnetometer_ga[i * 3 + 2] = vect(2);
 
-		raw.magnetometer_raw[0] = mag_report.x_raw;
-		raw.magnetometer_raw[1] = mag_report.y_raw;
-		raw.magnetometer_raw[2] = mag_report.z_raw;
+			raw.magnetometer_raw[i * 3 + 0] = mag_report.x_raw;
+			raw.magnetometer_raw[i * 3 + 1] = mag_report.y_raw;
+			raw.magnetometer_raw[i * 3 + 2] = mag_report.z_raw;
 
-		raw.magnetometer_timestamp = mag_report.timestamp;
-		raw.magnetometer_priority = _mag_prio;
-		raw.magnetometer_errcount = mag_report.error_count;
-		raw.magnetometer_temp = mag_report.temperature;
-	}
-
-	orb_check(_mag1_sub, &mag_updated);
-
-	if (mag_updated) {
-		struct mag_report	mag_report;
-
-		orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report);
-
-		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
-
-		vect = _mag_rotation[1] * vect;
-
-		raw.magnetometer1_ga[0] = vect(0);
-		raw.magnetometer1_ga[1] = vect(1);
-		raw.magnetometer1_ga[2] = vect(2);
-
-		raw.magnetometer1_raw[0] = mag_report.x_raw;
-		raw.magnetometer1_raw[1] = mag_report.y_raw;
-		raw.magnetometer1_raw[2] = mag_report.z_raw;
-
-		raw.magnetometer1_timestamp = mag_report.timestamp;
-		raw.magnetometer1_priority = _mag1_prio;
-		raw.magnetometer1_errcount = mag_report.error_count;
-		raw.magnetometer1_temp = mag_report.temperature;
-	}
-
-	orb_check(_mag2_sub, &mag_updated);
-
-	if (mag_updated) {
-		struct mag_report	mag_report;
-
-		orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report);
-
-		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
-
-		vect = _mag_rotation[2] * vect;
-
-		raw.magnetometer2_ga[0] = vect(0);
-		raw.magnetometer2_ga[1] = vect(1);
-		raw.magnetometer2_ga[2] = vect(2);
-
-		raw.magnetometer2_raw[0] = mag_report.x_raw;
-		raw.magnetometer2_raw[1] = mag_report.y_raw;
-		raw.magnetometer2_raw[2] = mag_report.z_raw;
-
-		raw.magnetometer2_timestamp = mag_report.timestamp;
-		raw.magnetometer2_priority = _mag2_prio;
-		raw.magnetometer2_errcount = mag_report.error_count;
-		raw.magnetometer2_temp = mag_report.temperature;
+			raw.magnetometer_timestamp[i] = mag_report.timestamp;
+			raw.magnetometer_errcount[i] = mag_report.error_count;
+			raw.magnetometer_temp[i] = mag_report.temperature;
+		}
 	}
 }
 
 void
 Sensors::baro_poll(struct sensor_combined_s &raw)
 {
-	bool baro_updated;
-	orb_check(_baro_sub, &baro_updated);
+	for (unsigned i = 0; i < _baro_count; i++) {
+		bool baro_updated;
+		orb_check(_baro_sub[i], &baro_updated);
 
-	if (baro_updated) {
+		if (baro_updated) {
 
-		orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
+			orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
 
-		raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar
-		raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
-		raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius
+			raw.baro_pres_mbar[i] = _barometer.pressure; // Pressure in mbar
+			raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters
+			raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius
 
-		raw.baro_timestamp = _barometer.timestamp;
-		raw.baro_priority = _baro_prio;
-	}
-
-	orb_check(_baro1_sub, &baro_updated);
-
-	if (baro_updated) {
-
-		struct baro_report	baro_report;
-
-		orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report);
-
-		raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar
-		raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters
-		raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
-
-		raw.baro1_timestamp = baro_report.timestamp;
-		raw.baro1_priority = _baro1_prio;
+			raw.baro_timestamp[i] = _barometer.timestamp;
+		}
 	}
 }
 
@@ -1325,12 +1142,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 	if (updated) {
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
-		raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa;
-		raw.differential_pressure_timestamp = _diff_pres.timestamp;
-		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
+		raw.differential_pressure_pa[0] = _diff_pres.differential_pressure_raw_pa;
+		raw.differential_pressure_timestamp[0] = _diff_pres.timestamp;
+		raw.differential_pressure_filtered_pa[0] = _diff_pres.differential_pressure_filtered_pa;
 
 		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
-						(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+						(raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG);
 
 		_airspeed.timestamp = _diff_pres.timestamp;
 
@@ -1338,11 +1155,11 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		_airspeed.indicated_airspeed_m_s = math::max(0.0f,
 						   calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
 		_airspeed.true_airspeed_m_s = math::max(0.0f,
-							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
-									raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
+							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f,
+									raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
 		_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
-							calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar * 1e2f,
-								raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
+							calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f,
+								raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius));
 
 		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
@@ -2117,6 +1934,17 @@ Sensors::task_main_trampoline(int argc, char *argv[])
 	sensors::g_sensors->task_main();
 }
 
+void
+Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
+	unsigned *priorities, unsigned *errcount)
+{
+	for (unsigned i = 0; i < (unsigned)orb_group_count(meta); i++) {
+		subs[i] = orb_subscribe_multi(meta, i);
+		orb_priority(subs[i], (int32_t*)&priorities[i]);
+		errcount[i] = 100000;
+	}
+}
+
 void
 Sensors::task_main()
 {
@@ -2147,71 +1975,43 @@ Sensors::task_main()
 		return;
 	}
 
+	struct sensor_combined_s raw = {};
+
 	/*
 	 * do subscriptions
 	 */
-	_gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
-	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
-	_mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
-	_gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1);
-	_accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1);
-	_mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1);
-	_gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2);
-	_accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2);
-	_mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2);
+
+	init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
+		&raw.gyro_priority[0], &raw.gyro_errcount[0]);
+
+	init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
+		&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
+
+	init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
+		&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
+
+	init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
+		&raw.baro_priority[0], &raw.baro_errcount[0]);
+
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
-	_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
-	_baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1);
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
-	/*
-	 * get sensor priorities
-	 */
-	orb_priority(_gyro_sub, &_gyro_prio);
-	orb_priority(_accel_sub, &_accel_prio);
-	orb_priority(_mag_sub, &_mag_prio);
-	orb_priority(_gyro1_sub, &_gyro1_prio);
-	orb_priority(_accel1_sub, &_accel1_prio);
-	orb_priority(_mag1_sub, &_mag1_prio);
-	orb_priority(_gyro2_sub, &_gyro2_prio);
-	orb_priority(_accel2_sub, &_accel2_prio);
-	orb_priority(_mag2_sub, &_mag2_prio);
-	orb_priority(_baro_sub, &_baro_prio);
-	orb_priority(_baro1_sub, &_baro1_prio);
-
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vcontrol_mode_sub, 200);
 
 	/*
 	 * do advertisements
 	 */
-	struct sensor_combined_s raw;
-	memset(&raw, 0, sizeof(raw));
 	raw.timestamp = hrt_absolute_time();
 	raw.adc_voltage_v[0] = 0.0f;
 	raw.adc_voltage_v[1] = 0.0f;
 	raw.adc_voltage_v[2] = 0.0f;
 	raw.adc_voltage_v[3] = 0.0f;
 
-	/* set high initial error counts to deselect gyros */
-	raw.gyro_errcount = 100000;
-	raw.gyro1_errcount = 100000;
-	raw.gyro2_errcount = 100000;
-
-	/* set high initial error counts to deselect accels */
-	raw.accelerometer_errcount = 100000;
-	raw.accelerometer1_errcount = 100000;
-	raw.accelerometer2_errcount = 100000;
-
-	/* set high initial error counts to deselect mags */
-	raw.magnetometer_errcount = 100000;
-	raw.magnetometer1_errcount = 100000;
-	raw.magnetometer2_errcount = 100000;
-
 	memset(&_battery_status, 0, sizeof(_battery_status));
 	_battery_status.voltage_v = -1.0f;
 	_battery_status.voltage_filtered_v = -1.0f;
@@ -2234,8 +2034,8 @@ Sensors::task_main()
 	/* wakeup source(s) */
 	px4_pollfd_struct_t fds[1];
 
-	/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
-	fds[0].fd = _gyro_sub;
+	/* use the gyro to pace output */
+	fds[0].fd = _gyro_sub[0];
 	fds[0].events = POLLIN;
 
 	_task_should_exit = false;
@@ -2268,13 +2068,13 @@ Sensors::task_main()
 		baro_poll(raw);
 
 		/* work out if main gyro timed out and fail over to alternate gyro */
-		if (hrt_elapsed_time(&raw.timestamp) > 20 * 1000) {
+		if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000) {
 
 			/* if the secondary failed as well, go to the tertiary */
-			if (hrt_elapsed_time(&raw.gyro1_timestamp) > 20 * 1000) {
-				fds[0].fd = _gyro2_sub;
+			if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) {
+				fds[0].fd = _gyro_sub[2];
 			} else {
-				fds[0].fd = _gyro1_sub;
+				fds[0].fd = _gyro_sub[1];
 			}
 		}
 

From fd0950f0463996286746679c55c5c6c8174662eb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:08:12 +0200
Subject: [PATCH 30/62] Attitude estimator Q: Update to new sensors combined
 topic

---
 .../attitude_estimator_q_main.cpp                    | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index a127f9d057..b8a5481466 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -288,12 +288,12 @@ void AttitudeEstimatorQ::task_main()
 		sensor_combined_s sensors;
 		if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
 			// Feed validator with recent sensor data
-			_voter_gyro.put(0, sensors.timestamp, sensors.gyro_rad_s, sensors.gyro_errcount);
-			_voter_gyro.put(1, sensors.gyro1_timestamp, sensors.gyro1_rad_s, sensors.gyro1_errcount);
-			_voter_accel.put(0, sensors.accelerometer_timestamp, sensors.accelerometer_m_s2, sensors.accelerometer_errcount);
-			_voter_accel.put(1, sensors.accelerometer1_timestamp, sensors.accelerometer1_m_s2, sensors.accelerometer1_errcount);
-			_voter_mag.put(0, sensors.magnetometer_timestamp, sensors.magnetometer_ga, sensors.magnetometer_errcount);
-			_voter_mag.put(1, sensors.magnetometer1_timestamp, sensors.magnetometer1_ga, sensors.magnetometer1_errcount);
+
+			for (unsigned i = 0; i < 2; i++) {
+				_voter_gyro.put(i, sensors.gyro_timestamp[i], &sensors.gyro_rad_s[i * 3], sensors.gyro_errcount[i]);
+				_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], sensors.accelerometer_errcount[i]);
+				_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], sensors.magnetometer_errcount[i]);
+			}
 
 			int best_gyro, best_accel, best_mag;
 

From b18b1c8b09f1f615bbb37e8d71b4de30192c2d15 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:08:45 +0200
Subject: [PATCH 31/62] FrSky telemetry: Update to new sensors combined topic

---
 src/drivers/frsky_telemetry/frsky_data.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index de22a888d3..d8650c382e 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -178,12 +178,12 @@ void frsky_send_frame1(int uart)
 			roundf(raw.accelerometer_m_s2[2] * 1000.0f));
 
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
-			raw.baro_alt_meter);
+			raw.baro_alt_meter[0]);
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
-			roundf(frac(raw.baro_alt_meter) * 100.0f));
+			roundf(frac(raw.baro_alt_meter[0]) * 100.0f));
 
 	frsky_send_data(uart, FRSKY_ID_TEMP1,
-			roundf(raw.baro_temp_celcius));
+			roundf(raw.baro_temp_celcius[0]));
 
 	frsky_send_data(uart, FRSKY_ID_VFAS,
 			roundf(battery.voltage_v * 10.0f));

From 2b090d62c96c4b34a41796d597c986f10df6a026 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:09:16 +0200
Subject: [PATCH 32/62] HoTT telemetry: Update to new sensors combined topic

---
 src/drivers/hott/messages.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index ef841c3f85..61daa302fb 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -148,12 +148,12 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	msg.eam_sensor_id = EAM_SENSOR_ID;
 	msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
 	
-	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
+	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius[0] + 20);
 	msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
 
 	msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
 
-	uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
+	uint16_t alt = (uint16_t)(raw.baro_alt_meter[0] + 500);
 	msg.altitude_L = (uint8_t)alt & 0xff;
 	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 

From 299ccab3aeeb5c3e6d16ce5c0cdb3e6072e64a4b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:09:29 +0200
Subject: [PATCH 33/62] MS5611: Fix build error

---
 src/drivers/ms5611/ms5611_nuttx.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/drivers/ms5611/ms5611_nuttx.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp
index 4a304e130b..5f91416e74 100644
--- a/src/drivers/ms5611/ms5611_nuttx.cpp
+++ b/src/drivers/ms5611/ms5611_nuttx.cpp
@@ -271,7 +271,7 @@ MS5611::init()
 	}
 
 	/* allocate basic report buffers */
-	_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
+	_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s));
 
 	if (_reports == nullptr) {
 		DEVICE_DEBUG("can't get memory for reports");

From 1cdcf8b5ddb4c69cebeaca0eef13b4a2ec74a3fb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:09:49 +0200
Subject: [PATCH 34/62] Commander: Update to new sensors combined topic

---
 src/modules/commander/commander.cpp | 2 +-
 src/modules/commander/module.mk     | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 397083a3b5..0be086f16d 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1435,7 +1435,7 @@ int commander_thread_main(int argc, char *argv[])
 			 * vertical separation from other airtraffic the operator has to know when the
 			 * barometer is inoperational.
 			 * */
-			if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
+			if (hrt_elapsed_time(&sensors.baro_timestamp[0]) < FAILSAFE_DEFAULT_TIMEOUT) {
 				/* handle the case where baro was regained */
 				if (status.barometer_failure) {
 					status.barometer_failure = false;
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index c16a606711..e4fcea2c69 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -55,6 +55,6 @@ MODULE_STACKSIZE = 5000
 MAXOPTIMIZATION	 = -Os
 
 ifeq ($(PX4_TARGET_OS),nuttx)
-EXTRACXXFLAGS = -Wframe-larger-than=2200
+EXTRACXXFLAGS = -Wframe-larger-than=2400
 endif
 

From ff8e4f2ea6f48334c18b81b76d0bd6a6e1e88ad3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:10:04 +0200
Subject: [PATCH 35/62] EKF: Update to new sensors combined topic

---
 .../ekf_att_pos_estimator_main.cpp            | 136 ++++++++----------
 1 file changed, 59 insertions(+), 77 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 58692e92ea..7de7e04613 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1230,14 +1230,14 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
-	if (last_accel != _sensor_combined.accelerometer_timestamp) {
+	if (last_accel != _sensor_combined.accelerometer_timestamp[_accel_main]) {
 		accel_updated = true;
 
 	} else {
 		accel_updated = false;
 	}
 
-	last_accel = _sensor_combined.accelerometer_timestamp;
+	last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
 
 
 	// Copy gyro and accel
@@ -1295,30 +1295,27 @@ void AttitudePositionEstimatorEKF::pollData()
 	_ekf->dtIMU = deltaT;
 
 	int last_gyro_main = _gyro_main;
+	unsigned best_gyro_err = UINT32_MAX - GYRO_SWITCH_HYSTERESIS - 1;
+	_gyro_valid = false;
 
-	if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
-	    PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
-	    PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
-	    (_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) {
+	for (unsigned i = 0; i < 3; i++) {
 
-		_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
-		_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
-		_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
-		_gyro_main = 0;
-		_gyro_valid = true;
+		if (PX4_ISFINITE(_sensor_combined.gyro_integral_rad[i * 3 + 0]) &&
+		    PX4_ISFINITE(_sensor_combined.gyro_integral_rad[i * 3 + 1]) &&
+		    PX4_ISFINITE(_sensor_combined.gyro_integral_rad[i * 3 + 2]) &&
+		    (_sensor_combined.gyro_errcount[i] < (best_gyro_err + GYRO_SWITCH_HYSTERESIS))) {
 
-	} else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) &&
-		   PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) &&
-		   PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) {
+			_ekf->angRate.x = _sensor_combined.gyro_rad_s[i * 3 + 0];
+			_ekf->angRate.y = _sensor_combined.gyro_rad_s[i * 3 + 1];
+			_ekf->angRate.z = _sensor_combined.gyro_rad_s[i * 3 + 2];
+			_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[i * 3 + 0];
+			_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[i * 3 + 1];
+			_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[i * 3 + 2];
+			_gyro_main = i;
+			best_gyro_err = _sensor_combined.gyro_errcount[i];
+			_gyro_valid = true;
 
-		_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
-		_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
-		_ekf->angRate.z = _sensor_combined.gyro1_rad_s[2];
-		_gyro_main = 1;
-		_gyro_valid = true;
-
-	} else {
-		_gyro_valid = false;
+		}
 	}
 
 	if (last_gyro_main != _gyro_main) {
@@ -1333,22 +1330,27 @@ void AttitudePositionEstimatorEKF::pollData()
 		perf_count(_perf_gyro);
 	}
 
-	if (accel_updated) {
+	if (accel_updated || hrt_elapsed_time(&last_accel) > 5000) {
 
 		int last_accel_main = _accel_main;
 
-		/* fail over to the 2nd accel if we know the first is down */
-		if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) {
-			_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
-			_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
-			_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
-			_accel_main = 0;
+		unsigned best_accel_err = UINT32_MAX - ACCEL_SWITCH_HYSTERESIS - 1;
+		_accel_valid = false;
 
-		} else {
-			_ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0];
-			_ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1];
-			_ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2];
-			_accel_main = 1;
+		for (unsigned i = 0; i < 3; i++) {
+
+			/* fail over to the 2nd accel if we know the first is down */
+			if (_sensor_combined.accelerometer_errcount[i] < (best_accel_err + ACCEL_SWITCH_HYSTERESIS)) {
+				_ekf->accel.x = _sensor_combined.accelerometer_m_s2[i * 3 + 0];
+				_ekf->accel.y = _sensor_combined.accelerometer_m_s2[i * 3 + 1];
+				_ekf->accel.z = _sensor_combined.accelerometer_m_s2[i * 3 + 2];
+				_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[i * 3 + 0];
+				_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[i * 3 + 1];
+				_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[i * 3 + 2];
+				_accel_main = i;
+				best_accel_err = _sensor_combined.accelerometer_errcount[i];
+				_accel_valid = true;
+			}
 		}
 
 		if (!_accel_valid) {
@@ -1358,25 +1360,16 @@ void AttitudePositionEstimatorEKF::pollData()
 		if (last_accel_main != _accel_main) {
 			mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main);
 		}
-
-		_accel_valid = true;
 	}
 
-	_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
-	_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
-	_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[2];
-	_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[0];
-	_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[1];
-	_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[2];
-
-	if (last_mag != _sensor_combined.magnetometer_timestamp) {
+	if (last_mag != _sensor_combined.magnetometer_timestamp[_mag_main]) {
 		_newDataMag = true;
 
 	} else {
 		_newDataMag = false;
 	}
 
-	last_mag = _sensor_combined.magnetometer_timestamp;
+	last_mag = _sensor_combined.magnetometer_timestamp[_mag_main];
 
 	//PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
 
@@ -1553,49 +1546,38 @@ void AttitudePositionEstimatorEKF::pollData()
 	}
 
 	//Update Magnetometer
-	if (_newDataMag) {
-
-		_mag_valid = true;
+	if (_newDataMag || hrt_elapsed_time(&last_mag) > 500000) {
 
 		perf_count(_perf_mag);
 
 		int last_mag_main = _mag_main;
 
-		Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
-			_sensor_combined.magnetometer_ga[2]);
+		unsigned best_mag_err = UINT32_MAX - MAG_SWITCH_HYSTERESIS - 1;
+		_mag_valid = false;
 
-		Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1],
-			_sensor_combined.magnetometer1_ga[2]);
+		for (unsigned i = 0; i < 3; i++) {
 
-		const unsigned mag_timeout_us = 200000;
+			Vector3f mag(_sensor_combined.magnetometer_ga[i * 3 + 0], _sensor_combined.magnetometer_ga[i * 3 + 1],
+				_sensor_combined.magnetometer_ga[i * 3 + 2]);
 
-		/* fail over to the 2nd mag if we know the first is down */
-		if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us &&
-			_sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) &&
-			mag0.length() > 0.1f) {
-			_ekf->magData.x = mag0.x;
-			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+			const unsigned mag_timeout_us = 200000;
 
-			_ekf->magData.y = mag0.y;
-			_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+			/* fail over to the 2nd mag if we know the first is down */
+			if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp[i]) < mag_timeout_us &&
+				_sensor_combined.magnetometer_errcount[i] < (best_mag_err + MAG_SWITCH_HYSTERESIS) &&
+				mag.length() > 0.1f) {
+				_ekf->magData.x = mag.x;
+				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
 
-			_ekf->magData.z = mag0.z;
-			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
-			_mag_main = 0;
+				_ekf->magData.y = mag.y;
+				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
 
-		} else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us &&
-			mag1.length() > 0.1f) {
-			_ekf->magData.x = mag1.x;
-			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
-
-			_ekf->magData.y = mag1.y;
-			_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
-
-			_ekf->magData.z = mag1.z;
-			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
-			_mag_main = 1;
-		} else {
-			_mag_valid = false;
+				_ekf->magData.z = mag.z;
+				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+				_mag_main = i;
+				best_mag_err = _sensor_combined.magnetometer_errcount[i];
+				_mag_valid = true;
+			}
 		}
 
 		if (last_mag_main != _mag_main) {

From caacb4f7774f79c146cb3a45387c77371a86fadf Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:10:28 +0200
Subject: [PATCH 36/62] MAVLink: Update to new sensors combined topic

---
 src/modules/mavlink/mavlink_messages.cpp | 24 ++++++++++----------
 src/modules/mavlink/mavlink_receiver.cpp | 28 ++++++++++++------------
 2 files changed, 26 insertions(+), 26 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 67809ed7be..3e22a09148 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -643,28 +643,28 @@ protected:
 		if (_sensor_sub->update(&_sensor_time, &sensor)) {
 			uint16_t fields_updated = 0;
 
-			if (_accel_timestamp != sensor.accelerometer_timestamp) {
+			if (_accel_timestamp != sensor.accelerometer_timestamp[0]) {
 				/* mark first three dimensions as changed */
 				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-				_accel_timestamp = sensor.accelerometer_timestamp;
+				_accel_timestamp = sensor.accelerometer_timestamp[0];
 			}
 
-			if (_gyro_timestamp != sensor.timestamp) {
+			if (_gyro_timestamp != sensor.gyro_timestamp[0]) {
 				/* mark second group dimensions as changed */
 				fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-				_gyro_timestamp = sensor.timestamp;
+				_gyro_timestamp = sensor.gyro_timestamp[0];
 			}
 
-			if (_mag_timestamp != sensor.magnetometer_timestamp) {
+			if (_mag_timestamp != sensor.magnetometer_timestamp[0]) {
 				/* mark third group dimensions as changed */
 				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-				_mag_timestamp = sensor.magnetometer_timestamp;
+				_mag_timestamp = sensor.magnetometer_timestamp[0];
 			}
 
-			if (_baro_timestamp != sensor.baro_timestamp) {
+			if (_baro_timestamp != sensor.baro_timestamp[0]) {
 				/* mark last group dimensions as changed */
 				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-				_baro_timestamp = sensor.baro_timestamp;
+				_baro_timestamp = sensor.baro_timestamp[0];
 			}
 
 			mavlink_highres_imu_t msg;
@@ -679,10 +679,10 @@ protected:
 			msg.xmag = sensor.magnetometer_ga[0];
 			msg.ymag = sensor.magnetometer_ga[1];
 			msg.zmag = sensor.magnetometer_ga[2];
-			msg.abs_pressure = sensor.baro_pres_mbar;
-			msg.diff_pressure = sensor.differential_pressure_pa;
-			msg.pressure_alt = sensor.baro_alt_meter;
-			msg.temperature = sensor.baro_temp_celcius;
+			msg.abs_pressure = sensor.baro_pres_mbar[0];
+			msg.diff_pressure = sensor.differential_pressure_pa[0];
+			msg.pressure_alt = sensor.baro_alt_meter[0];
+			msg.temperature = sensor.baro_temp_celcius[0];
 			msg.fields_updated = fields_updated;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 24d0940477..3b20fd6b76 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1433,9 +1433,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.accelerometer_m_s2[0] = imu.xacc;
 		hil_sensors.accelerometer_m_s2[1] = imu.yacc;
 		hil_sensors.accelerometer_m_s2[2] = imu.zacc;
-		hil_sensors.accelerometer_mode = 0; // TODO what is this?
-		hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
-		hil_sensors.accelerometer_timestamp = timestamp;
+		hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?
+		hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16
+		hil_sensors.accelerometer_timestamp[0] = timestamp;
 
 		hil_sensors.adc_voltage_v[0] = 0.0f;
 		hil_sensors.adc_voltage_v[1] = 0.0f;
@@ -1447,19 +1447,19 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.magnetometer_ga[0] = imu.xmag;
 		hil_sensors.magnetometer_ga[1] = imu.ymag;
 		hil_sensors.magnetometer_ga[2] = imu.zmag;
-		hil_sensors.magnetometer_range_ga = 32.7f; // int16
-		hil_sensors.magnetometer_mode = 0; // TODO what is this
-		hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
-		hil_sensors.magnetometer_timestamp = timestamp;
+		hil_sensors.magnetometer_range_ga[0] = 32.7f; // int16
+		hil_sensors.magnetometer_mode[0] = 0; // TODO what is this
+		hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f;
+		hil_sensors.magnetometer_timestamp[0] = timestamp;
 
-		hil_sensors.baro_pres_mbar = imu.abs_pressure;
-		hil_sensors.baro_alt_meter = imu.pressure_alt;
-		hil_sensors.baro_temp_celcius = imu.temperature;
-		hil_sensors.baro_timestamp = timestamp;
+		hil_sensors.baro_pres_mbar[0] = imu.abs_pressure;
+		hil_sensors.baro_alt_meter[0] = imu.pressure_alt;
+		hil_sensors.baro_temp_celcius[0] = imu.temperature;
+		hil_sensors.baro_timestamp[0] = timestamp;
 
-		hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
-		hil_sensors.differential_pressure_filtered_pa = hil_sensors.differential_pressure_pa;
-		hil_sensors.differential_pressure_timestamp = timestamp;
+		hil_sensors.differential_pressure_pa[0] = imu.diff_pressure * 1e2f; //from hPa to Pa
+		hil_sensors.differential_pressure_filtered_pa[0] = hil_sensors.differential_pressure_pa[0];
+		hil_sensors.differential_pressure_timestamp[0] = timestamp;
 
 		/* publish combined sensor topic */
 		if (_sensors_pub == nullptr) {

From 502a25d27fdd96b11abfd2627fbe2a11c2c8d63e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:10:50 +0200
Subject: [PATCH 37/62] Navigator: Update to new sensors combined topic

---
 src/modules/navigator/navigator_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index d63f5fe338..1055cb869b 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -406,7 +406,7 @@ Navigator::task_main()
 		/* Check geofence violation */
 		static hrt_abstime last_geofence_check = 0;
 		if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
-			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set);
+			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set);
 			last_geofence_check = hrt_absolute_time();
 			have_geofence_position_data = false;
 			if (!inside) {

From ae569ba5438466473a892f8fb15a59fe52bf6670 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:11:05 +0200
Subject: [PATCH 38/62] INAV: Update to new sensors combined topic

---
 .../position_estimator_inav_main.c             | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 892e50f3a1..ee8359f0c3 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -399,13 +399,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (fds_init[0].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
-					baro_timestamp = sensor.baro_timestamp;
+				if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
+					baro_timestamp = sensor.baro_timestamp[0];
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {
-						if (PX4_ISFINITE(sensor.baro_alt_meter)) {
-							baro_offset += sensor.baro_alt_meter;
+						if (PX4_ISFINITE(sensor.baro_alt_meter[0])) {
+							baro_offset += sensor.baro_alt_meter[0];
 							baro_init_cnt++;
 						}
 
@@ -474,7 +474,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (updated) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (sensor.accelerometer_timestamp != accel_timestamp) {
+				if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
 					if (att.R_valid) {
 						/* correct accel bias */
 						sensor.accelerometer_m_s2[0] -= acc_bias[0];
@@ -496,13 +496,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						memset(acc, 0, sizeof(acc));
 					}
 
-					accel_timestamp = sensor.accelerometer_timestamp;
+					accel_timestamp = sensor.accelerometer_timestamp[0];
 					accel_updates++;
 				}
 
-				if (sensor.baro_timestamp != baro_timestamp) {
-					corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
-					baro_timestamp = sensor.baro_timestamp;
+				if (sensor.baro_timestamp[0] != baro_timestamp) {
+					corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0];
+					baro_timestamp = sensor.baro_timestamp[0];
 					baro_updates++;
 				}
 			}

From ad14b471e3d72c1b9d0a981dc108dc10cd28ea9b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:11:23 +0200
Subject: [PATCH 39/62] sdlog2: Update to new sensors combined topic

---
 src/modules/sdlog2/module.mk |   2 +-
 src/modules/sdlog2/sdlog2.c  | 231 +++++++++++++----------------------
 2 files changed, 84 insertions(+), 149 deletions(-)

diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk
index d22d0a9d93..69afb39099 100644
--- a/src/modules/sdlog2/module.mk
+++ b/src/modules/sdlog2/module.mk
@@ -47,5 +47,5 @@ MODULE_STACKSIZE = 1200
 MAXOPTIMIZATION	 = -Os
 
 ifeq ($(PX4_TARGET_OS),nuttx)
-EXTRACFLAGS = -Wframe-larger-than=1400
+EXTRACFLAGS = -Wframe-larger-than=1600
 endif
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index c6d13f8756..7750b885f7 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1284,18 +1284,11 @@ int sdlog2_thread_main(int argc, char *argv[])
 	pthread_cond_init(&logbuffer_cond, NULL);
 
 	/* track changes in sensor_combined topic */
-	hrt_abstime gyro_timestamp = 0;
-	hrt_abstime accelerometer_timestamp = 0;
-	hrt_abstime magnetometer_timestamp = 0;
-	hrt_abstime barometer_timestamp = 0;
-	hrt_abstime differential_pressure_timestamp = 0;
-	hrt_abstime barometer1_timestamp = 0;
-	hrt_abstime gyro1_timestamp = 0;
-	hrt_abstime accelerometer1_timestamp = 0;
-	hrt_abstime magnetometer1_timestamp = 0;
-	hrt_abstime gyro2_timestamp = 0;
-	hrt_abstime accelerometer2_timestamp = 0;
-	hrt_abstime magnetometer2_timestamp = 0;
+	hrt_abstime gyro_timestamp[3] = {0, 0, 0};
+	hrt_abstime accelerometer_timestamp[3] = {0, 0, 0};
+	hrt_abstime magnetometer_timestamp[3] = {0, 0, 0};
+	hrt_abstime barometer_timestamp[3] = {0, 0, 0};
+	hrt_abstime differential_pressure_timestamp[3] = {0, 0, 0};
 
 	/* initialize calculated mean SNR */
 	float snr_mean = 0.0f;
@@ -1443,144 +1436,86 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- SENSOR COMBINED --- */
 		if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) {
-			bool write_IMU = false;
-			bool write_IMU1 = false;
-			bool write_IMU2 = false;
-			bool write_SENS = false;
-			bool write_SENS1 = false;
+			
 
-			if (buf.sensor.timestamp != gyro_timestamp) {
-				gyro_timestamp = buf.sensor.timestamp;
-				write_IMU = true;
+			for (unsigned i = 0; i < 3; i++) {
+				bool write_IMU = false;
+				bool write_SENS = false;
+
+				if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) {
+					gyro_timestamp[i] = buf.sensor.gyro_timestamp[i];
+					write_IMU = true;
+				}
+
+				if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) {
+					accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i];
+					write_IMU = true;
+				}
+
+				if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) {
+					magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i];
+					write_IMU = true;
+				}
+
+				if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) {
+					barometer_timestamp[i] = buf.sensor.baro_timestamp[i];
+					write_SENS = true;
+				}
+
+				if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) {
+					differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i];
+					write_SENS = true;
+				}
+
+				if (write_IMU) {
+					switch (i) {
+						case 0:
+							log_msg.msg_type = LOG_IMU_MSG;
+							break;
+						case 1:
+							log_msg.msg_type = LOG_IMU1_MSG;
+							break;
+						case 2:
+							log_msg.msg_type = LOG_IMU2_MSG;
+							break;
+					}
+
+					log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0];
+					log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1];
+					log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2];
+					log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0];
+					log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1];
+					log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2];
+					log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0];
+					log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1];
+					log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2];
+					log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i * 3 + 0];
+					log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i * 3 + 0];
+					log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i * 3 + 0];
+					LOGBUFFER_WRITE_AND_COUNT(IMU);
+				}
+
+				if (write_SENS) {
+					switch (i) {
+						case 0:
+							log_msg.msg_type = LOG_SENS_MSG;
+							break;
+						case 1:
+							log_msg.msg_type = LOG_AIR1_MSG;
+							break;
+						case 2:
+							continue;
+							break;
+					}
+
+					log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i];
+					log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i];
+					log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i];
+					log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i];
+					log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i];
+					LOGBUFFER_WRITE_AND_COUNT(SENS);
+				}
 			}
-
-			if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
-				accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
-				write_IMU = true;
-			}
-
-			if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
-				magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
-				write_IMU = true;
-			}
-
-			if (buf.sensor.baro_timestamp != barometer_timestamp) {
-				barometer_timestamp = buf.sensor.baro_timestamp;
-				write_SENS = true;
-			}
-
-			if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
-				differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
-				write_SENS = true;
-			}
-
-			if (write_IMU) {
-				log_msg.msg_type = LOG_IMU_MSG;
-				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0];
-				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1];
-				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2];
-				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
-				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
-				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
-				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
-				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
-				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
-				log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp;
-				log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp;
-				log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp;
-				LOGBUFFER_WRITE_AND_COUNT(IMU);
-			}
-
-			if (write_SENS) {
-				log_msg.msg_type = LOG_SENS_MSG;
-				log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar;
-				log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
-				log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
-				log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
-				log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa;
-				LOGBUFFER_WRITE_AND_COUNT(SENS);
-			}
-
-			if (buf.sensor.baro1_timestamp != barometer1_timestamp) {
-				barometer1_timestamp = buf.sensor.baro1_timestamp;
-				write_SENS1 = true;
-			}
-
-			if (write_SENS1) {
-				log_msg.msg_type = LOG_AIR1_MSG;
-				log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar;
-				log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter;
-				log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius;
-				log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa;
-				log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa;
-				// XXX moving to AIR0-AIR2 instead of SENS
-				LOGBUFFER_WRITE_AND_COUNT(SENS);
-			}
-
-			if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
-				accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
-				write_IMU1 = true;
-			}
-
-			if (buf.sensor.gyro1_timestamp != gyro1_timestamp) {
-				gyro1_timestamp = buf.sensor.gyro1_timestamp;
-				write_IMU1 = true;
-			}
-
-			if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) {
-				magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp;
-				write_IMU1 = true;
-			}
-
-			if (write_IMU1) {
-				log_msg.msg_type = LOG_IMU1_MSG;
-				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0];
-				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1];
-				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2];
-				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0];
-				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1];
-				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2];
-				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0];
-				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1];
-				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2];
-				log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro1_temp;
-				log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer1_temp;
-				log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer1_temp;
-				LOGBUFFER_WRITE_AND_COUNT(IMU);
-			}
-
-			if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) {
-				accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp;
-				write_IMU2 = true;
-			}
-
-			if (buf.sensor.gyro2_timestamp != gyro2_timestamp) {
-				gyro2_timestamp = buf.sensor.gyro2_timestamp;
-				write_IMU2 = true;
-			}
-
-			if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) {
-				magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp;
-				write_IMU2 = true;
-			}
-
-			if (write_IMU2) {
-				log_msg.msg_type = LOG_IMU2_MSG;
-				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0];
-				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1];
-				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2];
-				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0];
-				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1];
-				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2];
-				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0];
-				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1];
-				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2];
-				log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro2_temp;
-				log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer2_temp;
-				log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer2_temp;
-				LOGBUFFER_WRITE_AND_COUNT(IMU);
-			}
-
 		}
 
 		/* --- ATTITUDE --- */

From e76037233cccbc36d3ac6747f6931d1392377e7a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:16:45 +0200
Subject: [PATCH 40/62] Attitude only EKF: Update to new sensor combined topic

---
 .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp  | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index a320ffe518..02940241c7 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -383,10 +383,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					uint8_t update_vect[3] = {0, 0, 0};
 
 					/* Fill in gyro measurements */
-					if (sensor_last_timestamp[0] != raw.timestamp) {
+					if (sensor_last_timestamp[0] != raw.gyro_timestamp[0]) {
 						update_vect[0] = 1;
 						// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
-						sensor_last_timestamp[0] = raw.timestamp;
+						sensor_last_timestamp[0] = raw.gyro_timestamp[0];
 					}
 
 					z_k[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
@@ -394,7 +394,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) {
 						update_vect[1] = 1;
 						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
 						sensor_last_timestamp[1] = raw.accelerometer_timestamp;

From 221b2f1331b303b5ac5a0a81fa001d0252bb9381 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:18:39 +0200
Subject: [PATCH 41/62] Attitude only EKF: Build fix

---
 .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp  | 6 +++---
 src/modules/attitude_estimator_ekf/module.mk                | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 02940241c7..14f5470425 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -397,7 +397,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) {
 						update_vect[1] = 1;
 						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp[0];
 					}
 
 					hrt_abstime vel_t = 0;
@@ -437,14 +437,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* update magnetometer measurements */
-					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp &&
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] &&
 						/* check that the mag vector is > 0 */
 						fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
 							raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
 							raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
 						update_vect[2] = 1;
 						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp[0];
 					}
 
 					bool vision_updated = false;
diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk
index 20eda27690..716b0aefe4 100644
--- a/src/modules/attitude_estimator_ekf/module.mk
+++ b/src/modules/attitude_estimator_ekf/module.mk
@@ -46,5 +46,5 @@ MODULE_STACKSIZE = 1200
 EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700
 
 ifeq ($(PX4_TARGET_OS),nuttx)
-EXTRACXXFLAGS = -Wframe-larger-than=2400
+EXTRACXXFLAGS = -Wframe-larger-than=2600
 endif

From f539246e4fb81d4319e58b979a0f3320380395f7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 00:29:58 +0200
Subject: [PATCH 42/62] MAVLink app: Set gyro timestamp in HIL

---
 src/modules/mavlink/mavlink_receiver.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3b20fd6b76..3daeb32426 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1426,6 +1426,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.gyro_rad_s[0] = imu.xgyro;
 		hil_sensors.gyro_rad_s[1] = imu.ygyro;
 		hil_sensors.gyro_rad_s[2] = imu.zgyro;
+		hil_sensors.gyro_timestamp[0] = timestamp;
 
 		hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
 		hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;

From 0732ec650fa5db3306ee926417c8999436d820e0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 11:51:26 +0200
Subject: [PATCH 43/62] Q estimator: Use all available sensor instances

---
 src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index b8a5481466..df795b8c32 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -289,7 +289,7 @@ void AttitudeEstimatorQ::task_main()
 		if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
 			// Feed validator with recent sensor data
 
-			for (unsigned i = 0; i < 2; i++) {
+			for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
 				_voter_gyro.put(i, sensors.gyro_timestamp[i], &sensors.gyro_rad_s[i * 3], sensors.gyro_errcount[i]);
 				_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], sensors.accelerometer_errcount[i]);
 				_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], sensors.magnetometer_errcount[i]);

From a7e3232e7fbd87b9e1f2839fd4da24ec34758c4f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 11:52:31 +0200
Subject: [PATCH 44/62] sensors app: Initialize class count, remove magic
 numbers, ensure that the sensor combined struct cannot overflow

---
 src/modules/sensors/sensors.cpp | 66 ++++++++++++++++++++-------------
 1 file changed, 40 insertions(+), 26 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index edbcce6bb6..282401f58c 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -129,6 +129,8 @@
 #define STICK_ON_OFF_LIMIT		0.75f
 #define MAG_ROT_VAL_INTERNAL		-1
 
+#define SENSOR_COUNT_MAX		3
+
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -201,21 +203,21 @@ private:
 	bool		_hil_enabled;			/**< if true, HIL is active */
 	bool		_publishing;			/**< if true, we are publishing sensor data */
 
-	int		_gyro_sub[ORB_MULTI_MAX_INSTANCES - 1];			/**< raw gyro0 data subscription */
-	int		_accel_sub[ORB_MULTI_MAX_INSTANCES - 1];			/**< raw accel0 data subscription */
-	int		_mag_sub[ORB_MULTI_MAX_INSTANCES - 1];			/**< raw mag0 data subscription */
-	int		_baro_sub[ORB_MULTI_MAX_INSTANCES - 1];			/**< raw baro0 data subscription */
-	unsigned	_gyro_count;			/**< raw gyro0 data subscription */
-	unsigned	_accel_count;			/**< raw accel0 data subscription */
-	unsigned	_mag_count;			/**< raw mag0 data subscription */
-	unsigned	_baro_count;			/**< raw baro0 data subscription */
+	int		_gyro_sub[SENSOR_COUNT_MAX];	/**< raw gyro data subscription */
+	int		_accel_sub[SENSOR_COUNT_MAX];	/**< raw accel data subscription */
+	int		_mag_sub[SENSOR_COUNT_MAX];	/**< raw mag data subscription */
+	int		_baro_sub[SENSOR_COUNT_MAX];	/**< raw baro data subscription */
+	unsigned	_gyro_count;			/**< raw gyro data count */
+	unsigned	_accel_count;			/**< raw accel data count */
+	unsigned	_mag_count;			/**< raw mag data count */
+	unsigned	_baro_count;			/**< raw baro data count */
 	
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_diff_pres_sub;			/**< raw differential pressure subscription */
-	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
+	int		_vcontrol_mode_sub;		/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
-	int		_rc_parameter_map_sub;			/**< rc parameter map subscription */
-	int 		_manual_control_sub;			/**< notification of manual control updates */
+	int		_rc_parameter_map_sub;		/**< rc parameter map subscription */
+	int 		_manual_control_sub;		/**< notification of manual control updates */
 
 	orb_advert_t	_sensor_pub;			/**< combined sensor data topic */
 	orb_advert_t	_manual_control_pub;		/**< manual control signal topic */
@@ -236,7 +238,7 @@ private:
 	float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];	/**< parameter values for RC control */
 
 	math::Matrix<3, 3>	_board_rotation;	/**< rotation matrix for the orientation that the board is mounted */
-	math::Matrix<3, 3>	_mag_rotation[3];		/**< rotation matrix for the orientation that the external mag0 is mounted */
+	math::Matrix<3, 3>	_mag_rotation[3];	/**< rotation matrix for the orientation that the external mag0 is mounted */
 
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
 	hrt_abstime _battery_current_timestamp;		/**< timestamp of last battery current reading */
@@ -362,8 +364,8 @@ private:
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
-	void		init_sensor_class(const struct orb_metadata *meta, int *subs,
-		unsigned *priorities, unsigned *errcount);
+	int		init_sensor_class(const struct orb_metadata *meta, int *subs,
+				unsigned *priorities, unsigned *errcount);
 
 	/**
 	 * Update our local parameter cache.
@@ -1226,7 +1228,7 @@ Sensors::parameter_update_poll(bool forced)
 		unsigned accel_count = 0;
 
 		/* run through all gyro sensors */
-		for (unsigned s = 0; s < 3; s++) {
+		for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
 
 			res = ERROR;
 			(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
@@ -1240,7 +1242,7 @@ Sensors::parameter_update_poll(bool forced)
 			bool config_ok = false;
 
 			/* run through all stored calibrations */ 
-			for (unsigned i = 0; i < 3; i++) {
+			for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
 				/* initially status is ok per config */
 				failed = false;
 
@@ -1292,7 +1294,7 @@ Sensors::parameter_update_poll(bool forced)
 		}
 
 		/* run through all accel sensors */
-		for (unsigned s = 0; s < 3; s++) {
+		for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
 
 			res = ERROR;
 			(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
@@ -1306,7 +1308,7 @@ Sensors::parameter_update_poll(bool forced)
 			bool config_ok = false;
 
 			/* run through all stored calibrations */ 
-			for (unsigned i = 0; i < 3; i++) {
+			for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
 				/* initially status is ok per config */
 				failed = false;
 
@@ -1358,7 +1360,7 @@ Sensors::parameter_update_poll(bool forced)
 		}
 
 		/* run through all mag sensors */
-		for (unsigned s = 0; s < 3; s++) {
+		for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
 
 			/* set a valid default rotation (same as board).
 			 * if the mag is configured, this might be replaced
@@ -1379,7 +1381,7 @@ Sensors::parameter_update_poll(bool forced)
 			bool config_ok = false;
 
 			/* run through all stored calibrations */ 
-			for (unsigned i = 0; i < 3; i++) {
+			for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
 				/* initially status is ok per config */
 				failed = false;
 
@@ -1934,15 +1936,23 @@ Sensors::task_main_trampoline(int argc, char *argv[])
 	sensors::g_sensors->task_main();
 }
 
-void
+int
 Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
 	unsigned *priorities, unsigned *errcount)
 {
-	for (unsigned i = 0; i < (unsigned)orb_group_count(meta); i++) {
+	unsigned group_count = orb_group_count(meta);
+
+	if (group_count > SENSOR_COUNT_MAX) {
+		group_count = SENSOR_COUNT_MAX;
+	}
+
+	for (unsigned i = 0; i < group_count; i++) {
 		subs[i] = orb_subscribe_multi(meta, i);
 		orb_priority(subs[i], (int32_t*)&priorities[i]);
 		errcount[i] = 100000;
 	}
+
+	return group_count;
 }
 
 void
@@ -1977,20 +1987,24 @@ Sensors::task_main()
 
 	struct sensor_combined_s raw = {};
 
+	/* ensure no overflows can occur */
+	static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX,
+		"SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur");
+
 	/*
 	 * do subscriptions
 	 */
 
-	init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
+	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
 		&raw.gyro_priority[0], &raw.gyro_errcount[0]);
 
-	init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
+	_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
 		&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
 
-	init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
+	_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
 		&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
 
-	init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
+	_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
 		&raw.baro_priority[0], &raw.baro_errcount[0]);
 
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));

From fcb25fd02c2b7ec60d9f12005ee2422214ece2b7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 12:52:58 +0200
Subject: [PATCH 45/62] Data validator: add missing header

---
 src/lib/ecl/validation/data_validator.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h
index 26d55af11e..de87a49f63 100644
--- a/src/lib/ecl/validation/data_validator.h
+++ b/src/lib/ecl/validation/data_validator.h
@@ -41,6 +41,8 @@
 
 #pragma once
 
+#include <cmath>
+
 class DataValidator {
 public:
 	DataValidator(DataValidator *prev_sibling = nullptr);

From 68666aa393aac7cd4fc742e39393731c7090df70 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 12:53:22 +0200
Subject: [PATCH 46/62] EKF: Use voting class instead of special routines to
 select sensor

---
 .../AttitudePositionEstimatorEKF.h            |  19 +-
 .../ekf_att_pos_estimator_main.cpp            | 408 ++++++++----------
 2 files changed, 192 insertions(+), 235 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index 28abb0818b..6251f8f5cb 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -70,6 +70,7 @@
 
 #include <geo/geo.h>
 #include <systemlib/perf_counter.h>
+#include <lib/ecl/validation/data_validator_group.h>
 #include "estimator_22states.h"
 
 //Forward declaration
@@ -169,10 +170,8 @@ private:
     struct vehicle_land_detected_s      _landDetector;
     struct actuator_armed_s             _armed;
 
-    Vector3f lastAngRate;
-    Vector3f lastAccel;
-    hrt_abstime last_accel;
-    hrt_abstime last_mag;
+    hrt_abstime _last_accel;
+    hrt_abstime _last_mag;
 
     struct gyro_scale               _gyro_offsets[3];
     struct accel_scale              _accel_offsets[3];
@@ -185,6 +184,8 @@ private:
     float                       _filter_ref_offset;   /**< offset between initial baro reference and GPS init baro altitude */
     float                       _baro_gps_offset;   /**< offset between baro altitude (at GPS init time) and GPS altitude */
     hrt_abstime                 _last_debug_print = 0;
+    float       _vibration_warning_threshold = 1.0f;
+    hrt_abstime _vibration_warning_timestamp = 0;
 
     perf_counter_t  _loop_perf;         ///< loop performance counter
     perf_counter_t  _loop_intvl;        ///< loop rate counter
@@ -204,14 +205,16 @@ private:
     bool            _gps_initialized;
     hrt_abstime     _filter_start_time;
     hrt_abstime     _last_sensor_timestamp;
-    hrt_abstime     _last_run;
     hrt_abstime     _distance_last_valid;
-    bool            _gyro_valid;
-    bool            _accel_valid;
-    bool            _mag_valid;
+    DataValidatorGroup _voter_gyro;
+    DataValidatorGroup _voter_accel;
+    DataValidatorGroup _voter_mag;
     int             _gyro_main;         ///< index of the main gyroscope
     int             _accel_main;        ///< index of the main accelerometer
     int             _mag_main;          ///< index of the main magnetometer
+    bool            _data_good;         ///< all required filter data is ok
+    bool            _failsafe;          ///< failsafe on one of the sensors
+    bool            _vibration_warning; ///< high vibration levels detected
     bool            _ekf_logging;       ///< log EKF state
     unsigned        _debug;             ///< debug level - default 0
 
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 7de7e04613..95b4a90251 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -68,7 +68,6 @@
 #include <mavlink/mavlink_log.h>
 #include <platforms/px4_defines.h>
 
-static uint64_t IMUmsec = 0;
 static uint64_t IMUusec = 0;
 
 //Constants
@@ -88,13 +87,18 @@ static constexpr float EPV_LARGE_VALUE = 1000.0f;
  */
 extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
 
-__EXPORT uint32_t millis();
-
-__EXPORT uint64_t getMicros();
+uint32_t millis();
+uint64_t getMicros();
+uint32_t getMillis();
 
 uint32_t millis()
 {
-	return IMUmsec;
+	return getMillis();
+}
+
+uint32_t getMillis()
+{
+	return getMicros() / 1000;
 }
 
 uint64_t getMicros()
@@ -133,89 +137,88 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_landDetectorSub(-1),
 	_armedSub(-1),
 
-/* publications */
+	/* publications */
 	_att_pub(nullptr),
 	_global_pos_pub(nullptr),
 	_local_pos_pub(nullptr),
 	_estimator_status_pub(nullptr),
 	_wind_pub(nullptr),
 
-	_att({}),
-    _gyro({}),
-    _accel({}),
-    _mag({}),
-    _airspeed({}),
-    _baro({}),
-    _vstatus({}),
-    _global_pos({}),
-    _local_pos({}),
-    _gps({}),
-    _wind({}),
-    _distance {},
-    _landDetector {},
-    _armed {},
+	_att{},
+	_gyro{},
+	_accel{},
+	_mag{},
+	_airspeed{},
+	_baro{},
+	_vstatus{},
+	_global_pos{},
+	_local_pos{},
+	_gps{},
+	_wind{},
+	_distance{},
+	_landDetector{},
+	_armed{},
 
-    lastAngRate{},
-    lastAccel{},
-    last_accel(0),
-    last_mag(0),
+	_last_accel(0),
+	_last_mag(0),
 
-    _gyro_offsets({}),
-    _accel_offsets({}),
-    _mag_offsets({}),
+	_gyro_offsets{},
+	_accel_offsets{},
+	_mag_offsets{},
 
-    _sensor_combined {},
+	_sensor_combined{},
 
-    _pos_ref{},
-    _filter_ref_offset(0.0f),
-    _baro_gps_offset(0.0f),
+	 _pos_ref{},
+	_filter_ref_offset(0.0f),
+	_baro_gps_offset(0.0f),
 
-    /* performance counters */
-    _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
-    _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
-    _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
-    _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
-    _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
-    _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
-    _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
-    _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
+	/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
+	_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
+	_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
+	_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
+	_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
+	_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
+	_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
+	_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
 
-    /* states */
-    _gps_alt_filt(0.0f),
-    _baro_alt_filt(0.0f),
-    _covariancePredictionDt(0.0f),
-    _gpsIsGood(false),
-    _previousGPSTimestamp(0),
-    _baro_init(false),
-    _gps_initialized(false),
-    _filter_start_time(0),
-    _last_sensor_timestamp(0),
-    _last_run(0),
-    _distance_last_valid(0),
-    _gyro_valid(false),
-    _accel_valid(false),
-    _mag_valid(false),
-    _gyro_main(0),
-    _accel_main(0),
-    _mag_main(0),
-    _ekf_logging(true),
-    _debug(0),
+	      /* states */
+	_gps_alt_filt(0.0f),
+	_baro_alt_filt(0.0f),
+	_covariancePredictionDt(0.0f),
+	_gpsIsGood(false),
+	_previousGPSTimestamp(0),
+	_baro_init(false),
+	_gps_initialized(false),
+	_filter_start_time(0),
+	_last_sensor_timestamp(hrt_absolute_time()),
+	_distance_last_valid(0),
+	_voter_gyro(3),
+	_voter_accel(3),
+	_voter_mag(3),
+	_gyro_main(-1),
+	_accel_main(-1),
+	_mag_main(-1),
+	_data_good(false),
+	_failsafe(false),
+	_vibration_warning(false),
+	_ekf_logging(true),
+	_debug(0),
 
-    _newHgtData(false),
-    _newAdsData(false),
-    _newDataMag(false),
-    _newRangeData(false),
+	_newHgtData(false),
+	_newAdsData(false),
+	_newDataMag(false),
+	_newRangeData(false),
 
-    _mavlink_fd(-1),
-    _parameters{},
-    _parameter_handles{},
-    _ekf(nullptr),
+	_mavlink_fd(-1),
+	_parameters{},
+	_parameter_handles{},
+	_ekf(nullptr),
 
-    _LP_att_P(100.0f, 10.0f),
-    _LP_att_Q(100.0f, 10.0f),
-    _LP_att_R(100.0f, 10.0f)
+	_LP_att_P(100.0f, 10.0f),
+	_LP_att_Q(100.0f, 10.0f),
+	_LP_att_R(100.0f, 10.0f)
 {
-	_last_run = hrt_absolute_time();
 
 	_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
 	_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
@@ -613,15 +616,14 @@ void AttitudePositionEstimatorEKF::task_main()
 				orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
 				/* set sensors to de-initialized state */
-				_gyro_valid = false;
-				_accel_valid = false;
-				_mag_valid = false;
+				_gyro_main = -1;
+				_accel_main = -1;
+				_mag_main = -1;
 
 				_baro_init = false;
 				_gps_initialized = false;
 
 				_last_sensor_timestamp = hrt_absolute_time();
-				_last_run = _last_sensor_timestamp;
 
 				_ekf->ZeroVariables();
 				_ekf->dtIMU = 0.01f;
@@ -649,7 +651,7 @@ void AttitudePositionEstimatorEKF::task_main()
 			 *    We run the filter only once all data has been fetched
 			 **/
 
-			if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
+			if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) {
 
 				// maintain filtered baro and gps altitudes to calculate weather offset
 				// baro sample rate is ~70Hz and measurement bandwidth is high
@@ -1012,7 +1014,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 	_ekf->UpdateStrapdownEquationsNED();
 
 	// store the predicted states for subsequent use by measurement fusion
-	_ekf->StoreStates(IMUmsec);
+	_ekf->StoreStates(getMillis());
 
 	// sum delta angles and time used by covariance prediction
 	_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
@@ -1038,8 +1040,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 		_ekf->fusePosData = true;
 
 		// recall states stored at time of measurement after adjusting for delays
-		_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-		_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+		_ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms));
+		_ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms));
 
 		// run the fusion step
 		_ekf->FuseVelposNED();
@@ -1062,8 +1064,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 		_ekf->fusePosData = true;
 
 		// recall states stored at time of measurement after adjusting for delays
-		_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-		_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+		_ekf->RecallStates(_ekf->statesAtVelTime, (getMillis() - _parameters.vel_delay_ms));
+		_ekf->RecallStates(_ekf->statesAtPosTime, (getMillis() - _parameters.pos_delay_ms));
 
 		// run the fusion step
 		_ekf->FuseVelposNED();
@@ -1079,7 +1081,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 		_ekf->fuseHgtData = true;
 
 		// recall states stored at time of measurement after adjusting for delays
-		_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
+		_ekf->RecallStates(_ekf->statesAtHgtTime, (getMillis() - _parameters.height_delay_ms));
 
 		// run the fusion step
 		_ekf->FuseVelposNED();
@@ -1092,7 +1094,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 	if (fuseMag) {
 		_ekf->fuseMagData = true;
 		_ekf->RecallStates(_ekf->statesAtMagMeasTime,
-				   (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
+				   (getMillis() - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
 
 		_ekf->magstate.obsIndex = 0;
 		_ekf->FuseMagnetometer();
@@ -1107,7 +1109,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 	if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) {
 		_ekf->fuseVtasData = true;
 		_ekf->RecallStates(_ekf->statesAtVtasMeasTime,
-				   (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
+				   (getMillis() - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
 		_ekf->FuseAirspeed();
 
 	} else {
@@ -1120,7 +1122,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 			// _ekf->rngMea is set in sensor readout already
 			_ekf->fuseRngData = true;
 			_ekf->fuseOptFlowData = false;
-			_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
+			_ekf->RecallStates(_ekf->statesAtRngTime, (getMillis() - 100.0f));
 			_ekf->OpticalFlowEKF();
 			_ekf->fuseRngData = false;
 		}
@@ -1166,7 +1168,7 @@ void AttitudePositionEstimatorEKF::print_status()
 	// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
 	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
-	PX4_INFO("dtIMU: %8.6f filt: %8.6f IMUmsec: %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec);
+	PX4_INFO("dtIMU: %8.6f filt: %8.6f IMU (ms): %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)getMillis());
 	PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt);
 	PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt);
 	PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset,
@@ -1213,6 +1215,13 @@ void AttitudePositionEstimatorEKF::print_status()
 	       (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
 	       (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
 	       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
+
+	PX4_INFO("gyro status:");
+	_voter_gyro.print();
+	PX4_INFO("accel status:");
+	_voter_accel.print();
+	PX4_INFO("mag status:");
+	_voter_mag.print();
 }
 
 void AttitudePositionEstimatorEKF::pollData()
@@ -1225,27 +1234,99 @@ void AttitudePositionEstimatorEKF::pollData()
 		orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
 	}
 
-	//Update Gyro and Accelerometer
-	bool accel_updated = false;
-
 	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
-	if (last_accel != _sensor_combined.accelerometer_timestamp[_accel_main]) {
-		accel_updated = true;
+	// Feed validator with recent sensor data
 
-	} else {
-		accel_updated = false;
+	for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
+		_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], _sensor_combined.gyro_errcount[i]);
+		_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], _sensor_combined.accelerometer_errcount[i]);
+		_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], _sensor_combined.magnetometer_errcount[i]);
 	}
 
-	last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
+	// Get best measurement values
+	hrt_abstime curr_time = hrt_absolute_time();
+	(void)_voter_gyro.get_best(curr_time, &_gyro_main);
+	if (_gyro_main >= 0) {
+		_ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0];
+		_ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1];
+		_ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2];
+		_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
+		_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
+		_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
+		perf_count(_perf_gyro);
+	}
 
+	(void)_voter_accel.get_best(curr_time, &_accel_main);
+	if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
+		_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
+		_ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1];
+		_ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2];
+		_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
+		_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
+		_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
+		_last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
+	}
+
+	(void)_voter_mag.get_best(curr_time, &_mag_main);
+	if (_mag_main >= 0) {
+		Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1],
+				_sensor_combined.magnetometer_ga[_mag_main * 3 + 2]);
+
+		/* fail over to the 2nd mag if we know the first is down */
+		if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) {
+			_ekf->magData.x = mag.x;
+			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+
+			_ekf->magData.y = mag.y;
+			_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+			_ekf->magData.z = mag.z;
+			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+			_newDataMag = true;
+			_last_mag = _sensor_combined.magnetometer_timestamp[_mag_main];
+			perf_count(_perf_mag);
+		}
+	}
+
+	if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
+		_voter_accel.failover_count() > 0 ||
+		_voter_mag.failover_count() > 0)) {
+
+		_failsafe = true;
+		mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
+	}
+
+	if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
+		_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
+		_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
+
+		if (_vibration_warning_timestamp == 0) {
+			_vibration_warning_timestamp = curr_time;
+		} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
+			_vibration_warning = true;
+			mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
+				(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
+				(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
+				(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
+		}
+	} else {
+		_vibration_warning_timestamp = 0;
+	}
 
-	// Copy gyro and accel
-	_last_sensor_timestamp = _sensor_combined.timestamp;
-	IMUmsec = _sensor_combined.timestamp / 1e3;
 	IMUusec = _sensor_combined.timestamp;
 
-	float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
+	float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
+	_last_sensor_timestamp = _sensor_combined.timestamp;
+
+	/* guard against too large deltaT's */
+	if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
+		deltaT = 0.01f;
+	}
+
+	// Always store data, independent of init status
+	/* fill in last data set */
+	_ekf->dtIMU = deltaT;
 
 	// XXX this is for assessing the filter performance
 	// leave this in as long as larger improvements are still being made.
@@ -1283,93 +1364,7 @@ void AttitudePositionEstimatorEKF::pollData()
 	}
 	#endif
 
-	/* guard against too large deltaT's */
-	if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
-		deltaT = 0.01f;
-	}
-
-	_last_run = _sensor_combined.timestamp;
-
-	// Always store data, independent of init status
-	/* fill in last data set */
-	_ekf->dtIMU = deltaT;
-
-	int last_gyro_main = _gyro_main;
-	unsigned best_gyro_err = UINT32_MAX - GYRO_SWITCH_HYSTERESIS - 1;
-	_gyro_valid = false;
-
-	for (unsigned i = 0; i < 3; i++) {
-
-		if (PX4_ISFINITE(_sensor_combined.gyro_integral_rad[i * 3 + 0]) &&
-		    PX4_ISFINITE(_sensor_combined.gyro_integral_rad[i * 3 + 1]) &&
-		    PX4_ISFINITE(_sensor_combined.gyro_integral_rad[i * 3 + 2]) &&
-		    (_sensor_combined.gyro_errcount[i] < (best_gyro_err + GYRO_SWITCH_HYSTERESIS))) {
-
-			_ekf->angRate.x = _sensor_combined.gyro_rad_s[i * 3 + 0];
-			_ekf->angRate.y = _sensor_combined.gyro_rad_s[i * 3 + 1];
-			_ekf->angRate.z = _sensor_combined.gyro_rad_s[i * 3 + 2];
-			_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[i * 3 + 0];
-			_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[i * 3 + 1];
-			_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[i * 3 + 2];
-			_gyro_main = i;
-			best_gyro_err = _sensor_combined.gyro_errcount[i];
-			_gyro_valid = true;
-
-		}
-	}
-
-	if (last_gyro_main != _gyro_main) {
-		mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main);
-	}
-
-	if (!_gyro_valid) {
-		/* keep last value if he hit an out of band value */
-		lastAngRate = _ekf->angRate;
-
-	} else {
-		perf_count(_perf_gyro);
-	}
-
-	if (accel_updated || hrt_elapsed_time(&last_accel) > 5000) {
-
-		int last_accel_main = _accel_main;
-
-		unsigned best_accel_err = UINT32_MAX - ACCEL_SWITCH_HYSTERESIS - 1;
-		_accel_valid = false;
-
-		for (unsigned i = 0; i < 3; i++) {
-
-			/* fail over to the 2nd accel if we know the first is down */
-			if (_sensor_combined.accelerometer_errcount[i] < (best_accel_err + ACCEL_SWITCH_HYSTERESIS)) {
-				_ekf->accel.x = _sensor_combined.accelerometer_m_s2[i * 3 + 0];
-				_ekf->accel.y = _sensor_combined.accelerometer_m_s2[i * 3 + 1];
-				_ekf->accel.z = _sensor_combined.accelerometer_m_s2[i * 3 + 2];
-				_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[i * 3 + 0];
-				_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[i * 3 + 1];
-				_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[i * 3 + 2];
-				_accel_main = i;
-				best_accel_err = _sensor_combined.accelerometer_errcount[i];
-				_accel_valid = true;
-			}
-		}
-
-		if (!_accel_valid) {
-			lastAccel = _ekf->accel;
-		}
-
-		if (last_accel_main != _accel_main) {
-			mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main);
-		}
-	}
-
-	if (last_mag != _sensor_combined.magnetometer_timestamp[_mag_main]) {
-		_newDataMag = true;
-
-	} else {
-		_newDataMag = false;
-	}
-
-	last_mag = _sensor_combined.magnetometer_timestamp[_mag_main];
+	_data_good = true;
 
 	//PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
 
@@ -1391,7 +1386,6 @@ void AttitudePositionEstimatorEKF::pollData()
 		_ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s;
 	}
 
-
 	bool gps_update;
 	orb_check(_gps_sub, &gps_update);
 
@@ -1545,46 +1539,6 @@ void AttitudePositionEstimatorEKF::pollData()
 		perf_count(_perf_baro);
 	}
 
-	//Update Magnetometer
-	if (_newDataMag || hrt_elapsed_time(&last_mag) > 500000) {
-
-		perf_count(_perf_mag);
-
-		int last_mag_main = _mag_main;
-
-		unsigned best_mag_err = UINT32_MAX - MAG_SWITCH_HYSTERESIS - 1;
-		_mag_valid = false;
-
-		for (unsigned i = 0; i < 3; i++) {
-
-			Vector3f mag(_sensor_combined.magnetometer_ga[i * 3 + 0], _sensor_combined.magnetometer_ga[i * 3 + 1],
-				_sensor_combined.magnetometer_ga[i * 3 + 2]);
-
-			const unsigned mag_timeout_us = 200000;
-
-			/* fail over to the 2nd mag if we know the first is down */
-			if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp[i]) < mag_timeout_us &&
-				_sensor_combined.magnetometer_errcount[i] < (best_mag_err + MAG_SWITCH_HYSTERESIS) &&
-				mag.length() > 0.1f) {
-				_ekf->magData.x = mag.x;
-				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
-
-				_ekf->magData.y = mag.y;
-				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
-
-				_ekf->magData.z = mag.z;
-				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
-				_mag_main = i;
-				best_mag_err = _sensor_combined.magnetometer_errcount[i];
-				_mag_valid = true;
-			}
-		}
-
-		if (last_mag_main != _mag_main) {
-			mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main);
-		}
-	}
-
 	//Update range data
 	orb_check(_distance_sub, &_newRangeData);
 

From 425d4316d1bb18b930aa324e50754673c26927f6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 13:59:51 +0200
Subject: [PATCH 47/62] Data validator: Increase timeout interval

---
 src/lib/ecl/validation/data_validator.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h
index de87a49f63..c7ec8526c9 100644
--- a/src/lib/ecl/validation/data_validator.h
+++ b/src/lib/ecl/validation/data_validator.h
@@ -115,7 +115,7 @@ private:
 
 DataValidator::DataValidator(DataValidator *prev_sibling) :
 	_time_last(0),
-	_timeout_interval(50000),
+	_timeout_interval(70000),
 	_event_count(0),
 	_error_count(0),
 	_mean{0.0f},

From dee7f1d956460b8391713aeeccf9aff773af0a9c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 14:23:20 +0200
Subject: [PATCH 48/62] Data validator: Add option to configure timeout

---
 src/lib/ecl/validation/data_validator.h       |  7 +++++++
 src/lib/ecl/validation/data_validator_group.h | 20 ++++++++++++++++++-
 2 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h
index c7ec8526c9..f768c278bd 100644
--- a/src/lib/ecl/validation/data_validator.h
+++ b/src/lib/ecl/validation/data_validator.h
@@ -92,6 +92,13 @@ public:
 	 */
 	void			print();
 
+	/**
+	 * Set the timeout value
+	 *
+	 * @param timeout_interval_us The timeout interval in microseconds
+	 */
+	void			set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
+
 private:
 	static const unsigned _dimensions = 3;
 	uint64_t _time_last;			/**< last timestamp */
diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h
index 6b420b3f27..e67f878eb8 100644
--- a/src/lib/ecl/validation/data_validator_group.h
+++ b/src/lib/ecl/validation/data_validator_group.h
@@ -85,6 +85,13 @@ public:
 	 */
 	void			print();
 
+	/**
+	 * Set the timeout value
+	 *
+	 * @param timeout_interval_us The timeout interval in microseconds
+	 */
+	void			set_timeout(uint64_t timeout_interval_us);
+
 private:
 	DataValidator *_first;		/**< sibling in the group */
 	int _curr_best;		/**< currently best index */
@@ -118,6 +125,17 @@ DataValidatorGroup::~DataValidatorGroup()
 
 }
 
+void
+DataValidatorGroup::set_timeout(uint64_t timeout_interval_us)
+{
+	DataValidator *next = _first;
+
+	while (next != nullptr) {
+		next->set_timeout(timeout_interval_us);
+		next = next->sibling();
+	}
+}
+
 void
 DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count)
 {
@@ -141,7 +159,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 
 	// XXX This should eventually also include voting
 	int pre_check_best = _curr_best;
-	float max_confidence = 0.0f;
+	float max_confidence = -1.0f;
 	int max_index = -1;
 	uint64_t min_error_count = 30000;
 	DataValidator *best = nullptr;

From 46f7404078c7e6cc1ef0f51ce3ea40a1a7ebd3e3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 14:23:35 +0200
Subject: [PATCH 49/62] Q estimator: Set mag timeout

---
 src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index df795b8c32..7e1dd26ca5 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -180,6 +180,8 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_voter_accel(3),
 	_voter_mag(3)
 {
+	_voter_mag.set_timeout(200000);
+	
 	_params_handles.w_acc		= param_find("ATT_W_ACC");
 	_params_handles.w_mag		= param_find("ATT_W_MAG");
 	_params_handles.w_gyro_bias	= param_find("ATT_W_GYRO_BIAS");

From 293bd026d0197f87896b3941651340cfcc10a4ca Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 14:23:46 +0200
Subject: [PATCH 50/62] EKF: Set mag timeout

---
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp       | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 95b4a90251..e8ddce57e2 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -219,6 +219,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_LP_att_Q(100.0f, 10.0f),
 	_LP_att_R(100.0f, 10.0f)
 {
+	_voter_mag.set_timeout(200000);
 
 	_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
 	_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
@@ -1136,7 +1137,7 @@ int AttitudePositionEstimatorEKF::start()
 	/* start the task */
 	_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 40,
+					 SCHED_PRIORITY_MAX - 20,
 					 4800,
 					 (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
 					 nullptr);

From 8da3f1f8f99b5b01e40312c61722883b909e35f6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 18:47:21 +0200
Subject: [PATCH 51/62] data validator: Move implementations to CPP files

---
 src/lib/ecl/module.mk                         |   4 +-
 src/lib/ecl/validation/data_validator.cpp     | 135 ++++++++++++
 src/lib/ecl/validation/data_validator.h       |  96 ---------
 .../ecl/validation/data_validator_group.cpp   | 192 ++++++++++++++++++
 src/lib/ecl/validation/data_validator_group.h | 152 --------------
 5 files changed, 330 insertions(+), 249 deletions(-)
 create mode 100644 src/lib/ecl/validation/data_validator.cpp
 create mode 100644 src/lib/ecl/validation/data_validator_group.cpp

diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk
index 03974c9503..747a7c5be0 100644
--- a/src/lib/ecl/module.mk
+++ b/src/lib/ecl/module.mk
@@ -39,6 +39,8 @@ SRCS		 = 	attitude_fw/ecl_controller.cpp \
 			attitude_fw/ecl_pitch_controller.cpp \
 			attitude_fw/ecl_roll_controller.cpp \
 			attitude_fw/ecl_yaw_controller.cpp \
-			l1/ecl_l1_pos_controller.cpp
+			l1/ecl_l1_pos_controller.cpp \
+			validation/data_validator.cpp \
+			validation/data_validator_group.cpp
 
 MAXOPTIMIZATION	 = -Os
diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp
new file mode 100644
index 0000000000..992a1018b7
--- /dev/null
+++ b/src/lib/ecl/validation/data_validator.cpp
@@ -0,0 +1,135 @@
+DataValidator::DataValidator(DataValidator *prev_sibling) :
+	_time_last(0),
+	_timeout_interval(70000),
+	_event_count(0),
+	_error_count(0),
+	_mean{0.0f},
+	_lp{0.0f},
+	_M2{0.0f},
+	_rms{0.0f},
+	_value{0.0f},
+	_value_equal_count(0),
+	_sibling(prev_sibling)
+{
+
+}
+
+DataValidator::~DataValidator()
+{
+
+}
+
+void
+DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in)
+{
+	_event_count++;
+	_error_count = error_count_in;
+
+	for (unsigned i = 0; i < _dimensions; i++) {
+		if (_time_last == 0) {
+			_mean[i] = 0;
+			_lp[i] = val[i];
+			_M2[i] = 0;
+		} else {
+			float lp_val = val[i] - _lp[i];
+
+			float delta_val = lp_val - _mean[i];
+			_mean[i] += delta_val / _event_count;
+			_M2[i] += delta_val * (lp_val - _mean[i]);
+			_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
+
+			if (fabsf(_value[i] - val[i]) < 0.000001f) {
+				_value_equal_count++;
+			} else {
+				_value_equal_count = 0;
+			}
+		}
+
+		// XXX replace with better filter, make it auto-tune to update rate
+		_lp[i] = _lp[i] * 0.5f + val[i] * 0.5f;
+
+		_value[i] = val[i];
+	}
+
+	_time_last = timestamp;
+}
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file data_validator.c
+ *
+ * A data validation class to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+float
+DataValidator::confidence(uint64_t timestamp)
+{
+	/* check if we have any data */
+	if (_time_last == 0) {
+		return 0.0f;
+	}
+
+	/* check error count limit */
+	if (_error_count > NORETURN_ERRCOUNT) {
+		return 0.0f;
+	}
+
+	/* we got the exact same sensor value N times in a row */
+	if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
+		return 0.0f;
+	}
+
+	/* timed out - that's it */
+	if (timestamp - _time_last > _timeout_interval) {
+		return 0.0f;
+	}
+
+	return 1.0f;
+}
+
+void
+DataValidator::print()
+{
+	if (_time_last == 0) {
+		printf("\tno data\n");
+		return;
+	}
+
+	for (unsigned i = 0; i < _dimensions; i++) {
+		printf("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n",
+			(double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]);
+	}
+}
diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h
index f768c278bd..dfd88b6e42 100644
--- a/src/lib/ecl/validation/data_validator.h
+++ b/src/lib/ecl/validation/data_validator.h
@@ -119,99 +119,3 @@ private:
 	DataValidator(const DataValidator&);
 	DataValidator operator=(const DataValidator&);
 };
-
-DataValidator::DataValidator(DataValidator *prev_sibling) :
-	_time_last(0),
-	_timeout_interval(70000),
-	_event_count(0),
-	_error_count(0),
-	_mean{0.0f},
-	_lp{0.0f},
-	_M2{0.0f},
-	_rms{0.0f},
-	_value{0.0f},
-	_value_equal_count(0),
-	_sibling(prev_sibling)
-{
-
-}
-
-DataValidator::~DataValidator()
-{
-
-}
-
-void
-DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in)
-{
-	_event_count++;
-	_error_count = error_count_in;
-
-	for (unsigned i = 0; i < _dimensions; i++) {
-		if (_time_last == 0) {
-			_mean[i] = 0;
-			_lp[i] = val[i];
-			_M2[i] = 0;
-		} else {
-			float lp_val = val[i] - _lp[i];
-
-			float delta_val = lp_val - _mean[i];
-			_mean[i] += delta_val / _event_count;
-			_M2[i] += delta_val * (lp_val - _mean[i]);
-			_rms[i] = sqrtf(_M2[i] / (_event_count - 1));
-
-			if (fabsf(_value[i] - val[i]) < 0.000001f) {
-				_value_equal_count++;
-			} else {
-				_value_equal_count = 0;
-			}
-		}
-
-		// XXX replace with better filter, make it auto-tune to update rate
-		_lp[i] = _lp[i] * 0.5f + val[i] * 0.5f;
-
-		_value[i] = val[i];
-	}
-
-	_time_last = timestamp;
-}
-
-float
-DataValidator::confidence(uint64_t timestamp)
-{
-	/* check if we have any data */
-	if (_time_last == 0) {
-		return 0.0f;
-	}
-
-	/* check error count limit */
-	if (_error_count > NORETURN_ERRCOUNT) {
-		return 0.0f;
-	}
-
-	/* we got the exact same sensor value N times in a row */
-	if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
-		return 0.0f;
-	}
-
-	/* timed out - that's it */
-	if (timestamp - _time_last > _timeout_interval) {
-		return 0.0f;
-	}
-
-	return 1.0f;
-}
-
-void
-DataValidator::print()
-{
-	if (_time_last == 0) {
-		printf("\tno data\n");
-		return;
-	}
-
-	for (unsigned i = 0; i < _dimensions; i++) {
-		printf("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n",
-			(double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]);
-	}
-}
diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp
new file mode 100644
index 0000000000..5e3232b701
--- /dev/null
+++ b/src/lib/ecl/validation/data_validator_group.cpp
@@ -0,0 +1,192 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file data_validator_group.cpp
+ *
+ * A data validation group to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
+	_first(nullptr),
+	_curr_best(-1),
+	_prev_best(-1),
+	_first_failover_time(0),
+	_toggle_count(0)
+{
+	DataValidator *next = _first;
+
+	for (unsigned i = 0; i < siblings; i++) {
+		next = new DataValidator(next);
+	}
+
+	_first = next;
+}
+
+DataValidatorGroup::~DataValidatorGroup()
+{
+
+}
+
+void
+DataValidatorGroup::set_timeout(uint64_t timeout_interval_us)
+{
+	DataValidator *next = _first;
+
+	while (next != nullptr) {
+		next->set_timeout(timeout_interval_us);
+		next = next->sibling();
+	}
+}
+
+void
+DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count)
+{
+	DataValidator *next = _first;
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		if (i == index) {
+			next->put(timestamp, val, error_count);
+			break;
+		}
+		next = next->sibling();
+		i++;
+	}
+}
+
+float*
+DataValidatorGroup::get_best(uint64_t timestamp, int *index)
+{
+	DataValidator *next = _first;
+
+	// XXX This should eventually also include voting
+	int pre_check_best = _curr_best;
+	float max_confidence = -1.0f;
+	int max_index = -1;
+	uint64_t min_error_count = 30000;
+	DataValidator *best = nullptr;
+
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		float confidence = next->confidence(timestamp);
+		if (confidence > max_confidence ||
+			(fabsf(confidence - max_confidence) < 0.01f && next->error_count() < min_error_count)) {
+			max_index = i;
+			max_confidence = confidence;
+			min_error_count = next->error_count();
+			best = next;
+		}
+
+		next = next->sibling();
+		i++;
+	}
+
+	/* the current best sensor is not matching the previous best sensor */
+	if (max_index != _curr_best) {
+
+		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
+		if (_curr_best < 0) {
+			_prev_best = max_index;
+		} else {
+			/* we were initialized before, this is a real failsafe */
+			_prev_best = pre_check_best;
+			_toggle_count++;
+
+			/* if this is the first time, log when we failed */
+			if (_first_failover_time == 0) {
+				_first_failover_time = timestamp;
+			}
+		}
+
+		/* for all cases we want to keep a record of the best index */
+		_curr_best = max_index;
+	}
+	*index = max_index;
+	return (best) ? best->value() : nullptr;
+}
+
+float
+DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
+{
+	DataValidator *next = _first;
+
+	float vibe = 0.0f;
+
+	/* find the best RMS value of a non-timed out sensor */
+	while (next != nullptr) {
+
+		if (next->confidence(timestamp) > 0.5f) {
+			float* rms = next->rms();
+
+			for (unsigned j = 0; j < 3; j++) {
+				if (rms[j] > vibe) {
+					vibe = rms[j];
+				}
+			}
+		}
+
+		next = next->sibling();
+	}
+
+	return vibe;
+}
+
+void
+DataValidatorGroup::print()
+{
+	/* print the group's state */
+	warnx("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
+		_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
+		_toggle_count);
+
+
+	DataValidator *next = _first;
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		printf("sensor #%u:\n", i);
+		next->print();
+		next = next->sibling();
+		i++;
+	}
+}
+
+unsigned
+DataValidatorGroup::failover_count()
+{
+	return _toggle_count;
+}
diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h
index e67f878eb8..dbbe3bdce1 100644
--- a/src/lib/ecl/validation/data_validator_group.h
+++ b/src/lib/ecl/validation/data_validator_group.h
@@ -103,155 +103,3 @@ private:
 	DataValidatorGroup(const DataValidatorGroup&);
 	DataValidatorGroup operator=(const DataValidatorGroup&);
 };
-
-DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
-	_first(nullptr),
-	_curr_best(-1),
-	_prev_best(-1),
-	_first_failover_time(0),
-	_toggle_count(0)
-{
-	DataValidator *next = _first;
-
-	for (unsigned i = 0; i < siblings; i++) {
-		next = new DataValidator(next);
-	}
-
-	_first = next;
-}
-
-DataValidatorGroup::~DataValidatorGroup()
-{
-
-}
-
-void
-DataValidatorGroup::set_timeout(uint64_t timeout_interval_us)
-{
-	DataValidator *next = _first;
-
-	while (next != nullptr) {
-		next->set_timeout(timeout_interval_us);
-		next = next->sibling();
-	}
-}
-
-void
-DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count)
-{
-	DataValidator *next = _first;
-	unsigned i = 0;
-
-	while (next != nullptr) {
-		if (i == index) {
-			next->put(timestamp, val, error_count);
-			break;
-		}
-		next = next->sibling();
-		i++;
-	}
-}
-
-float*
-DataValidatorGroup::get_best(uint64_t timestamp, int *index)
-{
-	DataValidator *next = _first;
-
-	// XXX This should eventually also include voting
-	int pre_check_best = _curr_best;
-	float max_confidence = -1.0f;
-	int max_index = -1;
-	uint64_t min_error_count = 30000;
-	DataValidator *best = nullptr;
-
-	unsigned i = 0;
-
-	while (next != nullptr) {
-		float confidence = next->confidence(timestamp);
-		if (confidence > max_confidence ||
-			(fabsf(confidence - max_confidence) < 0.01f && next->error_count() < min_error_count)) {
-			max_index = i;
-			max_confidence = confidence;
-			min_error_count = next->error_count();
-			best = next;
-		}
-
-		next = next->sibling();
-		i++;
-	}
-
-	/* the current best sensor is not matching the previous best sensor */
-	if (max_index != _curr_best) {
-
-		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
-		if (_curr_best < 0) {
-			_prev_best = max_index;
-		} else {
-			/* we were initialized before, this is a real failsafe */
-			_prev_best = pre_check_best;
-			_toggle_count++;
-
-			/* if this is the first time, log when we failed */
-			if (_first_failover_time == 0) {
-				_first_failover_time = timestamp;
-			}
-		}
-
-		/* for all cases we want to keep a record of the best index */
-		_curr_best = max_index;
-	}
-	*index = max_index;
-	return (best) ? best->value() : nullptr;
-}
-
-float
-DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
-{
-	DataValidator *next = _first;
-
-	float vibe = 0.0f;
-
-	/* find the best RMS value of a non-timed out sensor */
-	while (next != nullptr) {
-
-		if (next->confidence(timestamp) > 0.5f) {
-			float* rms = next->rms();
-
-			for (unsigned j = 0; j < 3; j++) {
-				if (rms[j] > vibe) {
-					vibe = rms[j];
-				}
-			}
-		}
-
-		next = next->sibling();
-	}
-
-	return vibe;
-}
-
-void
-DataValidatorGroup::print()
-{
-	/* print the group's state */
-	warnx("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
-		_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
-		_toggle_count);
-
-
-	DataValidator *next = _first;
-	unsigned i = 0;
-
-	while (next != nullptr) {
-		printf("sensor #%u:\n", i);
-		next->print();
-		next = next->sibling();
-		i++;
-	}
-}
-
-unsigned
-DataValidatorGroup::failover_count()
-{
-	return _toggle_count;
-}

From 9f5140eebb1a7bb636cbf7c3a25681b8b96e1ddc Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 30 Aug 2015 19:35:24 +0200
Subject: [PATCH 52/62] Data validator: Build as library

---
 src/lib/ecl/ecl.h                             |  3 +
 src/lib/ecl/validation/data_validator.cpp     | 88 ++++++++++---------
 src/lib/ecl/validation/data_validator.h       |  3 +-
 .../ecl/validation/data_validator_group.cpp   |  7 +-
 src/lib/ecl/validation/data_validator_group.h |  2 +-
 5 files changed, 57 insertions(+), 46 deletions(-)

diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h
index aa3c5000a4..39e1f50809 100644
--- a/src/lib/ecl/ecl.h
+++ b/src/lib/ecl/ecl.h
@@ -38,6 +38,9 @@
  */
 
 #include <drivers/drv_hrt.h>
+#include <px4_log.h>
 
 #define ecl_absolute_time hrt_absolute_time
 #define ecl_elapsed_time hrt_elapsed_time
+#define ECL_WARN PX4_WARN
+#define ECL_INFO PX4_INFO
diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp
index 992a1018b7..810ab7f6ff 100644
--- a/src/lib/ecl/validation/data_validator.cpp
+++ b/src/lib/ecl/validation/data_validator.cpp
@@ -1,3 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file data_validator.c
+ *
+ * A data validation class to identify anomalies in data streams
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#include "data_validator.h"
+#include <ecl/ecl.h>
+
 DataValidator::DataValidator(DataValidator *prev_sibling) :
 	_time_last(0),
 	_timeout_interval(70000),
@@ -53,46 +97,6 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in)
 
 	_time_last = timestamp;
 }
-/****************************************************************************
- *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file data_validator.c
- *
- * A data validation class to identify anomalies in data streams
- *
- * @author Lorenz Meier <lorenz@px4.io>
- */
 
 float
 DataValidator::confidence(uint64_t timestamp)
@@ -124,12 +128,12 @@ void
 DataValidator::print()
 {
 	if (_time_last == 0) {
-		printf("\tno data\n");
+		ECL_INFO("\tno data\n");
 		return;
 	}
 
 	for (unsigned i = 0; i < _dimensions; i++) {
-		printf("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n",
+		ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n",
 			(double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]);
 	}
 }
diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h
index dfd88b6e42..efcec93745 100644
--- a/src/lib/ecl/validation/data_validator.h
+++ b/src/lib/ecl/validation/data_validator.h
@@ -42,8 +42,9 @@
 #pragma once
 
 #include <cmath>
+#include <stdint.h>
 
-class DataValidator {
+class __EXPORT DataValidator {
 public:
 	DataValidator(DataValidator *prev_sibling = nullptr);
 	virtual ~DataValidator();
diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp
index 5e3232b701..25002de0f6 100644
--- a/src/lib/ecl/validation/data_validator_group.cpp
+++ b/src/lib/ecl/validation/data_validator_group.cpp
@@ -39,6 +39,9 @@
  * @author Lorenz Meier <lorenz@px4.io>
  */
 
+#include "data_validator_group.h"
+#include <ecl/ecl.h>
+
 DataValidatorGroup::DataValidatorGroup(unsigned siblings) :
 	_first(nullptr),
 	_curr_best(-1),
@@ -169,7 +172,7 @@ void
 DataValidatorGroup::print()
 {
 	/* print the group's state */
-	warnx("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
+	ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
 		_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
 		_toggle_count);
 
@@ -178,7 +181,7 @@ DataValidatorGroup::print()
 	unsigned i = 0;
 
 	while (next != nullptr) {
-		printf("sensor #%u:\n", i);
+		ECL_INFO("sensor #%u:\n", i);
 		next->print();
 		next = next->sibling();
 		i++;
diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h
index dbbe3bdce1..ec6162c606 100644
--- a/src/lib/ecl/validation/data_validator_group.h
+++ b/src/lib/ecl/validation/data_validator_group.h
@@ -43,7 +43,7 @@
 
 #include "data_validator.h"
 
-class DataValidatorGroup {
+class __EXPORT DataValidatorGroup {
 public:
 	DataValidatorGroup(unsigned siblings);
 	virtual ~DataValidatorGroup();

From 536321f5dd49fccae748bbd3f2d3aed87b1f629e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Sep 2015 17:27:25 +0200
Subject: [PATCH 53/62] Stay with 8g sensor ranges for now

---
 ROMFS/px4fmu_common/init.d/rc.sensors | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index ef9e086f1e..ff1a8092be 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -24,7 +24,7 @@ then
 	fi
 
 	# external MPU6K is rotated 180 degrees yaw
-	if mpu6000 -X -R 4 -a 16 start
+	if mpu6000 -X -R 4 start
 	then
 		set BOARD_FMUV3 true
 	else
@@ -39,12 +39,12 @@ then
 		fi
 
 		# external LSM303D is rotated 270 degrees yaw
-		if lsm303d -X -R 6 -a 16 start
+		if lsm303d -X -R 6 start
 		then
 		fi
 
 		# internal MPU6000 is rotated 180 deg roll, 270 deg yaw
-		if mpu6000 -R 14 -a 16 start
+		if mpu6000 -R 14 start
 		then
 		fi
 
@@ -54,7 +54,7 @@ then
 
 	else
 		# FMUv2
-		if mpu6000 -a 16 start
+		if mpu6000 start
 		then
 		fi
 
@@ -62,7 +62,7 @@ then
 		then
 		fi
 
-		if lsm303d -a 16 start
+		if lsm303d start
 		then
 		fi
 	fi

From 1f66c26a6256b5ae9c81b39fac0f86b3260f5ac0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Sep 2015 17:27:43 +0200
Subject: [PATCH 54/62] Q estimator: Use delta angles when available

---
 .../attitude_estimator_q_main.cpp             | 25 +++++++++++++++++--
 1 file changed, 23 insertions(+), 2 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 7e1dd26ca5..9dd2a38071 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -227,7 +227,7 @@ int AttitudeEstimatorQ::start()
 	_control_task = px4_task_spawn_cmd("attitude_estimator_q",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2000,
+				       2100,
 				       (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
 				       nullptr);
 
@@ -292,7 +292,23 @@ void AttitudeEstimatorQ::task_main()
 			// Feed validator with recent sensor data
 
 			for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
-				_voter_gyro.put(i, sensors.gyro_timestamp[i], &sensors.gyro_rad_s[i * 3], sensors.gyro_errcount[i]);
+
+				/* ignore empty fields */
+				if (sensors.gyro_timestamp[i] > 0) {
+
+					float gyro[3];
+
+					for (unsigned j = 0; j < 3; j++) {
+						if (sensors.gyro_integral_dt[i] > 0) {
+							gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
+						} else {
+							/* fall back to angular rate */
+							gyro[j] = sensors.gyro_rad_s[i * 3 + j];
+						}
+					}
+
+					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i]);
+				}
 				_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], sensors.accelerometer_errcount[i]);
 				_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], sensors.magnetometer_errcount[i]);
 			}
@@ -305,6 +321,11 @@ void AttitudeEstimatorQ::task_main()
 			_accel.set(_voter_accel.get_best(curr_time, &best_accel));
 			_mag.set(_voter_mag.get_best(curr_time, &best_mag));
 
+			if (_accel.length() < 0.01f || _mag.length() < 0.01f) {
+				warnx("WARNING: degenerate accel / mag!");
+				continue;
+			}
+
 			_data_good = true;
 
 			if (!_failsafe && (_voter_gyro.failover_count() > 0 ||

From 597bfc340aa5f8097e35bb255f70852763e2b454 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Sep 2015 17:28:00 +0200
Subject: [PATCH 55/62] Control lib: Add option to store parameters

---
 src/modules/controllib/block/BlockParam.cpp | 5 +++++
 src/modules/controllib/block/BlockParam.hpp | 1 +
 2 files changed, 6 insertions(+)

diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp
index 532a037d40..80f1bae1b0 100644
--- a/src/modules/controllib/block/BlockParam.cpp
+++ b/src/modules/controllib/block/BlockParam.cpp
@@ -97,6 +97,11 @@ void BlockParam<T>::update() {
 	if (_handle != PARAM_INVALID) param_get(_handle, &_val);
 }
 
+template <class T>
+void BlockParam<T>::commit() {
+	if (_handle != PARAM_INVALID) param_set(_handle, &_val);
+}
+
 template <class T>
 BlockParam<T>::~BlockParam() {};
 
diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp
index cab28c65fd..db035f9f91 100644
--- a/src/modules/controllib/block/BlockParam.hpp
+++ b/src/modules/controllib/block/BlockParam.hpp
@@ -79,6 +79,7 @@ public:
 	BlockParam(Block *block, const char *name,
 		   bool parent_prefix = true);
 	T get();
+	void commit();
 	void set(T val);
 	void update();
 	virtual ~BlockParam();

From 9950d5e9501bc6921cd7d9d8b84f61521ba4f7b7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 5 Sep 2015 17:30:52 +0200
Subject: [PATCH 56/62] EKF: Use delta angles / velocities if available, fall
 back to rates / acceleration when needed. Remove unused sensor offsets. Store
 estimated offset on landing, but do not yet load it on boot (we want to check
 these after real flights in logs first)

---
 .../AttitudePositionEstimatorEKF.h            |  14 ++-
 .../ekf_att_pos_estimator_main.cpp            | 108 ++++++++----------
 .../ekf_att_pos_estimator_params.c            |  36 ++++++
 3 files changed, 94 insertions(+), 64 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index 6251f8f5cb..e4d924396a 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -52,7 +52,6 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/parameter_update.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/home_position.h>
@@ -73,10 +72,13 @@
 #include <lib/ecl/validation/data_validator_group.h>
 #include "estimator_22states.h"
 
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
 //Forward declaration
 class AttPosEKF;
 
-class AttitudePositionEstimatorEKF
+class AttitudePositionEstimatorEKF : public control::SuperBlock
 {
 public:
     /**
@@ -173,10 +175,6 @@ private:
     hrt_abstime _last_accel;
     hrt_abstime _last_mag;
 
-    struct gyro_scale               _gyro_offsets[3];
-    struct accel_scale              _accel_offsets[3];
-    struct mag_scale                _mag_offsets[3];
-
     struct sensor_combined_s            _sensor_combined;
 
     struct map_projection_reference_s   _pos_ref;
@@ -225,6 +223,10 @@ private:
 
     int             _mavlink_fd;
 
+    control::BlockParamFloat _mag_offset_x;
+    control::BlockParamFloat _mag_offset_y;
+    control::BlockParamFloat _mag_offset_z;
+
     struct {
         int32_t vel_delay_ms;
         int32_t pos_delay_ms;
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index e8ddce57e2..6339be23c9 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -119,6 +119,7 @@ AttitudePositionEstimatorEKF	*g_estimator = nullptr;
 }
 
 AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
+	SuperBlock(NULL, "EKF"),
 	_task_should_exit(false),
 	_task_running(false),
 	_estimator_task(-1),
@@ -162,10 +163,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_last_accel(0),
 	_last_mag(0),
 
-	_gyro_offsets{},
-	_accel_offsets{},
-	_mag_offsets{},
-
 	_sensor_combined{},
 
 	 _pos_ref{},
@@ -211,6 +208,9 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_newRangeData(false),
 
 	_mavlink_fd(-1),
+	_mag_offset_x(this, "PE_MAGB_X"),
+	_mag_offset_y(this, "PE_MAGB_Y"),
+	_mag_offset_z(this, "PE_MAGB_Z"),
 	_parameters{},
 	_parameter_handles{},
 	_ekf(nullptr),
@@ -246,48 +246,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 
 	/* fetch initial parameter values */
 	parameters_update();
-
-	/* get offsets */
-	int fd, res;
-
-	for (unsigned s = 0; s < 3; s++) {
-		char str[30];
-		(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
-		fd = px4_open(str, O_RDONLY);
-
-		if (fd >= 0) {
-			res = px4_ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]);
-			px4_close(fd);
-
-			if (res) {
-				PX4_WARN("G%u SCALE FAIL", s);
-			}
-		}
-
-		(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
-		fd = px4_open(str, O_RDONLY);
-
-		if (fd >= 0) {
-			res = px4_ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets[s]);
-			px4_close(fd);
-
-			if (res) {
-				PX4_WARN("A%u SCALE FAIL", s);
-			}
-		}
-
-		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
-		fd = px4_open(str, O_RDONLY);
-
-		if (fd >= 0) {
-			res = px4_ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets[s]);
-			px4_close(fd);
-
-			if (res) {
-				PX4_WARN("M%u SCALE FAIL", s);
-			}
-		}
-	}
 }
 
 AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF()
@@ -326,6 +284,8 @@ int AttitudePositionEstimatorEKF::enable_logging(bool logging)
 
 int AttitudePositionEstimatorEKF::parameters_update()
 {
+	/* update C++ param system */
+	updateParams();
 
 	param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms));
 	param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms));
@@ -363,6 +323,13 @@ int AttitudePositionEstimatorEKF::parameters_update()
 		_ekf->accelProcessNoise = _parameters.acc_pnoise;
 		_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
 		_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
+		#if 0
+		// Initially disable loading until
+		// convergence is flight-test proven
+		_ekf->magBias.x = _mag_offset_x.get();
+		_ekf->magBias.y = _mag_offset_y.get();
+		_ekf->magBias.z = _mag_offset_z.get();
+		#endif
 	}
 
 	return OK;
@@ -375,12 +342,24 @@ void AttitudePositionEstimatorEKF::vehicle_status_poll()
 	/* Check HIL state if vehicle status has changed */
 	orb_check(_vstatus_sub, &vstatus_updated);
 
+	bool landed = _vstatus.condition_landed;
+
 	if (vstatus_updated) {
 
 		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
 
-		//Tell EKF that the vehicle is a fixed wing or multi-rotor
+		// Tell EKF that the vehicle is a fixed wing or multi-rotor
 		_ekf->setIsFixedWing(!_vstatus.is_rotary_wing);
+
+		// Save params on landed
+		if (!landed && _vstatus.condition_landed) {
+			_mag_offset_x.set(_ekf->magBias.x);
+			_mag_offset_x.commit();
+			_mag_offset_y.set(_ekf->magBias.y);
+			_mag_offset_y.commit();
+			_mag_offset_z.set(_ekf->magBias.z);
+			_mag_offset_z.commit();
+		}
 	}
 }
 
@@ -1249,23 +1228,41 @@ void AttitudePositionEstimatorEKF::pollData()
 	hrt_abstime curr_time = hrt_absolute_time();
 	(void)_voter_gyro.get_best(curr_time, &_gyro_main);
 	if (_gyro_main >= 0) {
+
+		// Use pre-integrated values if possible
+		if (_sensor_combined.gyro_integral_dt[_gyro_main] > 0) {
+			_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
+			_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
+			_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
+		} else {
+			_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]);
+			_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]);
+			_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]);
+		}
+
 		_ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0];
 		_ekf->angRate.y = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1];
 		_ekf->angRate.z = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2];
-		_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
-		_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
-		_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
 		perf_count(_perf_gyro);
 	}
 
 	(void)_voter_accel.get_best(curr_time, &_accel_main);
 	if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
+
+		// Use pre-integrated values if possible
+		if (_sensor_combined.accelerometer_integral_dt[_accel_main] > 0) {
+			_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
+			_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
+			_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
+		} else {
+			_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
+			_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
+			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
+		}
+
 		_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
 		_ekf->accel.y = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1];
 		_ekf->accel.z = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2];
-		_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
-		_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
-		_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
 		_last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
 	}
 
@@ -1277,13 +1274,8 @@ void AttitudePositionEstimatorEKF::pollData()
 		/* fail over to the 2nd mag if we know the first is down */
 		if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) {
 			_ekf->magData.x = mag.x;
-			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
-
 			_ekf->magData.y = mag.y;
-			_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
-
 			_ekf->magData.z = mag.z;
-			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
 			_newDataMag = true;
 			_last_mag = _sensor_combined.magnetometer_timestamp[_mag_main];
 			perf_count(_perf_mag);
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
index f496f9ed96..482d1ca897 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
@@ -258,6 +258,42 @@ PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
  */
 PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
 
+/**
+ * Magnetometer X bias
+ *
+ * The magnetometer bias. This bias is learnt by the filter over time and
+ * persists between boots.
+ *
+ * @min -0.6
+ * @max 0.6
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_MAGB_X, 0.0f);
+
+/**
+ * Magnetometer Y bias
+ *
+ * The magnetometer bias. This bias is learnt by the filter over time and
+ * persists between boots.
+ *
+ * @min -0.6
+ * @max 0.6
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_MAGB_Y, 0.0f);
+
+/**
+ * Magnetometer Z bias
+ *
+ * The magnetometer bias. This bias is learnt by the filter over time and
+ * persists between boots.
+ *
+ * @min -0.6
+ * @max 0.6
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_MAGB_Z, 0.0f);
+
 /**
  * Threshold for filter initialization.
  *

From dce31a76a82ceffd42c510b1ae2487ae80d05110 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 6 Sep 2015 20:10:14 +0200
Subject: [PATCH 57/62] EKF: Set correct interval / update rate

---
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp    | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 6339be23c9..3d6c8ddd34 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -215,9 +215,9 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_parameter_handles{},
 	_ekf(nullptr),
 
-	_LP_att_P(100.0f, 10.0f),
-	_LP_att_Q(100.0f, 10.0f),
-	_LP_att_R(100.0f, 10.0f)
+	_LP_att_P(250.0f, 20.0f),
+	_LP_att_Q(250.0f, 20.0f),
+	_LP_att_R(250.0f, 20.0f)
 {
 	_voter_mag.set_timeout(200000);
 

From d5e152f2cddaab6ace64f656baef7569c9b7c44d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 6 Sep 2015 20:15:59 +0200
Subject: [PATCH 58/62] Attitude estimator Q: Add output filter for rate
 outputs to bring noise level into manageable range

---
 .../attitude_estimator_q_main.cpp              | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 9dd2a38071..56929b75ae 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -58,7 +58,8 @@
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
-#include <lib/mathlib/mathlib.h>
+#include <mathlib/mathlib.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/geo/geo.h>
 #include <lib/ecl/validation/data_validator_group.h>
 #include <mavlink/mavlink_log.h>
@@ -153,6 +154,10 @@ private:
 	DataValidatorGroup _voter_accel;
 	DataValidatorGroup _voter_mag;
 
+	/* Low pass filter for attitude rates */
+	math::LowPassFilter2p _lp_roll_rate;
+	math::LowPassFilter2p _lp_pitch_rate;
+
 	hrt_abstime _vel_prev_t = 0;
 
 	bool		_inited = false;
@@ -178,7 +183,9 @@ private:
 AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_voter_gyro(3),
 	_voter_accel(3),
-	_voter_mag(3)
+	_voter_mag(3),
+	_lp_roll_rate(250.0f, 20.0f),
+	_lp_pitch_rate(250.0f, 20.0f)
 {
 	_voter_mag.set_timeout(200000);
 	
@@ -408,8 +415,11 @@ void AttitudeEstimatorQ::task_main()
 		att.pitch = euler(1);
 		att.yaw = euler(2);
 
-		att.rollspeed = _rates(0);
-		att.pitchspeed = _rates(1);
+		/* the complimentary filter should reflect the true system
+		 * state, but we need smoother outputs for the control system
+		 */
+		att.rollspeed = _lp_roll_rate.apply(_rates(0));
+		att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
 		att.yawspeed = _rates(2);
 
 		for (int i = 0; i < 3; i++) {

From ae56496ba3e96a431f3e3b54d85d7bedaf63a92d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 7 Sep 2015 09:57:42 +0200
Subject: [PATCH 59/62] Data validator: Add priority support

---
 src/lib/ecl/validation/data_validator.cpp       | 10 +++++++++-
 src/lib/ecl/validation/data_validator.h         | 13 ++++++++++---
 src/lib/ecl/validation/data_validator_group.cpp | 10 +++++++---
 src/lib/ecl/validation/data_validator_group.h   | 10 ++++++----
 4 files changed, 32 insertions(+), 11 deletions(-)

diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp
index 810ab7f6ff..037cb88645 100644
--- a/src/lib/ecl/validation/data_validator.cpp
+++ b/src/lib/ecl/validation/data_validator.cpp
@@ -47,6 +47,7 @@ DataValidator::DataValidator(DataValidator *prev_sibling) :
 	_timeout_interval(70000),
 	_event_count(0),
 	_error_count(0),
+	_priority(0),
 	_mean{0.0f},
 	_lp{0.0f},
 	_M2{0.0f},
@@ -64,10 +65,11 @@ DataValidator::~DataValidator()
 }
 
 void
-DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in)
+DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in)
 {
 	_event_count++;
 	_error_count = error_count_in;
+	_priority = priority_in;
 
 	for (unsigned i = 0; i < _dimensions; i++) {
 		if (_time_last == 0) {
@@ -124,6 +126,12 @@ DataValidator::confidence(uint64_t timestamp)
 	return 1.0f;
 }
 
+int
+DataValidator::priority()
+{
+	return _priority;
+}
+
 void
 DataValidator::print()
 {
diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h
index efcec93745..686a810c4e 100644
--- a/src/lib/ecl/validation/data_validator.h
+++ b/src/lib/ecl/validation/data_validator.h
@@ -54,7 +54,7 @@ public:
 	 *
 	 * @param val		Item to put
 	 */
-	void			put(uint64_t timestamp, float val[3], uint64_t error_count);
+	void			put(uint64_t timestamp, float val[3], uint64_t error_count, int priority);
 
 	/**
 	 * Get the next sibling in the group
@@ -81,6 +81,12 @@ public:
 	 */
 	float*			value() { return _value; }
 
+	/**
+	 * Get the priority of this validator
+	 * @return		the stored priority
+	 */
+	int			priority();
+
 	/**
 	 * Get the RMS values of this validator
 	 * @return		the stored RMS
@@ -106,14 +112,15 @@ private:
 	uint64_t _timeout_interval;		/**< interval in which the datastream times out in us */
 	uint64_t _event_count;			/**< total data counter */
 	uint64_t _error_count;			/**< error count */
-	float _mean[_dimensions];				/**< mean of value */
+	int _priority;				/**< sensor nominal priority */
+	float _mean[_dimensions];		/**< mean of value */
 	float _lp[3];				/**< low pass value */
 	float _M2[3];				/**< RMS component value */
 	float _rms[3];				/**< root mean square error */
 	float _value[3];			/**< last value */
 	float _value_equal_count;		/**< equal values in a row */
 	DataValidator *_sibling;		/**< sibling in the group */
-	const unsigned NORETURN_ERRCOUNT = 1000;	/**< if the error count reaches this value, return sensor as invalid */
+	const unsigned NORETURN_ERRCOUNT = 100;	/**< if the error count reaches this value, return sensor as invalid */
 	const unsigned VALUE_EQUAL_COUNT_MAX = 100;	/**< if the sensor value is the same (accumulated also between axes) this many times, flag it */
 
 	/* we don't want this class to be copied */
diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp
index 25002de0f6..cdd1f6a677 100644
--- a/src/lib/ecl/validation/data_validator_group.cpp
+++ b/src/lib/ecl/validation/data_validator_group.cpp
@@ -75,14 +75,14 @@ DataValidatorGroup::set_timeout(uint64_t timeout_interval_us)
 }
 
 void
-DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count)
+DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority)
 {
 	DataValidator *next = _first;
 	unsigned i = 0;
 
 	while (next != nullptr) {
 		if (i == index) {
-			next->put(timestamp, val, error_count);
+			next->put(timestamp, val, error_count, priority);
 			break;
 		}
 		next = next->sibling();
@@ -98,6 +98,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 	// XXX This should eventually also include voting
 	int pre_check_best = _curr_best;
 	float max_confidence = -1.0f;
+	int max_priority = -1000;
 	int max_index = -1;
 	uint64_t min_error_count = 30000;
 	DataValidator *best = nullptr;
@@ -107,9 +108,12 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 	while (next != nullptr) {
 		float confidence = next->confidence(timestamp);
 		if (confidence > max_confidence ||
-			(fabsf(confidence - max_confidence) < 0.01f && next->error_count() < min_error_count)) {
+			(fabsf(confidence - max_confidence) < 0.01f &&
+				((next->error_count() < min_error_count) &&
+				(next->priority() >= max_priority)))) {
 			max_index = i;
 			max_confidence = confidence;
+			max_priority = next->priority();
 			min_error_count = next->error_count();
 			best = next;
 		}
diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h
index ec6162c606..f35e0d71c1 100644
--- a/src/lib/ecl/validation/data_validator_group.h
+++ b/src/lib/ecl/validation/data_validator_group.h
@@ -51,12 +51,14 @@ public:
 	/**
 	 * Put an item into the validator group.
 	 *
-	 * @param x		X Item to put
-	 * @param y		Y Item to put
-	 * @param z		Z Item to put
+	 * @param index		Sensor index
+	 * @param timestamp	The timestamp of the measurement
+	 * @param val		The 3D vector
+	 * @param error_count	The current error count of the sensor
+	 * @param priority	The priority of the sensor
 	 */
 	void			put(unsigned index, uint64_t timestamp,
-					float val[3], uint64_t error_count);
+					float val[3], uint64_t error_count, int priority);
 
 	/**
 	 * Get the best data triplet of the group

From d558819a243283b3086305bf6838e38ad8cef0fd Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 7 Sep 2015 09:59:49 +0200
Subject: [PATCH 60/62] Attitude estimator Q: Add sensor priority support

---
 .../attitude_estimator_q/attitude_estimator_q_main.cpp    | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 56929b75ae..9b60701413 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -314,10 +314,12 @@ void AttitudeEstimatorQ::task_main()
 						}
 					}
 
-					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i]);
+					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
 				}
-				_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], sensors.accelerometer_errcount[i]);
-				_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], sensors.magnetometer_errcount[i]);
+				_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
+					sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
+				_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
+					sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
 			}
 
 			int best_gyro, best_accel, best_mag;

From 52a29468270fae607b97dd3cd8e766309a795b9a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 7 Sep 2015 10:00:03 +0200
Subject: [PATCH 61/62] EKF: Add sensor priority support

---
 .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 3d6c8ddd34..c75a9e547d 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1219,9 +1219,12 @@ void AttitudePositionEstimatorEKF::pollData()
 	// Feed validator with recent sensor data
 
 	for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
-		_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], _sensor_combined.gyro_errcount[i]);
-		_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], _sensor_combined.accelerometer_errcount[i]);
-		_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], _sensor_combined.magnetometer_errcount[i]);
+		_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3],
+			_sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]);
+		_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3],
+			_sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]);
+		_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3],
+			_sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]);
 	}
 
 	// Get best measurement values

From ad2058427dfd2c3d4886ce0bed55dee6d247f90e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 7 Sep 2015 10:16:50 +0200
Subject: [PATCH 62/62] sensors app: Keep looking for new sensors until system
 is fully booted

---
 src/modules/sensors/sensors.cpp | 35 +++++++++++++++++++++++++++++----
 1 file changed, 31 insertions(+), 4 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 282401f58c..86df382df8 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -202,6 +202,7 @@ private:
 
 	bool		_hil_enabled;			/**< if true, HIL is active */
 	bool		_publishing;			/**< if true, we are publishing sensor data */
+	bool		_armed;				/**< arming status of the vehicle */
 
 	int		_gyro_sub[SENSOR_COUNT_MAX];	/**< raw gyro data subscription */
 	int		_accel_sub[SENSOR_COUNT_MAX];	/**< raw accel data subscription */
@@ -485,6 +486,7 @@ Sensors::Sensors() :
 	_sensors_task(-1),
 	_hil_enabled(false),
 	_publishing(true),
+	_armed(false),
 
 	/* subscriptions */
 	_gyro_sub{-1, -1, -1},
@@ -520,6 +522,14 @@ Sensors::Sensors() :
 	_battery_discharged(0),
 	_battery_current_timestamp(0)
 {
+	/* initialize subscriptions */
+	for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
+		_gyro_sub[i] = -1;
+		_accel_sub[i] = -1;
+		_mag_sub[i] = -1;
+		_baro_sub[i] = -1;
+	}
+
 	memset(&_rc, 0, sizeof(_rc));
 	memset(&_diff_pres, 0, sizeof(_diff_pres));
 	memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
@@ -1189,16 +1199,17 @@ Sensors::vehicle_control_mode_poll()
 		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
 
 		/* switching from non-HIL to HIL mode */
-		//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
 		if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
 			_hil_enabled = true;
 			_publishing = false;
+			_armed = vcontrol_mode.flag_armed;
 
 			/* switching from HIL to non-HIL mode */
 
 		} else if (!_publishing && !_hil_enabled) {
 			_hil_enabled = false;
 			_publishing = true;
+			_armed = vcontrol_mode.flag_armed;
 		}
 	}
 }
@@ -1947,9 +1958,10 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
 	}
 
 	for (unsigned i = 0; i < group_count; i++) {
-		subs[i] = orb_subscribe_multi(meta, i);
-		orb_priority(subs[i], (int32_t*)&priorities[i]);
-		errcount[i] = 100000;
+		if (subs[i] < 0) {
+			subs[i] = orb_subscribe_multi(meta, i);
+			orb_priority(subs[i], (int32_t*)&priorities[i]);
+		}
 	}
 
 	return group_count;
@@ -2074,6 +2086,21 @@ Sensors::task_main()
 		/* check vehicle status for changes to publication state */
 		vehicle_control_mode_poll();
 
+		/* keep adding sensors as long as we are not armed */
+		if (!_armed) {
+			_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
+				&raw.gyro_priority[0], &raw.gyro_errcount[0]);
+
+			_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
+				&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
+
+			_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
+				&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
+
+			_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
+				&raw.baro_priority[0], &raw.baro_errcount[0]);
+		}
+
 		/* the timestamp of the raw struct is updated by the gyro_poll() method */
 		/* copy most recent sensor data */
 		gyro_poll(raw);