Merge branch 'ekf_voting_priority'

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

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/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(report.x);
-	report.y = _gyro_filter_y.apply(report.y);
-	report.z = _gyro_filter_z.apply(report.z);
+	report.x = _gyro_filter_x.apply(xin);
+	report.y = _gyro_filter_y.apply(yin);
+	report.z = _gyro_filter_z.apply(zin);
+
+	math::Vector<3> gval(xin, yin, zin);
+	math::Vector<3> gval_integrated;
+
+	bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt);
+	report.x_integral = gval_integrated(0);
+	report.y_integral = gval_integrated(1);
+	report.z_integral = gval_integrated(2);
 
 	report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
 
@@ -1044,13 +1057,15 @@ L3GD20::measure()
 
 	_reports->force(&report);
 
-	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
+	if (gyro_notify) {
+		/* notify anyone waiting for data */
+		poll_notify(POLLIN);
 
-	/* publish for subscribers */
-	if (!(_pub_blocked)) {
-		/* publish it */
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+		/* publish for subscribers */
+		if (!(_pub_blocked)) {
+			/* publish it */
+			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+		}
 	}
 
 	_read++;

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/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[]) {
+void AttitudeEstimatorQ::print()
+{
+	warnx("gyro status:");
+	_voter_gyro.print();
+	warnx("accel status:");
+	_voter_accel.print();
+	warnx("mag status:");
+	_voter_mag.print();
+}
+
+void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
+{
 	attitude_estimator_q::instance->task_main();
 }
 
-void AttitudeEstimatorQ::task_main() {
-
+void AttitudeEstimatorQ::task_main()
+{
 	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -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

@@ -1438,7 +1438,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_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

@@ -1410,6 +1410,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;
@@ -1417,9 +1418,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;
@@ -1431,19 +1432,19 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.magnetometer_ga[0] = imu.xmag;
 		hil_sensors.magnetometer_ga[1] = imu.ymag;
 		hil_sensors.magnetometer_ga[2] = imu.zmag;
-		hil_sensors.magnetometer_range_ga = 32.7f; // int16
-		hil_sensors.magnetometer_mode = 0; // TODO what is this
-		hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
-		hil_sensors.magnetometer_timestamp = timestamp;
+		hil_sensors.magnetometer_range_ga[0] = 32.7f; // int16
+		hil_sensors.magnetometer_mode[0] = 0; // TODO what is this
+		hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f;
+		hil_sensors.magnetometer_timestamp[0] = timestamp;
 
-		hil_sensors.baro_pres_mbar = imu.abs_pressure;
-		hil_sensors.baro_alt_meter = imu.pressure_alt;
-		hil_sensors.baro_temp_celcius = imu.temperature;
-		hil_sensors.baro_timestamp = timestamp;
+		hil_sensors.baro_pres_mbar[0] = imu.abs_pressure;
+		hil_sensors.baro_alt_meter[0] = imu.pressure_alt;
+		hil_sensors.baro_temp_celcius[0] = imu.temperature;
+		hil_sensors.baro_timestamp[0] = timestamp;
 
-		hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
-		hil_sensors.differential_pressure_filtered_pa = hil_sensors.differential_pressure_pa;
-		hil_sensors.differential_pressure_timestamp = timestamp;
+		hil_sensors.differential_pressure_pa[0] = imu.diff_pressure * 1e2f; //from hPa to Pa
+		hil_sensors.differential_pressure_filtered_pa[0] = hil_sensors.differential_pressure_pa[0];
+		hil_sensors.differential_pressure_timestamp[0] = timestamp;
 
 		/* publish combined sensor topic */
 		if (_sensors_pub == nullptr) {

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

@@ -1287,18 +1287,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;
@@ -1446,144 +1439,86 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- SENSOR COMBINED --- */
 		if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) {
-			bool write_IMU = false;
-			bool write_IMU1 = false;
-			bool write_IMU2 = false;
-			bool write_SENS = false;
-			bool write_SENS1 = false;
+			
 
-			if (buf.sensor.timestamp != gyro_timestamp) {
-				gyro_timestamp = buf.sensor.timestamp;
-				write_IMU = true;
+			for (unsigned i = 0; i < 3; i++) {
+				bool write_IMU = false;
+				bool write_SENS = false;
+
+				if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) {
+					gyro_timestamp[i] = buf.sensor.gyro_timestamp[i];
+					write_IMU = true;
+				}
+
+				if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) {
+					accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i];
+					write_IMU = true;
+				}
+
+				if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) {
+					magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i];
+					write_IMU = true;
+				}
+
+				if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) {
+					barometer_timestamp[i] = buf.sensor.baro_timestamp[i];
+					write_SENS = true;
+				}
+
+				if (buf.sensor.differential_pressure_timestamp[i] != differential_pressure_timestamp[i]) {
+					differential_pressure_timestamp[i] = buf.sensor.differential_pressure_timestamp[i];
+					write_SENS = true;
+				}
+
+				if (write_IMU) {
+					switch (i) {
+						case 0:
+							log_msg.msg_type = LOG_IMU_MSG;
+							break;
+						case 1:
+							log_msg.msg_type = LOG_IMU1_MSG;
+							break;
+						case 2:
+							log_msg.msg_type = LOG_IMU2_MSG;
+							break;
+					}
+
+					log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[i * 3 + 0];
+					log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[i * 3 + 1];
+					log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[i * 3 + 2];
+					log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[i * 3 + 0];
+					log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[i * 3 + 1];
+					log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[i * 3 + 2];
+					log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0];
+					log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1];
+					log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2];
+					log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp[i * 3 + 0];
+					log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp[i * 3 + 0];
+					log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp[i * 3 + 0];
+					LOGBUFFER_WRITE_AND_COUNT(IMU);
+				}
+
+				if (write_SENS) {
+					switch (i) {
+						case 0:
+							log_msg.msg_type = LOG_SENS_MSG;
+							break;
+						case 1:
+							log_msg.msg_type = LOG_AIR1_MSG;
+							break;
+						case 2:
+							continue;
+							break;
+					}
+
+					log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar[i];
+					log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i];
+					log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i];
+					log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa[i];
+					log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa[i];
+					LOGBUFFER_WRITE_AND_COUNT(SENS);
+				}
 			}
