diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index d37cf57133..f23684ea6f 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -262,13 +262,13 @@ bool Rover::nav_scripting_enable(uint8_t mode) } // lua scripts use this to retrieve the contents of the active command -bool Rover::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) +bool Rover::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) { if (control_mode != &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/Rover/Rover.h b/Rover/Rover.h index 0c42456706..4b3fd5d00b 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -260,7 +260,7 @@ private: bool set_desired_speed(float speed) override; bool get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value) 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; #endif // AP_SCRIPTING_ENABLED void stats_update(); diff --git a/Rover/mode.h b/Rover/mode.h index 44330deb28..761602b6ff 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -267,7 +267,7 @@ public: void start_RTL(); // 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{ @@ -376,6 +376,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/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 41c6920e7c..fbcb06b546 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -273,7 +273,7 @@ void ModeAuto::start_RTL() } // 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 (_submode == AutoSubMode::Auto_NavScriptTime) { @@ -281,6 +281,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 @@ -891,6 +893,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; } else { // for safety we set nav_scripting to done to protect against the mission getting stuck nav_scripting.done = true;