sensors: move voting into sensors module

- voting is now at a central place instead of duplicated within the
  estimators
  -> this also means that estimators that did not do voting so far,
     now have voting, like ekf2
- estimators requiring more than that can still subscribe to the raw
  sensors
- allows sensors_combined to be 3 times smaller
  - reduces logger, memcpy (cache) & RAM overhead
- all modules requiring only 1 or 2 sensor values now automatically get
  the voted result
- this also adds voting to baro

msg/sensor_combined.msg

@@ -5,18 +5,18 @@
 # change with board revisions and sensor updates.
 #
 
-uint64[3] gyro_timestamp			# Gyro timestamps
+# gyro timstamp is equal to the timestamp of the message
-float32[9] gyro_integral_rad		# delta angle in the NED body frame in rad in the integration time frame
+float32[3] gyro_integral_rad		# delta angle in the NED body frame in rad in the integration time frame
-uint64[3] gyro_integral_dt			# delta time for gyro integral in us
+uint64 gyro_integral_dt			# delta time for gyro integral in us
 
-float32[9] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
+uint64 accelerometer_timestamp	# Accelerometer timestamp
-uint64[3] accelerometer_integral_dt		# delta time for accel integral in us
+float32[3] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
-uint64[3] accelerometer_timestamp	# Accelerometer timestamp
+uint64 accelerometer_integral_dt		# delta time for accel integral in us
 
-float32[9] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
+uint64 magnetometer_timestamp	# Magnetometer timestamp
-uint64[3] magnetometer_timestamp	# Magnetometer timestamp
+float32[3] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
 
-float32[3] baro_alt_meter			# Altitude, already temp. comp.
+uint64 baro_timestamp		# Barometer timestamp
-float32[3] baro_temp_celcius		# Temperature in degrees celsius
+float32 baro_alt_meter			# Altitude, already temp. comp.
-uint64[3] baro_timestamp		# Barometer timestamp
+float32 baro_temp_celcius		# Temperature in degrees celsius
 

src/drivers/frsky_telemetry/frsky_data.c

@@ -205,7 +205,7 @@ void frsky_send_frame1(int uart)
 {
 	/* send formatted frame */
 	float acceleration[3];
-	float accel_dt = sensor_combined->accelerometer_integral_dt[0] / 1.e6f;
+	float accel_dt = sensor_combined->accelerometer_integral_dt / 1.e6f;
 	acceleration[0] = sensor_combined->accelerometer_integral_m_s[0] / accel_dt;
 	acceleration[1] = sensor_combined->accelerometer_integral_m_s[1] / accel_dt;
 	acceleration[2] = sensor_combined->accelerometer_integral_m_s[2] / accel_dt;
@@ -214,12 +214,12 @@ void frsky_send_frame1(int uart)
 	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(acceleration[2] * 1000.0f));
 
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
-			sensor_combined->baro_alt_meter[0]);
+			sensor_combined->baro_alt_meter);
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
-			roundf(frac(sensor_combined->baro_alt_meter[0]) * 100.0f));
+			roundf(frac(sensor_combined->baro_alt_meter) * 100.0f));
 
 	frsky_send_data(uart, FRSKY_ID_TEMP1,
-			roundf(sensor_combined->baro_temp_celcius[0]));
+			roundf(sensor_combined->baro_temp_celcius));
 
 	frsky_send_data(uart, FRSKY_ID_VFAS,
 			roundf(battery_status->voltage_v * 10.0f));

src/drivers/frsky_telemetry/sPort_data.c

@@ -236,7 +236,7 @@ void sPort_send_CUR(int uart)
 void sPort_send_ALT(int uart)
 {
 	/* send data */
-	uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter[0]);
+	uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter);
 	sPort_send_data(uart, SMARTPORT_ID_ALT, alt);
 }
 

src/drivers/hott/messages.cpp

@@ -149,12 +149,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[0] + 20);
+	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 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[0] + 500);
+	uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
 	msg.altitude_L = (uint8_t)alt & 0xff;
 	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 

src/examples/px4_simple_app/px4_simple_app.c

@@ -102,7 +102,7 @@ int px4_simple_app_main(int argc, char *argv[])
 				/* copy sensors raw data into local buffer */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
 				float sensor_accel[3];
-				float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f;
+				float accel_dt = raw.accelerometer_integral_dt / 1.e6f;
 				sensor_accel[0] = raw.accelerometer_integral_m_s[0] / accel_dt;
 				sensor_accel[1] = raw.accelerometer_integral_m_s[1] / accel_dt;
 				sensor_accel[2] = raw.accelerometer_integral_m_s[2] / accel_dt;

src/lib/terrain_estimation/terrain_estimator.cpp

@@ -68,7 +68,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
 	if (attitude->R_valid) {
 		matrix::Matrix<float, 3, 3> R_att(attitude->R);
 		matrix::Vector<float, 3> a;
-		float accel_dt = sensor->accelerometer_integral_dt[0] / 1.e6f;
+		float accel_dt = sensor->accelerometer_integral_dt / 1.e6f;
 		a(0) = sensor->accelerometer_integral_m_s[0] / accel_dt;
 		a(1) = sensor->accelerometer_integral_m_s[1] / accel_dt;
 		a(2) = sensor->accelerometer_integral_m_s[2] / accel_dt;

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -396,7 +396,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 				}
 
 				float gyro_rad_s[3];
-				float gyro_dt = raw.gyro_integral_dt[0] / 1.e6f;
+				float gyro_dt = raw.gyro_integral_dt / 1.e6f;
 				gyro_rad_s[0] = raw.gyro_integral_rad[0] / gyro_dt;
 				gyro_rad_s[1] = raw.gyro_integral_rad[1] / gyro_dt;
 				gyro_rad_s[2] = raw.gyro_integral_rad[2] / gyro_dt;
@@ -427,10 +427,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.gyro_timestamp[0]) {
+					if (sensor_last_timestamp[0] != raw.timestamp) {
 						update_vect[0] = 1;
 						// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
-						sensor_last_timestamp[0] = raw.gyro_timestamp[0];
+						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
 					z_k[0] =  gyro_rad_s[0] - gyro_offsets[0];
@@ -438,10 +438,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[2] =  gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp[0]) {
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
 						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.accelerometer_timestamp[0];
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
 					hrt_abstime vel_t = 0;
@@ -477,7 +477,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					}
 
 					matrix::Vector3f raw_accel;
-					float accel_dt = raw.accelerometer_integral_dt[0] / 1.e6f;
+					float accel_dt = raw.accelerometer_integral_dt / 1.e6f;
 					raw_accel(0) = raw.accelerometer_integral_m_s[0] / accel_dt;
 					raw_accel(1) = raw.accelerometer_integral_m_s[1] / accel_dt;
 					raw_accel(2) = raw.accelerometer_integral_m_s[2] / accel_dt;
@@ -487,14 +487,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					z_k[5] = raw_accel(2) - acc(2);
 
 					/* update magnetometer measurements */
-					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp[0] &&
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp &&
 						/* 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[0];
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
 					bool vision_updated = false;

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -67,7 +67,6 @@
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/geo/geo.h>
-#include <lib/ecl/validation/data_validator_group.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -139,7 +138,6 @@ private:
 		param_t	mag_decl_auto;
 		param_t	acc_comp;
 		param_t	bias_max;
-		param_t vibe_thresh;
 		param_t	ext_hdg_mode;
 		param_t airspeed_mode;
 	}		_params_handles;		/**< handles for interesting parameters */
@@ -152,8 +150,6 @@ private:
 	bool		_mag_decl_auto = false;
 	bool		_acc_comp = false;
 	float		_bias_max = 0.0f;
-	float		_vibration_warning_threshold = 2.0f;
-	hrt_abstime	_vibration_warning_timestamp = 0;
 	int		_ext_hdg_mode = 0;
 	int 	_airspeed_mode = 0;
 
@@ -177,10 +173,6 @@ private:
 	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;
@@ -190,8 +182,6 @@ private:
 
 	bool		_inited = false;
 	bool		_data_good = false;
-	bool		_failsafe = false;
-	bool		_vibration_warning = false;
 	bool		_ext_hdg_good = false;
 
 	orb_advert_t	_mavlink_log_pub = nullptr;
@@ -215,15 +205,10 @@ private:
 AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_vel_prev(0, 0, 0),
 	_pos_acc(0, 0, 0),
-	_voter_gyro(3),
-	_voter_accel(3),
-	_voter_mag(3),
 	_lp_roll_rate(250.0f, 30.0f),
 	_lp_pitch_rate(250.0f, 30.0f),
 	_lp_yaw_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_ext_hdg	= param_find("ATT_W_EXT_HDG");
@@ -232,7 +217,6 @@ 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");
 	_params_handles.ext_hdg_mode	= param_find("ATT_EXT_HDG_M");
 	_params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
 }
@@ -286,12 +270,6 @@ int AttitudeEstimatorQ::start()
 
 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[])
@@ -346,129 +324,40 @@ void AttitudeEstimatorQ::task_main()
 		// Update sensors
 		sensor_combined_s sensors;
 
-		int best_gyro = 0;
-		int best_accel = 0;
-		int best_mag = 0;
-
 		if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
 			// Feed validator with recent sensor data
 
