move initial sensor priority to parameters and purge ORB_PRIORITY

- CAL_ACCx_EN -> CAL_ACCx_PRIO
 - CAL_GYROx_EN -> CAL_GYROx_PRIO
 - CAL_MAGx_EN -> CAL_MAGx_PRIO

src/modules/sensors/sensor_params_acc0.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,13 +40,19 @@
 PARAM_DEFINE_INT32(CAL_ACC0_ID, 0);
 
 /**
- * Accelerometer 0 enabled
+ * Accelerometer 0 priority.
+ *
+ * @value 0   Disabled
+ * @value 1   Min
+ * @value 25  Low
+ * @value 50  Medium (Default)
+ * @value 75  High
+ * @value 100 Max
  *
- * @boolean
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_ACC0_EN, 1);
+PARAM_DEFINE_INT32(CAL_ACC0_PRIO, 50);
 
 /**
  * Accelerometer X-axis offset

src/modules/sensors/sensor_params_acc1.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,13 +40,19 @@
 PARAM_DEFINE_INT32(CAL_ACC1_ID, 0);
 
 /**
- * Accelerometer 1 enabled
+ * Accelerometer 1 priority.
+ *
+ * @value 0   Disabled
+ * @value 1   Min
+ * @value 25  Low
+ * @value 50  Medium (Default)
+ * @value 75  High
+ * @value 100 Max
  *
- * @boolean
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_ACC1_EN, 1);
+PARAM_DEFINE_INT32(CAL_ACC1_PRIO, 50);
 
 /**
  * Accelerometer X-axis offset

src/modules/sensors/sensor_params_acc2.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,13 +40,19 @@
 PARAM_DEFINE_INT32(CAL_ACC2_ID, 0);
 
 /**
- * Accelerometer 2 enabled
+ * Accelerometer 2 priority.
+ *
+ * @value 0   Disabled
+ * @value 1   Min
+ * @value 25  Low
+ * @value 50  Medium (Default)
+ * @value 75  High
+ * @value 100 Max
  *
- * @boolean
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_ACC2_EN, 1);
+PARAM_DEFINE_INT32(CAL_ACC2_PRIO, 50);
 
 /**
  * Accelerometer X-axis offset

src/modules/sensors/sensor_params_accel.c

@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * Primary accel ID
- *
- * @category system
- * @group Sensor Calibration
- */
-PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0);

src/modules/sensors/sensor_params_gyro.c

@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * Primary gyro ID
- *
- * @category system
- * @group Sensor Calibration
- */
-PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0);

src/modules/sensors/sensor_params_gyro0.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,13 +40,19 @@
 PARAM_DEFINE_INT32(CAL_GYRO0_ID, 0);
 
 /**
- * Gyro 0 enabled
+ * Gyro 0 priority.
+ *
+ * @value 0   Disabled
+ * @value 1   Min
+ * @value 25  Low
+ * @value 50  Medium (Default)
+ * @value 75  High
+ * @value 100 Max
  *
- * @boolean
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_GYRO0_EN, 1);
+PARAM_DEFINE_INT32(CAL_GYRO0_PRIO, 50);
 
 /**
  * Gyro X-axis offset

src/modules/sensors/sensor_params_gyro1.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,13 +40,19 @@
 PARAM_DEFINE_INT32(CAL_GYRO1_ID, 0);
 
 /**
- * Gyro 1 enabled
+ * Gyro 1 priority.
+ *
+ * @value 0   Disabled
+ * @value 1   Min
+ * @value 25  Low
+ * @value 50  Medium (Default)
+ * @value 75  High
+ * @value 100 Max
  *
- * @boolean
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_GYRO1_EN, 1);
+PARAM_DEFINE_INT32(CAL_GYRO1_PRIO, 50);
 
 /**
  * Gyro X-axis offset

src/modules/sensors/sensor_params_gyro2.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,13 +40,19 @@
 PARAM_DEFINE_INT32(CAL_GYRO2_ID, 0);
 
 /**
- * Gyro 2 enabled
+ * Gyro 2 priority.
+ *
+ * @value 0   Disabled
+ * @value 1   Min
+ * @value 25  Low
+ * @value 50  Medium (Default)
+ * @value 75  High
+ * @value 100 Max
  *
- * @boolean
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_GYRO2_EN, 1);
+PARAM_DEFINE_INT32(CAL_GYRO2_PRIO, 50);
 
 /**
  * Gyro X-axis offset

src/modules/sensors/sensor_params_mag.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,14 +31,6 @@
  *
  ****************************************************************************/
 
