sensors: sensor_preflight_imu -> sensors_status_imu and run continuously

- inconsistency checks now run continuously instead of only preflight
 - keep inconsistencies for all sensors
 - add per sensor data validator state as overall health flag

src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp

@@ -38,53 +38,88 @@
 #include <lib/parameters/param.h>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/sensor_preflight_imu.h>
+#include <uORB/topics/sensors_status_imu.h>
 #include <uORB/topics/subsystem_info.h>
 
 bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
 		const bool report_status)
 {
-	float test_limit = 1.0f; // pass limit re-used for each test
-
 	// Get sensor_preflight data if available and exit with a fail recorded if not
-	uORB::SubscriptionData<sensor_preflight_imu_s> sensors_sub{ORB_ID(sensor_preflight_imu)};
-	sensors_sub.update();
-	const sensor_preflight_imu_s &sensors = sensors_sub.get();
+	uORB::SubscriptionData<sensors_status_imu_s> sensors_status_imu_sub{ORB_ID(sensors_status_imu)};
+	const sensors_status_imu_s &imu = sensors_status_imu_sub.get();
 
 	// Use the difference between IMU's to detect a bad calibration.
 	// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
-	param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
+	float accel_test_limit = 1.f;
+	param_get(param_find("COM_ARM_IMU_ACC"), &accel_test_limit);
 
-	if (sensors.accel_inconsistency_m_s_s > test_limit) {
-		if (report_status) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accels inconsistent - Check Cal");
-			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status);
-			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status);
-		}
+	for (unsigned i = 0; i < (sizeof(imu.accel_inconsistency_m_s_s) / sizeof(imu.accel_inconsistency_m_s_s[0])); i++) {
+		if (imu.accel_device_ids[i] != 0) {
+			if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
+				set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, imu.accel_healthy[i], status);
 
-		return false;
+			} else {
+				set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, imu.accel_healthy[i], status);
+			}
 
-	} else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) {
-		if (report_status) {
-			mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accels inconsistent - Check Cal");
+			const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i];
+
+			if (accel_inconsistency_m_s_s > accel_test_limit) {
+				if (report_status) {
+					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u inconsistent - Check Cal", i);
+
+					if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
+						set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status);
+
+					} else {
+						set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status);
+					}
+				}
+
+				return false;
+
+			} else if (accel_inconsistency_m_s_s > accel_test_limit * 0.8f) {
+				if (report_status) {
+					mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accel %u inconsistent - Check Cal", i);
+				}
+			}
 		}
 	}
 
 	// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
-	param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
+	float gyro_test_limit = 1.f;
+	param_get(param_find("COM_ARM_IMU_GYR"), &gyro_test_limit);
 
-	if (sensors.gyro_inconsistency_rad_s > test_limit) {
-		if (report_status) {
-			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyros inconsistent - Check Cal");
-			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
-			set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
-		}
+	for (unsigned i = 0; i < (sizeof(imu.gyro_inconsistency_rad_s) / sizeof(imu.gyro_inconsistency_rad_s[0])); i++) {
+		if (imu.gyro_device_ids[i] != 0) {
+			if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {
+				set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, imu.accel_healthy[i], status);
 
-		return false;
+			} else {
+				set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, imu.accel_healthy[i], status);
+			}
 
-	} else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) {
-		if (report_status) {
-			mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyros inconsistent - Check Cal");
+			const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i];
+
+			if (gyro_inconsistency_rad_s > gyro_test_limit) {
+				if (report_status) {
+					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u inconsistent - Check Cal", i);
+
+					if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {
+						set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
+
+					} else {
+						set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
+					}
+				}
+
+				return false;
+
+			} else if (gyro_inconsistency_rad_s > gyro_test_limit * 0.5f) {
+				if (report_status) {
+					mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyro %u inconsistent - Check Cal", i);
+				}
+			}
 		}
 	}
 

src/modules/logger/logged_topics.cpp

@@ -80,7 +80,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("safety");
 	add_topic("sensor_combined");
 	add_topic("sensor_correction");
-	add_topic("sensor_preflight_imu", 200);
+	add_topic("sensors_status_imu", 200);
 	add_topic("sensor_preflight_mag", 500);
 	add_topic("sensor_selection");
 	add_topic("system_power", 500);

