ekf2: default to in air and not at rest

- this is a more conservative default if a vehicle isn't set (no land detector running)
 - handled horizontal preflight failures in commander when disarmed
rather than overloading xy_valid and v_xy_valid flags
 - ekf2 no longer depend on arming or standby status

src/modules/commander/Commander.cpp

@@ -3251,8 +3251,8 @@ Commander::reset_posvel_validity()
 	_lpos_probation_time_us = POSVEL_PROBATION_MIN;
 	_lvel_probation_time_us = POSVEL_PROBATION_MIN;
 
-	// recheck validity
+	// recheck validity (force update)
-	UpdateEstimateValidity();
+	estimator_check(true);
 }
 
 bool
@@ -3968,7 +3968,7 @@ void Commander::battery_status_check()
 	}
 }
 
-void Commander::estimator_check()
+void Commander::estimator_check(bool force)
 {
 	// Check if quality checking of position accuracy and consistency is to be performed
 	const bool run_quality_checks = !_status_flags.circuit_breaker_engaged_posfailure_check;
@@ -3990,7 +3990,7 @@ void Commander::estimator_check()
 	const bool gnss_heading_fault_prev = _estimator_status_flags_sub.get().cs_gps_yaw_fault;
 
 	// use primary estimator_status
-	if (_estimator_selector_status_sub.updated()) {
+	if (_estimator_selector_status_sub.updated() || force) {
 		estimator_selector_status_s estimator_selector_status;
 
 		if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
@@ -4042,47 +4042,56 @@ void Commander::estimator_check()
 	* if this does not fix the issue we need to stop using a position controlled
 	* mode to prevent flyaway crashes.
 	*/
-	if (_estimator_status_sub.updated() && run_quality_checks
+	bool pre_flt_fail_innov_heading = false;
-	    && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+	bool pre_flt_fail_innov_vel_horiz = false;
+
+	if (_estimator_status_sub.updated() || force) {
 
 		estimator_status_s estimator_status;
 
 		if (_estimator_status_sub.copy(&estimator_status)) {
-			if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
-				_nav_test_failed = false;
-				_nav_test_passed = false;
 
-			} else {
+			pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading;
-				if (!_nav_test_passed) {
+			pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz;
-					// 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);
 
-					const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
+			if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 
-					if (innovation_pass) {
+				if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
-						_time_last_innov_pass = hrt_absolute_time();
+					_nav_test_failed = false;
+					_nav_test_passed = false;
 
-						// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
+				} else {
-						const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s);
+					if (!_nav_test_passed) {
-						const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
+						// 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);
 
-						// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
+						const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
-						if (hrt_elapsed_time(&_time_last_innov_fail) > 10_s
-						    && (sufficient_time || sufficient_speed)) {
-							_nav_test_passed = true;
-							_nav_test_failed = false;
-						}
 
-					} else if (innovation_fail) {
+						if (innovation_pass) {
-						_time_last_innov_fail = hrt_absolute_time();
+							_time_last_innov_pass = hrt_absolute_time();
 
-						if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 2_s) {
+							// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
-							// if the innovation test has failed continuously, declare the nav as failed
+							const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s);
-							_nav_test_failed = true;
+							const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
-							mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Land and recalibrate sensors\t");
+
-							events::send(events::ID("commander_navigation_failure"), events::Log::Emergency,
+							// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
-								     "Navigation failure! Land and recalibrate the sensors");
+							if (hrt_elapsed_time(&_time_last_innov_fail) > 10_s
+							    && (sufficient_time || sufficient_speed)) {
+								_nav_test_passed = true;
+								_nav_test_failed = false;
+							}
+
+						} else if (innovation_fail) {
+							_time_last_innov_fail = hrt_absolute_time();
+
+							if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 2_s) {
+								// if the innovation test has failed continuously, declare the nav as failed
+								_nav_test_failed = true;
+								mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Land and recalibrate sensors\t");
+								events::send(events::ID("commander_navigation_failure"), events::Log::Emergency,
+									     "Navigation failure! Land and recalibrate the sensors");
+							}
 						}
 					}
 				}
@@ -4093,9 +4102,51 @@ void Commander::estimator_check()
 	// run position and velocity accuracy checks
 	// Check if quality checking of position accuracy and consistency is to be performed
 	if (run_quality_checks) {
-		UpdateEstimateValidity();
+		float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get();
+
+		// relax local position eph threshold in operator controlled position mode
+		if (_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL &&
+		    ((_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)
+		     || (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL))) {
+
+			// Set the allowable position uncertainty based on combination of flight and estimator state
+			// When we are in a operator demanded position control mode and are solely reliant on optical flow,
+			// do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
+			if (_status_flags.position_reliant_on_optical_flow) {
+				lpos_eph_threshold_adj = INFINITY;
+			}
+		}
+
+		bool xy_valid = lpos.xy_valid && !_nav_test_failed;
+		bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
+
+		if (!_armed.armed) {
+			if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) {
+				xy_valid = false;
+			}
+
+			if (pre_flt_fail_innov_vel_horiz) {
+				v_xy_valid = false;
+			}
+		}
+
+		const vehicle_global_position_s &gpos = _global_position_sub.get();
+
+		_status_flags.condition_global_position_valid =
+			check_posvel_validity(xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
+					      &_last_gpos_fail_time_us, &_gpos_probation_time_us, _status_flags.condition_global_position_valid);
+
+		_status_flags.condition_local_position_valid =
+			check_posvel_validity(xy_valid, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp,
+					      &_last_lpos_fail_time_us, &_lpos_probation_time_us, _status_flags.condition_local_position_valid);
+
+		_status_flags.condition_local_velocity_valid =
+			check_posvel_validity(v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
+					      &_last_lvel_fail_time_us, &_lvel_probation_time_us, _status_flags.condition_local_velocity_valid);
 	}
 
+
+	// altitude
 	_status_flags.condition_local_altitude_valid = lpos.z_valid
 			&& (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s));
 
@@ -4147,7 +4198,7 @@ void Commander::estimator_check()
 	// gps
 	const bool condition_gps_position_was_valid = _status_flags.condition_gps_position_valid;
 
-	if (_vehicle_gps_position_sub.updated()) {
+	if (_vehicle_gps_position_sub.updated() || force) {
 		vehicle_gps_position_s vehicle_gps_position;
 
 		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) {
@@ -4179,38 +4230,6 @@ void Commander::estimator_check()
 	}
 }
 
-void Commander::UpdateEstimateValidity()
-{
-	const vehicle_local_position_s &lpos = _local_position_sub.get();
-	const vehicle_global_position_s &gpos = _global_position_sub.get();
-
-	float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get();
-
-	// relax local position eph threshold in operator controlled position mode
-	if (_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL &&
-	    ((_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)
-	     || (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL))) {
-
-		// Set the allowable position uncertainty based on combination of flight and estimator state
-		// When we are in a operator demanded position control mode and are solely reliant on optical flow, do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
-		if (_status_flags.position_reliant_on_optical_flow) {
-			lpos_eph_threshold_adj = INFINITY;
-		}
-	}
-
-	_status_flags.condition_global_position_valid =
-		check_posvel_validity(lpos.xy_valid && !_nav_test_failed, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
-				      &_last_gpos_fail_time_us, &_gpos_probation_time_us, _status_flags.condition_global_position_valid);
-
-	_status_flags.condition_local_position_valid =
-		check_posvel_validity(lpos.xy_valid && !_nav_test_failed, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp,
-				      &_last_lpos_fail_time_us, &_lpos_probation_time_us, _status_flags.condition_local_position_valid);
-
-	_status_flags.condition_local_velocity_valid =
-		check_posvel_validity(lpos.v_xy_valid && !_nav_test_failed, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
-				      &_last_lvel_fail_time_us, &_lvel_probation_time_us, _status_flags.condition_local_velocity_valid);
-}
-
 void
 Commander::offboard_control_update()
 {

src/modules/commander/Commander.hpp

@@ -146,7 +146,7 @@ private:
 
 	void esc_status_check();
 
-	void estimator_check();
+	void estimator_check(bool force = false);
 
 	bool handle_command(const vehicle_command_s &cmd);
 
@@ -174,8 +174,6 @@ private:
 
 	void update_control_mode();
 
-	void UpdateEstimateValidity();
-
 	bool shutdown_if_allowed();
 
 	bool stabilization_required();

src/modules/ekf2/EKF/ekf.cpp

@@ -82,8 +82,6 @@ void Ekf::reset()
 
 	_prev_dvel_bias_var.zero();
 
-	_control_status.flags.vehicle_at_rest = true;
-
 	resetGpsDriftCheckFilters();
 }
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -258,7 +258,12 @@ public:
 
 protected:
 
-	EstimatorInterface() = default;
+	EstimatorInterface()
+	{
+		_control_status.flags.in_air = true;
+		_control_status.flags.vehicle_at_rest = false;
+	};
+
 	virtual ~EstimatorInterface();
 
 	virtual bool init(uint64_t timestamp) = 0;

src/modules/ekf2/EKF2.cpp

@@ -496,18 +496,6 @@ void EKF2::Run()
 
 				_preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type !=
 						vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
-
-				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-
-				// update standby (arming state) flag
-				const bool standby = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
-
-				if (_standby != standby) {
-					_standby = standby;
-
-					// reset preflight checks if transitioning in or out of standby arming state
-					_preflt_checker.reset();
-				}
 			}
 		}
 
