From 83f285bbfcd053438ac2e6aa2a92285ea9dd5e0e Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Sat, 1 Feb 2020 19:58:07 +0900 Subject: [PATCH] Rover: add Hold and SmartRTL to FENCE_ACTION --- APMrover2/fence.cpp | 33 ++++++++++++++++++++++++++++----- 1 file changed, 28 insertions(+), 5 deletions(-) diff --git a/APMrover2/fence.cpp b/APMrover2/fence.cpp index b146ea967e..3a335f90b5 100644 --- a/APMrover2/fence.cpp +++ b/APMrover2/fence.cpp @@ -16,13 +16,36 @@ void Rover::fence_check() // if there is a new breach take action if (new_breaches) { + gcs().send_text(MAV_SEVERITY_NOTICE,"Geofence triggered"); + // if the user wants some kind of response and motors are armed - if (g2.fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) { - // if we are within 100m of the fence, RTL + if (g2.fence.get_action() != Failsafe_Action_None) { + // if we are within 100m of the fence, HOLD if (g2.fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) { - if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) { - set_mode(mode_hold, ModeReason::FENCE_BREACHED); - } + switch (g2.fence.get_action()) { + case Failsafe_Action_None: + break; + case Failsafe_Action_RTL: + if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) { + set_mode(mode_hold, ModeReason::FENCE_BREACHED); + } + break; + case Failsafe_Action_Hold: + set_mode(mode_hold, ModeReason::FENCE_BREACHED); + break; + case Failsafe_Action_SmartRTL: + if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) { + if (!set_mode(mode_rtl, ModeReason::FENCE_BREACHED)) { + set_mode(mode_hold, ModeReason::FENCE_BREACHED); + } + } + break; + case Failsafe_Action_SmartRTL_Hold: + if (!set_mode(mode_smartrtl, ModeReason::FENCE_BREACHED)) { + set_mode(mode_hold, ModeReason::FENCE_BREACHED); + } + break; + } } else { // if more than 100m outside the fence just force to HOLD set_mode(mode_hold, ModeReason::FENCE_BREACHED);