-			for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
+			if (sensors.timestamp > 0) {
+				float gyro_dt = sensors.gyro_integral_dt / 1e6;
+				_gyro(0) = sensors.gyro_integral_rad[0] / gyro_dt;
+				_gyro(1) = sensors.gyro_integral_rad[1] / gyro_dt;
+				_gyro(2) = sensors.gyro_integral_rad[2] / gyro_dt;
+			}
 
-				/* ignore empty fields */
+			if (sensors.accelerometer_timestamp > 0) {
-				if (sensors.gyro_timestamp[i] > 0) {
+				float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
+				_accel(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
+				_accel(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
+				_accel(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
 
-					float gyro[3];
+				if (_accel.length() < 0.01f) {
-					float gyro_dt = sensors.gyro_integral_dt[i] / 1e6;
+					warnx("WARNING: degenerate accel!");
-					gyro[0] = sensors.gyro_integral_rad[i * 3 + 0] / gyro_dt;
+					continue;
-					gyro[1] = sensors.gyro_integral_rad[i * 3 + 1] / gyro_dt;
-					gyro[2] = sensors.gyro_integral_rad[i * 3 + 2] / gyro_dt;
-
-					//TODO: note: voter will be moved into sensors module
-					//_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
-					_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], 0, 75);
-				}
-
-				if (sensors.accelerometer_timestamp[i] > 0) {
-					float acceleration[3];
-					float accel_dt = sensors.accelerometer_integral_dt[i] / 1.e6f;
-					acceleration[0] = sensors.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
-					acceleration[1] = sensors.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
-					acceleration[2] = sensors.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
-					_voter_accel.put(i, sensors.accelerometer_timestamp[i], acceleration, 0, 75);
-				}
-
-				if (sensors.magnetometer_timestamp[i] > 0) {
-					_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], 0, 75);
 				}
 			}
 
-			// Get best measurement values
+			if (sensors.magnetometer_timestamp > 0) {
-			hrt_abstime curr_time = hrt_absolute_time();
+				_mag(0) = sensors.magnetometer_ga[0];
-			_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
+				_mag(1) = sensors.magnetometer_ga[1];
-			_accel.set(_voter_accel.get_best(curr_time, &best_accel));
+				_mag(2) = sensors.magnetometer_ga[2];
-			_mag.set(_voter_mag.get_best(curr_time, &best_mag));
 
-			if (_accel.length() < 0.01f) {
+				if (_mag.length() < 0.01f) {
-				warnx("WARNING: degenerate accel!");
+					warnx("WARNING: degenerate mag!");
-				continue;
+					continue;
-			}
+				}
-
-			if (_mag.length() < 0.01f) {
-				warnx("WARNING: degenerate mag!");
-				continue;
 			}
 
 			_data_good = true;
-
-			if (!_failsafe) {
-				uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
-
-#ifdef __PX4_POSIX
-				perf_end(_perf_accel);
-				perf_end(_perf_mpu);
-				perf_end(_perf_mag);
-#endif
-
-				if (_voter_gyro.failover_count() > 0) {
-					_failsafe = true;
-					flags = _voter_gyro.failover_state();
-					mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!",
-									  _voter_gyro.failover_index(),
-									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
-				}
-
-				if (_voter_accel.failover_count() > 0) {
-					_failsafe = true;
-					flags = _voter_accel.failover_state();
-					mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!",
-									  _voter_accel.failover_index(),
-									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
-				}
-
-				if (_voter_mag.failover_count() > 0) {
-					_failsafe = true;
-					flags = _voter_mag.failover_state();
-					mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!",
-									  _voter_mag.failover_index(),
-									  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
-									  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
-				}
-
-				if (_failsafe) {
-					mavlink_and_console_log_emergency(&_mavlink_log_pub, "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_log_pub, "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;
-			}
 		}
 
 		// Update vision and motion capture heading
@@ -596,10 +485,6 @@ void AttitudeEstimatorQ::task_main()
 		memcpy(&att.q[0], _q.data, sizeof(att.q));
 		att.q_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());
-
 		/* the instance count is not used here */
 		int att_inst;
 		orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
@@ -656,12 +541,9 @@ void AttitudeEstimatorQ::task_main()
 		}
 
 		{
-			struct estimator_status_s est = {};
+			//struct estimator_status_s est = {};
 
-			est.timestamp = sensors.timestamp;
+			//est.timestamp = sensors.timestamp;
-			est.vibe[0] = _voter_accel.get_vibration_offset(est.timestamp, 0);
-			est.vibe[1] = _voter_accel.get_vibration_offset(est.timestamp, 1);
-			est.vibe[2] = _voter_accel.get_vibration_offset(est.timestamp, 2);
 
 			/* the instance count is not used here */
 			//int est_inst;
@@ -672,6 +554,12 @@ void AttitudeEstimatorQ::task_main()
 		}
 	}
 
+#ifdef __PX4_POSIX
+	perf_end(_perf_accel);
+	perf_end(_perf_mpu);
+	perf_end(_perf_mag);
+#endif
+
 	orb_unsubscribe(_sensors_sub);
 	orb_unsubscribe(_vision_sub);
 	orb_unsubscribe(_mocap_sub);
@@ -706,7 +594,6 @@ 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);
 		param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
 		param_get(_params_handles.airspeed_mode, &_airspeed_mode);
 	}

src/modules/commander/calibration_routines.cpp

@@ -273,7 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 
 			for (unsigned i = 0; i < ndim; i++) {
 
-				float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt[0] / 1.e6f);
+				float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt / 1.e6f);
 
 				float d = di - accel_ema[i];
 				accel_ema[i] += d * w;

src/modules/commander/commander.cpp

@@ -1788,7 +1788,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[0]) < FAILSAFE_DEFAULT_TIMEOUT) {
+			if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
 				/* handle the case where baro was regained */
 				if (status_flags.barometer_failure) {
 					status_flags.barometer_failure = false;

src/modules/ekf2/ekf2_main.cpp

@@ -505,14 +505,14 @@ void Ekf2::task_main()
 		}
 
 		// push imu data into estimator
-		_ekf.setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
+		_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt,
-				&sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);
+				sensors.gyro_integral_rad, sensors.accelerometer_integral_m_s);
 
 		// read mag data
-		_ekf.setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]);
+		_ekf.setMagData(sensors.magnetometer_timestamp, sensors.magnetometer_ga);
 
 		// read baro data
-		_ekf.setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]);
+		_ekf.setBaroData(sensors.baro_timestamp, &sensors.baro_alt_meter);
 
 		// read gps data if available
 		if (gps_updated) {
@@ -615,7 +615,7 @@ void Ekf2::task_main()
 			float gyro_bias[3] = {};
 			_ekf.get_gyro_bias(gyro_bias);
 			float gyro_rad_s[3];
-			float gyro_dt = sensors.gyro_integral_dt[0] / 1.e6f;
+			float gyro_dt = sensors.gyro_integral_dt / 1.e6f;
 			gyro_rad_s[0] = sensors.gyro_integral_rad[0] / gyro_dt - gyro_bias[0];
 			gyro_rad_s[1] = sensors.gyro_integral_rad[1] / gyro_dt - gyro_bias[1];
 			gyro_rad_s[2] = sensors.gyro_integral_rad[2] / gyro_dt - gyro_bias[2];
@@ -650,7 +650,7 @@ void Ekf2::task_main()
 
 			// Acceleration data
 			matrix::Vector<float, 3> acceleration;
-			float accel_dt = sensors.accelerometer_integral_dt[0] / 1.e6f;
+			float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
 			acceleration(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
 			acceleration(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
 			acceleration(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
@@ -811,7 +811,7 @@ void Ekf2::task_main()
 				// TODO use innovatun consistency check timouts to set this
 				global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
 
-				global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)
+				global_pos.pressure_alt = sensors.baro_alt_meter; // Pressure altitude AMSL (m)
 
 				if (_vehicle_global_position_pub == nullptr) {
 					_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
@@ -905,15 +905,15 @@ void Ekf2::task_main()
 		if (publish_replay_message) {
 			struct ekf2_replay_s replay = {};
 			replay.time_ref = now;
-			replay.gyro_integral_dt = sensors.gyro_integral_dt[0];
+			replay.gyro_integral_dt = sensors.gyro_integral_dt;
-			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt[0];
+			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
-			replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0];
+			replay.magnetometer_timestamp = sensors.magnetometer_timestamp;
-			replay.baro_timestamp = sensors.baro_timestamp[0];
+			replay.baro_timestamp = sensors.baro_timestamp;
-			memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad));
+			memcpy(replay.gyro_integral_rad, sensors.gyro_integral_rad, sizeof(replay.gyro_integral_rad));
-			memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0],
+			memcpy(replay.accelerometer_integral_m_s, sensors.accelerometer_integral_m_s,
 			       sizeof(replay.accelerometer_integral_m_s));
-			memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga));
+			memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga));
-			replay.baro_alt_meter = sensors.baro_alt_meter[0];
+			replay.baro_alt_meter = sensors.baro_alt_meter;
 
 			// only write gps data if we had a gps update.
 			if (gps_updated) {

src/modules/ekf2_replay/ekf2_replay_main.cpp

@@ -377,10 +377,10 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		uint8_t *dest_ptr = (uint8_t *)&replay_part1.time_ref;
 		parseMessage(data, dest_ptr, type);
 		_sensors.timestamp = replay_part1.time_ref;
-		_sensors.gyro_integral_dt[0] = replay_part1.gyro_integral_dt;
+		_sensors.gyro_integral_dt = replay_part1.gyro_integral_dt;
-		_sensors.accelerometer_integral_dt[0] = replay_part1.accelerometer_integral_dt;
+		_sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt;
-		_sensors.magnetometer_timestamp[0] = replay_part1.magnetometer_timestamp;
+		_sensors.magnetometer_timestamp = replay_part1.magnetometer_timestamp;
-		_sensors.baro_timestamp[0] = replay_part1.baro_timestamp;
+		_sensors.baro_timestamp = replay_part1.baro_timestamp;
 		_sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad;
 		_sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad;
 		_sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad;
@@ -390,7 +390,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		_sensors.magnetometer_ga[0] = replay_part1.magnetometer_x_ga;
 		_sensors.magnetometer_ga[1] = replay_part1.magnetometer_y_ga;
 		_sensors.magnetometer_ga[2] = replay_part1.magnetometer_z_ga;
-		_sensors.baro_alt_meter[0] = replay_part1.baro_alt_meter;
+		_sensors.baro_alt_meter = replay_part1.baro_alt_meter;
 		_part1_counter_ref = _message_counter;
 
 	} else if (type == LOG_RPL2_MSG) {

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -71,7 +71,6 @@
 #include <geo/geo.h>
 #include <terrain_estimation/terrain_estimator.h>
 #include <systemlib/perf_counter.h>
-#include <lib/ecl/validation/data_validator_group.h>
 #include "estimator_22states.h"
 
 #include <controllib/blocks.hpp>
@@ -191,8 +190,6 @@ 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 = 2.0f;
-    hrt_abstime _vibration_warning_timestamp = 0;
 
     perf_counter_t  _loop_perf;         ///< loop performance counter
     perf_counter_t  _loop_intvl;        ///< loop rate counter
@@ -213,15 +210,7 @@ private:
     hrt_abstime     _filter_start_time;
     hrt_abstime     _last_sensor_timestamp;
     hrt_abstime     _distance_last_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
     bool            _was_landed;        ///< indicates if was landed in previous iteration

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -205,15 +205,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_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),
 	_was_landed(true),
