sensors: add accel/gyro current priority to sensors_status_imu

- later the sensor priorities (user configurable) can be factored into the estimator selection criteria

msg/sensors_status_imu.msg

@@ -8,10 +8,11 @@ uint32 accel_device_id_primary       # current primary accel device id for refer
 uint32[4] accel_device_ids
 float32[4] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and mean in m/s^2.
 bool[4] accel_healthy
-
+uint8[4] accel_priority
 
 uint32 gyro_device_id_primary       # current primary gyro device id for reference
 
 uint32[4] gyro_device_ids
 float32[4] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and mean in (rad/s).
 bool[4] gyro_healthy
+uint8[4] gyro_priority

src/modules/sensors/data_validator/DataValidatorGroup.cpp

@@ -305,3 +305,21 @@ uint32_t DataValidatorGroup::get_sensor_state(unsigned index)
 	// sensor index not found
 	return UINT32_MAX;
 }
+
+uint8_t DataValidatorGroup::get_sensor_priority(unsigned index)
+{
+	DataValidator *next = _first;
+	unsigned i = 0;
+
+	while (next != nullptr) {
+		if (i == index) {
+			return next->priority();
+		}
+
+		next = next->sibling();
+		i++;
+	}
+
+	// sensor index not found
+	return 0;
+}

src/modules/sensors/data_validator/DataValidatorGroup.hpp

@@ -104,6 +104,13 @@ public:
 	 */
 	uint32_t get_sensor_state(unsigned index);
 
+	/**
+	 * Get the priority of the sensor with the specified index
+	 *
+	 * @return		priority
+	 */
+	uint8_t get_sensor_priority(unsigned index);
+
 	/**
 	 * Print the validator value
 	 *

src/modules/sensors/voted_sensors_update.cpp

@@ -407,12 +407,14 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
 			status.accel_device_ids[i] = _accel_device_id[i];
 			status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm();
 			status.accel_healthy[i] = (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
+			status.accel_priority[i] = _accel.voter.get_sensor_priority(i);
 		}
 
 		if ((_gyro_device_id[i] != 0) && (_gyro.priority[i] > 0)) {
 			status.gyro_device_ids[i] = _gyro_device_id[i];
 			status.gyro_inconsistency_rad_s[i] = _gyro_diff[i].norm();
 			status.gyro_healthy[i] = (_gyro.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR);
+			status.gyro_priority[i] = _gyro.voter.get_sensor_priority(i);
 		}
 	}