src/modules/sensors/sensors.cpp

@@ -64,7 +64,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_preflight_imu.h>
+#include <uORB/topics/sensors_status_imu.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_imu.h>
@@ -115,7 +115,6 @@ private:
 	hrt_abstime     _sensor_combined_prev_timestamp{0};
 
 	sensor_combined_s _sensor_combined{};
-	sensor_preflight_imu_s _sensor_preflight_imu{};
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[3] {
 		{this, ORB_ID(vehicle_imu), 0},
@@ -130,7 +129,6 @@ private:
 
 	uORB::Publication<airspeed_s>             _airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<sensor_combined_s>      _sensor_pub{ORB_ID(sensor_combined)};
-	uORB::Publication<sensor_preflight_imu_s> _sensor_preflight_imu_pub{ORB_ID(sensor_preflight_imu)};
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -584,16 +582,6 @@ void Sensors::Run()
 		_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
 		_sensor_pub.publish(_sensor_combined);
 		_sensor_combined_prev_timestamp = _sensor_combined.timestamp;
-
-		// If the the vehicle is disarmed calculate the length of the maximum difference between
-		// IMU units as a consistency metric and publish to the sensor preflight topic
-		if (!_armed) {
-			_voted_sensors_update.calcAccelInconsistency(_sensor_preflight_imu);
-			_voted_sensors_update.calcGyroInconsistency(_sensor_preflight_imu);
-
-			_sensor_preflight_imu.timestamp = hrt_absolute_time();
-			_sensor_preflight_imu_pub.publish(_sensor_preflight_imu);
-		}
 	}
 
 	// keep adding sensors as long as we are not armed,
@@ -725,7 +713,7 @@ The provided functionality includes:
 - Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or
   on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the
   sensor drivers must already be running when `sensors` is started.
-- Do preflight sensor consistency checks and publish the `sensor_preflight_imu` topic.
+- Do sensor consistency checks and publish the `sensors_status_imu` topic.
 
 ### Implementation
 It runs in its own thread and polls on the currently selected gyro topic.

src/modules/sensors/voted_sensors_update.cpp

@@ -187,8 +187,8 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 	_accel.voter.get_best(hrt_absolute_time(), &accel_best_index);
 	_gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index);
 
-	checkFailover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC);
-	checkFailover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO);
+	checkFailover(_accel, "Accel");
+	checkFailover(_gyro, "Gyro");
 
 	// write data for the best sensor to output variables
 	if ((accel_best_index >= 0) && (gyro_best_index >= 0)) {
@@ -229,7 +229,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 	}
 }
 
-bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type)
+bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name)
 {
 	if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) {
 
@@ -261,34 +261,6 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na
 
 				// reduce priority of failed sensor to the minimum
 				sensor.priority[failover_index] = 1;
-
-				int ctr_valid = 0;
-
-				for (uint8_t i = 0; i < sensor.subscription_count; i++) {
-					if (sensor.priority[i] > 1) {
-						ctr_valid++;
-					}
-				}
-
-				if (ctr_valid < 2) {
-					if (ctr_valid == 0) {
-						// Zero valid sensors remain! Set even the primary sensor health to false
-						_info.subsystem_type = type;
-
-					} else if (ctr_valid == 1) {
-						// One valid sensor remains, set secondary sensor health to false
-						if (type == subsystem_info_s::SUBSYSTEM_TYPE_GYRO) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; }
-
-						if (type == subsystem_info_s::SUBSYSTEM_TYPE_ACC) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; }
-					}
-
-					_info.timestamp = hrt_absolute_time();
-					_info.present = true;
-					_info.enabled = true;
-					_info.ok = false;
-
-					_info_pub.publish(_info);
-				}
 			}
 		}
 
@@ -357,8 +329,41 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw)
 			_selection.timestamp = hrt_absolute_time();
 			_sensor_selection_pub.publish(_selection);
 			_selection_changed = false;
+
+			for (int sensor_index = 0; sensor_index < ACCEL_COUNT_MAX; sensor_index++) {
+				_accel_diff[sensor_index].zero();
+			}
+
+			for (int sensor_index = 0; sensor_index < GYRO_COUNT_MAX; sensor_index++) {
+				_gyro_diff[sensor_index].zero();
+			}
 		}
 	}
