Merged master into cmake-2

ROMFS/px4fmu_common/init.d/rc.uavcan

@@ -3,26 +3,10 @@
 # UAVCAN initialization script.
 #
 
-#
-# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable.
-# TODO there should be a smarter way.
-#
-set UAVCAN_ENABLE 0
-if param compare UAVCAN_ENABLE 1
-then
-	set UAVCAN_ENABLE 1
-fi
-if param compare UAVCAN_ENABLE 2
-then
-	set UAVCAN_ENABLE 2
-fi
-
-echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE"
-
 #
 # Starting stuff according to UAVCAN_ENABLE value
 #
-if [ $UAVCAN_ENABLE -ge 1 ]
+if param greater UAVCAN_ENABLE 0
 then
 	if uavcan start
 	then
@@ -33,7 +17,7 @@ then
 	fi
 fi
 
-if [ $UAVCAN_ENABLE -ge 2 ]
+if param greater UAVCAN_ENABLE 1
 then
 	if uavcan start fw
 	then
@@ -43,10 +27,3 @@ then
 		tone_alarm $TUNE_ERR
 	fi
 fi
-
-if [ $UAVCAN_ENABLE -ge 1 ]
-then
-	# First sensor publisher to initialize takes lowest instance ID
-	# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs
-	sleep 8
-fi

Tools/px4params/srcparser.py

@@ -200,7 +200,7 @@ class SourceParser(object):
                     m = self.re_px4_parameter_definition.match(line)
                     if m:
                         tp, name = m.group(1, 2)
-                        if default_var.has_key(name+'_DEFAULT'):
+                        if (name+'_DEFAULT') in default_var:
                             defval = default_var[name+'_DEFAULT']
                 if tp is not None:
                     # Remove trailing type specifier from numbers: 0.1f => 0.1

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

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

msg/sensor_combined.msg

@@ -26,96 +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
-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
-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[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[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
+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[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[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
+
+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

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

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)

src/drivers/device/integrator.h

@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ *   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
+
+#include <mathlib/mathlib.h>
+
+#include <mathlib/mathlib.h>
+
+class Integrator {
+public:
+	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(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
+	 */
+	math::Vector<3>		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
+	 */
+	math::Vector<3>		read(bool auto_reset);
+
+	/**
+	 * Get current integral start time
+	 */
+	hrt_abstime		current_integral_start() { return _last_auto; }
+
+private:
+	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::Integrator(hrt_abstime auto_reset_interval, bool coning_compensation) :
+	_auto_reset_interval(auto_reset_interval),
+	_last_integration(0),
+	_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()
+{
+
+}
+
+bool
+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;
+	}
+
+	// Integrate
+	double dt = (double)(timestamp - _last_integration) / 1000000.0;
+	math::Vector<3> i = (val + _last_val) * dt * 0.5f;
+
+	// 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_read += i;
+
+	_last_integration = timestamp;
+	_last_val = val;
+	_last_delta = i;
+
+	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) = 0.0f;
+		_integral_auto(1) = 0.0f;
+		_integral_auto(2) = 0.0f;
+	}
+
+	return auto_reset;
+}
+
+math::Vector<3>
+Integrator::read(bool auto_reset)
+{
+	math::Vector<3> val = _integral_read;
+	if (auto_reset) {
+		_integral_read(0) = 0.0f;
+		_integral_read(1) = 0.0f;
+		_integral_read(2) = 0.0f;
+	}
+
+	return val;
+}

src/drivers/device/vdev_posix.cpp

@@ -195,7 +195,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 {
 	sem_t sem;
 	int count = 0;
-	int ret;
+	int ret = -1;
 	unsigned int i;
 
 	PX4_DEBUG("Called px4_poll timeout = %d", timeout);
@@ -222,7 +222,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 
 	if (ret >= 0)
 	{
-		if (timeout >= 0)
+		if (timeout > 0)
 		{
 			// Use a work queue task
 			work_s _hpwork;
@@ -234,7 +234,7 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 			// out of scope
 			hrt_work_cancel(&_hpwork);
         	}
-		else
+		else if (timeout < 0) 
 		{
 			sem_wait(&sem);
 		}

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

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;
 

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(xin);
+	report.y = _gyro_filter_y.apply(yin);
+	report.z = _gyro_filter_z.apply(zin);
 
-	report.x = _gyro_filter_x.apply(report.x);
-	report.y = _gyro_filter_y.apply(report.y);
-	report.z = _gyro_filter_z.apply(report.z);
+	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++;

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),
@@ -1411,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
@@ -1575,17 +1587,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++;
@@ -1841,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();
@@ -1856,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) {
@@ -1884,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);
 
@@ -1980,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);
@@ -2084,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;
@@ -2094,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);
@@ -2107,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.

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),
@@ -1525,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
@@ -1759,6 +1775,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 +1810,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 +1828,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 +1844,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);
 	}
