Skip to content

Commit

Permalink
Copter: notify when fence breach has cleared
Browse files Browse the repository at this point in the history
output fence breach type
when switching mode without a fence action do not go into manual recovery
only recover if there is a fence action
  • Loading branch information
andyp1per committed Jan 2, 2024
1 parent 319202a commit 70acbad
Show file tree
Hide file tree
Showing 2 changed files with 12 additions and 8 deletions.
10 changes: 6 additions & 4 deletions ArduCopter/fence.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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);
}
Expand Down
10 changes: 6 additions & 4 deletions ArduCopter/mode.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 70acbad

Please sign in to comment.