From 2647bed484846bbe25b2dc2a76a62c9039b1f949 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 2 Jul 2016 17:15:05 +0900
Subject: [PATCH] Copter: guided mode uses modified
 check_destination_within_fence

---
 ArduCopter/Copter.cpp         |  2 +-
 ArduCopter/control_guided.cpp | 14 ++++++--------
 2 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index d093a92452..cb6da235c8 100644
--- a/ArduCopter/Copter.cpp
+++ b/ArduCopter/Copter.cpp
@@ -92,7 +92,7 @@ Copter::Copter(void) :
     camera_mount(ahrs, current_loc),
 #endif
 #if AC_FENCE == ENABLED
-    fence(inertial_nav),
+    fence(ahrs, inertial_nav),
 #endif
 #if AC_RALLY == ENABLED
     rally(ahrs),
diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp
index bcc4d9b4c5..9918cb8fb1 100644
--- a/ArduCopter/control_guided.cpp
+++ b/ArduCopter/control_guided.cpp
@@ -177,7 +177,8 @@ bool Copter::guided_set_destination(const Vector3f& destination)
 
 #if AC_FENCE == ENABLED
     // reject destination if outside the fence
-    if (!fence.check_destination_within_fence(pv_alt_above_home(destination.z)*0.01f, pv_distance_to_home_cm(destination)*0.01f)) {
+    Location_Class dest_loc(destination);
+    if (!fence.check_destination_within_fence(dest_loc)) {
         Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
         // failure is propagated to GCS with NAK
         return false;
@@ -205,13 +206,10 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc)
 #if AC_FENCE == ENABLED
     // reject destination outside the fence.
     // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
-    int32_t alt_target_cm;
-    if (dest_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target_cm)) {
-        if (!fence.check_destination_within_fence(alt_target_cm*0.01f, get_distance_cm(ahrs.get_home(), dest_loc)*0.01f)) {
-            Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
-            // failure is propagated to GCS with NAK
-            return false;
-        }
+    if (!fence.check_destination_within_fence(dest_loc)) {
+        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
+        // failure is propagated to GCS with NAK
+        return false;
     }
 #endif