@@ -236,8 +228,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_LP_att_Q(250.0f, 20.0f),
 	_LP_att_R(250.0f, 20.0f)
 {
-	_voter_mag.set_timeout(200000);
-
 	_terrain_estimator = new TerrainEstimator();
 
 	_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
@@ -620,19 +610,9 @@ void AttitudePositionEstimatorEKF::task_main()
 				/* system is in HIL now, wait for measurements to come in one last round */
 				usleep(60000);
 
-				/* HIL is slow, set permissive timeouts */
-				_voter_gyro.set_timeout(500000);
-				_voter_accel.set_timeout(500000);
-				_voter_mag.set_timeout(500000);
-
 				/* now read all sensor publications to ensure all real sensor data is purged */
 				orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
-				/* set sensors to de-initialized state */
-				_gyro_main = -1;
-				_accel_main = -1;
-				_mag_main = -1;
-
 				_baro_init = false;
 				_gps_initialized = false;
 
@@ -664,7 +644,8 @@ void AttitudePositionEstimatorEKF::task_main()
 			 *    We run the filter only once all data has been fetched
 			 **/
 
-			if (_baro_init && (_gyro_main >= 0) && (_accel_main >= 0) && (_mag_main >= 0)) {
+			if (_baro_init && _sensor_combined.accelerometer_timestamp && _sensor_combined.timestamp
+					&& _sensor_combined.magnetometer_timestamp) {
 
 				// maintain filtered baro and gps altitudes to calculate weather offset
 				// baro sample rate is ~70Hz and measurement bandwidth is high
@@ -1343,13 +1324,6 @@ 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()
@@ -1382,97 +1356,37 @@ void AttitudePositionEstimatorEKF::pollData()
 	/* fill in last data set */
 	_ekf->dtIMU = deltaT;
 
-	// Feed validator with recent sensor data
+	_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[0];
+	_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[1];
+	_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[2];
 
-	//TODO: note, we will move voters into sensors module
+	float gyro_dt = _sensor_combined.gyro_integral_dt / 1.e6f;
-	for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
+	_ekf->angRate = _ekf->dAngIMU / gyro_dt;
-		float gyro_rad_s[3];
-		float gyro_dt = _sensor_combined.gyro_integral_dt[i] / 1.e6f;
-		gyro_rad_s[0] = _sensor_combined.gyro_integral_rad[i * 3 + 0] / gyro_dt;
-		gyro_rad_s[1] = _sensor_combined.gyro_integral_rad[i * 3 + 1] / gyro_dt;
-		gyro_rad_s[2] = _sensor_combined.gyro_integral_rad[i * 3 + 2] / gyro_dt;
-		_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], gyro_rad_s, 0, 75);
-		float acceleration[3];
-		float accel_dt = _sensor_combined.accelerometer_integral_dt[i] / 1.e6f;
-		acceleration[0] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
-		acceleration[1] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
-		acceleration[2] = _sensor_combined.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
-		_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], acceleration, 0, 75);
-		_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], 0, 75);
-	}
 
-	// Get best measurement values
+	perf_count(_perf_gyro);
-	hrt_abstime curr_time = hrt_absolute_time();
-	(void)_voter_gyro.get_best(curr_time, &_gyro_main);
 
-	if (_gyro_main >= 0) {
+	if (_last_accel != _sensor_combined.accelerometer_timestamp) {
 
-		_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
+		_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
-		_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
+		_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
-		_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
+		_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[2];
 
-		float gyro_dt = _sensor_combined.gyro_integral_dt[_gyro_main] / 1.e6f;
+		float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
-		_ekf->angRate = _ekf->dAngIMU / gyro_dt;
-
-		perf_count(_perf_gyro);
-	}
-
-	(void)_voter_accel.get_best(curr_time, &_accel_main);
-
-	if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
-
-		_ekf->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];
-
-		float accel_dt = _sensor_combined.accelerometer_integral_dt[_accel_main] / 1.e6f;
 		_ekf->accel = _ekf->dVelIMU / accel_dt;
 
-		_last_accel = _sensor_combined.accelerometer_timestamp[_accel_main];
+		_last_accel = _sensor_combined.accelerometer_timestamp;
 	}
 
-	(void)_voter_mag.get_best(curr_time, &_mag_main);
+	Vector3f mag(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
+			_sensor_combined.magnetometer_ga[2]);
 
-	if (_mag_main >= 0) {
+	if (mag.length() > 0.1f && _last_mag != _sensor_combined.magnetometer_timestamp) {
-		Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1],
+		_ekf->magData.x = mag.x;
-			     _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]);
+		_ekf->magData.y = mag.y;
-
+		_ekf->magData.z = mag.z;
-		/* fail over to the 2nd mag if we know the first is down */
+		_newDataMag = true;
-		if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) {
+		_last_mag = _sensor_combined.magnetometer_timestamp;
-			_ekf->magData.x = mag.x;
+		perf_count(_perf_mag);
-			_ekf->magData.y = mag.y;
-			_ekf->magData.z = mag.z;
-			_newDataMag = true;
-			_last_mag = _sensor_combined.magnetometer_timestamp[_mag_main];
-			perf_count(_perf_mag);
-		}
-	}
-
-	if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
-			   _voter_accel.failover_count() > 0 ||
-			   _voter_mag.failover_count() > 0)) {
-
-		_failsafe = true;
-		mavlink_and_console_log_emergency(&_mavlink_log_pub, "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_log_pub, "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;
 	}
 
 	_last_sensor_timestamp = _sensor_combined.timestamp;
@@ -1481,8 +1395,8 @@ void AttitudePositionEstimatorEKF::pollData()
 	// leave this in as long as larger improvements are still being made.
 #if 0
 
-	float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f;
+	float deltaTIntegral = _sensor_combined.gyro_integral_dt / 1e6f;
-	float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f;
+	float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt / 1e6f;
 
 	static unsigned dtoverflow5 = 0;
 	static unsigned dtoverflow10 = 0;

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1231,7 +1231,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	/* filter speed and altitude for controller */
 	math::Vector<3> accel_body;
-	float accel_dt = _sensor_combined.accelerometer_integral_dt[0] / 1.e6f;
+	float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
 	accel_body(0) = _sensor_combined.accelerometer_integral_m_s[0] / accel_dt;
 	accel_body(1) = _sensor_combined.accelerometer_integral_m_s[1] / accel_dt;
 	accel_body(2) = _sensor_combined.accelerometer_integral_m_s[2] / accel_dt;

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -744,7 +744,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
 		_pub_gpos.get().terrain_alt_valid = _validTZ;
 		_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
-		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0];
+		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter;
 		_pub_gpos.update();
 	}
 }
