From 713a1d08a3d1ce9b8d584a219829e0f9aa2f94c5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 30 Jul 2021 09:54:19 -0400
Subject: [PATCH] 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                     |  3 ++-
 .../data_validator/DataValidatorGroup.cpp      | 18 ++++++++++++++++++
 .../data_validator/DataValidatorGroup.hpp      |  7 +++++++
 src/modules/sensors/voted_sensors_update.cpp   |  2 ++
 4 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/msg/sensors_status_imu.msg b/msg/sensors_status_imu.msg
index 29bc7b1ad1..cfad3419c3 100644
--- a/msg/sensors_status_imu.msg
+++ b/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
diff --git a/src/modules/sensors/data_validator/DataValidatorGroup.cpp b/src/modules/sensors/data_validator/DataValidatorGroup.cpp
index 20c1358525..6ff8748fd7 100644
--- a/src/modules/sensors/data_validator/DataValidatorGroup.cpp
+++ b/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;
+}
diff --git a/src/modules/sensors/data_validator/DataValidatorGroup.hpp b/src/modules/sensors/data_validator/DataValidatorGroup.hpp
index 8a2d4b8a1b..5ba6ea1cae 100644
--- a/src/modules/sensors/data_validator/DataValidatorGroup.hpp
+++ b/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
 	 *
diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp
index 5d12a98b97..63ed4d2d63 100644
--- a/src/modules/sensors/voted_sensors_update.cpp
+++ b/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);
 		}
 	}