navigator: set 1 second hysteresis time before triggering navigator_f…

…ail_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 <[email protected]>
PX4 · Dec 19, 2024 · ba6f3f1 · ba6f3f1
1 parent 367a8a7
commit ba6f3f1
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 2 deletions.
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
@@ -1023,10 +1023,19 @@ void MissionBlock::updateFailsafeChecks()
 
 void MissionBlock::updateMaxHaglFailsafe()
 {
+
+	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->get_global_position()->terrain_alt_valid
-	    && ((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
@@ -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 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 	 */
 	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 @@ class Navigator : public ModuleBase<Navigator>, public ModuleParams
 
 	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();