From ffc553f34f1263d74ef2840ff9081aaf916c7bfe Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 12 Sep 2023 08:31:11 +0930 Subject: [PATCH] AC_AttitudeControl: Fix yaw limit calculations --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 24 +++++++++++-------- .../AC_AttitudeControl/AC_AttitudeControl.h | 1 + 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index a64a277d883cf..0b37b8d59affb 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -756,7 +756,7 @@ void AC_AttitudeControl::attitude_controller_run_quat() } // thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion. -// The maximum error in the yaw axis is limited based on the angle yaw P value and acceleration. +// The maximum error in the yaw axis is limited based on static output saturation. void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const { Quaternion thrust_vector_correction; @@ -764,14 +764,17 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& attitude_tar // Todo: Limit roll an pitch error based on output saturation and maximum error. - // Limit Yaw Error based on maximum acceleration - Update to include output saturation and maximum error. - // Currently the limit is based on the maximum acceleration using the linear part of the SQRT controller. - // This should be updated to be based on an angle limit, saturation, or unlimited based on user defined parameters. - Quaternion yaw_vec_correction_quat; - if (!is_zero(_p_angle_yaw.kP()) && fabsf(attitude_error.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP()) { - attitude_error.z = constrain_float(wrap_PI(attitude_error.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP()); - yaw_vec_correction_quat.from_axis_angle(Vector3f{0.0f, 0.0f, attitude_error.z}); - attitude_target = attitude_body * thrust_vector_correction * yaw_vec_correction_quat; + // Limit Yaw Error based to the maximum that would saturate the output when yaw rate is zero. + Quaternion heading_vec_correction_quat; + + float heading_accel_max = constrain_float(get_accel_yaw_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS); + if (!is_zero(get_rate_yaw_pid().kP())) { + float heading_error_max = MIN(inv_sqrt_controller(1.0 / get_rate_yaw_pid().kP(), _p_angle_yaw.kP(), heading_accel_max), AC_ATTITUDE_YAW_MAX_ERROR_ANGLE); + if (!is_zero(_p_angle_yaw.kP()) && fabsf(attitude_error.z) > heading_error_max) { + attitude_error.z = constrain_float(wrap_PI(attitude_error.z), -heading_error_max, heading_error_max); + heading_vec_correction_quat.from_axis_angle(Vector3f{0.0f, 0.0f, attitude_error.z}); + attitude_target = attitude_body * thrust_vector_correction * heading_vec_correction_quat; + } } } @@ -910,6 +913,7 @@ Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const float sin_phi = constrain_float(fabsf(sinf(euler_rad.x)), 0.1f, 1.0f); float cos_phi = constrain_float(fabsf(cosf(euler_rad.x)), 0.1f, 1.0f); float sin_theta = constrain_float(fabsf(sinf(euler_rad.y)), 0.1f, 1.0f); + float cos_theta = constrain_float(fabsf(cosf(euler_rad.y)), 0.1f, 1.0f); Vector3f rot_accel; if (is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || is_negative(euler_accel.x) || is_negative(euler_accel.y) || is_negative(euler_accel.z)) { @@ -919,7 +923,7 @@ Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const } else { rot_accel.x = euler_accel.x; rot_accel.y = MIN(euler_accel.y / cos_phi, euler_accel.z / sin_phi); - rot_accel.z = MIN(MIN(euler_accel.x / sin_theta, euler_accel.y / sin_phi), euler_accel.z / cos_phi); + rot_accel.z = MIN(MIN(euler_accel.x / sin_theta, euler_accel.y / (sin_phi * cos_theta)), euler_accel.z / (cos_phi * cos_theta)); } return rot_accel; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index c6568a50c2acb..a035d3e31bcea 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -27,6 +27,7 @@ #define AC_ATTITUDE_RATE_RELAX_TC 0.16f // This is used to decay the rate I term to 5% in half a second. #define AC_ATTITUDE_THRUST_ERROR_ANGLE radians(30.0f) // Thrust angle error above which yaw corrections are limited +#define AC_ATTITUDE_YAW_MAX_ERROR_ANGLE radians(45.0f) // Thrust angle error above which yaw corrections are limited #define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT 1 // body-frame rate feedforward enabled by default