From 9ec0000691c2481f9561f24f2f155cb367afb8ff Mon Sep 17 00:00:00 2001
From: Niti Rohilla <niti.rohilla@tcs.com>
Date: Thu, 19 May 2016 19:35:42 +0900
Subject: [PATCH] AP_Mission: add check_takeoff_cmd

This checks that the first command in the mission is a takeoff command which helps avoid mission setup errors in which users forget to start a mission with a takeoff command
---
 libraries/AP_Mission/AP_Mission.cpp | 24 ++++++++++++++++++++++++
 libraries/AP_Mission/AP_Mission.h   |  1 +
 2 files changed, 25 insertions(+)

diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
index 5bb2d93c45..7909dca918 100644
--- a/libraries/AP_Mission/AP_Mission.cpp
+++ b/libraries/AP_Mission/AP_Mission.cpp
@@ -113,6 +113,30 @@ void AP_Mission::resume()
     }
 }
 
+// return false if next command in the mission is not takeoff
+bool AP_Mission::check_takeoff_cmd()
+{
+    Mission_Command cmd = {};
+    uint16_t cmd_index;
+
+    // get starting point for search or Reset cmd_index, if _restart is set
+    cmd_index = _restart ? AP_MISSION_CMD_INDEX_NONE : _nav_cmd.index;
+
+    if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
+        // start from beginning of the mission command list
+        cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
+        get_next_cmd(cmd_index, cmd, true);
+
+    } else {
+        read_cmd_from_storage(cmd_index, cmd);
+    }
+
+    if (cmd.id != MAV_CMD_NAV_TAKEOFF) {
+        return false;
+    }
+    return true;
+}
+
 /// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()
 void AP_Mission::start_or_resume()
 {
diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h
index 6e6af7a83d..161f317d68 100644
--- a/libraries/AP_Mission/AP_Mission.h
+++ b/libraries/AP_Mission/AP_Mission.h
@@ -304,6 +304,7 @@ public:
 
     /// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()
     void start_or_resume();
+    bool check_takeoff_cmd();
 
     /// reset - reset mission to the first command
     void reset();