AC_AttitudeControl: Squash Feedback

ArduPilot · 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