Plane: added parameter RTL_RADIUS

this allows the loiter radius for RTL to be controlled separately from
the radius for loiter (as requested by a user)

ArduPlane/ArduPlane.cpp

@@ -728,7 +728,8 @@ void Plane::update_navigation()
     // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
     // ------------------------------------------------------------------------
 
-    // distance and bearing calcs only
+    uint16_t radius = 0;
+    
     switch(control_mode) {
     case AUTO:
         update_commands();
@@ -756,16 +757,24 @@ void Plane::update_navigation()
             auto_state.checked_for_autoland = true;
         }
         // fall through to LOITER
+        if (g.rtl_radius < 0) {
+            loiter.direction = -1;
+        } else {
+            loiter.direction = 1;
+        }
+        radius = abs(g.rtl_radius);
 
     case LOITER:
     case GUIDED:
         // allow loiter direction to be changed in flight
-        if (g.loiter_radius < 0) {
-            loiter.direction = -1;
-        } else {
-            loiter.direction = 1;
+        if (radius == 0) {
+            if (g.loiter_radius < 0) {
+                loiter.direction = -1;
+            } else {
+                loiter.direction = 1;
+            }
         }
-        update_loiter();
+        update_loiter(abs(radius));
         break;
 
     case CRUISE:

ArduPlane/Parameters.cpp

@@ -319,6 +319,15 @@ const AP_Param::Info Plane::var_info[] = {
     // @User: Standard
     GSCALAR(loiter_radius,          "WP_LOITER_RAD",  LOITER_RADIUS_DEFAULT),
 
+    // @Param: RTL_RADIUS
+    // @DisplayName: RTL loiter radius
+    // @Description: Defines the radius of the loiter circle when in RTL mode. If this is zero then WP_LOITER_RAD is used. If the radius is negative then a counter-clockwise is used. If positive then a clockwise loiter is used.
+    // @Units: Meters
+    // @Range: -32767 32767
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(rtl_radius,             "RTL_RADIUS",  0),
+    
 #if GEOFENCE_ENABLED == ENABLED
     // @Param: FENCE_ACTION
     // @DisplayName: Action on geofence breach

ArduPlane/Parameters.h

@@ -277,6 +277,7 @@ public:
         k_param_kff_throttle_to_pitch,
         k_param_scaling_speed,
         k_param_quadplane,
+        k_param_rtl_radius,
 
         //
         // 210: flight modes
@@ -383,6 +384,7 @@ public:
     AP_Int16 waypoint_radius;
     AP_Int16 waypoint_max_radius;
     AP_Int16 loiter_radius;
+    AP_Int16 rtl_radius;
 
 #if GEOFENCE_ENABLED == ENABLED
     AP_Int8 fence_action;

ArduPlane/Plane.h

@@ -859,7 +859,7 @@ private:
     void navigate();
     void calc_airspeed_errors();
     void calc_gndspeed_undershoot();
-    void update_loiter();
+    void update_loiter(uint16_t radius);
     void update_cruise();
     void update_fbwb_speed_height(void);
     void setup_turn_angle(void);

ArduPlane/commands_logic.cpp

@@ -600,13 +600,18 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 
 bool Plane::verify_loiter_unlim()
 {
-    update_loiter();
+    if (g.rtl_radius < 0) {
+        loiter.direction = -1;
+    } else {
+        loiter.direction = 1;
+    }
+    update_loiter(abs(g.rtl_radius));
     return false;
 }
 
 bool Plane::verify_loiter_time()
 {
-    update_loiter();
+    update_loiter(0);
     if (loiter.start_time_ms == 0) {
         if (nav_controller->reached_loiter_target()) {
             // we've reached the target, start the timer
@@ -621,7 +626,7 @@ bool Plane::verify_loiter_time()
 
 bool Plane::verify_loiter_turns()
 {
-    update_loiter();
+    update_loiter(0);
     if (loiter.sum_cd > loiter.total_cd) {
         loiter.total_cd = 0;
         gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER orbits complete");
@@ -638,7 +643,7 @@ bool Plane::verify_loiter_turns()
  */
 bool Plane::verify_loiter_to_alt() 
 {
-    update_loiter();
+    update_loiter(0);
 
     //has target altitude been reached?
     if (condition_value != 0) {
@@ -705,7 +710,12 @@ bool Plane::verify_loiter_to_alt()
 
 bool Plane::verify_RTL()
 {
-    update_loiter();
+    if (g.rtl_radius < 0) {
+        loiter.direction = -1;
+    } else {
+        loiter.direction = 1;
+    }
+    update_loiter(abs(g.rtl_radius));
 	if (auto_state.wp_distance <= (uint32_t)MAX(g.waypoint_radius,0) || 
         nav_controller->reached_loiter_target()) {
 			gcs_send_text(MAV_SEVERITY_INFO,"Reached HOME");

ArduPlane/navigation.cpp

@@ -127,9 +127,11 @@ void Plane::calc_gndspeed_undershoot()
     }
 }
 
-void Plane::update_loiter()
+void Plane::update_loiter(uint16_t radius)
 {
-    int16_t radius = abs(g.loiter_radius);
+    if (radius == 0) {
+        radius = abs(g.loiter_radius);
+    }
 
     if (loiter.start_time_ms == 0 &&
         control_mode == AUTO &&