From d984ddc1638ded3b408859218a9896aeb0d3446c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 30 Nov 2020 08:43:05 +1100
Subject: [PATCH] Plane: apply circular limits to QLOITER pilot angles

This uses the same logic as Copters get_pilot_desired_angle() to apply
a circular limit and the limits in Q_ANGLE_MAX and Q_LOIT_ANG_MAX to
loiter pilot angles.

Co-authored-by: Kris <kris968658@gmail.com>
---
 ArduPlane/quadplane.cpp | 45 +++++++++++++++++++++++++++++++++++++----
 ArduPlane/quadplane.h   |  3 +++
 2 files changed, 44 insertions(+), 4 deletions(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 4b01aec5e6..6d533a78d8 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -1074,6 +1074,43 @@ float QuadPlane::get_pilot_throttle()
     }
 }
 
+/*
+  get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle.
+  The angle_max_cd and angle_limit_cd are mode dependent
+*/
+void QuadPlane::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const
+{
+    // failsafe check
+    if (plane.failsafe.rc_failsafe || plane.failsafe.throttle_counter > 0) {
+        roll_out_cd = 0;
+        pitch_out_cd = 0;
+        return;
+    }
+
+    // fetch roll and pitch inputs
+    roll_out_cd = plane.channel_roll->get_control_in();
+    pitch_out_cd = plane.channel_pitch->get_control_in();
+
+    // limit max lean angle, always allow for 10 degrees
+    angle_limit_cd = constrain_float(angle_limit_cd, 1000.0f, angle_max_cd);
+
+    // scale roll and pitch inputs to ANGLE_MAX parameter range
+    float scaler = angle_max_cd/4500.0;
+    roll_out_cd *= scaler;
+    pitch_out_cd *= scaler;
+
+    // apply circular limit
+    float total_in = norm(pitch_out_cd, roll_out_cd);
+    if (total_in > angle_limit_cd) {
+        float ratio = angle_limit_cd / total_in;
+        roll_out_cd *= ratio;
+        pitch_out_cd *= ratio;
+    }
+
+    // apply lateral tilt to euler roll conversion
+    roll_out_cd = 100 * degrees(atanf(cosf(radians(pitch_out_cd*0.01))*tanf(radians(roll_out_cd*0.01))));
+}
+
 /*
   control QACRO mode
  */
@@ -1285,10 +1322,10 @@ void QuadPlane::control_loiter()
     pos_control->set_max_accel_z(pilot_accel_z);
 
     // process pilot's roll and pitch input
-    loiter_nav->set_pilot_desired_acceleration(plane.channel_roll->get_control_in(),
-                                               plane.channel_pitch->get_control_in(),
-                                               plane.G_Dt);
-
+    float target_roll_cd, target_pitch_cd;
+    get_pilot_desired_lean_angles(target_roll_cd, target_pitch_cd, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
+    loiter_nav->set_pilot_desired_acceleration(target_roll_cd, target_pitch_cd, plane.G_Dt);
+    
     // run loiter controller
     loiter_nav->update();
 
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index d21ac91203..15daf859af 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -228,6 +228,9 @@ private:
     // get desired climb rate in cm/s
     float get_pilot_desired_climb_rate_cms(void) const;
 
+    // get pilot lean angle
+    void get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const;
+
     // initialise throttle_wait when entering mode
     void init_throttle_wait();