From a6d5c78d1034245a206de5e7a5666afa25648393 Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 26 Nov 2025 09:14:06 +0100 Subject: [PATCH] Ignore max HAGL failsafe in front transition (#25982) * mission_block: readibility improvement * mission_block: ignore max hagl failsafe in front transition --- src/modules/navigator/mission_block.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 168debf23a..63a51042b1 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -1027,10 +1027,18 @@ void MissionBlock::updateFailsafeChecks() void MissionBlock::updateMaxHaglFailsafe() { const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt; + const float max_alt = math::min(_navigator->get_local_position()->hagl_max_z, _navigator->get_local_position()->hagl_max_xy); + const float terrain_alt = _navigator->get_global_position()->terrain_alt; + const bool terrain_alt_valid = _navigator->get_global_position()->terrain_alt_valid; - if (_navigator->get_global_position()->terrain_alt_valid - && ((target_alt - _navigator->get_global_position()->terrain_alt) - > math::min(_navigator->get_local_position()->hagl_max_z, _navigator->get_local_position()->hagl_max_xy))) { + // If the HAGL failsafe is declared during front transition, we enter a + // FW hold at the current low altitude while not having finished the + // transition. This is dangerous and worse than possibly fusing neither + // optical flow nor airspeed for a couple seconds, so we bypass the + // failsafe here. + const bool in_transition_to_fw = _navigator->get_vstatus()->in_transition_to_fw; + + if (!in_transition_to_fw && terrain_alt_valid && (target_alt - terrain_alt) > max_alt) { // 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");