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

PX4 · Aug 7, 2024 · 087e5d8 · 087e5d8
1 parent 588eedb
commit 087e5d8
Show file tree

Hide file tree

Showing 2 changed files with 29 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()
@@ -157,6 +158,32 @@ 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
+		_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_point_s loiter_pos{};
+		loiter_pos.lat = _destination.lat;
+		loiter_pos.lon = _destination.lon;
+		loiter_pos.height_m = _destination.alt;
+
+		_land_approach = sanitizeLandApproach(loiter_pos);
+
+		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;
+		}
+
+		_print_return_alt_higher_than_max_hagl = true;
+	}
+
 	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);

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,