From 3744826790fef77beb987d6b93244596e8e95d64 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 6 Oct 2024 09:59:38 +1030 Subject: [PATCH] AC_AttitudeControl: Squash Feedback --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 12 ++++++++---- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 2 +- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 83a3727428040c..6c52ed1a78e799 100644 --- 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 index 641be5ff6b6962..4b95b680b7b917 100644 --- 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