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