diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 122d49a96544a5..767203521da90b 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -5,13 +5,13 @@ #if AP_FENCE_ENABLED // fence_check - ask fence library to check for breaches and initiate the response -// called at 1hz +// called at 25hz void Copter::fence_check() { const uint8_t orig_breaches = fence.get_breaches(); // check for new breaches; new_breaches is bitmask of fence types breached - const uint8_t new_breaches = fence.check(); + const uint8_t new_breaches = fence.check(motors->armed() && !copter.ap.land_complete); // we still don't do anything when disarmed, but we do check for fence breaches. // fence pre-arm check actually checks if any fence has been breached @@ -24,7 +24,8 @@ void Copter::fence_check() if (new_breaches) { if (!copter.ap.land_complete) { - GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached"); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "%s Fence Breached", new_breaches & AC_FENCE_TYPE_CIRCLE ? "Circle" : + new_breaches & AC_FENCE_TYPE_ALT_MAX ? "Max Alt" : new_breaches & AC_FENCE_TYPE_ALT_MIN ? "Min Alt" : "Polygon"); } // if the user wants some kind of response and motors are armed @@ -81,7 +82,8 @@ void Copter::fence_check() AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); - } else if (orig_breaches) { + } else if (orig_breaches && fence.get_breaches() == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared"); // record clearing of breach AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 3313ab35509bb9..0f5054317c6187 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -368,10 +368,12 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) #endif #if AP_FENCE_ENABLED - // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover - // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) - // but it should be harmless to disable the fence temporarily in these situations as well - fence.manual_recovery_start(); + if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) { + // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover + // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) + // but it should be harmless to disable the fence temporarily in these situations as well + fence.manual_recovery_start(); + } #endif #if AP_CAMERA_ENABLED