ekf2: push preflight filtered innovation checks into backend

- move flags to EstimatorStatusFlags and update commander HealthAndArmingChecks usage

msg/EstimatorStatus.msg

@@ -113,12 +113,6 @@ 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_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
 uint32 gyro_device_id
 uint32 baro_device_id

msg/EstimatorStatusFlags.msg

@@ -73,3 +73,10 @@ bool reject_sideslip                      #  9 - true if the synthetic sideslip
 bool reject_hagl                          # 10 - true if the height above ground observation has been rejected
 bool reject_optflow_x                     # 11 - true if the X optical flow observation has been rejected
 bool reject_optflow_y                     # 12 - true if the Y optical flow observation has been rejected
+
+# preflight failure checks
+bool pre_flt_fail_innov_heading
+bool pre_flt_fail_innov_vel_horiz
+bool pre_flt_fail_innov_vel_vert
+bool pre_flt_fail_innov_pos_horiz
+bool pre_flt_fail_innov_height

src/lib/mathlib/math/filter/AlphaFilter.hpp

@@ -52,7 +52,7 @@ class AlphaFilter
 {
 public:
 	AlphaFilter() = default;
-	explicit AlphaFilter(float alpha) : _alpha(alpha) {}
+	AlphaFilter(float alpha) : _alpha(alpha) {}
 
 	~AlphaFilter() = default;
 
@@ -99,7 +99,7 @@ public:
 	 *
 	 * @param sample new initial value
 	 */
-	void reset(const T &sample) { _filter_state = sample; }
+	void reset(const T &sample = {}) { _filter_state = sample; }
 
 	/**
 	 * Add a new raw value to the filter

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

@@ -88,14 +88,14 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 	}
 
 	if (!missing_data) {
-		estimator_status_s estimator_status;
+		estimator_status_flags_s estimator_status_flags;
 
-		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;
+		if (_estimator_status_flags_sub.copy(&estimator_status_flags)) {
+			pre_flt_fail_innov_heading = estimator_status_flags.pre_flt_fail_innov_heading;
+			pre_flt_fail_innov_vel_horiz = estimator_status_flags.pre_flt_fail_innov_vel_horiz;
 
-			checkEstimatorStatus(context, reporter, estimator_status, required_groups);
-			checkEstimatorStatusFlags(context, reporter, estimator_status, lpos);
+			checkEstimatorStatus(context, reporter, lpos, required_groups);
+			checkEstimatorStatusFlags(context, reporter, estimator_status_flags, required_groups);
 
 		} else {
 			missing_data = true;
@@ -129,327 +129,261 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 }
 
 void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter,
-		const estimator_status_s &estimator_status, NavModes required_groups)
+		const vehicle_local_position_s &lpos, NavModes required_groups)
 {
-	if (!context.isArmed() && estimator_status.pre_flt_fail_innov_heading) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_heading_not_stable"),
-					    events::Log::Error, "Heading estimate not stable");
+	estimator_status_s estimator_status;
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate not stable");
+	if (_estimator_status_sub.copy(&estimator_status)) {
+
+		// 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");
+			}
 		}
 
-	} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_vel_horiz) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_hor_vel_not_stable"),
-					    events::Log::Error, "Horizontal velocity unstable");
+		// 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: horizontal velocity unstable");
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error");
+			}
 		}
 
-	} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_vel_vert) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_vert_vel_not_stable"),
-					    events::Log::Error, "Vertical velocity unstable");
+		// 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: vertical velocity unstable");
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error");
+			}
 		}
 
-	} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_height) {
-		/* EVENT
-		 */
-		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_hgt_not_stable"),
-					    events::Log::Error, "Height estimate not stable");
+		// 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: height estimate not stable");
-		}
-	}
-
-
-	if ((_param_com_arm_mag_str.get() >= 1)
-	    && (!context.isArmed() && estimator_status.pre_flt_fail_mag_field_disturbed)) {
-
-		NavModes required_groups_mag = required_groups;
-
-		if (_param_com_arm_mag_str.get() != 1) {
-			required_groups_mag = NavModes::None; // optional
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error");
+			}
 		}
 
-		/* EVENT
-		 * @description
-		 * <profile name="dev">
-		 * This check can be configured via <param>COM_ARM_MAG_STR</param> and <param>EKF2_MAG_CHECK</param> parameters.
-		 * </profile>
-		 */
-		reporter.armingCheckFailure(required_groups_mag, health_component_t::local_position_estimate,
-					    events::ID("check_estimator_mag_interference"),
-					    events::Log::Warning, "Strong magnetic interference");
+		// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
+		if (!context.isArmed() && _param_sys_has_gps.get()) {
+			const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
+			const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;
 
-		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference");
-		}
-	}
-
-	// 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 (!context.isArmed() && _param_sys_has_gps.get()) {
-		const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
-		const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;
-
-		if (ekf_gps_fusion) {
-			reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly
-		}
-
-		if (ekf_gps_check_fail) {
-			NavModes required_groups_gps = required_groups;
-			events::Log log_level = events::Log::Error;
-
-			if (_param_com_arm_wo_gps.get()) {
-				required_groups_gps = NavModes::None; // optional
-				log_level = events::Log::Warning;
+			if (ekf_gps_fusion) {
+				reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly
 			}
 
-			// Only report the first failure to avoid spamming
-			const char *message = nullptr;
+			if (ekf_gps_check_fail) {
+				NavModes required_groups_gps = required_groups;
+				events::Log log_level = events::Log::Error;
 
-			if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
-				message = "Preflight%s: GPS fix too low";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_fix_too_low"),
-							    log_level, "GPS fix too low");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
-				message = "Preflight%s: not enough GPS Satellites";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_num_sats_too_low"),
-							    log_level, "Not enough GPS Satellites");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) {
-				message = "Preflight%s: GPS PDOP too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_pdop_too_high"),
-							    log_level, "GPS PDOP too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
-				message = "Preflight%s: GPS Horizontal Pos Error too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_hor_pos_err_too_high"),
-							    log_level, "GPS Horizontal Position Error too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
-				message = "Preflight%s: GPS Vertical Pos Error too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_vert_pos_err_too_high"),
-							    log_level, "GPS Vertical Position Error too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
-				message = "Preflight%s: GPS Speed Accuracy too low";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_speed_acc_too_low"),
-							    log_level, "GPS Speed Accuracy too low");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
-				message = "Preflight%s: GPS Horizontal Pos Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_hor_pos_drift_too_high"),
-							    log_level, "GPS Horizontal Position Drift too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
-				message = "Preflight%s: GPS Vertical Pos Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_vert_pos_drift_too_high"),
-							    log_level, "GPS Vertical Position Drift too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
-				message = "Preflight%s: GPS Hor Speed Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_hor_speed_drift_too_high"),
-							    log_level, "GPS Horizontal Speed Drift too high");
-
-			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
-				message = "Preflight%s: GPS Vert Speed Drift too high";
-				/* EVENT
-				 * @description
-				 * <profile name="dev">
-				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
-				 * </profile>
-				 */
-				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-							    events::ID("check_estimator_gps_vert_speed_drift_too_high"),
-							    log_level, "GPS Vertical Speed Drift too high");
-
-			} else {
-				if (!ekf_gps_fusion) {
-					// Likely cause unknown
-					message = "Preflight%s: Estimator not using GPS";
-					/* EVENT
-					 */
-					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-								    events::ID("check_estimator_gps_not_fusing"),
-								    log_level, "Estimator not using GPS");
-
-				} else {
-					// if we land here there was a new flag added and the code not updated. Show a generic message.
-					message = "Preflight%s: Poor GPS Quality";
-					/* EVENT
-					 */
-					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
-								    events::ID("check_estimator_gps_generic"),
-								    log_level, "Poor GPS Quality");
+				if (_param_com_arm_wo_gps.get()) {
+					required_groups_gps = NavModes::None; // optional
+					log_level = events::Log::Warning;
 				}