-/**
- * Primary mag ID
- *
- * @category system
- * @group Sensor Calibration
- */
-PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
-
 /**
  * Bitfield selecting mag sides for calibration
  *

src/modules/sensors/sensor_params_mag0.c

@@ -40,13 +40,19 @@
 PARAM_DEFINE_INT32(CAL_MAG0_ID, 0);
 
 /**
- * Mag 0 enabled
+ * Mag 0 priority.
+ *
+ * @value 0   Disabled
+ * @value 1   Min
+ * @value 25  Low
+ * @value 50  Medium (Default)
+ * @value 75  High
+ * @value 100 Max
  *
- * @boolean
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG0_EN, 1);
+PARAM_DEFINE_INT32(CAL_MAG0_PRIO, 50);
 
 /**
  * Rotation of magnetometer 0 relative to airframe.

src/modules/sensors/sensor_params_mag1.c

@@ -40,13 +40,19 @@
 PARAM_DEFINE_INT32(CAL_MAG1_ID, 0);
 
 /**
- * Mag 1 enabled
+ * Mag 1 priority.
+ *
+ * @value 0   Disabled
+ * @value 1   Min
+ * @value 25  Low
+ * @value 50  Medium (Default)
+ * @value 75  High
+ * @value 100 Max
  *
- * @boolean
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG1_EN, 1);
+PARAM_DEFINE_INT32(CAL_MAG1_PRIO, 50);
 
 /**
  * Rotation of magnetometer 1 relative to airframe.

src/modules/sensors/sensor_params_mag2.c

@@ -40,13 +40,19 @@
 PARAM_DEFINE_INT32(CAL_MAG2_ID, 0);
 
 /**
- * Mag 2 enabled
+ * Mag 2 priority.
+ *
+ * @value 0   Disabled
+ * @value 1   Min
+ * @value 25  Low
+ * @value 50  Medium (Default)
+ * @value 75  High
+ * @value 100 Max
  *
- * @boolean
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG2_EN, 1);
+PARAM_DEFINE_INT32(CAL_MAG2_PRIO, 50);
 
 /**
  * Rotation of magnetometer 2 relative to airframe.

src/modules/sensors/sensor_params_mag3.c

@@ -40,16 +40,22 @@
 PARAM_DEFINE_INT32(CAL_MAG3_ID, 0);
 
 /**
- * Mag 3 enabled
+ * Mag 3 priority.
+ *
+ * @value 0   Disabled
+ * @value 1   Min
+ * @value 25  Low
+ * @value 50  Medium (Default)
+ * @value 75  High
+ * @value 100 Max
  *
- * @boolean
  * @category system
  * @group Sensor Calibration
  */
-PARAM_DEFINE_INT32(CAL_MAG3_EN, 1);
+PARAM_DEFINE_INT32(CAL_MAG3_PRIO, 50);
 
 /**
- * Rotation of magnetometer 2 relative to airframe.
+ * Rotation of magnetometer 3 relative to airframe.
  *
  * An internal magnetometer will force a value of -1, so a GCS
  * should only attempt to configure the rotation if the value is

src/modules/sensors/sensors.cpp

@@ -340,8 +340,7 @@ void Sensors::diff_pres_poll()
 		/* push data into validator */
 		float airspeed_input[3] = { diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.0f };
 
-		_airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count,
-					ORB_PRIO_HIGH);
+		_airspeed_validator.put(airspeed.timestamp, airspeed_input, diff_pres.error_count, 100); // TODO: real priority?
 
 		airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time());
 

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -156,11 +156,6 @@ void VehicleAirData::Run()
 			updated[uorb_index] = _sensor_sub[uorb_index].update(&_last_data[uorb_index]);
 
 			if (updated[uorb_index]) {
-				if (_priority[uorb_index] == 0) {
-					// set initial priority
-					_priority[uorb_index] = _sensor_sub[uorb_index].get_priority();
-				}
-
 				// millibar to Pa
 				const float raw_pressure_pascals = _last_data[uorb_index].pressure * 100.f;
 
@@ -282,7 +277,7 @@ void VehicleAirData::Run()
 				}
 
 				// reduce priority of failed sensor to the minimum
-				_priority[failover_index] = ORB_PRIO_MIN;
+				_priority[failover_index] = 1;
 			}
 		}
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -122,6 +122,24 @@ void VehicleMagnetometer::ParametersUpdate(bool force)
 		}
 
 		_mag_comp_type = mag_comp_typ;
