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

Plane: fence: do not re-trigger if mode change for expected reason

This commit is contained in:
Iampete1 2022-07-08 23:10:01 +01:00 committed by Andrew Tridgell
parent 90f2bf3ba2
commit dd2221338b
3 changed files with 13 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/Plane.h
View File

@ -1210,6 +1210,9 @@ private:
float pitch_in_expo(bool use_dz) const;
float rudder_in_expo(bool use_dz) const;
// mode reason for entering previous mode
ModeReason previous_mode_reason = ModeReason::UNKNOWN;
public:
void failsafe_check(void);
#if AP_SCRIPTING_ENABLED

10
ArduPlane/fence.cpp
View File

@ -43,9 +43,13 @@ void Plane::fence_check()
return;
}
if (orig_breaches &&
(control_mode->is_guided_mode()
|| control_mode == &mode_rtl || fence.get_action() == AC_FENCE_ACTION_REPORT_ONLY)) {
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 (orig_breaches && (current_mode_breach || (previous_mode_breach && previous_mode_complete))) {
// we have already triggered, don't trigger again until the
// user disables/re-enables using the fence channel switch
return;

7
ArduPlane/system.cpp
View File

@ -257,7 +257,8 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
// TODO: move these to be after enter() once start_command_callback() no longer checks control_mode
previous_mode = control_mode;
control_mode = &new_mode;
const ModeReason previous_mode_reason = control_mode_reason;
const ModeReason old_previous_mode_reason = previous_mode_reason;
previous_mode_reason = control_mode_reason;
control_mode_reason = reason;
// attempt to enter new mode
@ -269,6 +270,7 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
previous_mode = &old_previous_mode;
control_mode = &old_mode;
control_mode_reason = previous_mode_reason;
previous_mode_reason = old_previous_mode_reason;
// make sad noise
if (reason != ModeReason::INITIALISED) {
@ -285,9 +287,6 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
// exit previous mode
old_mode.exit();
// record reasons
control_mode_reason = reason;
// log and notify mode change
logger.Write_Mode(control_mode->mode_number(), control_mode_reason);
notify_mode(*control_mode);