From e79768577138e422cfff79b1556aaee8f602124b Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Mon, 1 Jun 2020 00:29:13 +0100
Subject: [PATCH] Copter: add set guided mode angle for scripting

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

diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index 3de628138d..7aa2d31dbb 100644
--- a/ArduCopter/Copter.cpp
+++ b/ArduCopter/Copter.cpp
@@ -309,6 +309,21 @@ bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
     return true;
 }
 
+bool Copter::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)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    Quaternion q;
+    q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg));
+
+    mode_guided.set_angle(q,climb_rate_ms*100,use_yaw_rate,radians(yaw_rate_degs));
+    return true;
+}
+
+
 // rc_loops - reads user input from transmitter/receiver
 // called at 100hz
 void Copter::rc_loop()
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index e642263997..b490c0909c 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -643,6 +643,7 @@ private:
     bool start_takeoff(float alt) override;
     bool set_target_location(const Location& target_loc) override;
     bool set_target_velocity_NED(const Vector3f& vel_ned) override;
+    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;
     void rc_loop();
     void throttle_loop();
     void update_batt_compass(void);