@@ -845,7 +845,7 @@ void BlockLocalPositionEstimator::predict()
 	if (integrate && _sub_att.get().R_valid) {
 		Matrix3f R_att(_sub_att.get().R);
 		Vector3f a;
-		float accel_dt = _sub_sensor.get().accelerometer_integral_dt[0] / 1.e6f;
+		float accel_dt = _sub_sensor.get().accelerometer_integral_dt / 1.e6f;
 		a(0) = _sub_sensor.get().accelerometer_integral_m_s[0] / accel_dt;
 		a(1) = _sub_sensor.get().accelerometer_integral_m_s[1] / accel_dt;
 		a(2) = _sub_sensor.get().accelerometer_integral_m_s[2] / accel_dt;

src/modules/local_position_estimator/sensors/baro.cpp

@@ -41,7 +41,7 @@ int BlockLocalPositionEstimator::baroMeasure(Vector<float, n_y_baro> &y)
 {
 	//measure
 	y.setZero();
-	y(0) = _sub_sensor.get().baro_alt_meter[0];
+	y(0) = _sub_sensor.get().baro_alt_meter;
 	_baroStats.update(y);
 	_time_last_baro = _timeStamp;
 	return OK;

src/modules/mavlink/mavlink_messages.cpp

@@ -724,39 +724,39 @@ protected:
 		if (_sensor_sub->update(&_sensor_time, &sensor)) {
 			uint16_t fields_updated = 0;
 
-			if (_accel_timestamp != sensor.accelerometer_timestamp[0]) {
+			if (_accel_timestamp != sensor.accelerometer_timestamp) {
 				/* mark first three dimensions as changed */
 				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-				_accel_timestamp = sensor.accelerometer_timestamp[0];
+				_accel_timestamp = sensor.accelerometer_timestamp;
 			}
 
-			if (_gyro_timestamp != sensor.gyro_timestamp[0]) {
+			if (_gyro_timestamp != sensor.timestamp) {
 				/* mark second group dimensions as changed */
 				fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-				_gyro_timestamp = sensor.gyro_timestamp[0];
+				_gyro_timestamp = sensor.timestamp;
 			}
 
-			if (_mag_timestamp != sensor.magnetometer_timestamp[0]) {
+			if (_mag_timestamp != sensor.magnetometer_timestamp) {
 				/* mark third group dimensions as changed */
 				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-				_mag_timestamp = sensor.magnetometer_timestamp[0];
+				_mag_timestamp = sensor.magnetometer_timestamp;
 			}
 
-			if (_baro_timestamp != sensor.baro_timestamp[0]) {
+			if (_baro_timestamp != sensor.baro_timestamp) {
 				/* mark last group dimensions as changed */
 				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-				_baro_timestamp = sensor.baro_timestamp[0];
+				_baro_timestamp = sensor.baro_timestamp;
 			}
 			_differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure);
 
 			mavlink_highres_imu_t msg;
 
 			msg.time_usec = sensor.timestamp;
-			float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
+			float accel_dt = sensor.accelerometer_integral_dt / 1.e6f;
 			msg.xacc = sensor.accelerometer_integral_m_s[0] / accel_dt;
 			msg.yacc = sensor.accelerometer_integral_m_s[1] / accel_dt;
 			msg.zacc = sensor.accelerometer_integral_m_s[2] / accel_dt;
-			float gyro_dt = sensor.gyro_integral_dt[0] / 1.e6f;
+			float gyro_dt = sensor.gyro_integral_dt / 1.e6f;
 			msg.xgyro = sensor.gyro_integral_rad[0] / gyro_dt;
 			msg.ygyro = sensor.gyro_integral_rad[1] / gyro_dt;
 			msg.zgyro = sensor.gyro_integral_rad[2] / gyro_dt;
@@ -765,8 +765,8 @@ protected:
 			msg.zmag = sensor.magnetometer_ga[2];
 			msg.abs_pressure = 0;
 			msg.diff_pressure = differential_pressure.differential_pressure_raw_pa;
-			msg.pressure_alt = sensor.baro_alt_meter[0];
+			msg.pressure_alt = sensor.baro_alt_meter;
-			msg.temperature = sensor.baro_temp_celcius[0];
+			msg.temperature = sensor.baro_temp_celcius;
 			msg.fields_updated = fields_updated;
 
 			mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg);
@@ -1014,7 +1014,7 @@ protected:
 				/* fall back to baro altitude */
 				sensor_combined_s sensor;
 				(void)_sensor_sub->update(&_sensor_time, &sensor);
-				msg.alt = sensor.baro_alt_meter[0];
+				msg.alt = sensor.baro_alt_meter;
 			}
 			msg.climb = -pos.vel_d;
 
@@ -3121,7 +3121,7 @@ protected:
 
 			msg.time_usec = hrt_absolute_time();
 
-			msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter[0] : NAN;
+			msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter : NAN;
 			msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : NAN;
 			msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : NAN;
 			msg.altitude_relative = (_home_time > 0) ? (global_pos.alt - home.alt) : NAN;

src/modules/mavlink/mavlink_receiver.cpp

@@ -1673,16 +1673,14 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	{
 		struct sensor_combined_s hil_sensors = {};
 
-		hil_sensors.timestamp = timestamp;
-
 		hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt;
 		hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt;
 		hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt;
 		_hil_prev_gyro[0] = imu.xgyro;
 		_hil_prev_gyro[1] = imu.ygyro;
 		_hil_prev_gyro[2] = imu.zgyro;
-		hil_sensors.gyro_integral_dt[0] = dt * 1e6f;
+		hil_sensors.gyro_integral_dt = dt * 1e6f;
-		hil_sensors.gyro_timestamp[0] = timestamp;
+		hil_sensors.timestamp = timestamp;
 
 		hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt;
 		hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt;
@@ -1690,17 +1688,17 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		_hil_prev_accel[0] = imu.xacc;
 		_hil_prev_accel[1] = imu.yacc;
 		_hil_prev_accel[2] = imu.zacc;
-		hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
+		hil_sensors.accelerometer_integral_dt = dt * 1e6f;
-		hil_sensors.accelerometer_timestamp[0] = timestamp;
+		hil_sensors.accelerometer_timestamp = timestamp;
 
 		hil_sensors.magnetometer_ga[0] = imu.xmag;
 		hil_sensors.magnetometer_ga[1] = imu.ymag;
 		hil_sensors.magnetometer_ga[2] = imu.zmag;
-		hil_sensors.magnetometer_timestamp[0] = timestamp;
+		hil_sensors.magnetometer_timestamp = timestamp;
 
-		hil_sensors.baro_alt_meter[0] = imu.pressure_alt;
+		hil_sensors.baro_alt_meter = imu.pressure_alt;
-		hil_sensors.baro_temp_celcius[0] = imu.temperature;
+		hil_sensors.baro_temp_celcius = imu.temperature;
-		hil_sensors.baro_timestamp[0] = timestamp;
+		hil_sensors.baro_timestamp = timestamp;
 
 		/* publish combined sensor topic */
 		if (_sensors_pub == nullptr) {

src/modules/navigator/navigator_main.cpp

@@ -500,7 +500,7 @@ Navigator::task_main()
 		if (have_geofence_position_data &&
 			(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
 			(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
-			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid());
+			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid());
 			last_geofence_check = hrt_absolute_time();
 			have_geofence_position_data = false;
 

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -425,14 +425,14 @@ 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[0] != baro_timestamp) {
+				if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
-					baro_timestamp = sensor.baro_timestamp[0];
+					baro_timestamp = sensor.baro_timestamp;
 					baro_wait_for_sample_time = hrt_absolute_time();
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {
-						if (PX4_ISFINITE(sensor.baro_alt_meter[0])) {
+						if (PX4_ISFINITE(sensor.baro_alt_meter)) {
-							baro_offset += sensor.baro_alt_meter[0];
+							baro_offset += sensor.baro_alt_meter;
 							baro_init_cnt++;
 						}
 
@@ -502,10 +502,10 @@ 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[0] != accel_timestamp) {
+				if (sensor.accelerometer_timestamp != accel_timestamp) {
 					if (att.R_valid) {
 						float sensor_accel[3];
-						float accel_dt = sensor.accelerometer_integral_dt[0] / 1.e6f;
+						float accel_dt = sensor.accelerometer_integral_dt / 1.e6f;
 						sensor_accel[0] = sensor.accelerometer_integral_m_s[0] / accel_dt - acc_bias[0];
 						sensor_accel[1] = sensor.accelerometer_integral_m_s[1] / accel_dt - acc_bias[1];
 						sensor_accel[2] = sensor.accelerometer_integral_m_s[2] / accel_dt - acc_bias[2];
@@ -525,13 +525,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						memset(acc, 0, sizeof(acc));
 					}
 
-					accel_timestamp = sensor.accelerometer_timestamp[0];
+					accel_timestamp = sensor.accelerometer_timestamp;
 					accel_updates++;
 				}
 
-				if (sensor.baro_timestamp[0] != baro_timestamp) {
+				if (sensor.baro_timestamp != baro_timestamp) {
-					corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0];
+					corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
-					baro_timestamp = sensor.baro_timestamp[0];
+					baro_timestamp = sensor.baro_timestamp;
 					baro_updates++;
 				}
 			}
@@ -1363,7 +1363,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					global_pos.terrain_alt_valid = false;
 				}
 
-				global_pos.pressure_alt = sensor.baro_alt_meter[0];
+				global_pos.pressure_alt = sensor.baro_alt_meter;
 
 				if (vehicle_global_position_pub == NULL) {
 					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);

src/modules/sdlog2/sdlog2.c

@@ -1370,10 +1370,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 	pthread_cond_init(&logbuffer_cond, NULL);
 
 	/* track changes in sensor_combined topic */
-	hrt_abstime gyro_timestamp[3] = {0, 0, 0};
+	hrt_abstime gyro_timestamp = 0;
-	hrt_abstime accelerometer_timestamp[3] = {0, 0, 0};
+	hrt_abstime accelerometer_timestamp = 0;
-	hrt_abstime magnetometer_timestamp[3] = {0, 0, 0};
+	hrt_abstime magnetometer_timestamp = 0;
-	hrt_abstime barometer_timestamp[3] = {0, 0, 0};
+	hrt_abstime barometer_timestamp = 0;
 
 	/* initialize calculated mean SNR */
 	float snr_mean = 0.0f;
@@ -1634,80 +1634,58 @@ int sdlog2_thread_main(int argc, char *argv[])
 			// but we need to copy it again because we are re-using the buffer.
 			orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
 
-			for (unsigned i = 0; i < 3; i++) {
+			bool write_IMU = false;
-				bool write_IMU = false;
+			bool write_SENS = false;
-				bool write_SENS = false;
 
-				if (buf.sensor.gyro_timestamp[i] != gyro_timestamp[i]) {
+			if (buf.sensor.timestamp != gyro_timestamp) {
-					gyro_timestamp[i] = buf.sensor.gyro_timestamp[i];
+				gyro_timestamp = buf.sensor.timestamp;
-					write_IMU = true;
+				write_IMU = true;
-				}
+			}
 
-				if (buf.sensor.accelerometer_timestamp[i] != accelerometer_timestamp[i]) {
+			if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
-					accelerometer_timestamp[i] = buf.sensor.accelerometer_timestamp[i];
+				accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
-					write_IMU = true;
+				write_IMU = true;
-				}
+			}
 
-				if (buf.sensor.magnetometer_timestamp[i] != magnetometer_timestamp[i]) {
+			if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
-					magnetometer_timestamp[i] = buf.sensor.magnetometer_timestamp[i];
+				magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
-					write_IMU = true;
+				write_IMU = true;
-				}
+			}
 
-				if (buf.sensor.baro_timestamp[i] != barometer_timestamp[i]) {
+			if (buf.sensor.baro_timestamp != barometer_timestamp) {
-					barometer_timestamp[i] = buf.sensor.baro_timestamp[i];
+				barometer_timestamp = buf.sensor.baro_timestamp;
-					write_SENS = true;
+				write_SENS = true;
-				}
+			}
 
-				if (write_IMU) {
+			if (write_IMU) {
-					switch (i) {
+				log_msg.msg_type = LOG_IMU_MSG;
-						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;
-					}
 
-					float gyro_dt = buf.sensor.gyro_integral_dt[i] / 1.e6f;
+				float gyro_dt = buf.sensor.gyro_integral_dt / 1.e6f;
-					log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[i * 3 + 0] / gyro_dt;
+				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[0] / gyro_dt;
-					log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[i * 3 + 1] / gyro_dt;
+				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[1] / gyro_dt;
-					log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[i * 3 + 2] / gyro_dt;
+				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[2] / gyro_dt;
-					float accel_dt = buf.sensor.accelerometer_integral_dt[i] / 1.e6f;
+				float accel_dt = buf.sensor.accelerometer_integral_dt / 1.e6f;
-					log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[i * 3 + 0] / accel_dt;
+				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[0] / accel_dt;
-					log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[i * 3 + 1] / accel_dt;
+				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[1] / accel_dt;
-					log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[i * 3 + 2] / accel_dt;
+				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[2] / accel_dt;
-					log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[i * 3 + 0];
+				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
-					log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[i * 3 + 1];
+				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
-					log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[i * 3 + 2];
+				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];
-					log_msg.body.log_IMU.temp_gyro = 0;
+				log_msg.body.log_IMU.temp_gyro = 0;
-					log_msg.body.log_IMU.temp_acc = 0;
+				log_msg.body.log_IMU.temp_acc = 0;
-					log_msg.body.log_IMU.temp_mag = 0;
+				log_msg.body.log_IMU.temp_mag = 0;
-					LOGBUFFER_WRITE_AND_COUNT(IMU);
+				LOGBUFFER_WRITE_AND_COUNT(IMU);
-				}
+			}
 
-				if (write_SENS) {
+			if (write_SENS) {
-					switch (i) {
+				log_msg.msg_type = LOG_SENS_MSG;
-						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 = 0;
+				log_msg.body.log_SENS.baro_pres = 0;
-					log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter[i];
+				log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
-					log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius[i];
+				log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
-					log_msg.body.log_SENS.diff_pres = 0;
+				log_msg.body.log_SENS.diff_pres = 0;
-					log_msg.body.log_SENS.diff_pres_filtered = 0;
+				log_msg.body.log_SENS.diff_pres_filtered = 0;
-					LOGBUFFER_WRITE_AND_COUNT(SENS);
+				LOGBUFFER_WRITE_AND_COUNT(SENS);
-				}
 			}
 
 			/* --- VTOL VEHICLE STATUS --- */

src/modules/sensors/sensors.cpp

@@ -76,6 +76,7 @@
 #include <drivers/drv_px4flow.h>
 
 #include <systemlib/airspeed.h>
+#include <systemlib/mavlink_log.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -85,6 +86,7 @@
 #include <conversion/rotation.h>
 
 #include <lib/ecl/validation/data_validator.h>
+#include <lib/ecl/validation/data_validator_group.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
@@ -164,6 +166,9 @@ public:
 	 */
 	int		start();
 
+
+	void	print_status();
+
 private:
 	static const unsigned _rc_max_chan_count =
 		input_rc_s::RC_INPUT_MAX_CHANNELS;	/**< maximum number of r/c channels we handle */
@@ -200,19 +205,33 @@ private:
 	int 		_sensors_task;			/**< task handle for sensor task */
 
 	bool		_hil_enabled;			/**< if true, HIL is active */
-	bool		_publishing;			/**< if true, we are publishing sensor data */
+	bool		_publishing;			/**< if true, we are publishing sensor data (in HIL mode, we don't) */
 	bool		_armed;				/**< arming status of the vehicle */
 
-	int		_gyro_sub[SENSOR_COUNT_MAX];	/**< raw gyro data subscription */
+	struct SensorData {
-	int		_accel_sub[SENSOR_COUNT_MAX];	/**< raw accel data subscription */
+		SensorData()
-	int		_mag_sub[SENSOR_COUNT_MAX];	/**< raw mag data subscription */
+			: subscription_count(0),
-	int		_baro_sub[SENSOR_COUNT_MAX];	/**< raw baro data subscription */
+			  voter(SENSOR_COUNT_MAX),
-	int		_actuator_ctrl_0_sub;		/**< attitude controls sub */
+			  last_failover_count(0)
-	unsigned	_gyro_count;			/**< raw gyro data count */
+		{
-	unsigned	_accel_count;			/**< raw accel data count */
+			for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
-	unsigned	_mag_count;			/**< raw mag data count */
+				subscription[i] = -1;
-	unsigned	_baro_count;			/**< raw baro data count */
+			}
+		}
 
+		int subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
+		uint8_t priority[SENSOR_COUNT_MAX]; /**< sensor priority */
+		int subscription_count;
+		DataValidatorGroup voter;
+		unsigned int last_failover_count;
+	};
+
+	SensorData _gyro;
+	SensorData _accel;
+	SensorData _mag;
+	SensorData _baro;
+
+	int		_actuator_ctrl_0_sub;		/**< attitude controls sub */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vcontrol_mode_sub;		/**< vehicle control mode subscription */
@@ -227,6 +246,7 @@ private:
 	orb_advert_t	_battery_pub;			/**< battery status */
 	orb_advert_t	_airspeed_pub;			/**< airspeed */
 	orb_advert_t	_diff_pres_pub;			/**< differential_pressure */
+	orb_advert_t	_mavlink_log_pub;
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -234,7 +254,6 @@ private:
 
 	struct rc_channels_s _rc;			/**< r/c channel data */
 	struct battery_status_s _battery_status;	/**< battery status */
-	struct baro_report _barometer;			/**< barometer data */
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 	struct rc_parameter_map_s _rc_parameter_map;
@@ -245,9 +264,12 @@ private:
 
 	Battery		_battery;			/**< Helper lib to publish battery_status topic. */
 
-	float		_latest_baro_pressure[SENSOR_COUNT_MAX];
+	float _last_baro_pressure[SENSOR_COUNT_MAX]; /**< pressure from last baro sensors */
-	hrt_abstime	_last_accel_timestamp[SENSOR_COUNT_MAX];
+	float _last_best_baro_pressure; /**< pressure from last best baro */
-	hrt_abstime	_last_gyro_timestamp[SENSOR_COUNT_MAX];
+	sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */
+
+	hrt_abstime _vibration_warning_timestamp;
+	bool _vibration_warning;
 
 	struct {
 		float min[_rc_max_chan_count];
@@ -320,6 +342,8 @@ private:
 
 		float baro_qnh;
 
+		float vibration_warning_threshold;
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -387,10 +411,12 @@ private:
 
 		param_t baro_qnh;
 
+		param_t vibe_thresh; /**< vibration threshold */
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
-	int		init_sensor_class(const struct orb_metadata *meta, int *subs);
+	void	init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data);
 
 	/**
 	 * Update our local parameter cache.
@@ -495,6 +521,18 @@ private:
 	 */
 	void		adc_poll(struct sensor_combined_s &raw);
 
+	/**
+	 * Check & handle failover of a sensor
+	 * @return true if a switch occured (could be for a non-critical reason)
+	 */
+	bool check_failover(SensorData &sensor, const char *sensor_name);
+
+	/**
+	 * check vibration levels and output a warning if they're high
+	 * @return true on high vibration
+	 */
+	bool check_vibration();
+
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
@@ -521,10 +559,6 @@ Sensors::Sensors() :
 	_hil_enabled(false),
 	_publishing(true),
 	_armed(false),
-	_gyro_count(0),
-	_accel_count(0),
-	_mag_count(0),
-	_baro_count(0),
 	_rc_sub(-1),
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
@@ -539,6 +573,7 @@ Sensors::Sensors() :
 	_battery_pub(nullptr),
 	_airspeed_pub(nullptr),
 	_diff_pres_pub(nullptr),
+	_mavlink_log_pub(nullptr),
 
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
@@ -546,23 +581,20 @@ Sensors::Sensors() :
 
 	_param_rc_values{},
 	_board_rotation{},
-	_mag_rotation{}
+	_mag_rotation{},
+
+	_last_best_baro_pressure(0.f),
+
+	_vibration_warning_timestamp(0),
+	_vibration_warning(false)
 {
-	/* initialize subscriptions */
+	_mag.voter.set_timeout(200000);
-	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(&_parameters, 0, sizeof(_parameters));
 	memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
-	memset(&_latest_baro_pressure, 0, sizeof(_latest_baro_pressure));
+	memset(&_last_sensor_data, 0, sizeof(_last_sensor_data));
-	memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp));
-	memset(&_last_gyro_timestamp, 0, sizeof(_last_gyro_timestamp));
 
 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -661,6 +693,8 @@ Sensors::Sensors() :
 	/* Barometer QNH */
 	_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
 
+	_parameter_handles.vibe_thresh = param_find("ATT_VIBE_THRESH");
+
 	// These are parameters for which QGroundControl always expects to be returned in a list request.
 	// We do a param_find here to force them into the list.
 	(void)param_find("RC_CHAN_CNT");
@@ -993,6 +1027,8 @@ Sensors::parameters_update()
 
 #endif
 
+	param_get(_parameter_handles.vibe_thresh, &_parameters.vibration_warning_threshold);
+
 	return ret;
 }
 
@@ -1014,43 +1050,72 @@ Sensors::adc_init()
 void
 Sensors::accel_poll(struct sensor_combined_s &raw)
 {
-	for (unsigned i = 0; i < _accel_count; i++) {
+	bool got_update = false;
+
+	for (unsigned i = 0; i < _accel.subscription_count; i++) {
 		bool accel_updated;
-		orb_check(_accel_sub[i], &accel_updated);
+		orb_check(_accel.subscription[i], &accel_updated);
 
 		if (accel_updated) {
-			struct accel_report	accel_report;
+			struct accel_report accel_report;
 
-			orb_copy(ORB_ID(sensor_accel), _accel_sub[i], &accel_report);
+			orb_copy(ORB_ID(sensor_accel), _accel.subscription[i], &accel_report);
+
+			if (accel_report.timestamp == 0) {
+				continue; //ignore invalid data
+			}
+
+			got_update = true;
+			math::Vector<3> sensor_value;
 
 			if (accel_report.integral_dt != 0) {
 				math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
 				vect_int = _board_rotation * vect_int;
 
-				raw.accelerometer_integral_m_s[i * 3 + 0] = vect_int(0);
+				_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_int(0);
-				raw.accelerometer_integral_m_s[i * 3 + 1] = vect_int(1);
+				_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_int(1);
-				raw.accelerometer_integral_m_s[i * 3 + 2] = vect_int(2);
+				_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_int(2);
 
-				raw.accelerometer_integral_dt[i] = accel_report.integral_dt;
+				_last_sensor_data[i].accelerometer_integral_dt = accel_report.integral_dt;
+
+				float dt = accel_report.integral_dt / 1.e6f;
+				sensor_value = vect_int / dt;
 
 			} else {
 				//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
 				math::Vector<3> vect_val(accel_report.x, accel_report.y, accel_report.z);
 				vect_val = _board_rotation * vect_val;
 
-				if (_last_accel_timestamp[i] == 0) {
+				sensor_value = vect_val;
-					_last_accel_timestamp[i] = accel_report.timestamp - 1000;
+
+				if (_last_sensor_data[i].accelerometer_timestamp == 0) {
+					_last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp - 1000;
 				}
 
-				raw.accelerometer_integral_dt[i] = accel_report.timestamp - _last_accel_timestamp[i];
+				_last_sensor_data[i].accelerometer_integral_dt =
-				_last_accel_timestamp[i] = accel_report.timestamp;
+					accel_report.timestamp - _last_sensor_data[i].accelerometer_timestamp;
-				float dt = raw.accelerometer_integral_dt[i] / 1.e6f;
+				float dt = _last_sensor_data[i].accelerometer_integral_dt / 1.e6f;
-				raw.accelerometer_integral_m_s[i * 3 + 0] = vect_val(0) * dt;
+				_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_val(0) * dt;
-				raw.accelerometer_integral_m_s[i * 3 + 1] = vect_val(1) * dt;
+				_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_val(1) * dt;
-				raw.accelerometer_integral_m_s[i * 3 + 2] = vect_val(2) * dt;
+				_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_val(2) * dt;
 			}
 
-			raw.accelerometer_timestamp[i] = accel_report.timestamp;
+			_last_sensor_data[i].accelerometer_timestamp = accel_report.timestamp;
+			_accel.voter.put(i, accel_report.timestamp, sensor_value.data,
+					 accel_report.error_count, _accel.priority[i]);
+		}
+	}
+
+	if (got_update) {
+		int best_index;
+		_accel.voter.get_best(hrt_absolute_time(), &best_index);
+
+		if (best_index >= 0) {
+			raw.accelerometer_integral_m_s[0] = _last_sensor_data[best_index].accelerometer_integral_m_s[0];
+			raw.accelerometer_integral_m_s[1] = _last_sensor_data[best_index].accelerometer_integral_m_s[1];
+			raw.accelerometer_integral_m_s[2] = _last_sensor_data[best_index].accelerometer_integral_m_s[2];
+			raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
+			raw.accelerometer_timestamp = _last_sensor_data[best_index].accelerometer_timestamp;
 		}
 	}
 }
@@ -1058,47 +1123,72 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 void
 Sensors::gyro_poll(struct sensor_combined_s &raw)
 {
-	for (unsigned i = 0; i < _gyro_count; i++) {
+	bool got_update = false;
+
+	for (unsigned i = 0; i < _gyro.subscription_count; i++) {
 		bool gyro_updated;
-		orb_check(_gyro_sub[i], &gyro_updated);
+		orb_check(_gyro.subscription[i], &gyro_updated);
 
 		if (gyro_updated) {
-			struct gyro_report	gyro_report;
+			struct gyro_report gyro_report;
 
-			orb_copy(ORB_ID(sensor_gyro), _gyro_sub[i], &gyro_report);
+			orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[i], &gyro_report);
+
+			if (gyro_report.timestamp == 0) {
+				continue; //ignore invalid data
+			}
+
+			got_update = true;
+			math::Vector<3> sensor_value;
 
 			if (gyro_report.integral_dt != 0) {
 				math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
 				vect_int = _board_rotation * vect_int;
 
-				raw.gyro_integral_rad[i * 3 + 0] = vect_int(0);
+				_last_sensor_data[i].gyro_integral_rad[0] = vect_int(0);
-				raw.gyro_integral_rad[i * 3 + 1] = vect_int(1);
+				_last_sensor_data[i].gyro_integral_rad[1] = vect_int(1);
-				raw.gyro_integral_rad[i * 3 + 2] = vect_int(2);
+				_last_sensor_data[i].gyro_integral_rad[2] = vect_int(2);
 
-				raw.gyro_integral_dt[i] = gyro_report.integral_dt;
+				_last_sensor_data[i].gyro_integral_dt = gyro_report.integral_dt;
-				raw.gyro_timestamp[i] = gyro_report.timestamp;
+
+				float dt = gyro_report.integral_dt / 1.e6f;
+				sensor_value = vect_int / dt;
 
 			} else {
 				//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
 				math::Vector<3> vect_val(gyro_report.x, gyro_report.y, gyro_report.z);
 				vect_val = _board_rotation * vect_val;
 
-				if (_last_gyro_timestamp[i] == 0) {
+				sensor_value = vect_val;
-					_last_gyro_timestamp[i] = gyro_report.timestamp - 1000;
+
+				if (_last_sensor_data[i].timestamp == 0) {
+					_last_sensor_data[i].timestamp = gyro_report.timestamp - 1000;
 				}
 
-				raw.gyro_integral_dt[i] = gyro_report.timestamp - _last_gyro_timestamp[i];
+				_last_sensor_data[i].gyro_integral_dt =
-				_last_gyro_timestamp[i] = gyro_report.timestamp;
+					gyro_report.timestamp - _last_sensor_data[i].timestamp;
-				float dt = raw.gyro_integral_dt[i] / 1.e6f;
+				float dt = _last_sensor_data[i].gyro_integral_dt / 1.e6f;
-				raw.gyro_integral_rad[i * 3 + 0] = vect_val(0) * dt;
+				_last_sensor_data[i].gyro_integral_rad[0] = vect_val(0) * dt;
-				raw.gyro_integral_rad[i * 3 + 1] = vect_val(1) * dt;
+				_last_sensor_data[i].gyro_integral_rad[1] = vect_val(1) * dt;
-				raw.gyro_integral_rad[i * 3 + 2] = vect_val(2) * dt;
+				_last_sensor_data[i].gyro_integral_rad[2] = vect_val(2) * dt;
-
 			}
 
-			if (i == 0) {
+			_last_sensor_data[i].timestamp = gyro_report.timestamp;
-				raw.timestamp = gyro_report.timestamp;
+			_gyro.voter.put(i, gyro_report.timestamp, sensor_value.data,
-			}
+					gyro_report.error_count, _gyro.priority[i]);
+		}
+	}
+
+	if (got_update) {
+		int best_index;
+		_gyro.voter.get_best(hrt_absolute_time(), &best_index);
+
+		if (best_index >= 0) {
+			raw.gyro_integral_rad[0] = _last_sensor_data[best_index].gyro_integral_rad[0];
+			raw.gyro_integral_rad[1] = _last_sensor_data[best_index].gyro_integral_rad[1];
+			raw.gyro_integral_rad[2] = _last_sensor_data[best_index].gyro_integral_rad[2];
+			raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
+			raw.timestamp = _last_sensor_data[best_index].timestamp;
 		}
 	}
 }
@@ -1106,24 +1196,44 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 void
 Sensors::mag_poll(struct sensor_combined_s &raw)
 {
-	for (unsigned i = 0; i < _mag_count; i++) {
+	bool got_update = false;
+
+	for (unsigned i = 0; i < _mag.subscription_count; i++) {
 		bool mag_updated;
-		orb_check(_mag_sub[i], &mag_updated);
+		orb_check(_mag.subscription[i], &mag_updated);
 
 		if (mag_updated) {
-			struct mag_report	mag_report;
+			struct mag_report mag_report;
 
-			orb_copy(ORB_ID(sensor_mag), _mag_sub[i], &mag_report);
+			orb_copy(ORB_ID(sensor_mag), _mag.subscription[i], &mag_report);
 
+			if (mag_report.timestamp == 0) {
+				continue; //ignore invalid data
+			}
+
+			got_update = true;
 			math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
-
 			vect = _mag_rotation[i] * vect;
 
-			raw.magnetometer_ga[i * 3 + 0] = vect(0);
+			_last_sensor_data[i].magnetometer_ga[0] = vect(0);
-			raw.magnetometer_ga[i * 3 + 1] = vect(1);
+			_last_sensor_data[i].magnetometer_ga[1] = vect(1);
-			raw.magnetometer_ga[i * 3 + 2] = vect(2);
+			_last_sensor_data[i].magnetometer_ga[2] = vect(2);
 
-			raw.magnetometer_timestamp[i] = mag_report.timestamp;
+			_last_sensor_data[i].magnetometer_timestamp = mag_report.timestamp;
+			_mag.voter.put(i, mag_report.timestamp, vect.data,
+				       mag_report.error_count, _mag.priority[i]);
+		}
+	}
+
+	if (got_update) {
+		int best_index;
+		_mag.voter.get_best(hrt_absolute_time(), &best_index);
+
+		if (best_index >= 0) {
+			raw.magnetometer_ga[0] = _last_sensor_data[best_index].magnetometer_ga[0];
+			raw.magnetometer_ga[1] = _last_sensor_data[best_index].magnetometer_ga[1];
+			raw.magnetometer_ga[2] = _last_sensor_data[best_index].magnetometer_ga[2];
+			raw.magnetometer_timestamp = _last_sensor_data[best_index].magnetometer_timestamp;
 		}
 	}
 }
@@ -1131,19 +1241,43 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 void
 Sensors::baro_poll(struct sensor_combined_s &raw)
 {
-	for (unsigned i = 0; i < _baro_count; i++) {
+	bool got_update = false;
+
+	for (unsigned i = 0; i < _baro.subscription_count; i++) {
 		bool baro_updated;
-		orb_check(_baro_sub[i], &baro_updated);
+		orb_check(_baro.subscription[i], &baro_updated);
 
 		if (baro_updated) {
+			struct baro_report baro_report;
 
-			orb_copy(ORB_ID(sensor_baro), _baro_sub[i], &_barometer);
+			orb_copy(ORB_ID(sensor_baro), _baro.subscription[i], &baro_report);
 
-			_latest_baro_pressure[i] = _barometer.pressure;
+			if (baro_report.timestamp == 0) {
-			raw.baro_alt_meter[i] = _barometer.altitude; // Altitude in meters
+				continue; //ignore invalid data
-			raw.baro_temp_celcius[i] = _barometer.temperature; // Temperature in degrees celcius
+			}
 
-			raw.baro_timestamp[i] = _barometer.timestamp;
+			got_update = true;
+			math::Vector<3> vect(baro_report.altitude, 0.f, 0.f);
+
+			_last_sensor_data[i].baro_alt_meter = baro_report.altitude;
+			_last_sensor_data[i].baro_temp_celcius = baro_report.temperature;
+			_last_baro_pressure[i] = baro_report.pressure;
+
+			_last_sensor_data[i].baro_timestamp = baro_report.timestamp;
+			_baro.voter.put(i, baro_report.timestamp, vect.data,
+					baro_report.error_count, _baro.priority[i]);
+		}
+	}
+
+	if (got_update) {
+		int best_index;
+		_baro.voter.get_best(hrt_absolute_time(), &best_index);
+
+		if (best_index >= 0) {
+			raw.baro_alt_meter = _last_sensor_data[best_index].baro_alt_meter;
+			raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius;
+			raw.baro_timestamp = _last_sensor_data[best_index].baro_timestamp;
+			_last_best_baro_pressure = _last_baro_pressure[best_index];
 		}
 	}
 }
@@ -1158,7 +1292,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
 		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature :
-						(raw.baro_temp_celcius[0] - PCB_TEMP_ESTIMATE_DEG);
+						(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		_airspeed.timestamp = _diff_pres.timestamp;
 
@@ -1175,13 +1309,12 @@ 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));
 
-		//FIXME: we just use the baro pressure from the first baro. we should do voting instead.
 		_airspeed.true_airspeed_m_s = math::max(0.0f,
-							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _latest_baro_pressure[0] * 1e2f,
+							calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + _last_best_baro_pressure * 1e2f,
-									_latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
+									_last_best_baro_pressure * 1e2f, air_temperature_celsius));
 		_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
-				calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _latest_baro_pressure[0] * 1e2f,
+				calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + _last_best_baro_pressure * 1e2f,
-						   _latest_baro_pressure[0] * 1e2f, air_temperature_celsius));
+						   _last_best_baro_pressure * 1e2f, air_temperature_celsius));
 
 		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
@@ -1544,14 +1677,7 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *g
 #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
-	const int res = h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal);
+	return !h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal);
-
-	if (res) {
-		return false;
-
-	} else {
-		return true;
-	}
 
 #else
 	/* On QURT, the params are read directly in the respective wrappers. */
