From a86d20c8449108965986e8ed0093b44de9d40a45 Mon Sep 17 00:00:00 2001
From: Yuri <yuri.rage@gmail.com>
Date: Thu, 30 Jun 2022 13:43:32 -0500
Subject: [PATCH] Copter: implement set_desired_speed

---
 ArduCopter/Copter.cpp | 12 ++++++++++++
 ArduCopter/Copter.h   |  1 +
 2 files changed, 13 insertions(+)

diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index 70314c9b56..e8df39b61b 100644
--- a/ArduCopter/Copter.cpp
+++ b/ArduCopter/Copter.cpp
@@ -418,6 +418,18 @@ bool Copter::set_circle_rate(float rate_dps)
     return true;
 }
 
+// set desired speed (m/s). Used for scripting.
+bool Copter::set_desired_speed(float speed)
+{
+    // exit if vehicle is not in auto mode
+    if (!flightmode->is_autopilot()) {
+        return false;
+    }
+
+    wp_nav->set_speed_xy(speed * 100.0f);
+    return true;
+}
+
 // returns true if mode supports NAV_SCRIPT_TIME mission commands
 bool Copter::nav_scripting_enable(uint8_t mode)
 {
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 6d3cb7248b..c39e378951 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -653,6 +653,7 @@ 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 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;
     void nav_script_time_done(uint16_t id) override;