AC_AttitudeControl: Squash Feedback

lthall · Oct 5, 2024 · 3744826 · 3744826
1 parent 4284ce8
commit 3744826
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 5 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -276,7 +276,7 @@ void AC_AttitudeControl::landed_gain_reduction(bool landed)
 }
 
 // limit_target_thrust_angle - 
-void AC_AttitudeControl::limit_target_thrust_angle(float thrust_angle_max_cd)
+bool AC_AttitudeControl::limit_target_thrust_angle(float thrust_angle_max_cd)
 {
     float thrust_angle_max_rad = radians(thrust_angle_max_cd * 0.01f);
 
@@ -286,10 +286,12 @@ void AC_AttitudeControl::limit_target_thrust_angle(float thrust_angle_max_cd)
 
     Vector3f gyro = _ahrs.get_gyro_latest();
 
+    // angle_delta is the estimated rotation that the aircraft will experience during the correction
+    Vector3f angle_delta = Vector3f{gyro.x / get_angle_roll_p().kP(), gyro.y / get_angle_pitch_p().kP(), gyro.z / get_angle_yaw_p().kP() };
+
     // 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_update.from_axis_angle(angle_delta);
     attitude_body *= attitude_update;
 
     // The direction of thrust is [0,0,-1] is any body-fixed frame, inc. body frame and target frame.
@@ -302,7 +304,7 @@ void AC_AttitudeControl::limit_target_thrust_angle(float thrust_angle_max_cd)
     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;
+        return false;
     }
 
     // the cross product of the current thrust vector and vertical thrust vector defines the rotation vector
@@ -338,6 +340,8 @@ void AC_AttitudeControl::limit_target_thrust_angle(float thrust_angle_max_cd)
     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);
+
+    return true;
 }
 
 // The attitude controller works around the concept of the desired attitude, target attitude

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
@@ -168,7 +168,7 @@ 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);
+    bool 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