From 3aca35777f3e0f00cad941469878e0154ee79ff7 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 1 May 2013 17:06:58 +0900
Subject: [PATCH] Copter: use mavlink fence message

Note: we should consider moving this send via mavlink into the fence
library
---
 ArduCopter/ArduCopter.pde |  2 +-
 ArduCopter/fence.pde      | 48 ++++++++++++++++++++++-----------------
 2 files changed, 28 insertions(+), 22 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 47863cbfdd..b0efd2479c 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -821,7 +821,7 @@ static AP_Mount camera_mount2(&current_loc, g_gps, &ahrs, 1);
 // AC_Fence library to reduce fly-aways
 ////////////////////////////////////////////////////////////////////////////////
 #if AC_FENCE == ENABLED
-AC_Fence    fence(&inertial_nav, &g_gps);
+AC_Fence    fence(&inertial_nav);
 #endif
 
 ////////////////////////////////////////////////////////////////////////////////
diff --git a/ArduCopter/fence.pde b/ArduCopter/fence.pde
index 7f0587b52d..5ff0937815 100644
--- a/ArduCopter/fence.pde
+++ b/ArduCopter/fence.pde
@@ -18,8 +18,8 @@ void fence_check()
         return;
     }
 
-    // give fence library our current distance from home
-    fence.set_home_distance(home_distance);
+    // give fence library our current distance from home in meters
+    fence.set_home_distance(home_distance*0.01f);
 
     // check for a breach
     new_breaches = fence.check_fence();
@@ -30,23 +30,23 @@ void fence_check()
         // if the user wants some kind of response and motors are armed
         if(fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY ) {
 
-            // disarm immediately we think we are on the ground
+            // disarm immediately if we think we are on the ground
             // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
             if(control_mode <= ACRO && g.rc_3.control_in == 0 && !ap.failsafe_radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
                 init_disarm_motors();
             }else{
                 // if we have a GPS
                 if( ap.home_is_set ) {
-                    // if the breach is of the big circle LAND
-                    if((new_breaches & AC_FENCE_TYPE_BIG_CIRCLE) > 0) {
-                        if(control_mode != LAND) {
-                            set_mode(LAND);
-                        }
-                    }else{
-                        // must be a small circle or altitude breach so try to RTL
+                    // if we are within 100m of the fence, RTL
+                    if( fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
                         if(control_mode != RTL) {
                             set_mode(RTL);
                         }
+                    }else{
+                        // if more than 100m outside the fence just force a land
+                        if(control_mode != RTL) {
+                            set_mode(LAND);
+                        }
                     }
                 }else{
                     // we have no GPS so LAND
@@ -69,18 +69,24 @@ void fence_check()
 
 // fence_send_mavlink_status - send fence status to ground station
 static void fence_send_mavlink_status(mavlink_channel_t chan)
-{
+{   
     if (fence.enabled()) {
-        mavlink_msg_limits_status_send(chan,
-                                       (uint8_t) fence.enabled(),
-                                       (uint32_t) fence.get_breach_time(),
-                                       (uint32_t) 0,
-                                       (uint32_t) 0,
-                                       (uint32_t) 0,
-                                       (uint16_t) fence.get_breach_count(),
-                                       (uint8_t) fence.get_enabled_fences(),
-                                       (uint8_t) 0,
-                                       (uint8_t) fence.get_breaches());
+        // traslate fence library breach types to mavlink breach types
+        uint8_t mavlink_breach_type = FENCE_BREACH_NONE;
+        uint8_t breaches = fence.get_breaches();
+        if (breaches & AC_FENCE_TYPE_ALT_MAX != 0) {
+            mavlink_breach_type = FENCE_BREACH_MAXALT;
+        }
+        if (breaches & AC_FENCE_TYPE_CIRCLE != 0) {
+            mavlink_breach_type = FENCE_BREACH_BOUNDARY;
+        }
+
+        // send status
+        mavlink_msg_fence_status_send(chan,
+                                      (int8_t)(fence.get_breaches()!=0),
+                                      fence.get_breach_count(),
+                                      mavlink_breach_type,
+                                      fence.get_breach_time());
     }
 }