@@ -1925,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);
@@ -1942,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;
@@ -1979,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);
@@ -2076,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 */
 
@@ -2175,6 +2221,7 @@ usage()
 	warnx("options:");
 	warnx("    -X    (external bus)");
 	warnx("    -R rotation");
+	warnx("    -a accel range (in g)");
 }
 
 } // namespace
@@ -2185,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;
@@ -2195,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);
@@ -2208,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")) {

src/drivers/ms5611/ms5611_nuttx.cpp

@@ -274,7 +274,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");

src/drivers/rgbled/module.mk

@@ -4,6 +4,7 @@
 
 MODULE_COMMAND	 = rgbled
 
-SRCS		 = rgbled.cpp
+SRCS		 = rgbled.cpp \
+			   rgbled_params.c
 
 MAXOPTIMIZATION	 = -Os

src/drivers/rgbled/rgbled.cpp

@@ -101,11 +101,13 @@ private:
 	uint8_t			_g;
 	uint8_t			_b;
 	float			_brightness;
+	float			_max_brightness;
 
 	bool			_running;
 	int			_led_interval;
 	bool			_should_run;
 	int			_counter;
+	int			_param_sub;
 
 	void 			set_color(rgbled_color_t ledcolor);
 	void			set_mode(rgbled_mode_t mode);
@@ -117,6 +119,7 @@ private:
 	int			send_led_enable(bool enable);
 	int			send_led_rgb();
 	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
+	void		update_params();
 };
 
 /* for now, we only support one RGBLED */
@@ -140,10 +143,12 @@ RGBLED::RGBLED(int bus, int rgbled) :
 	_g(0),
 	_b(0),
 	_brightness(1.0f),
+	_max_brightness(1.0f),
 	_running(false),
 	_led_interval(0),
 	_should_run(false),
-	_counter(0)
+	_counter(0),
+	_param_sub(-1)
 {
 	memset(&_work, 0, sizeof(_work));
 	memset(&_pattern, 0, sizeof(_pattern));
@@ -285,6 +290,22 @@ RGBLED::led()
 		return;
 	}
 
+	if (_param_sub < 0) {
+		_param_sub = orb_subscribe(ORB_ID(parameter_update));
+	}
+
+	if (_param_sub >= 0) {
+		bool updated = false;
+		orb_check(_param_sub, &updated);
+		if (updated) {
+			parameter_update_s pupdate;
+			orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
+			update_params();
+			// Immediately update to change brightness
+			send_led_rgb();
+		}
+	}
+
 	switch (_mode) {
 	case RGBLED_MODE_BLINK_SLOW:
 	case RGBLED_MODE_BLINK_NORMAL:
@@ -546,9 +567,9 @@ RGBLED::send_led_rgb()
 {
 	/* To scale from 0..255 -> 0..15 shift right by 4 bits */
 	const uint8_t msg[6] = {
-		SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
-		SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
-		SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
+		SUB_ADDR_PWM0, static_cast<uint8_t>((_b >> 4) * _brightness * _max_brightness + 0.5f),
+		SUB_ADDR_PWM1, static_cast<uint8_t>((_g >> 4) * _brightness * _max_brightness + 0.5f),
+		SUB_ADDR_PWM2, static_cast<uint8_t>((_r >> 4) * _brightness * _max_brightness + 0.5f)
 	};
 	return transfer(msg, sizeof(msg), nullptr, 0);
 }
@@ -573,6 +594,21 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 	return ret;
 }
 
