From 55f66b7696c72222031656906d8933750863e6b3 Mon Sep 17 00:00:00 2001
From: Niti Rohilla <niti.rohilla@tcs.com>
Date: Tue, 12 Apr 2016 11:40:09 +0530
Subject: [PATCH] Copter: Add support for MAV_CMD_NAV_DELAY

---
 ArduCopter/Copter.h           |  8 +++++++-
 ArduCopter/commands_logic.cpp | 31 +++++++++++++++++++++++++++++++
 2 files changed, 38 insertions(+), 1 deletion(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 46c0c58e30..5a1e3441e7 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -379,6 +379,10 @@ private:
     uint32_t brake_timeout_start;
     uint32_t brake_timeout_ms;
 
+    // Delay the next navigation command
+    int32_t delay_time_max;  // used for delaying the navigation commands (eg land,takeoff etc.)
+    uint32_t delay_time_start;
+
     // Flip
     Vector3f flip_orig_attitude;         // original copter attitude before flip
 
@@ -1014,6 +1018,7 @@ private:
     void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
     void do_guided_limits(const AP_Mission::Mission_Command& cmd);
 #endif
+    void do_nav_delay(const AP_Mission::Mission_Command& cmd);
     void do_wait_delay(const AP_Mission::Mission_Command& cmd);
     void do_within_distance(const AP_Mission::Mission_Command& cmd);
     void do_yaw(const AP_Mission::Mission_Command& cmd);
@@ -1037,8 +1042,9 @@ private:
 #if NAV_GUIDED == ENABLED
     bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
 #endif
-    void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
+    bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
 
+    void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination);
     void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
     void log_init(void);
     void run_cli(AP_HAL::UARTDriver *port);
diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp
index f70d269811..06d1d2a2d4 100644
--- a/ArduCopter/commands_logic.cpp
+++ b/ArduCopter/commands_logic.cpp
@@ -53,6 +53,10 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 #endif
 
+    case MAV_CMD_NAV_DELAY:                    // 94 Delay the next navigation command
+        do_nav_delay(cmd);
+        break;
+
     //
     // conditional commands
     //
@@ -212,6 +216,9 @@ bool Copter::verify_command(const AP_Mission::Mission_Command& cmd)
         return verify_nav_guided_enable(cmd);
 #endif
 
+     case MAV_CMD_NAV_DELAY:
+        return verify_nav_delay(cmd);
+
     ///
     /// conditional commands
     ///
@@ -514,6 +521,20 @@ void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 }
 #endif  // NAV_GUIDED
 
+// do_nav_delay - Delay the next navigation command
+void Copter::do_nav_delay(const AP_Mission::Mission_Command& cmd)
+{
+    delay_time_start = millis();
+
+    if (cmd.content.nav_delay.seconds > 0) {
+        // relative delay
+        delay_time_max = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
+    } else {
+        // absolute delay to utc time
+        delay_time_max = hal.util->get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0);
+    }
+    gcs_send_text_fmt(MAV_SEVERITY_INFO, "Delaying %u sec",(unsigned int)(delay_time_max/1000));
+}
 
 #if PARACHUTE == ENABLED
 // do_parachute - configure or release parachute
@@ -738,6 +759,16 @@ bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 }
 #endif  // NAV_GUIDED
 
+// verify_nav_delay - check if we have waited long enough
+bool Copter::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
+{
+    if (millis() - delay_time_start > (uint32_t)MAX(delay_time_max,0)) {
+        delay_time_max = 0;
+        return true;
+    }
+    return false;
+}
+
 
 /********************************************************************************/
 //	Condition (May) commands