diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index ffc34f3a90..5042cd4899 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -940,9 +940,6 @@ 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); - bool is_land_command(uint16_t cmd) const; bool do_change_speed(uint8_t speedtype, float speed_target_ms, float rhtottle_pct); @@ -950,13 +947,7 @@ private: return true if in a specific AUTO mission command */ bool in_auto_mission_id(uint16_t command) const; - - // 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 e5ac80ae73..70653ba336 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -206,7 +206,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) #endif case MAV_CMD_NAV_DELAY: - do_nav_delay(cmd); + mode_auto.do_nav_delay(cmd); break; default: @@ -311,7 +311,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret #endif case MAV_CMD_NAV_DELAY: - return verify_nav_delay(cmd); + return mode_auto.verify_nav_delay(cmd); // do commands (always return true) case MAV_CMD_DO_CHANGE_SPEED: @@ -533,7 +533,7 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) } // do_nav_delay - Delay the next navigation command -void Plane::do_nav_delay(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) { nav_delay.time_start_ms = millis(); @@ -880,9 +880,9 @@ bool Plane::verify_altitude_wait(const AP_Mission::Mission_Command &cmd) } // verify_nav_delay - check if we have waited long enough -bool Plane::verify_nav_delay(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd) { - if (arming.is_armed_and_safety_off()) { + if (AP::arming().is_armed_and_safety_off()) { // don't delay while armed, we need a nav controller running return true; } diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 4da472cd06..a88dbec5b6 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -8,6 +8,7 @@ #include #include "quadplane.h" #include +#include class AC_PosControl; class AC_AttitudeControl_Multi; @@ -220,12 +221,24 @@ public: bool mode_allows_autotuning() const override { return true; } bool is_landing() const override; - + + void do_nav_delay(const AP_Mission::Mission_Command& cmd); + bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); + protected: bool _enter() override; void _exit() override; bool _pre_arm_checks(size_t buflen, char *buffer) const override; + +private: + + // Delay the next navigation command + struct { + uint32_t time_max_ms; + uint32_t time_start_ms; + } nav_delay; + };