-			}
 
-			if (message && reporter.mavlink_log_pub()) {
-				if (!_param_com_arm_wo_gps.get()) {
-					mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
+				// Only report the first failure to avoid spamming
+				const char *message = nullptr;
+
+				if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
+					message = "Preflight%s: GPS fix too low";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_fix_too_low"),
+								    log_level, "GPS fix too low");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
+					message = "Preflight%s: not enough GPS Satellites";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_num_sats_too_low"),
+								    log_level, "Not enough GPS Satellites");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) {
+					message = "Preflight%s: GPS PDOP too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_pdop_too_high"),
+								    log_level, "GPS PDOP too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
+					message = "Preflight%s: GPS Horizontal Pos Error too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_hor_pos_err_too_high"),
+								    log_level, "GPS Horizontal Position Error too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
+					message = "Preflight%s: GPS Vertical Pos Error too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_vert_pos_err_too_high"),
+								    log_level, "GPS Vertical Position Error too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
+					message = "Preflight%s: GPS Speed Accuracy too low";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_speed_acc_too_low"),
+								    log_level, "GPS Speed Accuracy too low");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
+					message = "Preflight%s: GPS Horizontal Pos Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_hor_pos_drift_too_high"),
+								    log_level, "GPS Horizontal Position Drift too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
+					message = "Preflight%s: GPS Vertical Pos Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_vert_pos_drift_too_high"),
+								    log_level, "GPS Vertical Position Drift too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
+					message = "Preflight%s: GPS Hor Speed Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_hor_speed_drift_too_high"),
+								    log_level, "GPS Horizontal Speed Drift too high");
+
+				} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
+					message = "Preflight%s: GPS Vert Speed Drift too high";
+					/* EVENT
+					 * @description
+					 * <profile name="dev">
+					 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+					 * </profile>
+					 */
+					reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+								    events::ID("check_estimator_gps_vert_speed_drift_too_high"),
+								    log_level, "GPS Vertical Speed Drift too high");
 
 				} else {
-					mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
+					if (!ekf_gps_fusion) {
+						// Likely cause unknown
+						message = "Preflight%s: Estimator not using GPS";
+						/* EVENT
+						 */
+						reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+									    events::ID("check_estimator_gps_not_fusing"),
+									    log_level, "Estimator not using GPS");
+
+					} else {
+						// if we land here there was a new flag added and the code not updated. Show a generic message.
+						message = "Preflight%s: Poor GPS Quality";
+						/* EVENT
+						 */
+						reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+									    events::ID("check_estimator_gps_generic"),
+									    log_level, "Poor GPS Quality");
+					}
+				}
+
+				if (message && reporter.mavlink_log_pub()) {
+					if (!_param_com_arm_wo_gps.get()) {
+						mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
+
+					} else {
+						mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
+					}
 				}
 			}
 		}
 	}
-
 }
 
 void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, NavModes required_groups)
@@ -530,111 +464,117 @@ void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter,
 }
 
 void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &reporter,
-		const estimator_status_s &estimator_status, const vehicle_local_position_s &lpos)
+		const estimator_status_flags_s &estimator_status_flags, NavModes required_groups)
 {
-	estimator_status_flags_s estimator_status_flags;
+	if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_heading) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_heading_not_stable"),
+					    events::Log::Error, "Heading estimate not stable");
 
-	if (_estimator_status_flags_sub.copy(&estimator_status_flags)) {
-
-		bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
-				      || estimator_status_flags.cs_inertial_dead_reckoning;
-
-		if (!dead_reckoning) {
-			// position requirements (update if not dead reckoning)
-			bool gps             = estimator_status_flags.cs_gps;
-			bool optical_flow    = estimator_status_flags.cs_opt_flow;
-			bool vision_position = estimator_status_flags.cs_ev_pos;
-
-			_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate not stable");
 		}
 
-		// Check for a magnetomer fault and notify the user
-		if (estimator_status_flags.cs_mag_fault) {
-			/* EVENT
-			 * @description
-			 * Land and calibrate the compass.
-			 */
-			reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
-						    events::ID("check_estimator_mag_fault"),
-						    events::Log::Critical, "Stopping compass use");
+	} else if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_vel_horiz) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_hor_vel_not_stable"),
+					    events::Log::Error, "Horizontal velocity unstable");
 
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t");
-			}
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal velocity unstable");
 		}
 
-		if (estimator_status_flags.cs_gps_yaw_fault) {
-			/* EVENT
-			 * @description
-			 * Land now
-			 */
-			reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
-						    events::ID("check_estimator_gnss_fault"),
-						    events::Log::Critical, "GNSS heading not reliable");
+	} else if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_vel_vert) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_vert_vel_not_stable"),
+					    events::Log::Error, "Vertical velocity unstable");
 
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS heading not reliable - Land now!\t");
-			}
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity unstable");
+		}
+
+	} else if (!context.isArmed() && estimator_status_flags.pre_flt_fail_innov_height) {
+		/* EVENT
+		 */
+		reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_hgt_not_stable"),
+					    events::Log::Error, "Height estimate not stable");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate not stable");
 		}
 	}
 
-	const hrt_abstime now = hrt_absolute_time();
 
