Plane: prevent failsafe from changing mode during landing

this prevents RC or GCS failsafe from triggering a mode change during
a landing
This commit is contained in:
Andrew Tridgell 2020-02-17 16:36:56 +11:00
parent 87486739d1
commit 5f4b5c4b94
1 changed files with 14 additions and 0 deletions

View File

@ -40,6 +40,13 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
break;
case Mode::Number::AUTO:
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND ||
quadplane.in_vtol_land_sequence()) {
// don't failsafe in a landing sequence
break;
}
FALLTHROUGH;
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::LOITER:
@ -110,6 +117,13 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
break;
case Mode::Number::AUTO:
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND ||
quadplane.in_vtol_land_sequence()) {
// don't failsafe in a landing sequence
break;
}
FALLTHROUGH;
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {