ekf2: dead reckon time exceeded, respect ZUPT preflight if air data o…

…r optical flow expected
PX4 · Jul 5, 2024 · f383042 · f383042
1 parent e03c28a
commit f383042
Show file tree

Hide file tree

Showing 4 changed files with 94 additions and 30 deletions.
diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp
@@ -42,13 +42,13 @@ ZeroVelocityUpdate::ZeroVelocityUpdate()
 
 void ZeroVelocityUpdate::reset()
 {
-	_time_last_zero_velocity_fuse = 0;
+	_time_last_fuse = 0;
 }
 
 bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 {
 	// Fuse zero velocity at a limited rate (every 200 milliseconds)
-	const bool zero_velocity_update_data_ready = (_time_last_zero_velocity_fuse + 200'000 < imu_delayed.time_us);
+	const bool zero_velocity_update_data_ready = (_time_last_fuse + 200'000 < imu_delayed.time_us);
 
 	if (zero_velocity_update_data_ready) {
 		const bool continuing_conditions_passing = ekf.control_status_flags().vehicle_at_rest
@@ -69,7 +69,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
 				ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i);
 			}
 
-			_time_last_zero_velocity_fuse = imu_delayed.time_us;
+			_time_last_fuse = imu_delayed.time_us;
 
 			return true;
 		}

diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp
@@ -45,9 +45,11 @@ class ZeroVelocityUpdate : public EstimatorAidSource
 	void reset() override;
 	bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override;
 
+	const auto &time_last_fuse() const { return _time_last_fuse; }
+
 private:
 
-	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
+	uint64_t _time_last_fuse{0}; ///< last time of zero velocity update (uSec)
 
 };
 

diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -568,44 +568,106 @@ void Ekf::updateDeadReckoningStatus()
 
 void Ekf::updateHorizontalDeadReckoningstatus()
 {
-	const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.aux_gpos)
-				  && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
-				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
+	bool inertial_dead_reckoning = true;
+
+	// velocity aiding active
+	if ((_control_status.flags.gps || _control_status.flags.ev_vel)
+	&& isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
+	) {
+		inertial_dead_reckoning = false;
+	}
+
+	// position aiding active
+	if ((_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.aux_gpos)
+	&& isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
+	) {
+		inertial_dead_reckoning = false;
+	}
 
-	bool optFlowAiding = false;
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
-#endif // CONFIG_EKF2_OPTICAL_FLOW
+	// optical flow active
+	if (_control_status.flags.opt_flow
+	&& isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)
+	) {
+		inertial_dead_reckoning = false;
 
-	bool airDataAiding = false;
+	} else {
+		if (!_control_status.flags.in_air) {
+			// currently landed, but optical flow aiding should be possible once in air
+			bool optical_flow_aiding_expected = (_params.flow_ctrl == 1)
+								&& isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max);
+
+			if (optical_flow_aiding_expected) {
+				// if we expect to have optical flow aiding available once in air, then consider the ZUPT a valid aiding source
+				if (isRecent(_zero_velocity_update.time_last_fuse(), _params.no_aid_timeout_max)) {
+					inertial_dead_reckoning = false;
+				}
+			}
+		}
+	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #if defined(CONFIG_EKF2_AIRSPEED)
-	airDataAiding = _control_status.flags.wind &&
-				   isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) &&
-				   isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max);
+	// air data aiding active
+	if ((_control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max))
+	&& (_control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max))
+	) {
 
-	_control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
-#else
-	_control_status.flags.wind_dead_reckoning = false;
-#endif // CONFIG_EKF2_AIRSPEED
+		if (inertial_dead_reckoning) {
+			if (!_control_status.flags.wind_dead_reckoning) {
+				// wind_dead_reckoning: no velocity, position, or optical flow aiding, but air data aiding is active
+				ECL_DEBUG("wind dead reckoning");
+				_control_status.flags.wind_dead_reckoning = true;
+			}
+
+			// air data aiding is active, we're not inertial dead reckoning
+			inertial_dead_reckoning = false;
 
-	_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
+		} else {
+			_control_status.flags.wind_dead_reckoning = false;
+		}
 
-	if (!_control_status.flags.inertial_dead_reckoning) {
-		if (_time_delayed_us > _params.no_aid_timeout_max) {
-			_time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max;
+	} else {
+		_control_status.flags.wind_dead_reckoning = false;
+
+		if (!_control_status.flags.in_air) {
+			// currently landed, but air data aiding should be possible once in air
+			bool air_data_aiding_expected = _control_status.flags.fixed_wing
+						&& (_params.beta_fusion_enabled == 1)
+						&& (_params.arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max);
+
+			if (air_data_aiding_expected) {
+				// if we expect to have air data aiding available once in air, then consider the ZUPT a valid aiding source
+				if (isRecent(_zero_velocity_update.time_last_fuse(), _params.no_aid_timeout_max)) {
+					inertial_dead_reckoning = false;
+				}
+			}
 		}
 	}
+#endif // CONFIG_EKF2_AIRSPEED
 
-	// report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present
-	bool deadreckon_time_exceeded = isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max);
 
-	if (!_horizontal_deadreckon_time_exceeded && deadreckon_time_exceeded) {
-		// deadreckon time now exceeded
-		ECL_WARN("dead reckon time exceeded");
-	}
+	if (inertial_dead_reckoning) {
+		if (!_control_status.flags.inertial_dead_reckoning) {
+			ECL_WARN("inertial dead reckoning");
+		}
 
-	_horizontal_deadreckon_time_exceeded = deadreckon_time_exceeded;
+		if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) {
+			// deadreckon time exceeded
+			if (!_horizontal_deadreckon_time_exceeded) {
+				ECL_WARN("horizontal dead reckon time exceeded");
+				_horizontal_deadreckon_time_exceeded = true;
+			}
+		}
+
+		_control_status.flags.inertial_dead_reckoning = true;
+
+	} else {
+		_time_last_horizontal_aiding = _time_delayed_us;
+		_horizontal_deadreckon_time_exceeded = false;
+
+		_control_status.flags.inertial_dead_reckoning = false;
+	}
 }
 
 void Ekf::updateVerticalDeadReckoningStatus()

diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp
@@ -105,7 +105,7 @@ TEST_F(EkfGpsTest, gpsFixLoss)
 	_sensor_simulator._gps.setFixType(0);
 
 	// THEN: after dead-reconing for a couple of seconds, the local position gets invalidated
-	_sensor_simulator.runSeconds(5);
+	_sensor_simulator.runSeconds(6);
 	EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning);
 	EXPECT_FALSE(_ekf->local_position_is_valid());