ekf2/commander: simplify navigation filter preflight checks

 - remove commander test ratio "tuning knobs" (COM_ARM_EKF_{HGT,POS,VEL,YAW})
   - these are effectively redundant with the actual tuning (noise & gate)
     in the estimator, plus most users have no idea why they'd be
     adjusting these other than to silence an annoying preflight complaint
 - remove ekf2 "PreFlightChecker" with hard coded innovation limits
 - ekf2 preflight innovation flags are now simply if any active source
   exceeds half the limit preflight

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

@@ -335,7 +335,6 @@
 *(.text._ZThn16_N7sensors22VehicleAngularVelocity3RunEv)
 *(.text._ZN29MavlinkStreamObstacleDistance4sendEv)
 *(.text._ZN24MavlinkStreamOrbitStatus4sendEv)
-*(.text._ZN16PreFlightChecker26preFlightCheckHeightFailedERK23estimator_innovations_sf)
 *(.text._ZN9Navigator3runEv)
 *(.text._ZN24MavlinkParametersManager11send_paramsEv)
 *(.text._ZN17MavlinkLogHandler4sendEv)
@@ -374,7 +373,6 @@
 *(.text.imxrt_ioctl)
 *(.text._ZN3Ekf25checkMagBiasObservabilityEv)
 *(.text._ZN36MavlinkStreamGimbalDeviceSetAttitude4sendEv)
-*(.text._ZN16PreFlightChecker6updateEfRK23estimator_innovations_s)
 *(.text._ZN4math13expo_deadzoneIfEEKT_RS2_S3_S3_.isra.0)
 *(.text._ZN19StickAccelerationXYC1EP12ModuleParams)
 *(.text.imxrt_epsubmit)
@@ -458,7 +456,6 @@
 *(.text._ZN14FlightTaskAuto17_evaluateTripletsEv)
 *(.text._ZN11calibration9Gyroscope23SensorCorrectionsUpdateEb)
 *(.text._ZN25MavlinkStreamMagCalReport4sendEv)
-*(.text._ZN16PreFlightChecker27preFlightCheckHeadingFailedERK23estimator_innovations_sf)
 *(.text.imxrt_config_gpio)
 *(.text.nxsig_timeout)
 *(.text._ZN11RateControl19setSaturationStatusERKN6matrix7Vector3IbEES4_)
@@ -593,7 +590,6 @@
 *(.text.uart_xmitchars_done)
 *(.text._ZN4EKF225PublishYawEstimatorStatusERKy)
 *(.text.sin)
-*(.text._ZN16PreFlightChecker27preFlightCheckVertVelFailedERK23estimator_innovations_sf)
 *(.text._ZN6Safety19safetyButtonHandlerEv)
 *(.text._ZN3Ekf19controlAuxVelFusionEv)
 *(.text._ZNK6matrix6MatrixIfLj2ELj1EEplERKS1_)
@@ -667,10 +663,8 @@
 *(.text._ZN26MavlinkStreamCameraTrigger8get_sizeEv)
 *(.text.iob_navail)
 *(.text._ZN12FailsafeBase25removeNonActivatedActionsEv)
-*(.text._ZN16PreFlightChecker28preFlightCheckHorizVelFailedERK23estimator_innovations_sf)
 *(.text._ZN15TakeoffHandling10updateRampEff)
 *(.text._Z7led_offi)
-*(.text._ZN16PreFlightChecker22selectHeadingTestLimitEv)
 *(.text.led_off)
 *(.text.udp_wrbuffer_test)
 *(.text._ZNK3Ekf34updateVerticalPositionAidSrcStatusERKyfffR24estimator_aid_source1d_s)

msg/EstimatorStatus.msg

@@ -102,9 +102,10 @@ uint8 reset_count_quat   # number of quaternion reset events (allow to wrap if c
 float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time
 
 bool pre_flt_fail_innov_heading
+bool pre_flt_fail_innov_height
+bool pre_flt_fail_innov_pos_horiz
 bool pre_flt_fail_innov_vel_horiz
 bool pre_flt_fail_innov_vel_vert
-bool pre_flt_fail_innov_height
 bool pre_flt_fail_mag_field_disturbed
 
 uint32 accel_device_id

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

@@ -63,6 +63,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 
 	bool pre_flt_fail_innov_heading = false;
 	bool pre_flt_fail_innov_vel_horiz = false;
+	bool pre_flt_fail_innov_pos_horiz = false;
 	bool missing_data = false;
 	const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude;
 
@@ -90,6 +91,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 		if (_estimator_status_sub.copy(&estimator_status)) {
 			pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading;
 			pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz;
+			pre_flt_fail_innov_pos_horiz = estimator_status.pre_flt_fail_innov_pos_horiz;
 
 			checkEstimatorStatus(context, reporter, estimator_status, required_groups);
 			checkEstimatorStatusFlags(context, reporter, estimator_status, lpos);
@@ -123,7 +125,8 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 	}
 
 	// set mode requirements
-	setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position,
+	setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, pre_flt_fail_innov_pos_horiz,
+				lpos, vehicle_gps_position,
 				reporter.failsafeFlags(), reporter);
 
 
