From 78a463472d68494ca42c0aef79f6f511e69a4fd8 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 24 May 2017 09:24:19 -0700 Subject: [PATCH] AP_Mission: Prohibit resuming mission waypoints that are no longer in the mission --- libraries/AP_Mission/AP_Mission.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 7433e8d792..92bd4c2093 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -92,9 +92,13 @@ void AP_Mission::resume() } // ensure cache coherence - // don't bother checking if the read is successful. If it fails _nav_cmd - // won't change and we'll continue flying the old cached command. - read_cmd_from_storage(_nav_cmd.index, _nav_cmd); + if (!read_cmd_from_storage(_nav_cmd.index, _nav_cmd)) { + // if we failed to read the command from storage, then the command may have + // been from a previously loaded mission it is illogical to ever resume + // flying to a command that has been excluded from the current mission + start(); + return; + } // restart active navigation command. We run these on resume() // regardless of whether the mission was stopped, as we may be