-
-			if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
-				accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
-				write_IMU = true;
-			}
-
-			if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
-				magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
-				write_IMU = true;
-			}
-
-			if (buf.sensor.baro_timestamp != barometer_timestamp) {
-				barometer_timestamp = buf.sensor.baro_timestamp;
-				write_SENS = true;
-			}
-
-			if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
-				differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
-				write_SENS = true;
-			}
-
-			if (write_IMU) {
-				log_msg.msg_type = LOG_IMU_MSG;
-				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0];
-				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1];
-				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2];
-				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
-				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
-				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
-				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
-				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
-				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
-				log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro_temp;
-				log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer_temp;
-				log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer_temp;
-				LOGBUFFER_WRITE_AND_COUNT(IMU);
-			}
-
-			if (write_SENS) {
-				log_msg.msg_type = LOG_SENS_MSG;
-				log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar;
-				log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
-				log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
-				log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
-				log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa;
-				LOGBUFFER_WRITE_AND_COUNT(SENS);
-			}
-
-			if (buf.sensor.baro1_timestamp != barometer1_timestamp) {
-				barometer1_timestamp = buf.sensor.baro1_timestamp;
-				write_SENS1 = true;
-			}
-
-			if (write_SENS1) {
-				log_msg.msg_type = LOG_AIR1_MSG;
-				log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar;
-				log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter;
-				log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius;
-				log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa;
-				log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa;
-				// XXX moving to AIR0-AIR2 instead of SENS
-				LOGBUFFER_WRITE_AND_COUNT(SENS);
-			}
-
-			if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
-				accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
-				write_IMU1 = true;
-			}
-
-			if (buf.sensor.gyro1_timestamp != gyro1_timestamp) {
-				gyro1_timestamp = buf.sensor.gyro1_timestamp;
-				write_IMU1 = true;
-			}
-
-			if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) {
-				magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp;
-				write_IMU1 = true;
-			}
-
-			if (write_IMU1) {
-				log_msg.msg_type = LOG_IMU1_MSG;
-				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0];
-				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1];
-				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2];
-				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0];
-				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1];
-				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2];
-				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0];
-				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1];
-				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2];
-				log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro1_temp;
-				log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer1_temp;
-				log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer1_temp;
-				LOGBUFFER_WRITE_AND_COUNT(IMU);
-			}
-
-			if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) {
-				accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp;
-				write_IMU2 = true;
-			}
-
-			if (buf.sensor.gyro2_timestamp != gyro2_timestamp) {
-				gyro2_timestamp = buf.sensor.gyro2_timestamp;
-				write_IMU2 = true;
-			}
-
-			if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) {
-				magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp;
-				write_IMU2 = true;
-			}
-
-			if (write_IMU2) {
-				log_msg.msg_type = LOG_IMU2_MSG;
-				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0];
-				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1];
-				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2];
-				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0];
-				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1];
-				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2];
-				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0];
-				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1];
-				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2];
-				log_msg.body.log_IMU.temp_gyro = buf.sensor.gyro2_temp;
-				log_msg.body.log_IMU.temp_acc = buf.sensor.accelerometer2_temp;
-				log_msg.body.log_IMU.temp_mag = buf.sensor.magnetometer2_temp;
-				LOGBUFFER_WRITE_AND_COUNT(IMU);
-			}
-
 		}
 
 		/* --- ATTITUDE --- */