+
+	calcAccelInconsistency();
+	calcGyroInconsistency();
+
+	sensors_status_imu_s status{};
+	status.accel_device_id_primary = _selection.accel_device_id;
+	status.gyro_device_id_primary = _selection.gyro_device_id;
+
+	for (int i = 0; i < SENSOR_COUNT_MAX; i++) {
+		if (_accel_device_id[i] != 0) {
+			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);
+		}
+
+		if (_gyro_device_id[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.timestamp = hrt_absolute_time();
+	_sensors_status_imu_pub.publish(status);
 }
 
 void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
@@ -369,100 +374,34 @@ void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw)
 	}
 }
 
-void VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_imu_s &preflt)
+void VotedSensorsUpdate::calcAccelInconsistency()
 {
-	float accel_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
-	unsigned check_index = 0; // the number of sensors the primary has been checked against
+	const Vector3f primary_accel{_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2};
 
 	// Check each sensor against the primary
-	for (int sensor_index = 0; sensor_index < _accel.subscription_count; sensor_index++) {
-
+	for (int sensor_index = 0; sensor_index < ACCEL_COUNT_MAX; sensor_index++) {
 		// check that the sensor we are checking against is not the same as the primary
-		if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) {
+		if (_accel.advertised[sensor_index] && (_accel.priority[sensor_index] > 0)
+		    && (_accel_device_id[sensor_index] != _selection.accel_device_id)) {
 
-			float accel_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison agains the primary
-
-			// calculate accel_diff_sum_sq for the specified sensor against the primary
-			for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
-				_accel_diff[axis_index][check_index] = 0.95f * _accel_diff[axis_index][check_index] + 0.05f *
-								       (_last_sensor_data[_accel.last_best_vote].accelerometer_m_s2[axis_index] -
-									_last_sensor_data[sensor_index].accelerometer_m_s2[axis_index]);
-				accel_diff_sum_sq += _accel_diff[axis_index][check_index] * _accel_diff[axis_index][check_index];
-
-			}
-
-			// capture the largest sum value
-			if (accel_diff_sum_sq > accel_diff_sum_max_sq) {
-				accel_diff_sum_max_sq = accel_diff_sum_sq;
-
-			}
-
-			// increment the check index
-			check_index++;
+			const Vector3f current_accel{_last_sensor_data[sensor_index].accelerometer_m_s2};
+			_accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (primary_accel - current_accel);
 		}
-
-		// check to see if the maximum number of checks has been reached and break
-		if (check_index >= 2) {
-			break;
-
-		}
-	}
-
-	// skip check if less than 2 sensors
-	if (check_index < 1) {
-		preflt.accel_inconsistency_m_s_s = 0.0f;
-
-	} else {
-		// get the vector length of the largest difference and write to the combined sensor struct
-		preflt.accel_inconsistency_m_s_s = sqrtf(accel_diff_sum_max_sq);
 	}
 }
 
-void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_imu_s &preflt)
+void VotedSensorsUpdate::calcGyroInconsistency()
 {
-	float gyro_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
-	unsigned check_index = 0; // the number of sensors the primary has been checked against
+	const Vector3f primary_gyro{_last_sensor_data[_gyro.last_best_vote].gyro_rad};
 
 	// Check each sensor against the primary
-	for (int sensor_index = 0; sensor_index < _gyro.subscription_count; sensor_index++) {
-
+	for (int sensor_index = 0; sensor_index < GYRO_COUNT_MAX; sensor_index++) {
 		// check that the sensor we are checking against is not the same as the primary
-		if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) {
+		if (_gyro.advertised[sensor_index] && (_gyro.priority[sensor_index] > 0)
+		    && (_gyro_device_id[sensor_index] != _selection.gyro_device_id)) {
 
-			float gyro_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary
-
-			// calculate gyro_diff_sum_sq for the specified sensor against the primary
-			for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
-				_gyro_diff[axis_index][check_index] = 0.95f * _gyro_diff[axis_index][check_index] + 0.05f *
-								      (_last_sensor_data[_gyro.last_best_vote].gyro_rad[axis_index] -
-								       _last_sensor_data[sensor_index].gyro_rad[axis_index]);
-				gyro_diff_sum_sq += _gyro_diff[axis_index][check_index] * _gyro_diff[axis_index][check_index];
-
-			}
-
-			// capture the largest sum value
-			if (gyro_diff_sum_sq > gyro_diff_sum_max_sq) {
-				gyro_diff_sum_max_sq = gyro_diff_sum_sq;
-
-			}
-
-			// increment the check index
-			check_index++;
+			const Vector3f current_gyro{_last_sensor_data[sensor_index].gyro_rad};
+			_gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (primary_gyro - current_gyro);
 		}
-
-		// check to see if the maximum number of checks has been reached and break
-		if (check_index >= 2) {
-			break;
-
-		}
-	}
-
-	// skip check if less than 2 sensors
-	if (check_index < 1) {
-		preflt.gyro_inconsistency_rad_s = 0.0f;
-
-	} else {
-		// get the vector length of the largest difference and write to the combined sensor struct
-		preflt.gyro_inconsistency_rad_s = sqrtf(gyro_diff_sum_max_sq);
 	}
 }

