From b010556f37f5234c3304495a830556296193e187 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Jan 2015 13:13:54 +1100
Subject: [PATCH] AP_Mission: prevent infinite loop with linked jump commands

this prevents a "jump loop" from causing a firmware lockup. Thanks to
dellarb for reporting this!
---
 libraries/AP_Mission/AP_Mission.cpp | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
index 2c6cb1625d..7a311dbcf2 100644
--- a/libraries/AP_Mission/AP_Mission.cpp
+++ b/libraries/AP_Mission/AP_Mission.cpp
@@ -964,7 +964,8 @@ bool AP_Mission::advance_current_nav_cmd()
     }
 
     // search until we find next nav command or reach end of command list
-    while (!_flags.nav_cmd_loaded) {
+    uint8_t max_loops = 64;
+    while (!_flags.nav_cmd_loaded && --max_loops) {
         // get next command
         if (!get_next_cmd(cmd_index, cmd, true)) {
             return false;
@@ -990,6 +991,11 @@ bool AP_Mission::advance_current_nav_cmd()
         cmd_index = cmd.index+1;
     }
 
+    if (max_loops == 0) {
+        // the mission is looping
+        return false;
+    }
+
     // if we got this far we must have successfully advanced the nav command
     return true;
 }
@@ -1045,6 +1051,7 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i
     uint16_t jump_index = AP_MISSION_CMD_INDEX_NONE;
 
     // search until the end of the mission command list
+    uint8_t max_loops = 64;
     while(cmd_index < (unsigned)_cmd_total) {
         // load the next command
         if (!read_cmd_from_storage(cmd_index, temp_cmd)) {
@@ -1055,6 +1062,10 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i
         // check for do-jump command
         if (temp_cmd.id == MAV_CMD_DO_JUMP) {
 
+            if (max_loops-- == 0) {
+                return false;
+            }
+
             // check for invalid target
             if (temp_cmd.content.jump.target >= (unsigned)_cmd_total) {
                 // To-Do: log an error?