AP_Mission: starts_with_takeoff_cmd skips past mav_cmd_nav_delay

libraries/AP_Mission/AP_Mission.cpp

@@ -137,13 +137,26 @@ bool AP_Mission::starts_with_takeoff_cmd()
         cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
     }
 
-    if (!get_next_nav_cmd(cmd_index, cmd)) {
-        return false;
-    }
-    if (cmd.id != MAV_CMD_NAV_TAKEOFF) {
-        return false;
+    // check a maximum of 16 items, remembering that missions can have
+    // loops in them
+    for (uint8_t i=0; i<16; i++, cmd_index++) {
+        if (!get_next_nav_cmd(cmd_index, cmd)) {
+            return false;
+        }
+        switch (cmd.id) {
+        // any of these are considered a takeoff command:
+        case MAV_CMD_NAV_TAKEOFF:
+        case MAV_CMD_NAV_TAKEOFF_LOCAL:
+            return true;
+        // any of these are considered "skippable" when determining if
+        // we "start with a takeoff command"
+        case MAV_CMD_NAV_DELAY:
+            continue;
+        default:
+            return false;
+        }
     }
-    return true;
+    return false;
 }
 
 /// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()