aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
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 // fence.cpp
void fence_check(); void fence_check();
bool fence_stickmixing() const; bool fence_stickmixing() const;
bool in_fence_recovery() const;
#endif #endif
// ArduPlane.cpp // ArduPlane.cpp

22
ArduPlane/fence.cpp
View File

@ -43,13 +43,7 @@ void Plane::fence_check()
return; return;
} }
const bool current_mode_breach = plane.control_mode_reason == ModeReason::FENCE_BREACHED; if (in_fence_recovery()) {
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)) {
// we have already triggered, don't trigger again until the // we have already triggered, don't trigger again until the
// user disables/re-enables using the fence channel switch // user disables/re-enables using the fence channel switch
return; return;
@ -131,7 +125,7 @@ bool Plane::fence_stickmixing(void) const
{ {
if (fence.enabled() && if (fence.enabled() &&
fence.get_breaches() && fence.get_breaches() &&
control_mode->is_guided_mode()) in_fence_recovery())
{ {
// don't mix in user input // don't mix in user input
return false; return false;
@ -140,4 +134,16 @@ bool Plane::fence_stickmixing(void) const
return true; 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 #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 #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 // backup current control_mode and previous_mode
Mode &old_previous_mode = *previous_mode; Mode &old_previous_mode = *previous_mode;
Mode &old_mode = *control_mode; Mode &old_mode = *control_mode;