From 4284ce83a01407f89b7b0f6d29e2ac86acebac66 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Sat, 5 Oct 2024 22:21:34 +0930
Subject: [PATCH] AC_AttitudeControl: Add function to limit target attitude
 thrust angle

---
 .../AC_AttitudeControl/AC_AttitudeControl.cpp | 65 +++++++++++++++++++
 .../AC_AttitudeControl/AC_AttitudeControl.h   |  2 +
 2 files changed, 67 insertions(+)

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
index fbfa3305be080b..83a3727428040c 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -275,6 +275,71 @@ void AC_AttitudeControl::landed_gain_reduction(bool landed)
     set_angle_P_scale_mult(scale_mult);
 }
 
+// limit_target_thrust_angle - 
+void AC_AttitudeControl::limit_target_thrust_angle(float thrust_angle_max_cd)
+{
+    float thrust_angle_max_rad = radians(thrust_angle_max_cd * 0.01f);
+
+    // attitude_body represents a quaternion rotation in NED frame to the body
+    Quaternion attitude_body;
+    _ahrs.get_quat_body_to_ned(attitude_body);
+    
+    Vector3f gyro = _ahrs.get_gyro_latest();
+
+    // attitude_update represents a quaternion rotation representing the expected rotation in the next time_constant
+    Quaternion attitude_update;
+    float time_constant = MAX(1.0 / get_angle_roll_p().kP(), 1.0 / get_angle_pitch_p().kP() );
+    attitude_update.from_axis_angle(gyro * time_constant);
+    attitude_body *= attitude_update;
+
+    // The direction of thrust is [0,0,-1] is any body-fixed frame, inc. body frame and target frame.
+    const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f};
+    
+    // Rotating [0,0,-1] by attitude_body expresses (gets a view of) the thrust vector in the inertial frame
+    Vector3f thrust_vec = attitude_body * thrust_vector_up;
+
+    // the dot product is used to calculate the current lean angle
+    float thrust_correction_angle = acosf(constrain_float(thrust_vec * thrust_vector_up, -1.0f, 1.0f));
+
+    if (abs(thrust_correction_angle) <= thrust_angle_max_rad) {
+        return;
+    }
+
+    // the cross product of the current thrust vector and vertical thrust vector defines the rotation vector
+    Vector3f thrust_vec_cross = thrust_vec % thrust_vector_up;
+
+    // Normalize the thrust rotation vector
+    if (thrust_vec_cross.is_zero()) {
+        thrust_vec_cross.xy() = gyro.xy();
+        thrust_vec_cross.z = 0.0;
+        if (thrust_vec_cross.is_zero()) {
+            // choose recovery in the pitch axis
+            const Vector3f y_axis{0.0f, 1.0f, 0.0f};
+            thrust_vec_cross = _attitude_target * y_axis;
+        } else {
+            // choose recovery in the current rotation direction
+            thrust_vec_cross.normalize();
+        }
+    } else {
+        thrust_vec_cross.normalize();
+    }
+
+    // subtract the maximum lean angle from the current thrust angle
+    thrust_correction_angle -= constrain_float(thrust_correction_angle, -thrust_angle_max_rad, thrust_angle_max_rad);
+
+    // calculate the closest _attitude_target using roll and pitch only, with a thrust angle of thrust_angle_max_rad
+    Quaternion attitude_target_offset;
+    attitude_target_offset.from_axis_angle(thrust_vec_cross, thrust_correction_angle);
+    _attitude_target = attitude_target_offset * attitude_body;
+
+    // Set angular velocity targets when further than 1.25 * thrust_angle_max_cd
+    // this ensures small angles past thrust_angle_max_cd do not zero angular velocity targets and prevent full recovery
+    // angular velocity targets must be zeroed at large thrust angles to prevent them from preventing efficient recovery
+    float ang_vel_scalar = MAX( 0.0, 1.0f - (abs(thrust_correction_angle) / (0.25 * thrust_angle_max_cd)));
+    _ang_vel_target *= ang_vel_scalar;
+    ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
+}
+
 // The attitude controller works around the concept of the desired attitude, target attitude
 // and measured attitude. The desired attitude is the attitude input into the attitude controller
 // that expresses where the higher level code would like the aircraft to move to. The target attitude is moved
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
index 319c2269d2471c..641be5ff6b6962 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
@@ -168,6 +168,8 @@ class AC_AttitudeControl {
     // Reduce attitude control gains while landed to stop ground resonance
     void landed_gain_reduction(bool landed);
 
+    void limit_target_thrust_angle(float thrust_angle_max_cd);
+
     // Sets attitude target to vehicle attitude and sets all rates to zero
     // If reset_rate is false rates are not reset to allow the rate controllers to run
     void reset_target_and_rate(bool reset_rate = true);