Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AC_AttitudeControl: always use smaller of slew yaw and rate max #19735

Merged
merged 2 commits into from Jan 31, 2022
Merged

AC_AttitudeControl: always use smaller of slew yaw and rate max #19735

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ba5f034
AC_AttitudeControl: always use smaller of slew yaw and rate max
IamPete1 Jan 27, 2022
28790d3
Copter: AutoYaw: use get_slew_yaw_max_degs in place of get_slew_yaw_cds
IamPete1 Jan 27, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
4 changes: 2 additions & 2 deletions ArduCopter/autoyaw.cpp
View file
Open in desktop
Expand Up @@ -124,9 +124,9 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di
// get turn speed
if (!is_positive(turn_rate_ds)) {
// default to default slew rate
_fixed_yaw_slewrate_cds = copter.attitude_control->get_slew_yaw_cds();
_fixed_yaw_slewrate_cds = copter.attitude_control->get_slew_yaw_max_degs() * 100.0;
} else {
_fixed_yaw_slewrate_cds = MIN(copter.attitude_control->get_slew_yaw_cds(), turn_rate_ds * 100.0);
_fixed_yaw_slewrate_cds = MIN(copter.attitude_control->get_slew_yaw_max_degs(), turn_rate_ds) * 100.0;
}

// set yaw mode
Expand Down
24 changes: 17 additions & 7 deletions libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View file
Open in desktop
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
Open in desktop
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