Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: disalow mode change during fence recovery if option is set

This commit is contained in:
Iampete1 2022-08-12 20:34:31 +01:00 committed by Andrew Tridgell
parent 259e70b2b1
commit 2b0492e6aa
3 changed files with 25 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -979,6 +979,7 @@ private:
// fence.cpp
void fence_check();
bool fence_stickmixing() const;
bool in_fence_recovery() const;
#endif
// ArduPlane.cpp

22
ArduPlane/fence.cpp
View File

@ -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

10
ArduPlane/system.cpp
View File

@ -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;