@@ -1565,14 +1691,7 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s
 #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
-	const int res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);
+	return !h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal);
-
-	if (res) {
-		return false;
-
-	} else {
-		return true;
-	}
 
 #else
 	/* On QURT, the params are read directly in the respective wrappers. */
@@ -1586,14 +1705,7 @@ Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mca
 #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI2)
 
 	/* On most systems, we can just use the IOCTL call to set the calibration params. */
-	const int res = h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal);
+	return !h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal);
-
-	if (res) {
-		return false;
-
-	} else {
-		return true;
-	}
 
 #else
 	/* On QURT, the params are read directly in the respective wrappers. */
@@ -2045,14 +2157,72 @@ Sensors::rc_poll()
 	}
 }
 
+bool
+Sensors::check_failover(SensorData &sensor, const char *sensor_name)
+{
+	if (sensor.last_failover_count != sensor.voter.failover_count()) {
+
+		uint32_t flags = sensor.voter.failover_state();
+
+		if (flags == DataValidator::ERROR_FLAG_NO_ERROR) {
+			//we switched due to a non-critical reason. No need to panic.
+			PX4_INFO("%s sensor switch from #%i", sensor_name, sensor.voter.failover_index());
+
+		} else {
+			mavlink_and_console_log_emergency(&_mavlink_log_pub, "%s #%i failure :%s%s%s%s%s!",
+							  sensor_name,
+							  sensor.voter.failover_index(),
+							  ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
+							  ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
+							  ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
+							  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
+							  ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
+		}
+
+		sensor.last_failover_count = sensor.voter.failover_count();
+		return true;
+	}
+
+	return false;
+}
+
+bool
+Sensors::check_vibration()
+{
+	bool ret = false;
+	hrt_abstime cur_time = hrt_absolute_time();
+
+	if (!_vibration_warning && (_gyro.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold ||
+				    _accel.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold ||
+				    _mag.voter.get_vibration_factor(cur_time) > _parameters.vibration_warning_threshold)) {
+
+		if (_vibration_warning_timestamp == 0) {
+			_vibration_warning_timestamp = cur_time;
+
+		} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000 * 1000) {
+			_vibration_warning = true;
+			mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
+							 (int)(100 * _gyro.voter.get_vibration_factor(cur_time)),
+							 (int)(100 * _accel.voter.get_vibration_factor(cur_time)),
+							 (int)(100 * _mag.voter.get_vibration_factor(cur_time)));
+			ret = true;
+		}
+
+	} else {
+		_vibration_warning_timestamp = 0;
+	}
+
+	return ret;
+}
+
 void
 Sensors::task_main_trampoline(int argc, char *argv[])
 {
 	sensors::g_sensors->task_main();
 }
 