+
+		// update mag priority (CAL_MAGx_PRIO)
+		for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) {
+			const int32_t priority_old = _calibration[mag].priority();
+			_calibration[mag].ParametersUpdate();
+			const int32_t priority_new = _calibration[mag].priority();
+
+			if (priority_old != priority_new) {
+				if (_priority[mag] == priority_old) {
+					_priority[mag] = priority_new;
+
+				} else {
+					// change relative priority to incorporate any sensor faults
+					int priority_change = priority_new - priority_old;
+					_priority[mag] = math::constrain(_priority[mag] + priority_change, 1, 100);
+				}
+			}
+		}
 	}
 }
 
@@ -207,7 +225,7 @@ void VehicleMagnetometer::Run()
 				if (_calibration[uorb_index].device_id() != report.device_id) {
 					_calibration[uorb_index].set_external(report.is_external);
 					_calibration[uorb_index].set_device_id(report.device_id);
-					_priority[uorb_index] = _sensor_sub[uorb_index].get_priority();
+					_priority[uorb_index] = _calibration[uorb_index].priority();
 				}
 
 				if (_calibration[uorb_index].enabled()) {

src/modules/sensors/voted_sensors_update.cpp

@@ -88,28 +88,46 @@ void VotedSensorsUpdate::parametersUpdate()
 
 		if (imu.get().timestamp > 0 && imu.get().accel_device_id > 0 && imu.get().gyro_device_id > 0) {
 
-			if (_accel.priority[uorb_index] == ORB_PRIO_UNINITIALIZED) {
-				// find corresponding sensor_accel publication
-				for (uint8_t i = 0; i < ACCEL_COUNT_MAX; i++) {
-					uORB::SubscriptionData<sensor_accel_s> sensor_accel{ORB_ID(sensor_accel), i};
-					sensor_accel.update();
+			// find corresponding configured accel priority
+			int8_t accel_cal_index = calibration::FindCalibrationIndex("ACC", imu.get().accel_device_id);
 
-					if (imu.get().accel_device_id == sensor_accel.get().device_id) {
-						_accel.priority[uorb_index] = sensor_accel.get_priority();
-						break;
+			if (accel_cal_index >= 0) {
+				// found matching CAL_ACCx_PRIO
+				int32_t accel_priority_old = _accel.priority_configured[uorb_index];
+
+				_accel.priority_configured[uorb_index] = calibration::GetCalibrationParam("ACC", "PRIO", accel_cal_index);
+
+				if (accel_priority_old != _accel.priority_configured[uorb_index]) {
+					if (_accel.priority_configured[uorb_index] == 0) {
+						// disabled
+						_accel.priority[uorb_index] = 0;
+
+					} else {
+						// change relative priority to incorporate any sensor faults
+						int priority_change = _accel.priority_configured[uorb_index] - accel_priority_old;
+						_accel.priority[uorb_index] = math::constrain(_accel.priority[uorb_index] + priority_change, 1, 100);
 					}
 				}
 			}
 
-			if (_gyro.priority[uorb_index] == ORB_PRIO_UNINITIALIZED) {
-				// find corresponding sensor_gyro publication
-				for (uint8_t i = 0; i < GYRO_COUNT_MAX; i++) {
-					uORB::SubscriptionData<sensor_accel_s> sensor_gyro{ORB_ID(sensor_gyro), i};
-					sensor_gyro.update();
+			// find corresponding configured gyro priority
+			int8_t gyro_cal_index = calibration::FindCalibrationIndex("GYRO", imu.get().gyro_device_id);
 
-					if (imu.get().gyro_device_id == sensor_gyro.get().device_id) {
-						_gyro.priority[uorb_index] = sensor_gyro.get_priority();
-						break;
+			if (gyro_cal_index >= 0) {
+				// found matching CAL_GYROx_PRIO
+				int32_t gyro_priority_old = _gyro.priority_configured[uorb_index];
+
+				_gyro.priority_configured[uorb_index] = calibration::GetCalibrationParam("GYRO", "PRIO", gyro_cal_index);
+
+				if (gyro_priority_old != _gyro.priority_configured[uorb_index]) {
+					if (_gyro.priority_configured[uorb_index] == 0) {
+						// disabled
+						_gyro.priority[uorb_index] = 0;
+
+					} else {
+						// change relative priority to incorporate any sensor faults
+						int priority_change = _gyro.priority_configured[uorb_index] - gyro_priority_old;
+						_gyro.priority[uorb_index] = math::constrain(_gyro.priority[uorb_index] + priority_change, 1, 100);
 					}
 				}
 			}
@@ -122,21 +140,13 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 	for (int uorb_index = 0; uorb_index < 3; uorb_index++) {
 		vehicle_imu_s imu_report;
 
-		if (_accel.enabled[uorb_index] && _gyro.enabled[uorb_index] && _vehicle_imu_sub[uorb_index].update(&imu_report)) {
+		if ((_accel.priority[uorb_index] > 0) && (_gyro.priority[uorb_index] > 0)
+		    && _vehicle_imu_sub[uorb_index].update(&imu_report)) {
 
 			// copy corresponding vehicle_imu_status for accel & gyro error counts
 			vehicle_imu_status_s imu_status{};
 			_vehicle_imu_status_sub[uorb_index].copy(&imu_status);
 
-			// First publication with data
-			if (_accel.priority[uorb_index] == 0) {
-				_accel.priority[uorb_index] = _accel.subscription[uorb_index].get_priority();
-			}
-
-			if (_gyro.priority[uorb_index] == 0) {
-				_gyro.priority[uorb_index] = _gyro.subscription[uorb_index].get_priority();
-			}
-
 			_accel_device_id[uorb_index] = imu_report.accel_device_id;
 			_gyro_device_id[uorb_index] = imu_report.gyro_device_id;
 
@@ -250,12 +260,12 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na
 				}
 
 				// reduce priority of failed sensor to the minimum
-				sensor.priority[failover_index] = ORB_PRIO_MIN;
+				sensor.priority[failover_index] = 1;
 
 				int ctr_valid = 0;
 
 				for (uint8_t i = 0; i < sensor.subscription_count; i++) {
-					if (sensor.enabled[i] && (sensor.priority[i] > ORB_PRIO_MIN)) {
+					if (sensor.priority[i] > 1) {
 						ctr_valid++;
 					}
 				}
@@ -300,6 +310,8 @@ void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor
 
 		if (!sensor_data.advertised[i] && sensor_data.subscription[i].advertised()) {
 			sensor_data.advertised[i] = true;
+			sensor_data.priority[i] = DEFAULT_PRIORITY;
+			sensor_data.priority_configured[i] = DEFAULT_PRIORITY;
 
 			if (i > 0) {
 				/* the first always exists, but for each further sensor, add a new validator */
@@ -366,7 +378,7 @@ void VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_imu_s &preflt)
 	for (int sensor_index = 0; sensor_index < _accel.subscription_count; sensor_index++) {
 
 		// check that the sensor we are checking against is not the same as the primary
-		if (_accel.enabled[sensor_index] && (_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) {
+		if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) {
 
 			float accel_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison agains the primary
 
@@ -415,7 +427,7 @@ void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_imu_s &preflt)
 	for (int sensor_index = 0; sensor_index < _gyro.subscription_count; sensor_index++) {
 
 		// check that the sensor we are checking against is not the same as the primary
-		if (_gyro.enabled[sensor_index] && (_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) {
+		if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) {
 
 			float gyro_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary
 

src/modules/sensors/voted_sensors_update.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -126,6 +126,8 @@ private:
 	static constexpr uint8_t GYRO_COUNT_MAX = 3;
 	static constexpr uint8_t SENSOR_COUNT_MAX = math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX);
 
+	static constexpr uint8_t DEFAULT_PRIORITY = 50;
+
 	struct SensorData {
 		SensorData() = delete;
 		explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}} {}
@@ -133,10 +135,10 @@ private:
 		uORB::Subscription subscription[SENSOR_COUNT_MAX]; /**< raw sensor data subscription */
 		DataValidatorGroup voter{1};
 		unsigned int last_failover_count{0};
-		ORB_PRIO priority[SENSOR_COUNT_MAX] {}; /**< sensor priority */
+		int32_t priority[SENSOR_COUNT_MAX] {};
+		int32_t priority_configured[SENSOR_COUNT_MAX] {};
 		uint8_t last_best_vote{0}; /**< index of the latest best vote */
 		uint8_t subscription_count{0};
-		bool enabled[SENSOR_COUNT_MAX] {true, true, true};
 		bool advertised[SENSOR_COUNT_MAX] {false, false, false};
 	};