-	/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
-	* for a short time interval after takeoff.
-	* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
-	* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
-	* if this does not fix the issue we need to stop using a position controlled
-	* mode to prevent flyaway crashes.
-	*/
+	if ((_param_com_arm_mag_str.get() >= 1)
+	    && (!context.isArmed() && estimator_status_flags.cs_mag_field_disturbed)) {
 
-	if (context.status().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		NavModes required_groups_mag = required_groups;
 
-		if (!context.isArmed()) {
-			_nav_test_failed = false;
-			_nav_test_passed = false;
+		if (_param_com_arm_mag_str.get() != 1) {
+			required_groups_mag = NavModes::None; // optional
+		}
 
-		} else {
-			if (!_nav_test_passed) {
-				// Both test ratios need to pass/fail together to change the nav test status
-				const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
-							     && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>COM_ARM_MAG_STR</param> and <param>EKF2_MAG_CHECK</param> parameters.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(required_groups_mag, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_mag_interference"),
+					    events::Log::Warning, "Strong magnetic interference");
 
-				const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference");
+		}
+	}
 
-				if (innovation_pass) {
-					_time_last_innov_pass = now;
 
-					// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
-					const bool sufficient_time = (context.status().takeoff_time != 0) && (now > context.status().takeoff_time + 30_s);
-					const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
+	bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
+			      || estimator_status_flags.cs_inertial_dead_reckoning;
 
-					// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
-					if ((now > _time_last_innov_fail + 10_s) && (sufficient_time || sufficient_speed)) {
-						_nav_test_passed = true;
-						_nav_test_failed = false;
-					}
+	if (!dead_reckoning) {
+		// position requirements (update if not dead reckoning)
+		bool gps             = estimator_status_flags.cs_gps;
+		bool optical_flow    = estimator_status_flags.cs_opt_flow;
+		bool vision_position = estimator_status_flags.cs_ev_pos;
 
-				} else if (innovation_fail) {
-					_time_last_innov_fail = now;
+		_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
+	}
 
-					if (now > _time_last_innov_pass + 2_s) {
-						// if the innovation test has failed continuously, declare the nav as failed
-						_nav_test_failed = true;
-						/* EVENT
-						 * @description
-						 * Land and recalibrate the sensors.
-						 */
-						reporter.healthFailure(NavModes::All, health_component_t::local_position_estimate,
-								       events::ID("check_estimator_nav_failure"),
-								       events::Log::Emergency, "Navigation failure");
+	// Check for a magnetomer fault and notify the user
+	if (estimator_status_flags.cs_mag_fault) {
+		/* EVENT
+		 * @description
+		 * Land and calibrate the compass.
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_mag_fault"),
+					    events::Log::Critical, "Stopping compass use");
 
-						if (reporter.mavlink_log_pub()) {
-							mavlink_log_critical(reporter.mavlink_log_pub(), "Navigation failure! Land and recalibrate sensors\t");
-						}
-					}
-				}
-			}
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t");
+		}
+	}
+
+	if (estimator_status_flags.cs_gps_yaw_fault) {
+		/* EVENT
+		 * @description
+		 * Land now
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
+					    events::ID("check_estimator_gnss_fault"),
+					    events::Log::Critical, "GNSS heading not reliable");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS heading not reliable - Land now!\t");
 		}
 	}
 }
@@ -693,8 +633,8 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 		lpos_eph_threshold_relaxed = INFINITY;
 	}
 
-	bool xy_valid = lpos.xy_valid && !_nav_test_failed;
-	bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
+	bool xy_valid = lpos.xy_valid;
+	bool v_xy_valid = lpos.v_xy_valid;
 
 	if (!context.isArmed()) {
 		if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) {

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

@@ -59,11 +59,11 @@ public:
 	void checkAndReport(const Context &context, Report &reporter) override;
 
 private:
-	void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
+	void checkEstimatorStatus(const Context &context, Report &reporter, const vehicle_local_position_s &lpos,
 				  NavModes required_groups);
 	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);
+	void checkEstimatorStatusFlags(const Context &context, Report &reporter,
+				       const estimator_status_flags_s &estimator_status_flags, NavModes required_groups);
 
 	void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
 	void gpsNoLongerValid(const Context &context, Report &reporter) const;
@@ -93,12 +93,6 @@ private:
 	hrt_abstime	_last_lpos_relaxed_fail_time_us{0};	///< Last time that the relaxed local position validity recovery check failed (usec)
 	hrt_abstime	_last_lvel_fail_time_us{0};	///< Last time that the local velocity validity recovery check failed (usec)
 
-	// variables used to check for navigation failure after takeoff
-	hrt_abstime	_time_last_innov_pass{0};	///< last time velocity and position innovations passed
-	hrt_abstime	_time_last_innov_fail{0};	///< last time velocity and position innovations failed
-	bool		_nav_test_passed{false};	///< true if the post takeoff navigation test has passed
-	bool		_nav_test_failed{false};	///< true if the post takeoff navigation test has failed
-
 	static constexpr hrt_abstime GPS_VALID_TIME{3_s};
 	systemlib::Hysteresis _vehicle_gps_position_valid{false};
 

src/modules/ekf2/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,8 +31,6 @@
 #
 #############################################################################
 
-add_subdirectory(Utility)
-
 # Symforce code generation TODO:fixme
 # execute_process(
 #     COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic
@@ -126,7 +124,6 @@ px4_add_module(
 		geo
 		hysteresis
 		perf
-		EKF2Utility
 		px4_work_queue
 		world_magnetic_model
 	UNITY_BUILD

src/modules/ekf2/EKF/baro_height_control.cpp

@@ -80,6 +80,11 @@ void Ekf::controlBaroHeightFusion()
 						   innov_gate,
 						   aid_src);
 
+		// filtered innovation for preflight checks
+		if (!aid_src.innovation_rejected) {
+			_baro_hgt_innov_lpf.update(aid_src.innovation);
+		}
+
 		// Compensate for positive static pressure transients (negative vertical position innovations)
 		// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
 		if (_control_status.flags.gnd_effect && (_params.gnd_effect_deadzone > 0.f)) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -117,6 +117,25 @@ public:
 			_time_last_in_air = _imu_sample_delayed.time_us;
 		}
 
+		// reset all preflight innovation filters on takeoff or landing
+		if (_control_status.flags.in_air != in_air) {
+			_baro_hgt_innov_lpf.reset();
+			_gnss_hgt_innov_lpf.reset();
+			_rng_hgt_innov_lpf.reset();
+			_ev_hgt_innov_lpf.reset();
+
+			_mag_heading_innov_lpf.reset();
+			_gnss_yaw_innov_lpf.reset();
+			_ev_yaw_innov_lpf.reset();
+
+			_ev_pos_innov_lpf.reset();
+			_gnss_pos_innov_lpf.reset();
+
+			_gnss_vel_innov_lpf.reset();
+			_ev_vel_innov_lpf.reset();
+			_optical_flow_vel_innov_lpf.reset();
+		}
+
 		_control_status.flags.in_air = in_air;
 	}
 
@@ -268,6 +287,97 @@ public:
 	float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
 	float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
 
+	float filteredHorizontalVelocityInnovation() const
+	{
+		float innov = 0.f;
+
+		if (_control_status.flags.gps) {
+			innov = math::max(innov, _gnss_vel_innov_lpf.getState().xy().norm());
+		}
+
+		if (_control_status.flags.ev_vel) {
+			innov = math::max(innov, _ev_vel_innov_lpf.getState().xy().norm());
+		}
+
+		if (_control_status.flags.opt_flow) {
+			innov = math::max(innov, _optical_flow_vel_innov_lpf.getState().norm());
+		}
+
+		return innov;
+	}
+
+	float filteredVerticalVelocityInnovation() const
+	{
+		float innov = 0.f;
+
+		if (_control_status.flags.gps) {
+			innov = math::max(innov, _gnss_vel_innov_lpf.getState()(2));
+		}
+
+		if (_control_status.flags.ev_pos) {
+			innov = math::max(innov, _ev_vel_innov_lpf.getState()(2));
+		}
+
+		return innov;
+	}
+
+	float filteredHorizontalPositionInnovation() const
+	{
+		float innov = 0.f;
+
+		if (_control_status.flags.gps) {
+			innov = math::max(innov, _gnss_pos_innov_lpf.getState().norm());
+		}
+
+		if (_control_status.flags.ev_pos) {
+			innov = math::max(innov, _ev_pos_innov_lpf.getState().norm());
+		}
+
+		return innov;
+	}
+
+	float filteredVerticalPositionInnovation() const
+	{
+		float innov = 0.f;
+
+		if (_control_status.flags.baro_hgt) {
+			innov = math::max(innov, _baro_hgt_innov_lpf.getState());
+		}
+
+		if (_control_status.flags.gps_hgt) {
+			innov = math::max(innov, _gnss_hgt_innov_lpf.getState());
+		}
+
+		if (_control_status.flags.ev_hgt) {
+			innov = math::max(innov, _ev_hgt_innov_lpf.getState());
+		}
+
+		if (_control_status.flags.rng_hgt) {
+			innov = math::max(innov, _rng_hgt_innov_lpf.getState());
+		}
+
+		return innov;
+	}
+
+	float filteredHeadingInnovation() const
+	{
+		float innov = 0.f;
+
+		if (_control_status.flags.mag_hdg) {
+			innov = math::max(innov, _mag_heading_innov_lpf.getState());
+		}
+
+		if (_control_status.flags.ev_yaw) {
+			innov = math::max(innov, _ev_yaw_innov_lpf.getState());
+		}
+
+		if (_control_status.flags.gps_yaw) {
+			innov = math::max(innov, _gnss_yaw_innov_lpf.getState());
+		}
+
+		return innov;
+	}
+
 protected:
 
 	EstimatorInterface() = default;
@@ -399,6 +509,25 @@ protected:
 	warning_event_status_u _warning_events{};
 	information_event_status_u _information_events{};
 
+	// Preflight low pass filter time constant inverse (1/sec)
+	static constexpr float INNOV_LPF_TAU_INV = 0.2f;
+
+	AlphaFilter<float>    _baro_hgt_innov_lpf{INNOV_LPF_TAU_INV};       ///< filtered height innovations (m)
+	AlphaFilter<float>    _gnss_hgt_innov_lpf{INNOV_LPF_TAU_INV};       ///< filtered height innovations (m)
+	AlphaFilter<float>    _rng_hgt_innov_lpf{INNOV_LPF_TAU_INV};        ///< filtered height innovations (m)
+	AlphaFilter<float>    _ev_hgt_innov_lpf{INNOV_LPF_TAU_INV};         ///< filtered height innovations (m)
+
+	AlphaFilter<float>    _mag_heading_innov_lpf{INNOV_LPF_TAU_INV};    ///< filtered heading innovations (rad)
+	AlphaFilter<float>    _gnss_yaw_innov_lpf{INNOV_LPF_TAU_INV};       ///< filtered yaw innovations (rad)
+	AlphaFilter<float>    _ev_yaw_innov_lpf{INNOV_LPF_TAU_INV};         ///< filtered yaw innovations (rad)
+
+	AlphaFilter<Vector2f> _ev_pos_innov_lpf{INNOV_LPF_TAU_INV};           ///< filtered position innovations (m)
+	AlphaFilter<Vector2f> _gnss_pos_innov_lpf{INNOV_LPF_TAU_INV};         ///< filtered position innovations (m)
+
+	AlphaFilter<Vector3f> _ev_vel_innov_lpf{INNOV_LPF_TAU_INV};           ///< filtered velocity innovations (m/s)
+	AlphaFilter<Vector3f> _gnss_vel_innov_lpf{INNOV_LPF_TAU_INV};         ///< filtered velocity innovations (m/s)
+	AlphaFilter<Vector2f> _optical_flow_vel_innov_lpf{INNOV_LPF_TAU_INV}; ///< filtered velocity innovations (m/s)
+
 private:
 
 	inline void setDragData(const imuSample &imu);

src/modules/ekf2/EKF/ev_height_control.cpp

@@ -90,6 +90,11 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 					   math::max(_params.ev_pos_innov_gate, 1.f),
 					   aid_src);
 
+	// filtered innovation for preflight checks
+	if (!aid_src.innovation_rejected) {
+		_ev_hgt_innov_lpf.update(aid_src.innovation);
+	}
+
 	// update the bias estimator before updating the main filter but after
 	// using its current state to compute the vertical position innovation
 	if (measurement_valid && quality_sufficient) {

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -160,6 +160,11 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 					     math::max(_params.ev_pos_innov_gate, 1.f),    // innovation gate
 					     aid_src);
 
+	// filtered innovation for preflight checks
+	if (!aid_src.innovation_rejected) {
+		_ev_pos_innov_lpf.update(Vector2f(aid_src.innovation));
+	}
+
 	// update the bias estimator before updating the main filter but after
 	// using its current state to compute the vertical position innovation
 	if (measurement_valid && quality_sufficient) {

src/modules/ekf2/EKF/ev_vel_control.cpp

@@ -130,6 +130,11 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 				   math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate
 				   aid_src);
 
+	// filtered innovation for preflight checks
+	if (!aid_src.innovation_rejected) {
+		_ev_vel_innov_lpf.update(Vector3f(aid_src.innovation));
+	}
+
 	if (!measurement_valid) {
 		continuing_conditions_passing = false;
 	}

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -67,6 +67,11 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 					     - wrap_pi(getEulerYaw(ev_sample.quat) - getEulerYaw(_ev_sample_prev.quat)));
 	}
 
+	// filtered innovation for preflight checks
+	if (!aid_src.innovation_rejected) {
+		_ev_yaw_innov_lpf.update(aid_src.innovation);
+	}
+
 	const bool starting_conditions_passing = common_starting_conditions_passing
 			&& continuing_conditions_passing
 			&& isTimedOut(aid_src.time_last_fuse, (uint32_t)1e6);

src/modules/ekf2/EKF/gnss_height_control.cpp

@@ -74,6 +74,11 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 						   innov_gate,
 						   aid_src);
 
+		// filtered innovation for preflight checks
+		if (!aid_src.innovation_rejected) {
+			_gnss_hgt_innov_lpf.update(aid_src.innovation);
+		}
+
 		const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
 		const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
 

src/modules/ekf2/EKF/gps_control.cpp

@@ -67,6 +67,11 @@ void Ekf::controlGpsFusion()
 					   _aid_src_gnss_vel);
 		_aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
 
+		// filtered innovation for preflight checks
+		if (!_aid_src_gnss_vel.innovation_rejected) {
+			_gnss_vel_innov_lpf.update(Vector3f(_aid_src_gnss_vel.innovation));
+		}
+
 		// GNSS position
 		const Vector2f position{gps_sample.pos};
 		// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
@@ -89,6 +94,11 @@ void Ekf::controlGpsFusion()
 						     _aid_src_gnss_pos);
 		_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
 
+		// filtered innovation for preflight checks
+		if (!_aid_src_gnss_pos.innovation_rejected) {
+			_gnss_pos_innov_lpf.update(Vector2f(_aid_src_gnss_pos.innovation));
+		}
+
 		// if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start
 		if (_control_status.flags.tilt_align && _NED_origin_initialised
 		    && gps_checks_passing && !gps_checks_failing) {

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -139,6 +139,9 @@ void Ekf::fuseGpsYaw()
 	if (is_fused) {
 		_time_last_heading_fuse = _imu_sample_delayed.time_us;
 		gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
+
+		// filtered innovation for preflight checks
+		_gnss_yaw_innov_lpf.update(gnss_yaw.innovation);
 	}
 }
 

src/modules/ekf2/EKF/mag_control.cpp

@@ -430,6 +430,11 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 		_aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg;
 
 		fuseYaw(innovation, obs_var, _aid_src_mag_heading);
+
+		// filtered innovation for preflight checks
+		if (!_aid_src_mag_heading.innovation_rejected) {
+			_mag_heading_innov_lpf.update(_aid_src_mag_heading.innovation);
+		}
 	}
 }
 

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -115,6 +115,9 @@ void Ekf::fuseOptFlow()
 		return;
 	}
 
+	// filtered optical flow velocity innovation (for preflight checks)
+	_optical_flow_vel_innov_lpf.update(Vector2f(_state.vel(0) - _flow_vel_ne(0), _state.vel(1) - _flow_vel_ne(1)));
+
 	bool fused[2] {false, false};
 
 	// fuse observation axes sequentially

src/modules/ekf2/EKF/range_height_control.cpp

@@ -63,6 +63,11 @@ void Ekf::controlRangeHeightFusion()
 						   innov_gate,
 						   aid_src);
 
+		// filtered innovation for preflight checks
+		if (!aid_src.innovation_rejected) {
+			_rng_hgt_innov_lpf.update(aid_src.innovation);
+		}
+
 		// update the bias estimator before updating the main filter but after
 		// using its current state to compute the vertical position innovation
 		if (measurement_valid && _range_sensor.isDataHealthy()) {

src/modules/ekf2/EKF2.cpp

@@ -1067,29 +1067,6 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 
 	innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovations_pub.publish(innovations);
-
-	// calculate noise filtered velocity innovations which are used for pre-flight checking
-	if (_ekf.control_status_prev_flags().in_air != _ekf.control_status_flags().in_air) {
-		// fully reset on takeoff or landing
-		_preflt_checker.reset();
-	}
-
-	if (!_ekf.control_status_flags().in_air) {
-		// TODO: move to run before publications
-		_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
-		_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
-		_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
-		_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
-
-		_preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt);
-		_preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt);
-		_preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt);
-		_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);
-
-		_preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing);
-
-		_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);
-	}
 }
 
 void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
@@ -1389,12 +1366,6 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 
 	status.time_slip = _last_time_slip_us * 1e-6f;
 
-	status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
-	status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
-	status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
-	status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
-	status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
-
 	status.accel_device_id = _device_id_accel;
 	status.baro_device_id = _device_id_baro;
 	status.gyro_device_id = _device_id_gyro;
@@ -1430,6 +1401,30 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		_innov_check_fail_status_changes++;
 	}
 
+	if (!_ekf.control_status_flags().in_air) {
+
+		bool fail_vel_horiz = (_ekf.filteredHorizontalVelocityInnovation() > kVelocityInnovationTestLimit);
+		bool fail_vel_vert  = (_ekf.filteredVerticalVelocityInnovation() > kVelocityInnovationTestLimit);
+		bool fail_pos_horiz = (_ekf.filteredHorizontalPositionInnovation() > kPositionInnovationTestLimit);
+		bool fail_height    = (_ekf.filteredVerticalPositionInnovation() > kPositionInnovationTestLimit);
+		bool fail_heading   = (_ekf.filteredHeadingInnovation() > kHeadingInnovationTestLimit);
+
+		if (fail_vel_horiz    != _pre_flt_fail_innov_vel_horiz
+		    || fail_vel_vert  != _pre_flt_fail_innov_vel_vert
+		    || fail_pos_horiz != _pre_flt_fail_innov_pos_horiz
+		    || fail_height    != _pre_flt_fail_innov_height
+		    || fail_heading   != _pre_flt_fail_innov_heading
+		   ) {
+			_pre_flt_fail_innov_vel_horiz = fail_vel_horiz;
+			_pre_flt_fail_innov_vel_vert  = fail_vel_vert;
+			_pre_flt_fail_innov_pos_horiz = fail_pos_horiz;
+			_pre_flt_fail_innov_height    = fail_height;
+			_pre_flt_fail_innov_heading   = fail_heading;
+
+			update = true;
+		}
+	}
+
 	if (update) {
 		estimator_status_flags_s status_flags{};
 		status_flags.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
@@ -1502,6 +1497,12 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.reject_optflow_x                = _ekf.innov_check_fail_status_flags().reject_optflow_X;
 		status_flags.reject_optflow_y                = _ekf.innov_check_fail_status_flags().reject_optflow_Y;
 
+		status_flags.pre_flt_fail_innov_vel_horiz = _pre_flt_fail_innov_vel_horiz;
+		status_flags.pre_flt_fail_innov_vel_vert  = _pre_flt_fail_innov_vel_vert;
+		status_flags.pre_flt_fail_innov_pos_horiz = _pre_flt_fail_innov_pos_horiz;
+		status_flags.pre_flt_fail_innov_height    = _pre_flt_fail_innov_height;
+		status_flags.pre_flt_fail_innov_heading   = _pre_flt_fail_innov_heading;
+
 		status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_status_flags_pub.publish(status_flags);
 

src/modules/ekf2/EKF2.hpp

@@ -42,7 +42,6 @@
 #define EKF2_HPP
 
 #include "EKF/ekf.h"
-#include "Utility/PreFlightChecker.hpp"
 
 #include "EKF2Selector.hpp"
 
@@ -221,6 +220,20 @@ private:
 	uint64_t _start_time_us = 0;		///< system time at EKF start (uSec)
 	int64_t _last_time_slip_us = 0;		///< Last time slip (uSec)
 
+	// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
+	static constexpr float kVelocityInnovationTestLimit = 0.5f;
+	// Maximum permissible position innovation to pass pre-flight checks (m)
+	static constexpr float kPositionInnovationTestLimit = 1.5f;
+	// Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad)
+	static constexpr float kHeadingInnovationTestLimit = 0.5f;
+
+	// preflight failure checks
+	bool _pre_flt_fail_innov_heading{false};
+	bool _pre_flt_fail_innov_vel_horiz{false};
+	bool _pre_flt_fail_innov_vel_vert{false};
+	bool _pre_flt_fail_innov_pos_horiz{false};
+	bool _pre_flt_fail_innov_height{false};
+
 	perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")};
 	perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")};
 	perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
@@ -386,9 +399,6 @@ private:
 	uORB::PublicationMulti<vehicle_odometry_s>           _odometry_pub;
 	uORB::PublicationMulti<wind_s>              _wind_pub;
 
-
-	PreFlightChecker _preflt_checker;
-
 	Ekf _ekf;
 
 	parameters *_params;	///< pointer to ekf parameter struct (located in _ekf class instance)

src/modules/ekf2/Utility/CMakeLists.txt

@@ -1,45 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-#############################################################################
-
-px4_add_library(EKF2Utility
-	PreFlightChecker.cpp
-)
-
-target_include_directories(EKF2Utility
-	PUBLIC
-	${CMAKE_CURRENT_SOURCE_DIR}
-)
-
-target_link_libraries(EKF2Utility PRIVATE mathlib)
-
-px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS EKF2Utility)

src/modules/ekf2/Utility/InnovationLpf.hpp

@@ -1,79 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * First order "alpha" IIR digital filter with input saturation
- */
-
-#include <mathlib/mathlib.h>
-
-class InnovationLpf final
-{
-public:
-	InnovationLpf() = default;
-	~InnovationLpf() = default;
-
-	void reset(float val = 0.f) { _x = val; }
-
-	/**
-	 * Update the filter with a new value and returns the filtered state
-	 * The new value is constained by the limit set in setSpikeLimit
-	 * @param val new input
-	 * @param alpha normalized weight of the new input
-	 * @param spike_limit the amplitude of the saturation at the input of the filter
-	 * @return filtered output
-	 */
-	float update(float val, float alpha, float spike_limit)
-	{
-		float val_constrained = math::constrain(val, -spike_limit, spike_limit);
-		float beta = 1.f - alpha;
-
-		_x = beta * _x + alpha * val_constrained;
-
-		return _x;
-	}
-
-	/**
-	 * Helper function to compute alpha from dt and the inverse of tau
-	 * @param dt sampling time in seconds
-	 * @param tau_inv inverse of the time constant of the filter
-	 * @return alpha, the normalized weight of a new measurement
-	 */
-	static float computeAlphaFromDtAndTauInv(float dt, float tau_inv)
-	{
-		return math::constrain(dt * tau_inv, 0.f, 1.f);
-	}
-
-private:
-	float _x{}; ///< current state of the filter
-};

src/modules/ekf2/Utility/PreFlightChecker.cpp

@@ -1,167 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file PreFlightCheckHelper.cpp
- * Class handling the EKF2 innovation pre flight checks
- */
-
-#include "PreFlightChecker.hpp"
-
-void PreFlightChecker::update(const float dt, const estimator_innovations_s &innov)
-{
-	const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv);
-
-	_has_heading_failed = preFlightCheckHeadingFailed(innov, alpha);
-	_has_horiz_vel_failed = preFlightCheckHorizVelFailed(innov, alpha);
-	_has_vert_vel_failed = preFlightCheckVertVelFailed(innov, alpha);
-	_has_height_failed = preFlightCheckHeightFailed(innov, alpha);
-}
-
-bool PreFlightChecker::preFlightCheckHeadingFailed(const estimator_innovations_s &innov, const float alpha)
-{
-	const float heading_test_limit = selectHeadingTestLimit();
-	const float heading_innov_spike_lim = 2.0f * heading_test_limit;
-
-	const float heading_innov_lpf = _filter_heading_innov.update(innov.heading, alpha, heading_innov_spike_lim);
-
-	return checkInnovFailed(heading_innov_lpf, innov.heading, heading_test_limit, heading_innov_spike_lim);
-}
-
-float PreFlightChecker::selectHeadingTestLimit()
-{
-	// Select the max allowed heading innovaton depending on whether we are not aiding navigation using
-	// observations in the NE reference frame and if the vehicle can use GPS course to realign in flight (fixedwing sideslip fusion).
-	const bool is_ne_aiding = _is_using_gps_aiding || _is_using_ev_pos_aiding;
-
-	return (is_ne_aiding && !_can_observe_heading_in_flight)
-	       ? _nav_heading_innov_test_lim // more restrictive test limit
-	       : _heading_innov_test_lim; // less restrictive test limit
-}
-
-bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, const float alpha)
-{
-	bool has_failed = false;
-
-	if (_is_using_gps_aiding || _is_using_ev_vel_aiding) {
-		const Vector2f vel_ne_innov = Vector2f(fmaxf(fabsf(innov.gps_hvel[0]), fabsf(innov.ev_hvel[0])),
-						       fmaxf(fabsf(innov.gps_hvel[1]), fabsf(innov.ev_hvel[1])));
-		Vector2f vel_ne_innov_lpf;
-		vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim);
-		vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim);
-		has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
-	}
-
-	if (_is_using_flow_aiding) {
-		const Vector2f flow_innov = Vector2f(innov.flow);
-		Vector2f flow_innov_lpf;
-		flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
-		flow_innov_lpf(1) = _filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);
-		has_failed |= checkInnov2DFailed(flow_innov_lpf, flow_innov, _flow_innov_test_lim, 5.f * _flow_innov_spike_lim);
-	}
-
-	return has_failed;
-}
-
-bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s &innov, const float alpha)
-{
-	const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel));     // only temporary solution
-	const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
-	return checkInnovFailed(vel_d_innov_lpf, vel_d_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
-}
-
-bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)
-{
-	bool has_failed = false;
-
-	if (_is_using_baro_hgt_aiding) {
-		const float baro_hgt_innov_lpf = _filter_baro_hgt_innov.update(innov.baro_vpos, alpha, _hgt_innov_spike_lim);
-		has_failed |= checkInnovFailed(baro_hgt_innov_lpf, innov.baro_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
-	}
-
-	if (_is_using_gps_hgt_aiding) {
-		const float gps_hgt_innov_lpf = _filter_gps_hgt_innov.update(innov.gps_vpos, alpha, _hgt_innov_spike_lim);
-		has_failed |= checkInnovFailed(gps_hgt_innov_lpf, innov.gps_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
-	}
-
-	if (_is_using_rng_hgt_aiding) {
-		const float rng_hgt_innov_lpf = _filter_rng_hgt_innov.update(innov.rng_vpos, alpha, _hgt_innov_spike_lim);
-		has_failed |= checkInnovFailed(rng_hgt_innov_lpf, innov.rng_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
-	}
-
-	if (_is_using_ev_hgt_aiding) {
-		const float ev_hgt_innov_lpf = _filter_ev_hgt_innov.update(innov.ev_vpos, alpha, _hgt_innov_spike_lim);
-		has_failed |= checkInnovFailed(ev_hgt_innov_lpf, innov.ev_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
-	}
-
-	return has_failed;
-}
-
-bool PreFlightChecker::checkInnovFailed(const float innov_lpf, const float innov, const float test_limit,
-					const float spike_limit)
-{
-	return fabsf(innov_lpf) > test_limit || fabsf(innov) > spike_limit;
-}
-
-bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, const float test_limit,
-		const float spike_limit)
-{
-	return innov_lpf.norm_squared() > sq(test_limit)
-	       || innov.norm_squared() > sq(spike_limit);
-}
-
-void PreFlightChecker::reset()
-{
-	_is_using_gps_aiding = false;
-	_is_using_flow_aiding = false;
-	_is_using_ev_pos_aiding = false;
-	_is_using_ev_vel_aiding = false;
-	_is_using_baro_hgt_aiding = false;
-	_is_using_gps_hgt_aiding = false;
-	_is_using_rng_hgt_aiding = false;
-	_is_using_ev_hgt_aiding = false;
-	_has_heading_failed = false;
-	_has_horiz_vel_failed = false;
-	_has_vert_vel_failed = false;
-	_has_height_failed = false;
-	_filter_vel_n_innov.reset();
-	_filter_vel_e_innov.reset();
-	_filter_vel_d_innov.reset();
-	_filter_baro_hgt_innov.reset();
-	_filter_gps_hgt_innov.reset();
-	_filter_rng_hgt_innov.reset();
-	_filter_ev_hgt_innov.reset();
-	_filter_heading_innov.reset();
-	_filter_flow_x_innov.reset();
-	_filter_flow_y_innov.reset();
-}

src/modules/ekf2/Utility/PreFlightChecker.hpp

@@ -1,195 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file PreFlightChecker.hpp
- * Class handling the EKF2 innovation pre flight checks
- *
- * First call the update(...) function and then get the results
- * using the hasXxxFailed() getters
- */
-
-#ifndef EKF2_PREFLIGHTCHECKER_HPP
-#define EKF2_PREFLIGHTCHECKER_HPP
-
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/estimator_innovations.h>
-
-#include <matrix/matrix/math.hpp>
-
-#include "InnovationLpf.hpp"
-
-using matrix::Vector2f;
-
-class PreFlightChecker
-{
-public:
-	PreFlightChecker() = default;
-	~PreFlightChecker() = default;
-
-	/*
-	 * Reset all the internal states of the class to their default value
-	 */
-	void reset();
-
-	/*
-	 * Update the internal states
-	 * @param dt the sampling time
-	 * @param innov the ekf2_innovation_s struct containing the current innovations
-	 */
-	void update(float dt, const estimator_innovations_s &innov);
-
-	/*
-	 * If set to true, the checker will use a less conservative heading innovation check
-	 */
-	void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; }
-
-	void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
-	void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
-	void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
-	void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
-
-	void setUsingBaroHgtAiding(bool val) { _is_using_baro_hgt_aiding = val; }
-	void setUsingGpsHgtAiding(bool val) { _is_using_gps_hgt_aiding = val; }
-	void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; }
-	void setUsingEvHgtAiding(bool val) { _is_using_ev_hgt_aiding = val; }
-
-	bool hasHeadingFailed() const { return _has_heading_failed; }
-	bool hasHorizVelFailed() const { return _has_horiz_vel_failed; }
-	bool hasVertVelFailed() const { return _has_vert_vel_failed; }
-	bool hasHeightFailed() const { return _has_height_failed; }
-
-	/*
-	 * Overall state of the pre fligh checks
-	 * @return true if any of the check failed
-	 */
-	bool hasFailed() const { return hasHorizFailed() || hasVertFailed(); }
-
-	/*
-	 * Horizontal checks overall result
-	 * @return true if one of the horizontal checks failed
-	 */
-	bool hasHorizFailed() const { return _has_heading_failed || _has_horiz_vel_failed; }
-
-	/*
-	 * Vertical checks overall result
-	 * @return true if one of the vertical checks failed
-	 */
-	bool hasVertFailed() const { return _has_vert_vel_failed || _has_height_failed; }
-
-	/*
-	 * Check if the innovation fails the test
-	 * To pass the test, the following conditions should be true:
-	 * innov_lpf <= test_limit
-	 * innov <= spike_limit
-	 * @param innov_lpf the low-pass filtered innovation
-	 * @param innov the current unfiltered innovation
-	 * @param test_limit the magnitude test limit for innov_lpf
-	 * @param spike_limit the magnitude test limit for innov
-	 * @return true if the check failed the test, false otherwise
-	 */
-	static bool checkInnovFailed(float innov_lpf, float innov, float test_limit, float spike_limit);
-
-	/*
-	 * Check if the a innovation of a 2D vector fails the test
-	 * To pass the test, the following conditions should be true:
-	 * innov_lpf <= test_limit
-	 * innov <= spike_limit
-	 * @param innov_lpf the low-pass filtered innovation
-	 * @param innov the current unfiltered innovation
-	 * @param test_limit the magnitude test limit for innov_lpf
-	 * @param spike_limit the magnitude test limit for innov
-	 * @return true if the check failed the test, false otherwise
-	 */
-	static bool checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, float test_limit, float spike_limit);
-
-	static constexpr float sq(float var) { return var * var; }
-
-private:
-	bool preFlightCheckHeadingFailed(const estimator_innovations_s &innov, float alpha);
-	float selectHeadingTestLimit();
-
-	bool preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, float alpha);
-	bool preFlightCheckVertVelFailed(const estimator_innovations_s &innov, float alpha);
-	bool preFlightCheckHeightFailed(const estimator_innovations_s &innov, float alpha);
-
-	bool _has_heading_failed{};
-	bool _has_horiz_vel_failed{};
-	bool _has_vert_vel_failed{};
-	bool _has_height_failed{};
-
-	bool _can_observe_heading_in_flight{};
-	bool _is_using_gps_aiding{};
-	bool _is_using_flow_aiding{};
-	bool _is_using_ev_pos_aiding{};
-	bool _is_using_ev_vel_aiding{};
-
-	bool _is_using_baro_hgt_aiding{};
-	bool _is_using_gps_hgt_aiding{};
-	bool _is_using_rng_hgt_aiding{};
-	bool _is_using_ev_hgt_aiding{};
-
-	// Low-pass filters for innovation pre-flight checks
-	InnovationLpf _filter_vel_n_innov;	///< Preflight low pass filter N axis velocity innovations (m/sec)
-	InnovationLpf _filter_vel_e_innov;	///< Preflight low pass filter E axis velocity innovations (m/sec)
-	InnovationLpf _filter_vel_d_innov;	///< Preflight low pass filter D axis velocity innovations (m/sec)
-	InnovationLpf _filter_heading_innov;	///< Preflight low pass filter heading innovation magntitude (rad)
-	InnovationLpf _filter_flow_x_innov;	///< Preflight low pass filter optical flow innovation (rad)
-	InnovationLpf _filter_flow_y_innov;	///< Preflight low pass filter optical flow innovation (rad)
-
-	// Preflight low pass filter height innovation (m)
-	InnovationLpf _filter_baro_hgt_innov;
-	InnovationLpf _filter_gps_hgt_innov;
-	InnovationLpf _filter_rng_hgt_innov;
-	InnovationLpf _filter_ev_hgt_innov;
-
-	// Preflight low pass filter time constant inverse (1/sec)
-	static constexpr float _innov_lpf_tau_inv = 0.2f;
-	// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
-	static constexpr float _vel_innov_test_lim = 0.5f;
-	// Maximum permissible height innovation to pass pre-flight checks (m)
-	static constexpr float _hgt_innov_test_lim = 1.5f;
-	// Maximum permissible yaw innovation to pass pre-flight checks when aiding inertial nav using NE frame observations (rad)
-	static constexpr float _nav_heading_innov_test_lim = 0.25f;
-	// Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad)
-	static constexpr float _heading_innov_test_lim = 0.52f;
-	// Maximum permissible flow innovation to pass pre-flight checks
-	static constexpr float _flow_innov_test_lim = 0.25f;
-	// Preflight velocity innovation spike limit (m/sec)
-	static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim;
-	// Preflight position innovation spike limit (m)
-	static constexpr float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim;
-	// Preflight flow innovation spike limit (rad)
-	static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim;
-};
-#endif // !EKF2_PREFLIGHTCHECKER_HPP

