From ba6f3f1548781ecb045e72212232f75645c8c9d2 Mon Sep 17 00:00:00 2001
From: RomanBapst <bapstroman@gmail.com>
Date: Thu, 19 Dec 2024 14:07:57 +0300
Subject: [PATCH] navigator: set 1 second hysteresis time before triggering
 navigator_fail_max_hagl - this is needed to avoid a race condition with the
 estimator in cases where we transition from hover to forward flight and
 expect airspeed aiding to activate after the transition.

Signed-off-by: RomanBapst <bapstroman@gmail.com>
---
 src/modules/navigator/mission_block.cpp | 15 ++++++++++++---
 src/modules/navigator/navigator.h       |  8 ++++++++
 2 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index b07542984d..bcadc78110 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -1023,10 +1023,19 @@ void MissionBlock::updateFailsafeChecks()
 
 void MissionBlock::updateMaxHaglFailsafe()
 {
-	const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt;
 
-	if (_navigator->get_global_position()->terrain_alt_valid
-	    && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) {
+	if (!_navigator->get_position_setpoint_triplet()->current.valid ||
+	    !_navigator->get_global_position()->terrain_alt_valid ||
+	    !PX4_ISFINITE(_navigator->get_local_position()->hagl_max)) {
+		_navigator->update_and_get_terrain_alt_exceeded_state(false, hrt_absolute_time());
+		return;
+	}
+
+	const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt;
+	const bool exceeded = (target_alt - _navigator->get_global_position()->terrain_alt) >
+			      _navigator->get_local_position()->hagl_max;
+
+	if (_navigator->update_and_get_terrain_alt_exceeded_state(exceeded, hrt_absolute_time())) {
 		// Handle case where the altitude setpoint is above the maximum HAGL (height above ground level)
 		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t");
 		events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL");
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index b3057e5375..7da70c38c4 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -59,6 +59,7 @@
 
 #include <lib/adsb/AdsbConflict.h>
 #include <lib/perf/perf_counter.h>
+#include <lib/hysteresis/hysteresis.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/Publication.hpp>
@@ -240,6 +241,12 @@ public:
 	 */
 	void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; }
 
+	bool update_and_get_terrain_alt_exceeded_state(bool exceeded, hrt_abstime now)
+	{
+		_max_terrain_alt_exceeded_hyst.set_state_and_update(exceeded, now);
+		return _max_terrain_alt_exceeded_hyst.get_state();
+	}
+
 	/**
 	 * Get if the yaw acceptance is required at the current mission item
 	 *
@@ -385,6 +392,7 @@ private:
 
 	bool _is_capturing_images{false}; // keep track if we need to stop capturing images
 
+	systemlib::Hysteresis _max_terrain_alt_exceeded_hyst{false};
 
 	// update subscriptions
 	void params_update();