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

.ci/Jenkinsfile-hardware

@@ -862,6 +862,7 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_gyro_fifo"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensor_mag"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener sensors_status_imu"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener px4io_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener system_power"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "listener vehicle_air_data"'
@@ -934,6 +935,7 @@ void statusSEGGER() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_gyro_fifo"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensor_mag"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener sensors_status_imu"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener px4io_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener system_power"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "listener vehicle_air_data"'

Tools/ecl_ekf/analysis/post_processing.py

@@ -146,7 +146,7 @@ def get_gps_check_fail_flags(estimator_status: dict) -> dict:
     return gps_fail_flags
 
 
-def magnetic_field_estimates_from_status(estimator_states: dict) -> Tuple[float, float, float]:
+def magnetic_field_estimates_from_states(estimator_states: dict) -> Tuple[float, float, float]:
     """
 
     :param estimator_states:

Tools/ecl_ekf/plotting/pdf_report.py

@@ -11,7 +11,7 @@
 from matplotlib.backends.backend_pdf import PdfPages
 from pyulog import ULog
 
-from analysis.post_processing import magnetic_field_estimates_from_status, get_estimator_check_flags
+from analysis.post_processing import magnetic_field_estimates_from_states, get_estimator_check_flags
 from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \
     CheckFlagsPlot
 from analysis.detectors import PreconditionError
@@ -34,6 +34,12 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
     except:
         raise PreconditionError('could not find estimator_status data')
 
+    try:
+        estimator_states = ulog.get_dataset('estimator_states').data
+        print('found estimator_states data')
+    except:
+        raise PreconditionError('could not find estimator_states data')
+
     try:
         estimator_innovations = ulog.get_dataset('estimator_innovations').data
         estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances').data
@@ -45,12 +51,6 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
     except:
         raise PreconditionError('could not find innovation data')
 
-    try:
-        sensor_preflight_imu = ulog.get_dataset('sensor_preflight_imu').data
-        print('found sensor_preflight data')
-    except:
-        raise PreconditionError('could not find sensor_preflight_imu data')
-
     control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status)
 
     status_time = 1e-6 * estimator_status['timestamp']
@@ -60,17 +60,6 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
 
     with PdfPages(output_plot_filename) as pdf_pages:
 
-        # plot IMU consistency data
-        if ('accel_inconsistency_m_s_s' in sensor_preflight_imu.keys()) and (
-                'gyro_inconsistency_rad_s' in sensor_preflight_imu.keys()):
-            data_plot = TimeSeriesPlot(
-                sensor_preflight_imu, [['accel_inconsistency_m_s_s'], ['gyro_inconsistency_rad_s']],
-                x_labels=['data index', 'data index'],
-                y_labels=['acceleration (m/s/s)', 'angular rate (rad/s)'],
-                plot_title='IMU Consistency Check Levels', pdf_handle=pdf_pages)
-            data_plot.save()
-            data_plot.close()
-
         # vertical velocity and position innovations
         data_plot = InnovationPlot(
             innovation_data, [('gps_vpos', 'var_gps_vpos'),
@@ -292,10 +281,10 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
         data_plot.close()
 
         # Plot the earth frame magnetic field estimates
-        declination, field_strength, inclination = magnetic_field_estimates_from_status(
-            estimator_status)
+        declination, field_strength, inclination = magnetic_field_estimates_from_states(
+            estimator_states)
         data_plot = CheckFlagsPlot(
-            1e-6 * estimator_status['timestamp'],
+            1e-6 * estimator_states['timestamp'],
             {'strength': field_strength, 'declination': declination, 'inclination': inclination},
             [['declination'], ['inclination'], ['strength']],
             x_label='time (sec)', y_labels=['declination (deg)', 'inclination (deg)',
@@ -307,7 +296,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
 
         # Plot the body frame magnetic field estimates
         data_plot = CheckFlagsPlot(
-            1e-6 * estimator_status['timestamp'], estimator_status,
+            1e-6 * estimator_states['timestamp'], estimator_states,
             [['states[19]'], ['states[20]'], ['states[21]']],
             x_label='time (sec)', y_labels=['X (Gauss)', 'Y (Gauss)', 'Z (Gauss)'],
             plot_title='Magnetometer Bias Estimates', annotate=False, pdf_handle=pdf_pages)
@@ -316,7 +305,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
 
         # Plot the EKF wind estimates
         data_plot = CheckFlagsPlot(
-            1e-6 * estimator_status['timestamp'], estimator_status,
+            1e-6 * estimator_states['timestamp'], estimator_states,
             [['states[22]'], ['states[23]']], x_label='time (sec)',
             y_labels=['North (m/s)', 'East (m/s)'], plot_title='Wind Velocity Estimates',
             annotate=False, pdf_handle=pdf_pages)

msg/CMakeLists.txt

@@ -120,9 +120,9 @@ set(msg_files
 	sensor_gyro.msg
 	sensor_gyro_fifo.msg
 	sensor_mag.msg
-	sensor_preflight_imu.msg
 	sensor_preflight_mag.msg
 	sensor_selection.msg
+	sensors_status_imu.msg
 	subsystem_info.msg
 	system_power.msg
 	task_stack_info.msg

msg/sensors_status_imu.msg

@@ -0,0 +1,17 @@
+#
+# Sensor check metrics. This will be zero for a sensor that's primary or unpopulated.
+#
+uint64 timestamp # time since system start (microseconds)
+
+uint32 accel_device_id_primary       # current primary accel device id for reference
+
+uint32[3] accel_device_ids
+float32[3] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and primary in (m/s/s).
+bool[3] accel_healthy
+
+
+uint32 gyro_device_id_primary       # current primary gyro device id for reference
+
+uint32[3] gyro_device_ids
+float32[3] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and primary in (rad/s).
+bool[3] gyro_healthy

msg/tools/uorb_rtps_message_ids.yaml

@@ -144,7 +144,7 @@ rtps:
     id: 67
   - msg: sensor_mag
     id: 68
-  - msg: sensor_preflight_imu
+  - msg: sensors_status_imu
     id: 69
   - msg: sensor_selection
     id: 70

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);
+
+			} else {
+				set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, imu.accel_healthy[i], status);
+			}
+
+			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;
+				return false;
 
-	} 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");
+			} 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);
+
+			} else {
+				set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, imu.accel_healthy[i], status);
+			}
+
+			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;
+				return false;
 
-	} 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");
+			} 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 @@ class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::Sch
 	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 @@ class Sensors : public ModuleBase<Sensors>, public ModuleParams, public px4::Sch
 
 	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.