From 7f6b7dab2fa556bdd4b6d171566f4569f6e78c09 Mon Sep 17 00:00:00 2001 From: chobitsfan Date: Thu, 24 Jan 2019 19:01:26 +0900 Subject: [PATCH] Copter: support fence actions SmartRTL and Brake --- ArduCopter/fence.cpp | 40 +++++++++++++++++++++++++++++++--------- 1 file changed, 31 insertions(+), 9 deletions(-) diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index b0ae3fb2f7..432313dccb 100644 --- a/ArduCopter/fence.cpp +++ b/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); } } }