Copter: auto RTL: don't switch modes if already in auto

This commit is contained in:
Iampete1 2021-08-25 23:24:25 +01:00 committed by Randy Mackay
parent d31f3eb4c5
commit 09f1a3da8a

View File

@ -166,7 +166,8 @@ bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason)
{
if (mission.jump_to_landing_sequence()) {
mission.set_force_resume(true);
if (set_mode(Mode::Number::AUTO, reason)) {
// if not already in auto switch to auto
if ((copter.flightmode == &copter.mode_auto) || set_mode(Mode::Number::AUTO, reason)) {
auto_RTL = true;
return true;
}