+void
+RGBLED::update_params()
+{
+	int32_t maxbrt = 15;
+	param_get(param_find("LED_RGB_MAXBRT"), &maxbrt);
+	maxbrt = maxbrt > 15 ? 15 : maxbrt;
+	maxbrt = maxbrt <  0 ?  0 : maxbrt;
+
+	// A minimum of 2 "on" steps is required for breathe effect
+	if (maxbrt == 1) {
+		maxbrt = 2;
+	}
+	_max_brightness = maxbrt / 15.0f;
+}
+
 void
 rgbled_usage()
 {

src/drivers/rgbled/rgbled_params.c

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   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 rgbled_params.c
+ *
+ * Parameters defined by the RBG led driver
+ *
+ * @author Nate Weibley <nate.weibley@prioria.com>
+ */
+
+
+#include <px4_config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * RGB Led brightness limit
+ *
+ * Set to 0 to disable, 1 for minimum brightness up to 15 (max)
+ *
+ * @min 0
+ * @max 15
+ */
+PARAM_DEFINE_INT32(LED_RGB_MAXBRT, 15);

src/lib/dspal

@@ -1 +1 @@
-Subproject commit a88d55925cc21d4f9ebc728deab85cbcbb218a52
+Subproject commit 229f2f4d8471564f01fe8330e5de1554a9b7aeb6

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

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

src/lib/ecl/validation/data_validator.cpp

@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   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),
+	_event_count(0),
+	_error_count(0),
+	_priority(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, int priority_in)
+{
+	_event_count++;
+	_error_count = error_count_in;
+	_priority = priority_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;
+}
+
+int
+DataValidator::priority()
+{
+	return _priority;
+}
+
+void
+DataValidator::print()
+{
+	if (_time_last == 0) {
+		ECL_INFO("\tno data\n");
+		return;
+	}
+
+	for (unsigned i = 0; i < _dimensions; i++) {
+		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]);
+	}
+}

src/lib/ecl/validation/data_validator.h

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   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
+
+#include <cmath>
+#include <stdint.h>
+
+class __EXPORT 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, int priority);
+
+	/**
+	 * 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 error count of this validator
+	 * @return		the error count
+	 */
+	uint64_t		error_count() { return _error_count; }
+
+	/**
+	 * Get the values of this validator
+	 * @return		the stored value
+	 */
+	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
+	 */
+	float*			rms() { return _rms; }
+
+	/**
+	 * Print the validator value
+	 *
+	 */
+	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 */
+	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 */
+	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 = 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 */
+	DataValidator(const DataValidator&);
+	DataValidator operator=(const DataValidator&);
+};

src/lib/ecl/validation/data_validator_group.cpp

@@ -0,0 +1,199 @@
+/****************************************************************************
+ *
+ *   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>
+ */
+
+#include "data_validator_group.h"
+#include <ecl/ecl.h>
+
+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, int priority)
+{
+	DataValidator *next = _first;
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		if (i == index) {
+			next->put(timestamp, val, error_count, priority);
+			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_priority = -1000;
+	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) &&
+				(next->priority() >= max_priority)))) {
+			max_index = i;
+			max_confidence = confidence;
+			max_priority = next->priority();
+			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 */
+	ECL_INFO("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) {
+		ECL_INFO("sensor #%u:\n", i);
+		next->print();
+		next = next->sibling();
+		i++;
+	}
+}
+
+unsigned
+DataValidatorGroup::failover_count()
+{
+	return _toggle_count;
+}

src/lib/ecl/validation/data_validator_group.h

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   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 __EXPORT DataValidatorGroup {
+public:
+	DataValidatorGroup(unsigned siblings);
+	virtual ~DataValidatorGroup();
+
+	/**
+	 * Put an item into the validator group.
+	 *
+	 * @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, int priority);
+
+	/**
+	 * Get the best data triplet of the group
+	 *
+	 * @return		pointer to the array of best values
+	 */
+	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
+	 *
+	 */
+	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 */
+	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&);
+	DataValidatorGroup operator=(const DataValidatorGroup&);
+};

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,10 +394,10 @@ 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;
+						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;

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

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>
@@ -59,8 +58,11 @@
 #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>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -103,6 +105,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 */
@@ -121,6 +125,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;
@@ -130,6 +135,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;
@@ -142,10 +149,23 @@ private:
 	vehicle_global_position_s _gpos = {};
 	Vector<3>	_vel_prev;
 	Vector<3>	_pos_acc;
+
+	DataValidatorGroup _voter_gyro;
+	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;
 	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;
@@ -160,7 +180,15 @@ private:
 };
 
 
-AttitudeEstimatorQ::AttitudeEstimatorQ() {
+AttitudeEstimatorQ::AttitudeEstimatorQ() :
+	_voter_gyro(3),
+	_voter_accel(3),
+	_voter_mag(3),
+	_lp_roll_rate(250.0f, 20.0f),
+	_lp_pitch_rate(250.0f, 20.0f)
+{
+	_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");
@@ -168,12 +196,14 @@ 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");
 }
 
 /**
  * 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,14 +226,15 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() {
 	attitude_estimator_q::instance = nullptr;
 }
 
-int AttitudeEstimatorQ::start() {
+int AttitudeEstimatorQ::start()
+{
 	ASSERT(_control_task == -1);
 
 	/* start the task */
 	_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);
 
@@ -215,12 +246,23 @@ int AttitudeEstimatorQ::start() {
 	return OK;
 }
 
-void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) {
-	attitude_estimator_q::instance->task_main();
+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() {
+void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
+{
+	attitude_estimator_q::instance->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));
@@ -236,6 +278,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);
@@ -250,11 +296,71 @@ 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
+
+			for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); 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], sensors.gyro_priority[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;
+
+			// 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));
+
+			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 ||
+				_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;
@@ -311,8 +417,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++) {
@@ -328,6 +437,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 {
@@ -358,6 +471,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);
 	}
 }
 
@@ -501,6 +615,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;
 

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

src/modules/commander/commander.cpp

@@ -1436,7 +1436,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;

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
 

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

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

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>
@@ -70,11 +69,16 @@
 
 #include <geo/geo.h>
 #include <systemlib/perf_counter.h>
