RTL: use loiter radius from mission for loiter before enganging mission in RTL

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/navigator/mission.cpp

@@ -442,6 +442,7 @@ Mission::find_mission_land_start()
 			_landing_start_lon = missionitem.lon;
 			_landing_start_alt = missionitem.altitude_is_relative ?	missionitem.altitude +
 					     _navigator->get_home_position()->alt : missionitem.altitude;
+			_landing_loiter_radius = missionitem.loiter_radius;
 			_land_start_available = true;
 		}
 

src/modules/navigator/mission.h

@@ -96,6 +96,7 @@ public:
 	double get_landing_lat() { return _landing_lat; }
 	double get_landing_lon() { return _landing_lon; }
 	float get_landing_alt() { return _landing_alt; }
+	float get_landing_loiter_rad() { return _landing_loiter_radius; }
 
 	void set_closest_item_as_current();
 
@@ -263,6 +264,8 @@ private:
 	double _landing_lon{0.0};
 	float _landing_alt{0.0f};
 
+	float _landing_loiter_radius{0.f};
+
 	bool _need_takeoff{true};					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
 
 	hrt_abstime _time_mission_deactivated{0};

src/modules/navigator/navigator.h

@@ -301,6 +301,8 @@ public:
 	double get_mission_landing_lon() { return _mission.get_landing_lon(); }
 	float  get_mission_landing_alt() { return _mission.get_landing_alt(); }
 
+	float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); }
+
 	// RTL
 	bool mission_landing_required() { return _rtl.get_rtl_type() == RTL::RTL_TYPE_MISSION_LANDING; }
 

src/modules/navigator/rtl.cpp

@@ -265,8 +265,6 @@ void RTL::on_activation()
 
 	const vehicle_global_position_s &global_position = *_navigator->get_global_position();
 
-	_rtl_loiter_rad = _param_rtl_loiter_rad.get();
-
 	if (_navigator->get_land_detected()->landed) {
 		// For safety reasons don't go into RTL if landed.
 		_rtl_state = RTL_STATE_LANDED;
@@ -328,6 +326,10 @@ void RTL::set_rtl_item()
 	const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon);
 	const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
 
+	// if we will switch to mission for landing, already set the loiter radius (incl. direction) from mission
+	const float landing_loiter_radius = _destination.type == RTL_DESTINATION_MISSION_LANDING ?
+					    _navigator->get_mission_landing_loiter_radius() : _param_rtl_loiter_rad.get();
+
 	const RTLHeadingMode rtl_heading_mode = static_cast<RTLHeadingMode>(_param_rtl_hdg_md.get());
 
 	switch (_rtl_state) {
@@ -423,9 +425,8 @@ void RTL::set_rtl_item()
 				_mission_item.yaw = _destination.yaw;
 			}
 
-			if (_navigator->get_vstatus()->is_vtol
-			    || _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-				_mission_item.loiter_radius = _rtl_loiter_rad;
+			if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+				_mission_item.loiter_radius = landing_loiter_radius;
 			}
 
 			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
@@ -471,7 +472,10 @@ void RTL::set_rtl_item()
 				_mission_item.yaw = _destination.yaw;
 			}
 
-			_mission_item.loiter_radius = _navigator->get_loiter_radius();
+			if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+				_mission_item.loiter_radius = landing_loiter_radius;
+			}
+
 			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 			_mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f);
 			_mission_item.autocontinue = autocontinue;

src/modules/navigator/rtl.h

@@ -159,7 +159,6 @@ private:
 	hrt_abstime _destination_check_time{0};
 
 	float _rtl_alt{0.0f};	// AMSL altitude at which the vehicle should return to the home position
-	float _rtl_loiter_rad{50.0f};		// radius at which a fixed wing would loiter while descending
 
 	bool _rtl_alt_min{false};
 	bool _should_engange_mission_for_landing{false};