From d9ecf514575f1a1ecc4d54ae183561b911e479cd Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 21 Apr 2020 20:50:25 +0900
Subject: [PATCH] Copter: auto mode loiter-turns accepts pilot yaw input

---
 ArduCopter/autoyaw.cpp   | 11 +++++++++++
 ArduCopter/defines.h     |  1 +
 ArduCopter/mode_auto.cpp | 14 ++++++++++++--
 3 files changed, 24 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp
index 2c0a527b82..ee862b9460 100644
--- a/ArduCopter/autoyaw.cpp
+++ b/ArduCopter/autoyaw.cpp
@@ -94,6 +94,10 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
         // initialise target yaw rate to zero
         _rate_cds = 0.0f;
         break;
+
+    case AUTO_YAW_CIRCLE:
+        // no initialisation required
+        break;
     }
 }
 
@@ -197,6 +201,13 @@ float Mode::AutoYaw::yaw()
         // changes yaw to be same as when quad was armed
         return copter.initial_armed_bearing;
 
+    case AUTO_YAW_CIRCLE:
+        if (copter.circle_nav->is_active()) {
+            return copter.circle_nav->get_yaw();
+        }
+        // return the current attitude target
+        return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z);
+
     case AUTO_YAW_LOOK_AT_NEXT_WP:
     default:
         // point towards next waypoint.
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 34622220a0..aa0802e0d7 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -19,6 +19,7 @@ enum autopilot_yaw_mode {
     AUTO_YAW_LOOK_AHEAD =       4,  // point in the direction the copter is moving
     AUTO_YAW_RESETTOARMEDYAW =  5,  // point towards heading at time motors were armed
     AUTO_YAW_RATE =             6,  // turn at a specified rate (held in auto_yaw_rate)
+    AUTO_YAW_CIRCLE =           7,  // use AC_Circle's provided yaw (used during Loiter-Turns commands)
 };
 
 // Frame types
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index 4926b10346..4f04ed4d07 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -302,7 +302,7 @@ void ModeAuto::circle_start()
     copter.circle_nav->init(copter.circle_nav->get_center(), copter.circle_nav->center_is_terrain_alt());
 
     if (auto_yaw.mode() != AUTO_YAW_ROI) {
-        auto_yaw.set_mode(AUTO_YAW_HOLD);
+        auto_yaw.set_mode(AUTO_YAW_CIRCLE);
     }
 }
 
@@ -829,6 +829,16 @@ void ModeAuto::rtl_run()
 //      called by auto_run at 100hz or more
 void ModeAuto::circle_run()
 {
+    // process pilot's yaw input
+    float target_yaw_rate = 0;
+    if (!copter.failsafe.radio) {
+        // get pilot's desired yaw rate
+        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        if (!is_zero(target_yaw_rate)) {
+            auto_yaw.set_mode(AUTO_YAW_HOLD);
+        }
+    }
+
     // call circle controller
     copter.failsafe_terrain_set_status(copter.circle_nav->update());
 
@@ -837,7 +847,7 @@ void ModeAuto::circle_run()
 
     if (auto_yaw.mode() == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
-        attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true);
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), target_yaw_rate);
     } else {
         // roll, pitch from waypoint controller, yaw heading from auto_heading()
         attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), auto_yaw.yaw(), true);