+#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:
     /**
@@ -168,9 +172,8 @@ private:
     struct vehicle_land_detected_s      _landDetector;
     struct actuator_armed_s             _armed;
 
-    struct gyro_scale               _gyro_offsets[3];
-    struct accel_scale              _accel_offsets[3];
-    struct mag_scale                _mag_offsets[3];
+    hrt_abstime _last_accel;
+    hrt_abstime _last_mag;
 
     struct sensor_combined_s            _sensor_combined;
 
@@ -179,6 +182,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
@@ -198,14 +203,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
 
@@ -216,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;

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()
@@ -109,6 +113,7 @@ AttitudePositionEstimatorEKF	*g_estimator = nullptr;
 }
 
 AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
+	SuperBlock(NULL, "EKF"),
 	_task_should_exit(false),
 	_task_running(false),
 	_estimator_task(-1),
@@ -127,84 +132,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 {},
-
-    _gyro_offsets{},
-    _accel_offsets{},
-    _mag_offsets{},
-
-    _sensor_combined {},
-
-    _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")),
-
-    /* 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),
-
-    _newHgtData(false),
-    _newAdsData(false),
-    _newDataMag(false),
-    _newRangeData(false),
-
-    _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)
+	_att{},
+	_gyro{},
+	_accel{},
+	_mag{},
+	_airspeed{},
+	_baro{},
+	_vstatus{},
+	_global_pos{},
+	_local_pos{},
+	_gps{},
+	_wind{},
+	_distance{},
+	_landDetector{},
+	_armed{},
+
+	_last_accel(0),
+	_last_mag(0),
+
+	_sensor_combined{},
+
+	 _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")),
+
+	      /* 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),
+
+	_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),
+
+	_LP_att_P(250.0f, 20.0f),
+	_LP_att_Q(250.0f, 20.0f),
+	_LP_att_R(250.0f, 20.0f)
 {
-	_last_run = hrt_absolute_time();
+	_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");
@@ -231,48 +240,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()
@@ -311,6 +278,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));
@@ -348,6 +317,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;
@@ -360,12 +336,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();
+		}
 	}
 }
 
@@ -535,8 +523,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();
@@ -604,15 +590,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;
@@ -640,7 +625,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
@@ -830,9 +815,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;
@@ -1003,7 +988,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;
@@ -1029,8 +1014,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();
@@ -1053,8 +1038,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();
@@ -1070,7 +1055,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();
@@ -1083,7 +1068,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();
@@ -1098,7 +1083,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 {
@@ -1111,7 +1096,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;
 		}
@@ -1125,7 +1110,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);
@@ -1157,7 +1142,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,
@@ -1204,6 +1189,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()
@@ -1216,125 +1208,153 @@ void AttitudePositionEstimatorEKF::pollData()
 		orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
 	}
 
-	//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;
+	// 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], _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]);
 	}
 
-	last_accel = _sensor_combined.accelerometer_timestamp;
-
-
-	// Copy gyro and accel
-	_last_sensor_timestamp = _sensor_combined.timestamp;
-	IMUmsec = _sensor_combined.timestamp / 1e3;
-	IMUusec = _sensor_combined.timestamp;
+	// Get best measurement values
+	hrt_abstime curr_time = hrt_absolute_time();
+	(void)_voter_gyro.get_best(curr_time, &_gyro_main);
+	if (_gyro_main >= 0) {
 
-	float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
+		// 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]);
+		}
 
-	/* guard against too large deltaT's */
-	if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
-		deltaT = 0.01f;
+		_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];
+		perf_count(_perf_gyro);
 	}
 
-	_last_run = _sensor_combined.timestamp;
+	(void)_voter_accel.get_best(curr_time, &_accel_main);
+	if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
 
-	// Always store data, independent of init status
-	/* fill in last data set */
-	_ekf->dtIMU = deltaT;
+		// 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]);
+		}
 
-	int last_gyro_main = _gyro_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];
+		_last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
+	}
 
-	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))) {
+	(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]);
 
-		_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;
+		/* 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->magData.y = mag.y;
+			_ekf->magData.z = mag.z;
+			_newDataMag = true;
+			_last_mag = _sensor_combined.magnetometer_timestamp[_mag_main];
+			perf_count(_perf_mag);
+		}
+	}
 
-	} 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])) {
+	if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
+		_voter_accel.failover_count() > 0 ||
+		_voter_mag.failover_count() > 0)) {
 
-		_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;
+		_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 {
-		_gyro_valid = false;
+		_vibration_warning_timestamp = 0;
 	}
 
-	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);
-	}
+	IMUusec = _sensor_combined.timestamp;
 
-	if (!_gyro_valid) {
-		/* keep last value if he hit an out of band value */
-		lastAngRate = _ekf->angRate;
+	float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
+	_last_sensor_timestamp = _sensor_combined.timestamp;
 
-	} else {
-		perf_count(_perf_gyro);
+	/* guard against too large deltaT's */
+	if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
+		deltaT = 0.01f;
 	}
 
