From 1251a91430c43c5a913945be443ffcf9a559152e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 17 Feb 2022 21:59:16 +0900 Subject: [PATCH] Copter: add nav_script_time mission command support --- ArduCopter/Copter.cpp | 26 +++++++++++++ ArduCopter/Copter.h | 3 ++ ArduCopter/mode.h | 26 ++++++++++++- ArduCopter/mode_auto.cpp | 79 ++++++++++++++++++++++++++++++++++++++-- 4 files changed, 130 insertions(+), 4 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 3210b11714..70314c9b56 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -418,6 +418,32 @@ bool Copter::set_circle_rate(float rate_dps) return true; } +// returns true if mode supports NAV_SCRIPT_TIME mission commands +bool Copter::nav_scripting_enable(uint8_t mode) +{ + return mode == (uint8_t)mode_auto.mode_number(); +} + +// lua scripts use this to retrieve the contents of the active command +bool Copter::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) +{ + if (flightmode != &mode_auto) { + return false; + } + + return mode_auto.nav_script_time(id, cmd, arg1, arg2); +} + +// lua scripts use this to indicate when they have complete the command +void Copter::nav_script_time_done(uint16_t id) +{ + if (flightmode != &mode_auto) { + return; + } + + return mode_auto.nav_script_time_done(id); +} + #endif // AP_SCRIPTING_ENABLED diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index d852d9fe44..48b1ca783f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -653,6 +653,9 @@ private: bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override; bool get_circle_radius(float &radius_m) override; bool set_circle_rate(float rate_dps) override; + bool nav_scripting_enable(uint8_t mode) override; + bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) override; + void nav_script_time_done(uint16_t id) override; #endif // AP_SCRIPTING_ENABLED void rc_loop(); void throttle_loop(); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index bae3ed679b..cc27ab6247 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -390,7 +390,7 @@ public: bool has_manual_throttle() const override { return false; } bool allows_arming(AP_Arming::Method method) const override; bool is_autopilot() const override { return true; } - bool in_guided_mode() const override { return mode() == SubMode::NAVGUIDED; } + bool in_guided_mode() const override { return mode() == SubMode::NAVGUIDED || mode() == SubMode::NAV_SCRIPT_TIME; } // Auto modes enum class SubMode : uint8_t { @@ -404,6 +404,7 @@ public: LOITER, LOITER_TO_ALT, NAV_PAYLOAD_PLACE, + NAV_SCRIPT_TIME, }; // Auto @@ -438,6 +439,10 @@ public: // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode bool jump_to_landing_sequence_auto_RTL(ModeReason reason); + // lua accessors for nav script time support + bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2); + void nav_script_time_done(uint16_t id); + AP_Mission mission{ FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &), FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &), @@ -522,6 +527,9 @@ private: #endif void do_payload_place(const AP_Mission::Mission_Command& cmd); void do_RTL(void); +#if AP_SCRIPTING_ENABLED + void do_nav_script_time(const AP_Mission::Mission_Command& cmd); +#endif bool verify_takeoff(); bool verify_land(); @@ -540,6 +548,9 @@ private: bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd); #endif bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); +#if AP_SCRIPTING_ENABLED + bool verify_nav_script_time(); +#endif // Loiter control uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) @@ -582,6 +593,19 @@ private: // True if we have entered AUTO to perform a DO_LAND_START landing sequence and we should report as AUTO RTL mode bool auto_RTL; + +#if AP_SCRIPTING_ENABLED + // nav_script_time command variables + struct { + bool done; // true once lua script indicates it has completed + uint16_t id; // unique id to avoid race conditions between commands and lua scripts + uint32_t start_ms; // system time nav_script_time command was received (used for timeout) + uint8_t command; // command number provided by mission command + uint8_t timeout_s; // timeout (in seconds) provided by mission command + float arg1; // 1st argument provided by mission command + float arg2; // 2nd argument provided by mission command + } nav_scripting; +#endif }; #if AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 6b74938358..056732ebc5 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -135,7 +135,8 @@ void ModeAuto::run() break; case SubMode::NAVGUIDED: -#if NAV_GUIDED == ENABLED + case SubMode::NAV_SCRIPT_TIME: +#if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED nav_guided_run(); #endif break; @@ -199,6 +200,31 @@ bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason) return false; } +// lua scripts use this to retrieve the contents of the active command +bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) +{ +#if AP_SCRIPTING_ENABLED + if (_mode == SubMode::NAV_SCRIPT_TIME) { + id = nav_scripting.id; + cmd = nav_scripting.command; + arg1 = nav_scripting.arg1; + arg2 = nav_scripting.arg2; + return true; + } +#endif + return false; +} + +// lua scripts use this to indicate when they have complete the command +void ModeAuto::nav_script_time_done(uint16_t id) +{ +#if AP_SCRIPTING_ENABLED + if ((_mode == SubMode::NAV_SCRIPT_TIME) && (id == nav_scripting.id)) { + nav_scripting.done = true; + } +#endif +} + // auto_loiter_start - initialises loitering in auto mode // returns success/failure because this can be called by exit_mission bool ModeAuto::loiter_start() @@ -520,6 +546,12 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) do_payload_place(cmd); break; +#if AP_SCRIPTING_ENABLED + case MAV_CMD_NAV_SCRIPT_TIME: + do_nav_script_time(cmd); + break; +#endif + // // conditional commands // @@ -749,6 +781,12 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) cmd_complete = verify_nav_delay(cmd); break; +#if AP_SCRIPTING_ENABLED + case MAV_CMD_NAV_SCRIPT_TIME: + cmd_complete = verify_nav_script_time(); + break; +#endif + /// /// conditional commands /// @@ -904,7 +942,7 @@ void ModeAuto::circle_run() } } -#if NAV_GUIDED == ENABLED +#if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED // auto_nav_guided_run - allows control by external navigation controller // called by auto_run at 100hz or more void ModeAuto::nav_guided_run() @@ -912,7 +950,7 @@ void ModeAuto::nav_guided_run() // call regular guided flight mode run function copter.mode_guided.run(); } -#endif // NAV_GUIDED +#endif // NAV_GUIDED || AP_SCRIPTING_ENABLED // auto_loiter_run - loiter in AUTO flight mode // called by auto_run at 100hz or more @@ -1442,6 +1480,28 @@ void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd) gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000)); } +#if AP_SCRIPTING_ENABLED +// start accepting position, velocity and acceleration targets from lua scripts +void ModeAuto::do_nav_script_time(const AP_Mission::Mission_Command& cmd) +{ + // call regular guided flight mode initialisation + if (copter.mode_guided.init(true)) { + _mode = SubMode::NAV_SCRIPT_TIME; + nav_scripting.done = false; + nav_scripting.id++; + nav_scripting.start_ms = millis(); + nav_scripting.command = cmd.content.nav_script_time.command; + nav_scripting.timeout_s = cmd.content.nav_script_time.timeout_s; + nav_scripting.arg1 = cmd.content.nav_script_time.arg1; + nav_scripting.arg2 = cmd.content.nav_script_time.arg2; + } else { + // for safety we set nav_scripting to done to protect against the mission getting stuck + nav_scripting.done = true; + } +} +#endif + + /********************************************************************************/ // Condition (May) commands /********************************************************************************/ @@ -1968,4 +2028,17 @@ bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd) return false; } +#if AP_SCRIPTING_ENABLED +// check if verify_nav_script_time command has completed +bool ModeAuto::verify_nav_script_time() +{ + // if done or timeout then return true + if (nav_scripting.done || + (AP_HAL::millis() - nav_scripting.start_ms) > (nav_scripting.timeout_s * 1000)) { + return true; + } + return false; +} +#endif + #endif