From 1bb332382c1f96c17c249b8a8fe950da8044c614 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tomp@AionRobotics.com>
Date: Fri, 9 Aug 2019 11:42:19 -0700
Subject: [PATCH] Rover: add support for NAV_DELAY misison item

---
 APMrover2/mode.h        |  7 +++++++
 APMrover2/mode_auto.cpp | 40 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 47 insertions(+)

diff --git a/APMrover2/mode.h b/APMrover2/mode.h
index e226085650..792b7ee1db 100644
--- a/APMrover2/mode.h
+++ b/APMrover2/mode.h
@@ -302,6 +302,8 @@ private:
     bool do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination);
     void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
     void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd);
+    void do_nav_delay(const AP_Mission::Mission_Command& cmd);
+    bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
     bool verify_RTL();
     bool verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
@@ -346,6 +348,11 @@ private:
     // For example in a delay command the condition_start records that start time for the delay
     int32_t condition_start;
 
+    // Delay the next navigation command
+    uint32_t nav_delay_time_max_ms;  // used for delaying the navigation commands
+    uint32_t nav_delay_time_start_ms;
+    AutoSubMode nav_delay_submode_backup; // back up sub_mode
+
 };
 
 
diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp
index 8a2f2e3967..0b3a879c37 100644
--- a/APMrover2/mode_auto.cpp
+++ b/APMrover2/mode_auto.cpp
@@ -348,6 +348,10 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         do_nav_set_yaw_speed(cmd);
         break;
 
+    case MAV_CMD_NAV_DELAY:                    // 93 Delay the next navigation command
+        do_nav_delay(cmd);
+        break;
+
     // Conditional commands
     case MAV_CMD_CONDITION_DELAY:
         do_wait_delay(cmd);
@@ -472,6 +476,9 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_NAV_GUIDED_ENABLE:
         return verify_nav_guided_enable(cmd);
 
+    case MAV_CMD_NAV_DELAY:
+       return verify_nav_delay(cmd);
+
     case MAV_CMD_CONDITION_DELAY:
         return verify_wait_delay();
 
@@ -538,6 +545,27 @@ bool ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_sto
     return true;
 }
 
+// do_nav_delay - Delay the next navigation command
+void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
+{
+    nav_delay_time_start_ms = millis();
+
+    // act like we're just doing a heading hold that we instantly achieve.
+    // the behavior of this will put a boat into loiter, else stop
+    nav_delay_submode_backup = _submode;
+    _submode = Auto_HeadingAndSpeed;
+    _reached_heading = true;
+
+    if (cmd.content.nav_delay.seconds > 0) {
+        // relative delay
+        nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
+    } else {
+        // absolute delay to utc time
+        nav_delay_time_max_ms = AP::rtc().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(MAV_SEVERITY_INFO, "Delaying %u sec",nav_delay_time_max_ms/1000);
+}
+
 // start guided within auto to allow external navigation system to control vehicle
 void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
 {
@@ -601,6 +629,18 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
     }
 }
 
+// verify_nav_delay - check if we have waited long enough
+bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
+{
+    if (millis() - nav_delay_time_start_ms > nav_delay_time_max_ms) {
+        nav_delay_time_max_ms = 0;
+        _submode = nav_delay_submode_backup;
+        return true;
+    }
+
+    return false;
+}
+
 bool ModeAuto::verify_RTL()
 {
     return reached_destination();