navigator: check hagl failsafe centrally

src/modules/navigator/mission_block.cpp

@@ -1042,3 +1042,29 @@ void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_i
 		_mission_item.altitude_is_relative = false;
 	}
 }
+
+void MissionBlock::updateFailsafeChecks()
+{
+	updateMaxHaglFailsafe();
+}
+
+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)) {
+		// 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");
+
+		_navigator->trigger_hagl_failsafe(getNavigatorStateId());
+
+		// While waiting for a failsafe action from commander, keep the curren position
+		setLoiterItemFromCurrentPosition(&_mission_item);
+
+		mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current);
+
+		_navigator->set_position_setpoint_triplet_updated();
+	}
+}

src/modules/navigator/mission_block.h

@@ -145,6 +145,8 @@ public:
 	void set_align_mission_item(struct mission_item_s *const mission_item,
 				    const struct mission_item_s *const mission_item_next) const;
 
+	void updateFailsafeChecks() override;
+
 protected:
 	/**
 	 * @brief heading mode for setting navigation items
@@ -249,4 +251,8 @@ protected:
 	bool _payload_deploy_ack_successful{false};	// Flag to keep track of whether we received an acknowledgement for a successful payload deployment
 	hrt_abstime _payload_deployed_time{0};		// Last payload deployment start time to handle timeouts
 	float _payload_deploy_timeout_s{0.0f};		// Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received
+
+private:
+	void updateMaxHaglFailsafe();
+
 };

src/modules/navigator/navigator_mode.cpp

@@ -61,6 +61,7 @@ NavigatorMode::run(bool active)
 		} else {
 			/* periodic updates when active */
 			on_active();
+			updateFailsafeChecks();
 		}
 
 	} else {

src/modules/navigator/navigator_mode.h

@@ -80,6 +80,8 @@ public:
 	 */
 	virtual void on_active();
 
+	virtual void updateFailsafeChecks() {};
+
 protected:
 	Navigator *_navigator{nullptr};
 

src/modules/navigator/rtl.cpp

@@ -283,10 +283,12 @@ void RTL::on_active()
 	case RtlType::RTL_MISSION_FAST_REVERSE:
 	case RtlType::RTL_DIRECT_MISSION_LAND:
 		_rtl_mission_type_handle->on_active();
+		_rtl_mission_type_handle->updateFailsafeChecks();
 		break;
 
 	case RtlType::RTL_DIRECT:
 		_rtl_direct.on_active();
+		_rtl_direct.updateFailsafeChecks();
 		break;
 
 	default:

src/modules/navigator/rtl_direct.cpp

@@ -158,16 +158,6 @@ void RtlDirect::set_rtl_item()
 {
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	if (_global_pos_sub.get().terrain_alt_valid
-	    && ((_rtl_alt - _global_pos_sub.get().terrain_alt) > _navigator->get_local_position()->hagl_max)) {
-		// Handle case where the RTL altidude is above the maximum HAGL and land in place instead of RTL
-		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL\t");
-		events::send(events::ID("rtl_fail_max_hagl"), events::Log::Error, "RTL: return alt higher than max HAGL");
-
-		_navigator->trigger_hagl_failsafe(getNavigatorStateId());
-		_rtl_state = RTLState::IDLE;
-	}
-
 	const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon,
 				       _global_pos_sub.get().lat, _global_pos_sub.get().lon);
 	const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt);