From c87d72353fb5e5ca3b4b1cb311a54afdb84cd478 Mon Sep 17 00:00:00 2001
From: Michael Day <mday299@yahoo.com>
Date: Mon, 30 Dec 2013 14:33:02 -0800
Subject: [PATCH] Plane: Introducing FENCE_RETALT parameter.

---
 ArduPlane/Parameters.h   | 4 ++++
 ArduPlane/Parameters.pde | 9 +++++++++
 ArduPlane/geofence.pde   | 9 ++++++---
 3 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index d3204040bf..95010d18d5 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -103,6 +103,9 @@ public:
         // 100: Arming parameters
         k_param_arming = 100,
 
+        // 105: Extra parameters
+        k_param_fence_retalt = 105,
+
         // 110: Telemetry control
         //
         k_param_gcs0 = 110,         // stream rates for uartA
@@ -328,6 +331,7 @@ public:
     AP_Int8 fence_channel;
     AP_Int16 fence_minalt;    // meters
     AP_Int16 fence_maxalt;    // meters
+    AP_Int16 fence_retalt;    // meters
 #endif
 
     AP_Int8 rally_total;
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index b1d0f6a33a..8320e4fc8d 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -267,6 +267,15 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Increment: 1
     // @User: Standard
     GSCALAR(fence_maxalt,           "FENCE_MAXALT",   0),
+
+    // @Param: FENCE_RETALT
+    // @DisplayName: Fence Return Altitude
+    // @Description: Altitude the aircraft will transit to when a fence breach occurs.  If FENCE_RETALT is <= 0 then the midpoint between FENCE_MAXALT and FENCE_MINALT is used, unless FENCE_MAXALT < FENCE_MINALT.  If FENCE_MAXALT < FENCE_MINALT AND FENCE_RETALT is <= 0 then ALT_HOLD_RTL is the altitude used on a fence breach.
+    // @Units: meters
+    // @Range: 0 32767
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(fence_retalt,           "FENCE_RETALT",   0),
 #endif
 
     // @Param: RALLY_TOTAL
diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde
index fcceb54b55..823003391c 100644
--- a/ArduPlane/geofence.pde
+++ b/ArduPlane/geofence.pde
@@ -266,12 +266,15 @@ static void geofence_check(bool altitude_check_only)
 
     case FENCE_ACTION_GUIDED:
     case FENCE_ACTION_GUIDED_THR_PASS:
-        // fly to the return point, with an altitude half way between
-        // min and max
-        if (g.fence_minalt >= g.fence_maxalt) {
+        if (g.fence_retalt > 0) {
+            //fly to the return point using fence_retalt
+            guided_WP.alt = home.alt + 100.0*g.fence_retalt;
+        } else if (g.fence_minalt >= g.fence_maxalt) {
             // invalid min/max, use RTL_altitude
             guided_WP.alt = home.alt + g.RTL_altitude_cm;
         } else {
+            // fly to the return point, with an altitude half way between
+            // min and max
             guided_WP.alt = home.alt + 100.0*(g.fence_minalt + g.fence_maxalt)/2;
         }
         guided_WP.id = 0;