RTL: do not RTL if RTL alt is above max HAGL

PX4 · Aug 6, 2024 · bbe35ea · bbe35ea
1 parent 4883f21
commit bbe35ea
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 0 deletions.
diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp
@@ -59,6 +59,7 @@ RtlDirect::RtlDirect(Navigator *navigator) :
 	_land_approach.lat = static_cast<double>(NAN);
 	_land_approach.lon = static_cast<double>(NAN);
 	_land_approach.height_m = NAN;
+	_print_return_alt_higher_than_max_hagl = false;
 }
 
 void RtlDirect::on_inactivation()
@@ -91,6 +92,12 @@ void RtlDirect::on_activation()
 	events::send<int32_t, int32_t>(events::ID("vrtl_return_at"), events::Log::Info,
 				       "RTL: start return at {1m_v} ({2m_v} above destination)",
 				       (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt));
+
+	if (_print_return_alt_higher_than_max_hagl) {
+		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL, landing\t");
+		events::send(events::ID("rtl_fail_max_hagl"), events::Log::Warning, "RTL: return alt higher than max HAGL, landing");
+		_print_return_alt_higher_than_max_hagl = false;
+	}
 }
 
 void RtlDirect::on_active()
@@ -143,6 +150,23 @@ void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s
 			_destination.alt = _home_pos_sub.get().alt;
 		}
 
+		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
+			_print_return_alt_higher_than_max_hagl = true;
+
+			_destination.lat = _global_pos_sub.get().lat;
+			_destination.lon = _global_pos_sub.get().lon;
+			_destination.alt = _global_pos_sub.get().terrain_alt;
+			_destination.yaw = NAN;
+
+			_rtl_alt = _global_pos_sub.get().alt;
+
+			loiter_pos.lat = _destination.lat;
+			loiter_pos.lon = _destination.lon;
+			loiter_pos.height_m = _destination.alt;
+		}
+
 		_land_approach = sanitizeLandApproach(loiter_pos);
 
 		const float dist_to_destination{get_distance_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon)};

diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h
@@ -150,6 +150,8 @@ class RtlDirect : public MissionBlock, public ModuleParams
 
 	float _rtl_alt{0.0f};	///< AMSL altitude at which the vehicle should return to the home position
 
+	bool _print_return_alt_higher_than_max_hagl{false};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_rtl_descend_alt,
 		(ParamFloat<px4::params::RTL_LAND_DELAY>)  _param_rtl_land_delay,