-	if (accel_updated) {
+	// Always store data, independent of init status
+	/* fill in last data set */
+	_ekf->dtIMU = deltaT;
 
-		int last_accel_main = _accel_main;
+	// XXX this is for assessing the filter performance
+	// leave this in as long as larger improvements are still being made.
+	#if 0
 
-		/* 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;
+	float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f;
+	float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f;
 
-		} 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;
-		}
+	static unsigned dtoverflow5 = 0;
+	static unsigned dtoverflow10 = 0;
+	static hrt_abstime lastprint = 0;
 
-		if (!_accel_valid) {
-			lastAccel = _ekf->accel;
-		}
+	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);
 
-		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);
-		}
+		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);
 
-		_accel_valid = true;
-	}
+		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));
 
-	_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
-	lastAngRate = _ekf->angRate;
-	_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
-	lastAccel = _ekf->accel;
+		lastprint = hrt_absolute_time();
+	}
 
-	if (last_mag != _sensor_combined.magnetometer_timestamp) {
-		_newDataMag = true;
+	if (deltaT > 0.005f) {
+		dtoverflow5++;
+	}
 
-	} else {
-		_newDataMag = false;
+	if (deltaT > 0.01f) {
+		dtoverflow10++;
 	}
+	#endif
 
-	last_mag = _sensor_combined.magnetometer_timestamp;
+	_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);
 
@@ -1356,7 +1376,6 @@ void AttitudePositionEstimatorEKF::pollData()
 		_ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s;
 	}
 
-
 	bool gps_update;
 	orb_check(_gps_sub, &gps_update);
 
@@ -1510,57 +1529,6 @@ void AttitudePositionEstimatorEKF::pollData()
 		perf_count(_perf_baro);
 	}
 
-	//Update Magnetometer
-	if (_newDataMag) {
-
-		_mag_valid = true;
-
-		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]);
-
-		Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1],
-			_sensor_combined.magnetometer1_ga[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) < 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
-
-			_ekf->magData.y = mag0.y;
-			_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
-
-			_ekf->magData.z = mag0.z;
-			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
-			_mag_main = 0;
-
-		} 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;
-		}
-
-		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);
 

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.
  *

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

src/modules/mavlink/mavlink_receiver.cpp

@@ -1408,6 +1408,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;
@@ -1415,9 +1416,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;
@@ -1429,19 +1430,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.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.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.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[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[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) {

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) {

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

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

src/modules/sdlog2/sdlog2.c

@@ -1293,18 +1293,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;
@@ -1452,144 +1445,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;
-			}
-
-			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;
-			}
+			for (unsigned i = 0; i < 3; i++) {
+				bool write_IMU = false;
+				bool write_SENS = false;
 
-			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.gyro_timestamp[i] != gyro_timestamp[i]) {
+					gyro_timestamp[i] = buf.sensor.gyro_timestamp[i];
+					write_IMU = true;
+				}
 
-			if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
-				accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
-				write_IMU1 = true;
-			}
+				if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) {
+					accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i];
+					write_IMU = true;
+				}
 
-			if (buf.sensor.gyro1_timestamp != gyro1_timestamp) {
-				gyro1_timestamp = buf.sensor.gyro1_timestamp;
-				write_IMU1 = true;
-			}
+				if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) {
+					magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i];
+					write_IMU = true;
+				}
 
-			if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) {
-				magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp;
-				write_IMU1 = true;
-			}
+				if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) {
+					barometer_timestamp[i] = buf.sensor.baro_timestamp[i];
+					write_SENS = 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.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) {
+					differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i];
+					write_SENS = true;
+				}
 
-			if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) {
-				accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp;
-				write_IMU2 = 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;
+					}
 
-			if (buf.sensor.gyro2_timestamp != gyro2_timestamp) {
-				gyro2_timestamp = buf.sensor.gyro2_timestamp;
-				write_IMU2 = true;
-			}
+					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 (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) {
-				magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp;
-				write_IMU2 = true;
-			}
+				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;
+					}
 
-			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);
+					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);
+				}
 			}
-
 		}
 
 		/* --- ATTITUDE --- */

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
@@ -200,25 +202,23 @@ 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 */
+	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 */
+	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		_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		_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 */
@@ -228,18 +228,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 */
@@ -251,7 +239,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 */
@@ -377,6 +365,9 @@ private:
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
+	int		init_sensor_class(const struct orb_metadata *meta, int *subs,
+				unsigned *priorities, unsigned *errcount);
+
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -495,20 +486,18 @@ Sensors::Sensors() :
 	_sensors_task(-1),
 	_hil_enabled(false),
 	_publishing(true),
+	_armed(false),
 
 	/* 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 +512,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")),
 
@@ -546,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));
@@ -1032,269 +1016,132 @@ Sensors::adc_init()
 void
 Sensors::accel_poll(struct sensor_combined_s &raw)
 {
-	bool accel_updated;
-	orb_check(_accel_sub, &accel_updated);
-
-	if (accel_updated) {
-		struct accel_report	accel_report;
-
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
-
-		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);
+	for (unsigned i = 0; i < _accel_count; i++) {
+		bool accel_updated;
+		orb_check(_accel_sub[i], &accel_updated);
 
-		raw.accelerometer_raw[0] = accel_report.x_raw;
-		raw.accelerometer_raw[1] = accel_report.y_raw;
-		raw.accelerometer_raw[2] = accel_report.z_raw;
+		if (accel_updated) {
+			struct accel_report	accel_report;
 
-		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);
+			orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
 
-		raw.accelerometer1_raw[0] = accel_report.x_raw;
-		raw.accelerometer1_raw[1] = accel_report.y_raw;
-		raw.accelerometer1_raw[2] = accel_report.z_raw;
+			math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
+			vect = _board_rotation * vect;
 
-		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;
+			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);
 
-		orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report);
+			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(accel_report.x, accel_report.y, accel_report.z);
-		vect = _board_rotation * vect;
+			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.accelerometer2_m_s2[0] = vect(0);
-		raw.accelerometer2_m_s2[1] = vect(1);
-		raw.accelerometer2_m_s2[2] = vect(2);
+			raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
 
-		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.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.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);
-
-	if (gyro_updated) {
-		struct gyro_report	gyro_report;
+	for (unsigned i = 0; i < _gyro_count; i++) {
+		bool gyro_updated;
+		orb_check(_gyro_sub[i], &gyro_updated);
 
-		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
+		if (gyro_updated) {
+			struct gyro_report	gyro_report;
 
-		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
-		vect = _board_rotation * vect;
+			orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
 
-		raw.gyro_rad_s[0] = vect(0);
-		raw.gyro_rad_s[1] = vect(1);
-		raw.gyro_rad_s[2] = vect(2);
+			math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
+			vect = _board_rotation * vect;
 
-		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_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);
 
-		raw.timestamp = gyro_report.timestamp;
-		raw.gyro_priority = _gyro_prio;
-		raw.gyro_errcount = gyro_report.error_count;
-		raw.gyro_temp = gyro_report.temperature;
-	}
+			math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
+			vect_int = _board_rotation * vect_int;
 
-	orb_check(_gyro1_sub, &gyro_updated);
+			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);
 
-	if (gyro_updated) {
-		struct gyro_report	gyro_report;
+			raw.gyro_integral_dt[i] = gyro_report.integral_dt;
 
-		orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report);
+			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;
 
-		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);
-
-	if (mag_updated) {
-		struct mag_report	mag_report;
-
-		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
+	for (unsigned i = 0; i < _mag_count; i++) {
+		bool mag_updated;
+		orb_check(_mag_sub[i], &mag_updated);
 
-		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
+		if (mag_updated) {
+			struct mag_report	mag_report;
 
-		vect = _mag_rotation[0] * vect;
+			orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report);
 
-		raw.magnetometer_ga[0] = vect(0);
-		raw.magnetometer_ga[1] = vect(1);
-		raw.magnetometer_ga[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_timestamp = mag_report.timestamp;
-		raw.magnetometer_priority = _mag_prio;
-		raw.magnetometer_errcount = mag_report.error_count;
-		raw.magnetometer_temp = mag_report.temperature;
-	}
+			math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
-	orb_check(_mag1_sub, &mag_updated);
+			vect = _mag_rotation[i] * vect;
 
-	if (mag_updated) {
-		struct mag_report	mag_report;
+			raw.magnetometer_ga[i * 3 + 0] = vect(0);
+			raw.magnetometer_ga[i * 3 + 1] = vect(1);
+			raw.magnetometer_ga[i * 3 + 2] = vect(2);
 
-		orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report);
+			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;
 
-		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);
-
-		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_timestamp = _barometer.timestamp;
-		raw.baro_priority = _baro_prio;
-	}
+			orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
 
-	orb_check(_baro1_sub, &baro_updated);
+			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
 
-	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;
+		}
 	}
 }
 
@@ -1307,12 +1154,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;
 
@@ -1320,11 +1167,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;
 
@@ -1352,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;
 		}
 	}
 }
@@ -1391,7 +1239,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);
@@ -1405,7 +1253,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;
 
@@ -1457,7 +1305,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);
@@ -1471,7 +1319,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;
 
@@ -1523,7 +1371,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
@@ -1544,7 +1392,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;
 
@@ -2099,6 +1947,26 @@ Sensors::task_main_trampoline(int argc, char *argv[])
 	sensors::g_sensors->task_main();
 }
 
+int
+Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
+	unsigned *priorities, unsigned *errcount)
+{
+	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++) {
+		if (subs[i] < 0) {
+			subs[i] = orb_subscribe_multi(meta, i);
+			orb_priority(subs[i], (int32_t*)&priorities[i]);
+		}
+	}
+
+	return group_count;
+}
+
 void
 Sensors::task_main()
 {
@@ -2129,74 +1997,47 @@ Sensors::task_main()
 		return;
 	}
 
+	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
 	 */
-	_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);
+
+	_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]);
+
 	_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);
 
