From 23eef91c59b31b1796c34ee24ec5feee3484936a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 19 Jan 2016 15:04:30 +1100
Subject: [PATCH] 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      | 23 ++++++++++++++++-------
 ArduPlane/Parameters.cpp     |  9 +++++++++
 ArduPlane/Parameters.h       |  2 ++
 ArduPlane/Plane.h            |  2 +-
 ArduPlane/commands_logic.cpp | 20 +++++++++++++++-----
 ArduPlane/navigation.cpp     |  6 ++++--
 6 files changed, 47 insertions(+), 15 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index c6e2097f86..18dac70025 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/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
-
-    case LOITER:
-    case GUIDED:
-        // allow loiter direction to be changed in flight
-        if (g.loiter_radius < 0) {
+        if (g.rtl_radius < 0) {
             loiter.direction = -1;
         } else {
             loiter.direction = 1;
         }
-        update_loiter();
+        radius = abs(g.rtl_radius);
+
+    case LOITER:
+    case GUIDED:
+        // allow loiter direction to be changed in flight
+        if (radius == 0) {
+            if (g.loiter_radius < 0) {
+                loiter.direction = -1;
+            } else {
+                loiter.direction = 1;
+            }
+        }
+        update_loiter(abs(radius));
         break;
 
     case CRUISE:
diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
index 8a498f3c4c..266f4205e3 100644
--- a/ArduPlane/Parameters.cpp
+++ b/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
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 8f8ae66e0c..5039062b51 100644
--- a/ArduPlane/Parameters.h
+++ b/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;
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 6eb83601d4..5b5d0b64bb 100644
--- a/ArduPlane/Plane.h
+++ b/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);
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index 8002b75dca..50d36cfe5a 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/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");
diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp
index 937a88a3fa..e37d1204f4 100644
--- a/ArduPlane/navigation.cpp
+++ b/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 &&