From ffbd2298419a99e792aa3b0895bcb7556e49b947 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 15 Aug 2012 14:31:10 +1000
Subject: [PATCH] APM: use AHRS position for geo-fence

this allows it to take account of dead-reckoning
---
 ArduPlane/geofence.pde | 17 +++++++----------
 1 file changed, 7 insertions(+), 10 deletions(-)

diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde
index defdc6170f..a2ce6fc742 100644
--- a/ArduPlane/geofence.pde
+++ b/ArduPlane/geofence.pde
@@ -121,8 +121,9 @@ failed:
 static bool geofence_enabled(void)
 {
     if (g.fence_action == FENCE_ACTION_NONE ||
-        g.fence_channel == 0 ||
-        APM_RC.InputCh(g.fence_channel-1) < FENCE_ENABLE_PWM) {
+        (g.fence_action != FENCE_ACTION_REPORT &&
+         (g.fence_channel == 0 ||
+          APM_RC.InputCh(g.fence_channel-1) < FENCE_ENABLE_PWM))) {
         // geo-fencing is disabled
         if (geofence_state != NULL) {
             // re-arm for when the channel trigger is switched on
@@ -131,11 +132,6 @@ static bool geofence_enabled(void)
         return false;
     }
 
-    if (!g_gps->fix) {
-        // we can't do much without a GPS fix
-        return false;
-    }
-
     return true;
 }
 
@@ -203,6 +199,7 @@ static void geofence_check(bool altitude_check_only)
 
     bool outside = false;
     uint8_t breach_type = FENCE_BREACH_NONE;
+    struct Location loc;
 
     if (geofence_check_minalt()) {
         outside = true;
@@ -210,10 +207,10 @@ static void geofence_check(bool altitude_check_only)
     } else if (geofence_check_maxalt()) {
         outside = true;
         breach_type = FENCE_BREACH_MAXALT;
-    } else if (!altitude_check_only) {
+    } else if (!altitude_check_only && ahrs.get_position(&loc)) {
         Vector2l location;
-        location.x = current_loc.lat;
-        location.y = current_loc.lng;
+        location.x = loc.lat;
+        location.y = loc.lng;
         outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
         if (outside) {
             breach_type = FENCE_BREACH_BOUNDARY;