diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 95137132d7..7fb537e6ff 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -979,6 +979,7 @@ private: // fence.cpp void fence_check(); bool fence_stickmixing() const; + bool in_fence_recovery() const; #endif // ArduPlane.cpp diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 26b794d32d..28f4abdca6 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -43,13 +43,7 @@ void Plane::fence_check() return; } - const bool current_mode_breach = plane.control_mode_reason == ModeReason::FENCE_BREACHED; - const bool previous_mode_breach = plane.previous_mode_reason == ModeReason::FENCE_BREACHED; - const bool previous_mode_complete = (plane.control_mode_reason == ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL) || - (plane.control_mode_reason == ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND) || - (plane.control_mode_reason == ModeReason::QRTL_INSTEAD_OF_RTL) || - (plane.control_mode_reason == ModeReason::QLAND_INSTEAD_OF_RTL); - if (current_mode_breach || (previous_mode_breach && previous_mode_complete)) { + if (in_fence_recovery()) { // we have already triggered, don't trigger again until the // user disables/re-enables using the fence channel switch return; @@ -131,7 +125,7 @@ bool Plane::fence_stickmixing(void) const { if (fence.enabled() && fence.get_breaches() && - control_mode->is_guided_mode()) + in_fence_recovery()) { // don't mix in user input return false; @@ -140,4 +134,16 @@ bool Plane::fence_stickmixing(void) const return true; } +bool Plane::in_fence_recovery() const +{ + const bool current_mode_breach = plane.control_mode_reason == ModeReason::FENCE_BREACHED; + const bool previous_mode_breach = plane.previous_mode_reason == ModeReason::FENCE_BREACHED; + const bool previous_mode_complete = (plane.control_mode_reason == ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL) || + (plane.control_mode_reason == ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND) || + (plane.control_mode_reason == ModeReason::QRTL_INSTEAD_OF_RTL) || + (plane.control_mode_reason == ModeReason::QLAND_INSTEAD_OF_RTL); + + return current_mode_breach || (previous_mode_breach && previous_mode_complete); +} + #endif diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 91b96b7455..36ed578707 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -244,6 +244,16 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason) } #endif // HAL_QUADPLANE_ENABLED +#if AP_FENCE_ENABLED + // may not be allowed to change mode if recovering from fence breach + if (hal.util->get_soft_armed() && fence.enabled() && fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) && + fence.get_breaches() && in_fence_recovery()) { + gcs().send_text(MAV_SEVERITY_NOTICE,"Mode change to %s denied, in fence recovery", new_mode.name()); + AP_Notify::events.user_mode_change_failed = 1; + return false; + } +#endif + // backup current control_mode and previous_mode Mode &old_previous_mode = *previous_mode; Mode &old_mode = *control_mode;