diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 9ff910d3fe..d456f2c752 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/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; } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 453af5741a..097255a8d0 100644 --- a/ArduPlane/Plane.h +++ b/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); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index cb5388f3d0..76deb01bd3 100644 --- a/ArduPlane/commands_logic.cpp +++ b/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 /********************************************************************************/