-int
+void
-Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs)
+Sensors::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data)
 {
 	unsigned group_count = orb_group_count(meta);
 
@@ -2061,12 +2231,19 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs)
 	}
 
 	for (unsigned i = 0; i < group_count; i++) {
-		if (subs[i] < 0) {
+		if (sensor_data.subscription[i] < 0) {
-			subs[i] = orb_subscribe_multi(meta, i);
+			sensor_data.subscription[i] = orb_subscribe_multi(meta, i);
+			int32_t priority;
+			orb_priority(sensor_data.subscription[i], &priority);
+			sensor_data.priority[i] = (uint8_t)priority;
 		}
+
+		int32_t priority;
+		orb_priority(sensor_data.subscription[i], &priority);
+		sensor_data.priority[i] = (uint8_t)priority;
 	}
 
-	return group_count;
+	sensor_data.subscription_count = group_count;
 }
 
 void
@@ -2090,84 +2267,79 @@ Sensors::task_main()
 
 	struct sensor_combined_s raw = {};
 
-	/* ensure no overflows can occur */
-	static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX,
-		      "SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur");
-
 	/*
 	 * do subscriptions
 	 */
+	init_sensor_class(ORB_ID(sensor_gyro), _gyro);
 
-	unsigned gcount_prev = _gyro_count;
+	init_sensor_class(ORB_ID(sensor_mag), _mag);
 
