From 8c4732210c0ca9fb8abe601193acaaab4ce68d9f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 14 Oct 2022 11:37:52 +1100 Subject: [PATCH] Copter: two more arguments to NAV_SCRIPT_TIME --- ArduCopter/Copter.cpp | 4 ++-- ArduCopter/Copter.h | 2 +- ArduCopter/mode.h | 4 +++- ArduCopter/mode_auto.cpp | 6 +++++- 4 files changed, 11 insertions(+), 5 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 68f22382dd..a68d3ce890 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -411,13 +411,13 @@ bool Copter::nav_scripting_enable(uint8_t mode) } // 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) +bool Copter::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) { if (flightmode != &mode_auto) { return false; } - return mode_auto.nav_script_time(id, cmd, arg1, arg2); + return mode_auto.nav_script_time(id, cmd, arg1, arg2, arg3, arg4); } // lua scripts use this to indicate when they have complete the command diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index fc1aa937aa..a93fd85f9a 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -663,7 +663,7 @@ private: bool set_circle_rate(float rate_dps) override; bool set_desired_speed(float speed) override; bool nav_scripting_enable(uint8_t mode) override; - bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) override; + bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override; void nav_script_time_done(uint16_t id) override; // lua scripts use this to retrieve EKF failsafe state // returns true if the EKF failsafe has triggered diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 7772fa2689..8bb9c0bd86 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -468,7 +468,7 @@ public: 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); + bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4); void nav_script_time_done(uint16_t id); AP_Mission mission{ @@ -634,6 +634,8 @@ private: 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 + int16_t arg3; // 3rd argument provided by mission command + int16_t arg4; // 4th argument provided by mission command } nav_scripting; #endif diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 07fbeed3f7..5e2da22d44 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -233,7 +233,7 @@ bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason) } // 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) +bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) { #if AP_SCRIPTING_ENABLED if (_mode == SubMode::NAV_SCRIPT_TIME) { @@ -241,6 +241,8 @@ bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &a cmd = nav_scripting.command; arg1 = nav_scripting.arg1; arg2 = nav_scripting.arg2; + arg3 = nav_scripting.arg3; + arg4 = nav_scripting.arg4; return true; } #endif @@ -1650,6 +1652,8 @@ void ModeAuto::do_nav_script_time(const AP_Mission::Mission_Command& cmd) nav_scripting.timeout_s = cmd.content.nav_script_time.timeout_s; nav_scripting.arg1 = cmd.content.nav_script_time.arg1.get(); nav_scripting.arg2 = cmd.content.nav_script_time.arg2.get(); + nav_scripting.arg3 = cmd.content.nav_script_time.arg3; + nav_scripting.arg4 = cmd.content.nav_script_time.arg4; set_submode(SubMode::NAV_SCRIPT_TIME); } else { // for safety we set nav_scripting to done to protect against the mission getting stuck