From 181264aa05279aaa0550b25203f9baab6bb92851 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 9 Mar 2020 12:12:45 +0900
Subject: [PATCH] Rover: implement set_target_velocity_NED for use in scripting

---
 APMrover2/Rover.cpp | 20 ++++++++++++++++++++
 APMrover2/Rover.h   |  1 +
 2 files changed, 21 insertions(+)

diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp
index 93e9afd9ac..22a7b1f653 100644
--- a/APMrover2/Rover.cpp
+++ b/APMrover2/Rover.cpp
@@ -150,6 +150,26 @@ bool Rover::set_target_location(const Location& target_loc)
     return control_mode->set_desired_location(target_loc);
 }
 
+// set target velocity (for use by scripting)
+bool Rover::set_target_velocity_NED(const Vector3f& vel_ned)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!control_mode->in_guided_mode()) {
+        return false;
+    }
+
+    // convert vector length into speed
+    const float target_speed_m = safe_sqrt(sq(vel_ned.x) + sq(vel_ned.y));
+
+    // convert vector direction to target yaw
+    const float target_yaw_cd = degrees(atan2f(vel_ned.y, vel_ned.x)) * 100.0f;
+
+    // send target heading and speed
+    mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_speed_m);
+
+    return true;
+}
+
 #if STATS_ENABLED == ENABLED
 /*
   update AP_Stats
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 676bc576e5..23adc5350b 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -278,6 +278,7 @@ private:
 
     // Rover.cpp
     bool set_target_location(const Location& target_loc) override;
+    bool set_target_velocity_NED(const Vector3f& vel_ned) override;
     void stats_update();
     void ahrs_update();
     void gcs_failsafe_check(void);