Skip to content

Commit

Permalink
AC_AttitudeControl: always use smaller of slew yaw and rate max
Browse files Browse the repository at this point in the history
  • Loading branch information
@IamPete1 @rmackay9
IamPete1 authored and rmackay9 committed Jan 31, 2022
1 parent dee3f9d commit b364649
Show file tree
Hide file tree
Showing 2 changed files with 21 additions and 11 deletions.
24 changes: 17 additions & 7 deletions libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
{
Expand Down Expand Up @@ -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()});
Expand All @@ -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
Expand All @@ -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 {
Expand Down Expand Up @@ -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});
Expand All @@ -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
Expand All @@ -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;
Expand Down
8 changes: 4 additions & 4 deletions libraries/AC_AttitudeControl/AC_AttitudeControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,9 +116,9 @@ class AC_AttitudeControl {

// 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; }
Expand Down Expand Up @@ -373,7 +373,7 @@ class AC_AttitudeControl {
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;
Expand Down

0 comments on commit b364649

Please sign in to comment.