refactor(commander): one function per estimator status check (#28898)

* test(commander): pin the estimator checks before restructuring them

Sixteen cases on EstimatorChecks driven through its topics, one per
behaviour of the file that the next commit moves: the preflight
innovation and magnetic interference checks, GNSS fusion starting and
stopping, spoofing and jamming, a failing GNSS check under each
COM_ARM_WO_GPS setting, the sensor bias check, the compass fault and
heading reference checks, the imminent position failure warning, low
position accuracy, and the attitude, angular velocity and altitude
validity flags. They observe the health report of a cycle and the
events it sends. All pass on the file as it is.

Assisted-by: Claude:claude-fable-5-1
Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu>

* refactor(commander): one function per estimator status check

checkEstimatorStatus ran four hundred lines through eight levels of
nesting, covering the preflight innovation checks, the magnetic
interference check and everything about GNSS. Each of those is its own
function now, named for what it checks, with an early return where a
condition used to wrap the whole block, and the GNSS part is split
further into the fusion change, the spoofing and jamming latches and
the preflight quality check. In setModeRequirementFlags the one nested
block, the warning of an imminent position failure, is extracted the
same way, and the rest is left as flat sections.

Three small simplifications on the way: the mavlink severity of a
failed GNSS check follows the log level chosen for it instead of a
second switch on the parameter, the spoofing and jamming latches are one
comparison each, and the heading innovation flag that
setModeRequirementFlags never read is no longer passed to it.

The ITCM lists of the i.MX RT boards name setModeRequirementFlags by
its new signature and take the functions split out of
checkEstimatorStatus, so the same code stays in ITCM there.

No event, message, condition or order of side effects changes. The
extracted events are identical, and the twenty seven functional tests
pass before and after.

Assisted-by: Claude:claude-fable-5-1
Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu>

---------

Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu>

boards/agam/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld

@@ -458,7 +458,8 @@
 *(.text._ZN24ManualVelocitySmoothingZC1Ev) /* itcm-check-ignore */
 *(.text._ZN3ADC6sampleEj)
 *(.text._ZNK3Ekf22isTerrainEstimateValidEv)
-*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report)
+*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report)
+*(.text._ZN15EstimatorChecks29warnOfImminentPositionFailureER6ReportRKyRK25vehicle_global_position_sfRK16failsafe_flags_s)
 *(.text._ZN11ControlMath11addIfNotNanERff)
 *(.text._ZN9Commander21checkForMissionUpdateEv)
 *(.text._Z8set_tunei)
@@ -601,6 +602,12 @@
 *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_)
 *(.text._ZN13AnalogBattery19get_current_channelEv)
 *(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks25checkInnovationsPreflightERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks34checkMagneticInterferencePreflightERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks15checkGnssFusionERK7ContextR6ReportRK18estimator_status_s)
+*(.text._ZN15EstimatorChecks22reportGnssFusionChangeERK7ContextR6Reportb)
+*(.text._ZN15EstimatorChecks22reportGnssInterferenceER6Reportt)
+*(.text._ZN15EstimatorChecks30reportFailedGnssCheckPreflightER6ReportRK18estimator_status_sb)
 *(.text._ZN12FailsafeBase11updateDelayERKy)
 *(.text._ZN10FlightTask25_evaluateDistanceToGroundEv)
 *(.text._ZN4EKF218PublishGnssHgtBiasERKy)

boards/ark/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld

@@ -445,7 +445,8 @@
 *(.text._ZN24ManualVelocitySmoothingZC1Ev) /* itcm-check-ignore */
 *(.text._ZN3ADC6sampleEj)
 *(.text._ZNK3Ekf22isTerrainEstimateValidEv)
-*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report)
+*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report)
+*(.text._ZN15EstimatorChecks29warnOfImminentPositionFailureER6ReportRKyRK25vehicle_global_position_sfRK16failsafe_flags_s)
 *(.text._ZN11ControlMath11addIfNotNanERff)
 *(.text._ZN9Commander21checkForMissionUpdateEv)
 *(.text._Z8set_tunei)