src/modules/sensors/voted_sensors_update.h

@@ -53,11 +53,10 @@
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/sensor_preflight_imu.h>
+#include <uORB/topics/sensors_status_imu.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/vehicle_imu_status.h>
-#include <uORB/topics/subsystem_info.h>
 
 namespace sensors
 {
@@ -111,16 +110,6 @@ public:
 	 */
 	void setRelativeTimestamps(sensor_combined_s &raw);
 
-	/**
-	 * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor
-	 */
-	void calcAccelInconsistency(sensor_preflight_imu_s &preflt);
-
-	/**
-	 * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor
-	 */
-	void calcGyroInconsistency(sensor_preflight_imu_s &preflt);
-
 private:
 
 	static constexpr uint8_t ACCEL_COUNT_MAX = 3;
@@ -157,7 +146,17 @@ private:
 	 * Check & handle failover of a sensor
 	 * @return true if a switch occured (could be for a non-critical reason)
 	 */
-	bool checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type);
+	bool checkFailover(SensorData &sensor, const char *sensor_name);
+
+	/**
+	 * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor
+	 */
+	void calcAccelInconsistency();
+
+	/**
+	 * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor
+	 */
+	void calcGyroInconsistency();
 
 	SensorData _accel{ORB_ID::sensor_accel};
 	SensorData _gyro{ORB_ID::sensor_gyro};
@@ -166,7 +165,7 @@ private:
 	orb_advert_t _mavlink_log_pub{nullptr};
 
 	uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)};	/**< handle to the sensor selection uORB topic */
-	uORB::PublicationQueued<subsystem_info_s> _info_pub{ORB_ID(subsystem_info)};	/* subsystem info publication */
+	uORB::Publication<sensors_status_imu_s> _sensors_status_imu_pub{ORB_ID(sensors_status_imu)};
 
 	uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[3];
 	uORB::SubscriptionMultiArray<vehicle_imu_status_s, ACCEL_COUNT_MAX> _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status};
@@ -177,8 +176,8 @@ private:
 
 	bool _selection_changed{true};			/**< true when a sensor selection has changed and not been published */
 
-	float _accel_diff[3][2] {};			/**< filtered accel differences between IMU units (m/s/s) */
-	float _gyro_diff[3][2] {};			/**< filtered gyro differences between IMU uinits (rad/s) */
+	matrix::Vector3f _accel_diff[ACCEL_COUNT_MAX] {};		/**< filtered accel differences between IMU units (m/s/s) */
+	matrix::Vector3f _gyro_diff[GYRO_COUNT_MAX] {};			/**< filtered gyro differences between IMU uinits (rad/s) */
 
 	uint32_t _accel_device_id[SENSOR_COUNT_MAX] {};	/**< accel driver device id for each uorb instance */
 	uint32_t _gyro_device_id[SENSOR_COUNT_MAX] {};	/**< gyro driver device id for each uorb instance */
@@ -186,7 +185,6 @@ private:
 	uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {};	/**< latest full timestamp */
 
 	sensor_selection_s _selection {};		/**< struct containing the sensor selection to be published to the uORB */
-	subsystem_info_s _info {};			/**< subsystem info publication */
 };
 
 } /* namespace sensors */