-	/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
-	orb_set_interval(_gyro_sub, 4);
-
 	/*
 	 * 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;
@@ -2219,8 +2060,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;
@@ -2245,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);
@@ -2253,13 +2109,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];
 			}
 		}
 

src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp

@@ -303,6 +303,11 @@ int VCDevExample::main()
 		ret = 1;
 		goto fail2;
 	}
+	PX4_INFO("TEST: 100ms TIMEOUT POLL -----------");
+	if(do_poll(fd, 0, 30, 100)) {
+		ret = 1;
+		goto fail2;
+	}
 	PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------");
 	if(do_poll(fd, 1000, 3, 0)) {
 		ret = 1;

src/platforms/posix/work_queue/hrt_thread.c

@@ -41,6 +41,7 @@
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <stdint.h>
+#include <signal.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <queue.h>
@@ -73,6 +74,26 @@ sem_t _hrt_work_lock;
  ****************************************************************************/
 static void hrt_work_process(void);
 
+static void _sighandler(int sig_num);
+
+/****************************************************************************
+ * Name: _sighandler
+ *
+ * Description:
+ *   This is the handler for the signal to wake the queue processing thread
+ *
+ * Input parameters:
+ *   sig_num - the received signal
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+static void _sighandler(int sig_num)
+{
+	PX4_DEBUG("RECEIVED SIGNAL %d", sig_num);
+}
+
 /****************************************************************************
  * Name: work_process
  *
@@ -251,5 +272,11 @@ void hrt_work_queue_init(void)
 					    work_hrtthread,
 					    (char *const *)NULL);
 
+	
+#ifdef __PX4_QURT
+	signal(SIGALRM, _sighandler);
+#else
+	signal(SIGCONT, _sighandler);
+#endif
 }