@@ -586,6 +587,12 @@
 *(.text._ZN12SafetyButton3RunEv)
 *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_)
 *(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks25checkInnovationsPreflightERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks34checkMagneticInterferencePreflightERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks15checkGnssFusionERK7ContextR6ReportRK18estimator_status_s)
+*(.text._ZN15EstimatorChecks22reportGnssFusionChangeERK7ContextR6Reportb)
+*(.text._ZN15EstimatorChecks22reportGnssInterferenceER6Reportt)
+*(.text._ZN15EstimatorChecks30reportFailedGnssCheckPreflightER6ReportRK18estimator_status_sb)
 *(.text._ZN12FailsafeBase11updateDelayERKy)
 *(.text._ZN10FlightTask25_evaluateDistanceToGroundEv)
 *(.text._ZN4EKF218PublishGnssHgtBiasERKy)

boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld

@@ -450,7 +450,8 @@
 *(.text._ZN24ManualVelocitySmoothingZC1Ev) /* itcm-check-ignore */
 *(.text._ZN3ADC6sampleEj)
 *(.text._ZNK3Ekf22isTerrainEstimateValidEv)
-*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report)
+*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report)
+*(.text._ZN15EstimatorChecks29warnOfImminentPositionFailureER6ReportRKyRK25vehicle_global_position_sfRK16failsafe_flags_s)
 *(.text._ZN11ControlMath11addIfNotNanERff)
 *(.text._ZN9Commander21checkForMissionUpdateEv)
 *(.text._Z8set_tunei)
@@ -589,6 +590,12 @@
 *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_)
 *(.text._ZN13AnalogBattery19get_current_channelEv)
 *(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks25checkInnovationsPreflightERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks34checkMagneticInterferencePreflightERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks15checkGnssFusionERK7ContextR6ReportRK18estimator_status_s)
+*(.text._ZN15EstimatorChecks22reportGnssFusionChangeERK7ContextR6Reportb)
+*(.text._ZN15EstimatorChecks22reportGnssInterferenceER6Reportt)
+*(.text._ZN15EstimatorChecks30reportFailedGnssCheckPreflightER6ReportRK18estimator_status_sb)
 *(.text._ZN12FailsafeBase11updateDelayERKy)
 *(.text._ZN10FlightTask25_evaluateDistanceToGroundEv)
 *(.text._ZN4EKF218PublishGnssHgtBiasERKy)

boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld

@@ -450,7 +450,8 @@
 *(.text._ZN24ManualVelocitySmoothingZC1Ev) /* itcm-check-ignore */
 *(.text._ZN3ADC6sampleEj)
 *(.text._ZNK3Ekf22isTerrainEstimateValidEv)
-*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report)
+*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report)
+*(.text._ZN15EstimatorChecks29warnOfImminentPositionFailureER6ReportRKyRK25vehicle_global_position_sfRK16failsafe_flags_s)
 *(.text._ZN11ControlMath11addIfNotNanERff)
 *(.text._ZN9Commander21checkForMissionUpdateEv)
 *(.text._Z8set_tunei)
@@ -588,6 +589,12 @@
 *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_)
 *(.text._ZN13AnalogBattery19get_current_channelEv)
 *(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks25checkInnovationsPreflightERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks34checkMagneticInterferencePreflightERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks15checkGnssFusionERK7ContextR6ReportRK18estimator_status_s)
+*(.text._ZN15EstimatorChecks22reportGnssFusionChangeERK7ContextR6Reportb)
+*(.text._ZN15EstimatorChecks22reportGnssInterferenceER6Reportt)
+*(.text._ZN15EstimatorChecks30reportFailedGnssCheckPreflightER6ReportRK18estimator_status_sb)
 *(.text._ZN12FailsafeBase11updateDelayERKy)
 *(.text._ZN10FlightTask25_evaluateDistanceToGroundEv)
 *(.text._ZN4EKF218PublishGnssHgtBiasERKy)

boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld

@@ -457,7 +457,8 @@
 *(.text._ZN24ManualVelocitySmoothingZC1Ev) /* itcm-check-ignore */
 *(.text._ZN3ADC6sampleEj)
 *(.text._ZNK3Ekf22isTerrainEstimateValidEv)
-*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report)
+*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK14vehicle_gnss_sR16failsafe_flags_sR6Report)
+*(.text._ZN15EstimatorChecks29warnOfImminentPositionFailureER6ReportRKyRK25vehicle_global_position_sfRK16failsafe_flags_s)
 *(.text._ZN11ControlMath11addIfNotNanERff)
 *(.text._ZN9Commander21checkForMissionUpdateEv)
 *(.text._Z8set_tunei)
@@ -600,6 +601,12 @@
 *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_)
 *(.text._ZN13AnalogBattery19get_current_channelEv)
 *(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks25checkInnovationsPreflightERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks34checkMagneticInterferencePreflightERK7ContextR6ReportRK18estimator_status_s8NavModes)
+*(.text._ZN15EstimatorChecks15checkGnssFusionERK7ContextR6ReportRK18estimator_status_s)
+*(.text._ZN15EstimatorChecks22reportGnssFusionChangeERK7ContextR6Reportb)
+*(.text._ZN15EstimatorChecks22reportGnssInterferenceER6Reportt)
+*(.text._ZN15EstimatorChecks30reportFailedGnssCheckPreflightER6ReportRK18estimator_status_sb)
 *(.text._ZN12FailsafeBase11updateDelayERKy)
 *(.text._ZN10FlightTask25_evaluateDistanceToGroundEv)
 *(.text._ZN4EKF218PublishGnssHgtBiasERKy)

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -71,8 +71,19 @@ private:
 		WarningOnly = 2
 	};
 
+	// The checks on the estimator status: the preflight innovation and magnetic interference checks, and
+	// what the estimator makes of GNSS
 	void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
 				  NavModes required_groups);
+	void checkInnovationsPreflight(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
+				       NavModes required_groups);
+	void checkMagneticInterferencePreflight(const Context &context, Report &reporter,
+						const estimator_status_s &estimator_status, NavModes required_groups);
+	void checkGnssFusion(const Context &context, Report &reporter, const estimator_status_s &estimator_status);
+	void reportGnssFusionChange(const Context &context, Report &reporter, bool gnss_fused);
+	void reportGnssInterference(Report &reporter, uint16_t gps_check_fail_flags);
+	void reportFailedGnssCheckPreflight(Report &reporter, const estimator_status_s &estimator_status, bool gnss_fused);
+
 	void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups);
 	void checkEstimatorStatusFlags(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
 				       const vehicle_local_position_s &lpos);
@@ -84,10 +95,12 @@ private:
 	void reportGnssReasonForPositionLoss(const Context &context, Report &reporter, const hrt_abstime &now,
 					     const vehicle_gnss_s &vehicle_gnss) const;
 
-	void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
-				     bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz,
-				     const vehicle_local_position_s &lpos, const vehicle_gnss_s &vehicle_gnss,
-				     failsafe_flags_s &failsafe_flags, Report &reporter);
+	// The mode requirement flags, with the warning of an imminent position failure on its own
+	void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_vel_horiz,
+				     bool pre_flt_fail_innov_pos_horiz, const vehicle_local_position_s &lpos,
+				     const vehicle_gnss_s &vehicle_gnss, failsafe_flags_s &failsafe_flags, Report &reporter);
+	void warnOfImminentPositionFailure(Report &reporter, const hrt_abstime &now, const vehicle_global_position_s &gpos,
+					   float lpos_eph_threshold, const failsafe_flags_s &failsafe_flags);
 
 	bool checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy,
 				 const float required_accuracy,