Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: Fix yaw limit calculations

This commit is contained in:
Leonard Hall 2023-09-12 08:31:11 +09:30 committed by Peter Barker
parent ed7ca580c2
commit ffc553f34f
2 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -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;
}

1
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -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