src/modules/ekf2/Utility/PreFlightCheckerTest.cpp

@@ -1,97 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * Test code for PreFlightChecker class
- * Run this test only using make tests TESTFILTER=PreFlightChecker
- */
-
-#include <gtest/gtest.h>
-
-#include "PreFlightChecker.hpp"
-
-class PreFlightCheckerTest : public ::testing::Test
-{
-};
-
-TEST_F(PreFlightCheckerTest, testInnovFailed)
-{
-	const float test_limit = 1.0; ///< is the limit for innovation_lpf
-	const float spike_limit = 2.f * test_limit; ///< is the limit for innovation_lpf
-	const float innovations[9] = 	 {0.0, 1.5, 2.5, -1.5, -2.5, 1.5, -1.5, -2.5, -2.5};
-	const float innovations_lpf[9] = {0.0, 0.9, 0.9, -0.9, -0.9, 1.1, -1.1, -1.1, 1.1};
-	const bool expected_result[9] = {false, false, true, false, true, true, true, true, true};
-
-	for (int i = 0; i < 9; i++) {
-		EXPECT_EQ(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit),
-			  expected_result[i]);
-	}
-
-	// Smaller test limit, all the checks should fail except the first
-	EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[0], innovations[0], 0.0, 0.0));
-
-	for (int i = 1; i < 9; i++) {
-		EXPECT_TRUE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 0.0, 0.0));
-	}
-
-	// Larger test limit, none of the checks should fail
-	for (int i = 0; i < 9; i++) {
-		EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 2.0, 4.0));
-	}
-}
-
-TEST_F(PreFlightCheckerTest, testInnov2dFailed)
-{
-	const float test_limit = 1.0;
-	const float spike_limit = 2.0;
-	Vector2f innovations[4] = {{0.0, 0.0}, {0.0, 0.0}, {0.0, -2.5}, {1.5, -1.5}};
-	Vector2f innovations_lpf[4] = {{0.0, 0.0}, {1.1, 0.0}, {0.5, 0.5}, {1.0, -1.0}};
-	const bool expected_result[4] = {false, true, true, true};
-
-	for (int i = 0; i < 4; i++) {
-		EXPECT_EQ(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit),
-			  expected_result[i]);
-	}
-
-	// Smaller test limit, all the checks should fail except the first
-	EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations[0], innovations_lpf[0], 0.0, 0.0));
-
-	for (int i = 1; i < 4; i++) {
-		EXPECT_TRUE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 0.0, 0.0));
-	}
-
-	// Larger test limit, none of the checks should fail
-	for (int i = 0; i < 4; i++) {
-		EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 1.42, 2.84));
-	}
-}