-	unsigned mcount_prev = _mag_count;
+	init_sensor_class(ORB_ID(sensor_accel), _accel);
 
-	unsigned acount_prev = _accel_count;
+	init_sensor_class(ORB_ID(sensor_baro), _baro);
 
-	unsigned bcount_prev = _baro_count;
+	/* reload calibration params */
-
+	parameter_update_poll(true);
-	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
-
-	_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub);
-
-	_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
-
-	_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub);
-
-	if (gcount_prev != _gyro_count ||
-	    mcount_prev != _mag_count ||
-	    acount_prev != _accel_count ||
-	    bcount_prev != _baro_count) {
-
-		/* reload calibration params */
-		parameter_update_poll(true);
-	}
 
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
+
 	_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));
+
 	_actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0));
 
-	/*
+	raw.timestamp = 0;
-	 * do advertisements
-	 */
-	raw.timestamp = hrt_absolute_time();
 
 	_battery.reset(&_battery_status);
 
 	/* get a set of initial values */
 	accel_poll(raw);
+
 	gyro_poll(raw);
+
 	mag_poll(raw);
+
 	baro_poll(raw);
+
 	diff_pres_poll(raw);
 
 	parameter_update_poll(true /* forced */);
+
 	rc_parameter_map_poll(true /* forced */);
 
 	/* advertise the sensor_combined topic and make the initial publication */
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 
 	/* wakeup source(s) */
