Rally Points: enable rally points always, except for RTL type == RTL_…

…MISSION

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

src/modules/navigator/rtl.cpp

@@ -63,7 +63,7 @@ RTL::on_inactive()
 }
 
 void
-RTL::find_closest_landing_point()
+RTL::find_RTL_destination()
 {
 	// get home position:
 	home_position_s &home_landing_position = *_navigator->get_home_position();
@@ -79,7 +79,8 @@ RTL::find_closest_landing_point()
 
 	_destination.type = RTL_DESTINATION_HOME;
 
-	if (_navigator->get_mission_start_land_available()) {
+	// consider the mission landing if not RTL_HOME type set
+	if (rtl_type() != RTL_HOME && _navigator->get_mission_start_land_available()) {
 		double mission_landing_lat = _navigator->get_mission_landing_lat();
 		double mission_landing_lon = _navigator->get_mission_landing_lon();
 
@@ -88,7 +89,8 @@ RTL::find_closest_landing_point()
 		dlon = mission_landing_lon - global_position.lon;
 		double dist_squared = dlat * dlat + dlon * dlon;
 
-		if (dist_squared < min_dist_squared) {
+		// set destination to mission landing if closest or in RTL_LAND or RTL_MISSION (so not in RTL_CLOSEST)
+		if (dist_squared < min_dist_squared || rtl_type() != RTL_CLOSEST) {
 			min_dist_squared = dist_squared;
 			_destination.lat = _navigator->get_mission_landing_lat();
 			_destination.lon = _navigator->get_mission_landing_lon();
@@ -98,6 +100,11 @@ RTL::find_closest_landing_point()
 		}
 	}
 
+	// do not consider rally point if RTL type is set to RTL_MISSION, so exit function and use either home or mission landing
+	if (rtl_type() == RTL_MISSION) {
+		return;
+	}
+
 	// compare to safe landing positions
 	mission_safe_point_s closest_safe_point {} ;
 	mission_stats_entry_s stats;
@@ -160,49 +167,6 @@ RTL::find_closest_landing_point()
 
 }
 
-void
-RTL::find_RTL_destination()
-{
-	// get home position:
-	home_position_s &home_landing_position = *_navigator->get_home_position();
-
-	switch (rtl_type()) {
-	case RTL_HOME:
-		// always take home position as landing destination
-		_destination.set(home_landing_position);
-		_destination.type = RTL_DESTINATION_HOME;
-		break;
-
-	case RTL_LAND:
-
-	// take mission landing as landing destination (if available), otherwise home
-	case RTL_MISSION:
-
-		// inverse mission
-		if (_navigator->get_mission_start_land_available()) {
-			_destination.lat = _navigator->get_mission_landing_lat();
-			_destination.lon = _navigator->get_mission_landing_lon();
-			_destination.alt = _navigator->get_mission_landing_alt();
-			_destination.type = RTL_DESTINATION_MISSION_LANDING;
-
-		} else {
-			_destination.set(home_landing_position);
-			_destination.type = RTL_DESTINATION_HOME;
-		}
-
-		break;
-
-	case RTL_CLOSEST:
-		// choose closest possible landing point (consider home, mission landing and safe points)
-		find_closest_landing_point();
-		break;
-
-	default:
-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported RTL_TYPE. Change RTL_TYPE param.");
-		break;
-	}
-}
-
 int
 RTL::rtl_type() const
 {

src/modules/navigator/rtl.h

@@ -74,7 +74,6 @@ class RTL : public MissionBlock, public ModuleParams
 	void on_activation() override;
 	void on_active() override;
 
-	void find_closest_landing_point();
 	void find_RTL_destination();
 
 	void set_return_alt_min(bool min);

src/modules/navigator/rtl_params.c

@@ -107,13 +107,12 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 5.0f);
 /**
  * Return type
  *
- * Fly straight to the return location or planned mission landing and land there or
- * use the planned mission to get to those points.
+ * Return mode destination and flight path (home location, rally point, mission landing pattern, reverse mission)
  *
- * @value 0 Return home via direct path
- * @value 1 Return to a planned mission landing, if available, via direct path, else return to home via direct path
- * @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path
- * @value 3 Return via direct path to closest destination: home, mission landing pattern or safe point
+ * @value 0 Return to closest safe point (home or rally point) via direct path.
+ * @value 1 Return to closest safe point other than home (mission landing pattern or rally point), via direct path. If no mission landing or rally points are defined return home via direct path.
+ * @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path. Do not consider rally points.
+ * @value 3 Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land.
  * @group Return Mode
  */
 PARAM_DEFINE_INT32(RTL_TYPE, 0);