@@ -813,7 +801,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	_estimator_innovations_pub.publish(innovations);
 
 	// calculate noise filtered velocity innovations which are used for pre-flight checking
-	if (_standby) {
+	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);
@@ -821,6 +809,10 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 		_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
 
 		_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);
+
+	} else if (_ekf.control_status_flags().in_air != _ekf.control_status_prev_flags().in_air) {
+		// reset preflight checks if transitioning back to landed
+		_preflt_checker.reset();
 	}
 }
 
@@ -901,9 +893,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	lpos.az = vel_deriv(2);
 
 	// TODO: better status reporting
-	lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
+	lpos.xy_valid = _ekf.local_position_is_valid();
 	lpos.z_valid = !_preflt_checker.hasVertFailed();
-	lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
+	lpos.v_xy_valid = _ekf.local_position_is_valid();
 	lpos.v_z_valid = !_preflt_checker.hasVertFailed();
 
 	// Position of local NED origin in GPS / WGS84 frame
@@ -1923,22 +1915,19 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 		}
 	}
 
+	// update stored declination value
+	if (!_mag_decl_saved) {
+		float declination_deg;
 
-	if (!_armed) {
+		if (_ekf.get_mag_decl_deg(&declination_deg)) {
-		// update stored declination value
+			_param_ekf2_mag_decl.update();
-		if (!_mag_decl_saved) {
-			float declination_deg;
 
-			if (_ekf.get_mag_decl_deg(&declination_deg)) {
+			if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) {
-				_param_ekf2_mag_decl.update();
+				_param_ekf2_mag_decl.set(declination_deg);
-
+				_param_ekf2_mag_decl.commit_no_notification();
-				if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) {
-					_param_ekf2_mag_decl.set(declination_deg);
-					_param_ekf2_mag_decl.commit_no_notification();
-				}
-
-				_mag_decl_saved = true;
 			}
+
+			_mag_decl_saved = true;
 		}
 	}
 }

src/modules/ekf2/EKF2.hpp

@@ -267,9 +267,6 @@ private:
 
 	bool _callback_registered{false};
 
-	bool _armed{false};
-	bool _standby{false}; // standby arming state
-
 	hrt_abstime _last_status_flag_update{0};
 	hrt_abstime _last_range_sensor_update{0};