Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: always use smaller of slew yaw and rate max

Browse Source
This commit is contained in:
Iampete1 2022-01-27 21:03:32 +00:00 committed by Randy Mackay
parent dee3f9dce1
commit b3646494d4
2 changed files with 21 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -147,6 +147,15 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
AP_GROUPEND
};
// get the slew yaw rate limit in deg/s
float AC_AttitudeControl::get_slew_yaw_max_degs() const
{
if (!is_positive(_ang_vel_yaw_max)) {
return _slew_yaw * 0.01;
}
return MIN(_ang_vel_yaw_max, _slew_yaw * 0.01);
}
// Ensure attitude controller have zero errors to relax rate controller output
void AC_AttitudeControl::relax_attitude_controllers()
{
@ -313,6 +322,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle += get_roll_trim_rad();
const float slew_yaw_max_rads = get_slew_yaw_max_rads();
if (_rate_bf_ff_enabled) {
// translate the roll pitch and yaw acceleration limits to the euler axis
const Vector3f euler_accel = euler_accel_limit(_euler_angle_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()});
@ -324,7 +334,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
_euler_rate_target.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _euler_angle_target.y), _input_tc, euler_accel.y, _euler_rate_target.y, _dt);
_euler_rate_target.z = input_shaping_angle(wrap_PI(euler_yaw_angle - _euler_angle_target.z), _input_tc, euler_accel.z, _euler_rate_target.z, _dt);
if (slew_yaw) {
_euler_rate_target.z = constrain_float(_euler_rate_target.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
_euler_rate_target.z = constrain_float(_euler_rate_target.z, -slew_yaw_max_rads, slew_yaw_max_rads);
}
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
@ -339,7 +349,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle
_euler_angle_target.y = euler_pitch_angle;
if (slew_yaw) {
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(euler_yaw_angle - _euler_angle_target.z), -get_slew_yaw_rads() * _dt, get_slew_yaw_rads() * _dt);
float angle_error = constrain_float(wrap_PI(euler_yaw_angle - _euler_angle_target.z), -slew_yaw_max_rads * _dt, slew_yaw_max_rads * _dt);
// Update attitude target from constrained angle error
_euler_angle_target.z = wrap_PI(angle_error + _euler_angle_target.z);
} else {
@ -568,7 +578,7 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, heading_rate, get_accel_yaw_max_radss(), _dt);
// Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), MIN(radians(_ang_vel_yaw_max), get_slew_yaw_rads()));
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), get_slew_yaw_max_rads());
} else {
Quaternion yaw_quat;
yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, heading_rate * _dt});
@ -590,10 +600,10 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust
void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds)
{
// a zero _angle_vel_yaw_max means that setting is disabled
const float ang_vel_yaw_max_rads = is_zero(_ang_vel_yaw_max) ? get_slew_yaw_rads() : MIN(radians(_ang_vel_yaw_max), get_slew_yaw_rads());
const float slew_yaw_max_rads = get_slew_yaw_max_rads();
// Convert from centidegrees on public interface to radians
float heading_rate = constrain_float(radians(heading_rate_cds * 0.01f), -ang_vel_yaw_max_rads, ang_vel_yaw_max_rads);
float heading_rate = constrain_float(radians(heading_rate_cds * 0.01f), -slew_yaw_max_rads, slew_yaw_max_rads);
float heading_angle = radians(heading_angle_cd * 0.01f);
// calculate the attitude target euler angles
@ -614,10 +624,10 @@ void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vect
// the output rate towards the input rate.
_ang_vel_target.x = input_shaping_angle(attitude_error.x, _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
_ang_vel_target.y = input_shaping_angle(attitude_error.y, _input_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
_ang_vel_target.z = input_shaping_angle(attitude_error.z, _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, heading_rate, get_slew_yaw_rads(), _dt);
_ang_vel_target.z = input_shaping_angle(attitude_error.z, _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, heading_rate, slew_yaw_max_rads, _dt);
// Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), ang_vel_yaw_max_rads);
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), slew_yaw_max_rads);
} else {
// set persisted quaternion target attitude
_attitude_target = desired_attitude_quat;

8
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -116,9 +116,9 @@ public:
// get the yaw angular velocity limit in radians/s
float get_ang_vel_yaw_max_rads() const { return radians(_ang_vel_yaw_max); }
// get the yaw slew limit
float get_slew_yaw_cds() const { return _slew_yaw; }
// get the slew yaw rate limit in deg/s
float get_slew_yaw_max_degs() const;
// get the rate control input smoothing time constant
float get_input_tc() const { return _input_tc; }
@ -373,7 +373,7 @@ protected:
virtual float get_roll_trim_rad() { return 0;}
// Return the yaw slew rate limit in radians/s
float get_slew_yaw_rads() { return radians(_slew_yaw * 0.01f); }
float get_slew_yaw_max_rads() const { return radians(get_slew_yaw_max_degs()); }
// Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
AP_Float _slew_yaw;