Copter: support fence actions SmartRTL and Brake

ArduCopter/fence.cpp

@@ -24,7 +24,8 @@ void Copter::fence_check()
     if (new_breaches) {
 
         // if the user wants some kind of response and motors are armed
-        if(fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY ) {
+        uint8_t fence_act = fence.get_action();
+        if (fence_act != AC_FENCE_ACTION_REPORT_ONLY ) {
 
             // disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
             // 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
@@ -32,17 +33,38 @@ void Copter::fence_check()
                 init_disarm_motors();
 
             } else {
-                // if always land option mode is specified, land
-                if (fence.get_action() == AC_FENCE_ACTION_ALWAYS_LAND) {
+
+                // if more than 100m outside the fence just force a land
+                if (fence.get_breach_distance(new_breaches) > AC_FENCE_GIVE_UP_DISTANCE) {
                     set_mode(LAND, MODE_REASON_FENCE_BREACH);
-                } else if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
-                    if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
+                } else {
+                    switch (fence_act) {
+                    case AC_FENCE_ACTION_RTL_AND_LAND:
+                    default:
+                        // switch to RTL, if that fails then Land
+                        if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
+                            set_mode(LAND, MODE_REASON_FENCE_BREACH);
+                        }
+                        break;
+                    case AC_FENCE_ACTION_ALWAYS_LAND:
+                        // if always land option mode is specified, land
                         set_mode(LAND, MODE_REASON_FENCE_BREACH);
-                        // if we are within 100m of the fence, RTL
+                        break;
+                    case AC_FENCE_ACTION_SMART_RTL:
+                        // Try SmartRTL, if that fails, RTL, if that fails Land
+                        if (!set_mode(SMART_RTL, MODE_REASON_FENCE_BREACH)) {
+                            if (!set_mode(RTL, MODE_REASON_FENCE_BREACH)) {
+                                set_mode(LAND, MODE_REASON_FENCE_BREACH);
+                            }
+                        }
+                        break;
+                    case AC_FENCE_ACTION_BRAKE:
+                        // Try Brake, if that fails Land
+                        if (!set_mode(BRAKE, MODE_REASON_FENCE_BREACH)) {
+                            set_mode(LAND, MODE_REASON_FENCE_BREACH);
+                        }
+                        break;
                     }
-                } else {
-                    // if more than 100m outside the fence just force a land
-                    set_mode(LAND, MODE_REASON_FENCE_BREACH);
                 }
             }
         }