AC_Fence: Added a new function to check whether the guided waypoint i…

…s within the fence

libraries/AC_Fence/AC_Fence.cpp

@@ -197,6 +197,23 @@ uint8_t AC_Fence::check_fence(float curr_alt)
     //outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
 }
 
+/// check_fence_guided - returns false if the destination waypoint is outside the fence
+// returns true if the guided waypoint is within the fence
+bool AC_Fence::check_fence_location(float destination_alt, float home_destination_distance)
+{
+    // Altitude fence check
+    if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) && (round(destination_alt*0.01f) >= _alt_max)) {
+        return false;
+    }
+
+    // Circular fence check
+    if ((get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) && (home_destination_distance*0.01f >= _circle_radius)) {
+        return false;
+    }
+
+    return true;
+}
+
 /// record_breach - update breach bitmask, time and count
 void AC_Fence::record_breach(uint8_t fence_type)
 {

libraries/AC_Fence/AC_Fence.h

@@ -54,6 +54,9 @@ class AC_Fence
     ///     curr_alt is the altitude above home in meters
     uint8_t check_fence(float curr_alt);
 
+    // check_fence_location - returns true if the destination waypoint is within fence
+    bool check_fence_location(float destination_alt, float home_destination_distance);
+
     /// get_breaches - returns bit mask of the fence types that have been breached
     uint8_t get_breaches() const { return _breached_fences; }