From cb9ea6f5023e3e6e78ec313156b58cd1331715c1 Mon Sep 17 00:00:00 2001
From: Matt Lawrence <lawrence.matt@gmail.com>
Date: Mon, 6 Jan 2020 17:37:04 -0500
Subject: [PATCH] Copter: Circle mode pilot control of rate & radius

RC pitch stick controls circle mode radius.  RC roll stick controls circle mode rate (speed) and direction.
---
 ArduCopter/mode.h          |  1 +
 ArduCopter/mode_circle.cpp | 47 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 48 insertions(+)

diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 74cda23977..c41b11f6ff 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -596,6 +596,7 @@ private:
 
     // Circle
     bool pilot_yaw_override = false; // true if pilot is overriding yaw
+    bool speed_changing = false;     // true when the roll stick is being held to facilitate stopping at 0 rate
 };
 
 
diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp
index 587e77f8b5..021fc1fab3 100644
--- a/ArduCopter/mode_circle.cpp
+++ b/ArduCopter/mode_circle.cpp
@@ -10,6 +10,7 @@
 bool ModeCircle::init(bool ignore_checks)
 {
     pilot_yaw_override = false;
+    speed_changing = false;
 
     // initialize speeds and accelerations
     pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
@@ -39,6 +40,52 @@ void ModeCircle::run()
         pilot_yaw_override = true;
     }
 
+    // pilot changes to circle rate and radius
+    // skip if in radio failsafe
+    if (!copter.failsafe.radio && copter.circle_nav->pilot_control_enabled()) {
+        // update the circle controller's radius target based on pilot pitch stick inputs
+        const float radius_current = copter.circle_nav->get_radius();           // circle controller's radius target, which begins as the circle_radius parameter
+        const float pitch_stick = channel_pitch->norm_input_dz();               // pitch stick normalized -1 to 1
+        const float nav_speed = copter.wp_nav->get_default_speed_xy();          // copter WP_NAV parameter speed
+        const float radius_pilot_change = (pitch_stick * nav_speed) * G_Dt;     // rate of change
+        const float radius_new = MAX(radius_current + radius_pilot_change,0);   // new radius target
+
+        if (!is_equal(radius_current, radius_new)) {
+            copter.circle_nav->set_radius(radius_new);
+        }
+
+        // update the orbicular rate target based on pilot roll stick inputs
+        // skip if using CH6 tuning knob for circle rate
+        if (g.radio_tuning != TUNING_CIRCLE_RATE) {
+            const float roll_stick = channel_roll->norm_input_dz();         // roll stick normalized -1 to 1
+
+            if (is_zero(roll_stick)) {
+                // no speed change, so reset speed changing flag
+                speed_changing = false;
+            } else {
+                const float rate = copter.circle_nav->get_rate();           // circle controller's rate target, which begins as the circle_rate parameter
+                const float rate_current = copter.circle_nav->get_rate_current(); // current adjusted rate target, which is probably different from _rate
+                const float rate_pilot_change = (roll_stick * G_Dt);        // rate of change from 0 to 1 degrees per second
+                float rate_new = rate_current;                              // new rate target
+                if (is_positive(rate)) {
+                    // currently moving clockwise, constrain 0 to 90
+                    rate_new = constrain_float(rate_current + rate_pilot_change, 0, 90);
+
+                } else if (is_negative(rate)) {
+                    // currently moving counterclockwise, constrain -90 to 0
+                    rate_new = constrain_float(rate_current + rate_pilot_change, -90, 0);
+
+                } else if (is_zero(rate) && !speed_changing) {
+                    // Stopped, pilot has released the roll stick, and pilot now wants to begin moving with the roll stick
+                    rate_new = rate_pilot_change;
+                }
+
+                speed_changing = true;
+                copter.circle_nav->set_rate(rate_new);
+            }
+        }
+    }
+
     // get pilot desired climb rate (or zero if in radio failsafe)
     float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
     // adjust climb rate using rangefinder