Copter: use mavlink fence message

Note: we should consider moving this send via mavlink into the fence
library

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
 
 ////////////////////////////////////////////////////////////////////////////////

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,22 +30,22 @@ 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);
+                    // 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{
-                        // must be a small circle or altitude breach so try to RTL
+                        // if more than 100m outside the fence just force a land
                         if(control_mode != RTL) {
-                            set_mode(RTL);
+                            set_mode(LAND);
                         }
                     }
                 }else{
@@ -71,16 +71,22 @@ void fence_check()
 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());
     }
 }