-	px4_pollfd_struct_t fds[1] = {};
+	px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
 
-	/* use the gyro to pace output */
+	int num_poll_fds = 0;
-	fds[0].fd = _gyro_sub[0];
-	fds[0].events = POLLIN;
 
 	_task_should_exit = false;
 
-	raw.timestamp = 0;
-
 	uint64_t _last_config_update = hrt_absolute_time();
 
 	while (!_task_should_exit) {
 
+		/* use the gyro(s) to pace output */
+		if (num_poll_fds != _gyro.subscription_count) { //happens the first time we enter, or when new gyro added
+			num_poll_fds = _gyro.subscription_count;
+
+			for (int i = 0; i < _gyro.subscription_count; ++i) {
+				fds[i].fd = _gyro.subscription[i];
+				fds[i].events = POLLIN;
+			}
+		}
+
 		/* wait for up to 50ms for data */
-		int pret = px4_poll(fds, (sizeof(fds) / sizeof(fds[0])), 50);
+		int pret = px4_poll(fds, num_poll_fds, 50);
 
 		/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
 
@@ -2176,11 +2348,12 @@ Sensors::task_main()
 			/* if the polling operation failed because no gyro sensor is available yet,
 			 * then attempt to subscribe once again
 			 */
-			if (_gyro_count == 0) {
+			if (_gyro.subscription_count == 0) {
-				_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
+				init_sensor_class(ORB_ID(sensor_gyro), _gyro);
-				fds[0].fd = _gyro_sub[0];
 			}
 
+			usleep(1000);
+
 			continue;
 		}
 
@@ -2189,60 +2362,38 @@ Sensors::task_main()
 		/* check vehicle status for changes to publication state */
 		vehicle_control_mode_poll();
 
-		/* the timestamp of the raw struct is updated by the gyro_poll() method */
+		/* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro
-		/* copy most recent sensor data */
+		 * a mandatory sensor) */
 		gyro_poll(raw);
 		accel_poll(raw);
 		mag_poll(raw);
 		baro_poll(raw);
 
-		// FIXME TODO: this needs more thinking, otherwise we spam the console and keep switching.
-		/* Work out if main gyro timed out and fail over to alternate gyro.
-		 * However, don't do this if the secondary is not available. */
-		if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000 && _gyro_sub[1] >= 0) {
-			if (fds[0].fd == _gyro_sub[0]) {
-				PX4_WARN("gyro0 has timed out");
-			}
-
-			/* If the secondary failed as well, go to the tertiary, also only if available. */
-			if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000 && _gyro_sub[2] >= 0 && (fds[0].fd != _gyro_sub[2])) {
-				fds[0].fd = _gyro_sub[2];
-
-				if (!_hil_enabled) {
-					PX4_WARN("failing over to third gyro");
-				}
-
-			} else if (_gyro_sub[1] >= 0 && (fds[0].fd != _gyro_sub[1])) {
-				fds[0].fd = _gyro_sub[1];
-
-				if (!_hil_enabled) {
-					PX4_WARN("failing over to second gyro");
-				}
-			}
-		}
 
 		/* check battery voltage */
 		adc_poll(raw);
 
 		diff_pres_poll(raw);
 
-		/* Inform other processes that new data is available to copy */
 		if (_publishing && raw.timestamp > 0) {
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
+
+			check_failover(_accel, "Accel");
+			check_failover(_gyro, "Gyro");
+			check_failover(_mag, "Mag");
+			check_failover(_baro, "Baro");
+
+			//check_vibration(); //disabled for now, as it does not seem to be reliable
 		}
 
 		/* keep adding sensors as long as we are not armed,
 		 * when not adding sensors poll for param updates
 		 */
 		if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) {
-			_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), _gyro_sub);
+			init_sensor_class(ORB_ID(sensor_gyro), _gyro);
-
+			init_sensor_class(ORB_ID(sensor_mag), _mag);
-			_mag_count = init_sensor_class(ORB_ID(sensor_mag), _mag_sub);
+			init_sensor_class(ORB_ID(sensor_accel), _accel);
-
+			init_sensor_class(ORB_ID(sensor_baro), _baro);
-			_accel_count = init_sensor_class(ORB_ID(sensor_accel), _accel_sub);
-
-			_baro_count = init_sensor_class(ORB_ID(sensor_baro), _baro_sub);
-
 			_last_config_update = hrt_absolute_time();
 
 		} else {
@@ -2260,22 +2411,20 @@ Sensors::task_main()
 		perf_end(_loop_perf);
 	}
 
-	for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) {
+	for (unsigned i = 0; i < _gyro.subscription_count; i++) {
-		if (_gyro_sub[i] >= 0) {
+		orb_unsubscribe(_gyro.subscription[i]);
-			orb_unsubscribe(_gyro_sub[i]);
+	}
-		}
 
-		if (_accel_sub[i] >= 0) {
+	for (unsigned i = 0; i < _accel.subscription_count; i++) {
-			orb_unsubscribe(_accel_sub[i]);
+		orb_unsubscribe(_accel.subscription[i]);
-		}
+	}
 
-		if (_mag_sub[i] >= 0) {
+	for (unsigned i = 0; i < _mag.subscription_count; i++) {
-			orb_unsubscribe(_mag_sub[i]);
+		orb_unsubscribe(_mag.subscription[i]);
-		}
+	}
 
-		if (_baro_sub[i] >= 0) {
+	for (unsigned i = 0; i < _baro.subscription_count; i++) {
-			orb_unsubscribe(_baro_sub[i]);
+		orb_unsubscribe(_baro.subscription[i]);
-		}
 	}
 
 	orb_unsubscribe(_rc_sub);
@@ -2316,6 +2465,19 @@ Sensors::start()
 	return OK;
 }
 
+void Sensors::print_status()
+{
+	PX4_INFO("gyro status:");
+	_gyro.voter.print();
+	PX4_INFO("accel status:");
+	_accel.voter.print();
+	PX4_INFO("mag status:");
+	_mag.voter.print();
+	PX4_INFO("baro status:");
+	_baro.voter.print();
+}
+
+
 int sensors_main(int argc, char *argv[])
 {
 	if (argc < 2) {
@@ -2360,7 +2522,7 @@ int sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (sensors::g_sensors) {
-			PX4_INFO("is running");
+			sensors::g_sensors->print_status();
 			return 0;
 
 		} else {