@@ -166,6 +169,17 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity unstable");
 		}
 
+	} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_pos_horiz) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_hor_pos_not_stable"),
+					    events::Log::Error, "Horizontal position unstable");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal position unstable");
+		}
+
 	} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_height) {
 		/* EVENT
 		 */
@@ -208,82 +222,6 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 		}
 	}
 
-	// check vertical position innovation test ratio
-	if (!context.isArmed() && (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get())) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_HGT</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_hgt_est_err"),
-				events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error");
-		}
-	}
-
-	// check velocity innovation test ratio
-	if (!context.isArmed() && (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get())) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_VEL</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_vel_est_err"),
-				events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error");
-		}
-	}
-
-	// check horizontal position innovation test ratio
-	if (!context.isArmed() && (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get())) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_POS</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_pos_est_err"),
-				events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error");
-		}
-	}
-
-	// check magnetometer innovation test ratio
-	if (!context.isArmed() && (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get())) {
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * Test ratio: {1:.3}, limit: {2:.3}.
-		 *
-		 * This check can be configured via <param>COM_ARM_EKF_YAW</param> parameter.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
-				events::ID("check_estimator_yaw_est_err"),
-				events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get());
-
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error");
-		}
-	}
-
 	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
 	if (_param_sys_has_gps.get()) {
 		const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
@@ -767,7 +705,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
 }
 
 void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
-		bool pre_flt_fail_innov_vel_horiz,
+		bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz,
 		const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags,
 		Report &reporter)
 {
@@ -797,7 +735,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 	bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
 
 	if (!context.isArmed()) {
-		if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) {
+		if (pre_flt_fail_innov_heading || pre_flt_fail_innov_pos_horiz) {
 			xy_valid = false;
 		}
 

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

@@ -66,7 +66,9 @@ class EstimatorChecks : public HealthAndArmingCheckBase
 				       const vehicle_local_position_s &lpos);
 	void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
 	void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const;
-	void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
+
+	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 sensor_gps_s &vehicle_gps_position,
 				     failsafe_flags_s &failsafe_flags, Report &reporter);
 
@@ -108,10 +110,6 @@ class EstimatorChecks : public HealthAndArmingCheckBase
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
 					(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
-					(ParamFloat<px4::params::COM_ARM_EKF_HGT>) _param_com_arm_ekf_hgt,
-					(ParamFloat<px4::params::COM_ARM_EKF_VEL>) _param_com_arm_ekf_vel,
-					(ParamFloat<px4::params::COM_ARM_EKF_POS>) _param_com_arm_ekf_pos,
-					(ParamFloat<px4::params::COM_ARM_EKF_YAW>) _param_com_arm_ekf_yaw,
 					(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
 					(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 					(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,

src/modules/commander/commander_params.c

@@ -358,50 +358,6 @@ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
  */
 PARAM_DEFINE_FLOAT(COM_OBC_LOSS_T, 5.0f);
 
-/**
- * Maximum EKF position innovation test ratio that will allow arming
- *
- * @group Commander
- * @min 0.1
- * @max 1.0
- * @decimal 2
- * @increment 0.05
- */
-PARAM_DEFINE_FLOAT(COM_ARM_EKF_POS, 0.5f);
-
-/**
- * Maximum EKF velocity innovation test ratio that will allow arming
- *
- * @group Commander
- * @min 0.1
- * @max 1.0
- * @decimal 2
- * @increment 0.05
- */
-PARAM_DEFINE_FLOAT(COM_ARM_EKF_VEL, 0.5f);
-
-/**
- * Maximum EKF height innovation test ratio that will allow arming
- *
- * @group Commander
- * @min 0.1
- * @max 1.0
- * @decimal 2
- * @increment 0.05
- */
-PARAM_DEFINE_FLOAT(COM_ARM_EKF_HGT, 1.0f);
-
-/**
- * Maximum EKF yaw innovation test ratio that will allow arming
- *
- * @group Commander
- * @min 0.1
- * @max 1.0
- * @decimal 2
- * @increment 0.05
- */
-PARAM_DEFINE_FLOAT(COM_ARM_EKF_YAW, 0.5f);
-
 /**
  * Maximum accelerometer inconsistency between IMU units that will allow arming
  *

src/modules/ekf2/CMakeLists.txt

@@ -31,8 +31,6 @@
 #
 #############################################################################
 
-add_subdirectory(Utility)
-
 option(EKF2_SYMFORCE_GEN "ekf2 generate symforce output" OFF)
 
 # Symforce code generation TODO:fixme
@@ -254,7 +252,6 @@ px4_add_module(
 		geo
 		hysteresis
 		perf
-		EKF2Utility
 		px4_work_queue
 		world_magnetic_model
 

src/modules/ekf2/EKF/ekf.h

@@ -396,7 +396,8 @@ class Ekf final : public EstimatorInterface
 
 	float getHeadingInnovationTestRatio() const;
 
-	float getVelocityInnovationTestRatio() const;
+	float getHorizontalVelocityInnovationTestRatio() const;
+	float getVerticalVelocityInnovationTestRatio() const;
 
 	float getHorizontalPositionInnovationTestRatio() const;
 	float getVerticalPositionInnovationTestRatio() const;