Plane: support NAV_DELAY in plane

allow for delayed takeoff, and takeoff at a particular time (for swarm
takeoff)

also check for takeoff command in landing sequence arming check

this allows for takeoff->land->disarm->delay->arm->takeoff->land
missions

ArduPlane/AP_Arming.cpp

@@ -102,7 +102,8 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
        }
     }
 
-    if (plane.mission.get_in_landing_sequence_flag()) {
+    if (plane.mission.get_in_landing_sequence_flag() &&
+        !plane.mission.starts_with_takeoff_cmd()) {
         check_failed(display_failure,"In landing sequence");
         ret = false;
     }

ArduPlane/Plane.h

@@ -937,6 +937,15 @@ private:
     bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
     float get_wp_radius() const;
 
+    void do_nav_delay(const AP_Mission::Mission_Command& cmd);
+    bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
+
+    // Delay the next navigation command
+    struct {
+        uint32_t time_max_ms;
+        uint32_t time_start_ms;
+    } nav_delay;
+    
 #if AP_SCRIPTING_ENABLED
     // nav scripting support
     void do_nav_script_time(const AP_Mission::Mission_Command& cmd);

ArduPlane/commands_logic.cpp

@@ -201,6 +201,10 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         do_nav_script_time(cmd);
         break;
 #endif
+
+    case MAV_CMD_NAV_DELAY:
+        do_nav_delay(cmd);
+        break;
         
     default:
         // unable to use the command, allow the vehicle to try the next command
@@ -301,7 +305,10 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     case MAV_CMD_NAV_SCRIPT_TIME:
         return verify_nav_script_time(cmd);
 #endif
-        
+
+     case MAV_CMD_NAV_DELAY:
+         return verify_nav_delay(cmd);
+
     // do commands (always return true)
     case MAV_CMD_DO_CHANGE_SPEED:
     case MAV_CMD_DO_SET_HOME:
@@ -520,6 +527,21 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
     condition_value = 0;
 }
 
+// do_nav_delay - Delay the next navigation command
+void Plane::do_nav_delay(const AP_Mission::Mission_Command& cmd)
+{
+    nav_delay.time_start_ms = millis();
+
+    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", (unsigned)(nav_delay.time_max_ms/1000));
+}
+
 /********************************************************************************/
 //  Verify Nav (Must) commands
 /********************************************************************************/
@@ -852,6 +874,20 @@ bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd)
     return false;
 }
 
+// verify_nav_delay - check if we have waited long enough
+bool Plane::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
+{
+    if (hal.util->get_soft_armed()) {
+        // don't delay while armed, we need a nav controller running
+        return true;
+    }
+    if (millis() - nav_delay.time_start_ms > nav_delay.time_max_ms) {
+        nav_delay.time_max_ms = 0;
+        return true;
+    }
+    return false;
+}
+
 /********************************************************************************/
 //  Condition (May) commands
 /********************************************************************************/