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.

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

Move rate limits from Autotune to Attitude Control #19583

Merged
merged 2 commits into from Jan 10, 2022
Merged

Move rate limits from Autotune to Attitude Control #19583

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
b147c05
AC_AttitudeControl: Respect rate limit in max_rate_step
lthall Dec 21, 2021
f404b52
AC_AutoTune: Move rate limit to AC_AttitudeControl
lthall Dec 21, 2021
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
18 changes: 15 additions & 3 deletions libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View file
Open in desktop
Expand Up @@ -1020,7 +1020,11 @@ float AC_AttitudeControl::max_rate_step_bf_roll()
float alpha_remaining = 1 - alpha;
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
return 2.0f * throttle_hover * AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_roll_pid().kD()) / _dt + get_rate_roll_pid().kP());
float rate_max = 2.0f * throttle_hover * AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_roll_pid().kD()) / _dt + get_rate_roll_pid().kP());
if (is_positive(_ang_vel_roll_max)) {
rate_max = MIN(rate_max, get_ang_vel_roll_max_rads());
}
return rate_max;
}

// Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
Expand All @@ -1030,7 +1034,11 @@ float AC_AttitudeControl::max_rate_step_bf_pitch()
float alpha_remaining = 1 - alpha;
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
return 2.0f * throttle_hover * AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_pitch_pid().kD()) / _dt + get_rate_pitch_pid().kP());
float rate_max = 2.0f * throttle_hover * AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_pitch_pid().kD()) / _dt + get_rate_pitch_pid().kP());
if (is_positive(_ang_vel_pitch_max)) {
rate_max = MIN(rate_max, get_ang_vel_pitch_max_rads());
}
return rate_max;
}

// Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
Expand All @@ -1040,7 +1048,11 @@ float AC_AttitudeControl::max_rate_step_bf_yaw()
float alpha_remaining = 1 - alpha;
// todo: When a thrust_max is available we should replace 0.5f with 0.5f * _motors.thrust_max
float throttle_hover = constrain_float(_motors.get_throttle_hover(), 0.1f, 0.5f);
return 2.0f * throttle_hover * AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_yaw_pid().kD()) / _dt + get_rate_yaw_pid().kP());
float rate_max = 2.0f * throttle_hover * AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX / ((alpha_remaining * alpha_remaining * alpha_remaining * alpha * get_rate_yaw_pid().kD()) / _dt + get_rate_yaw_pid().kP());
if (is_positive(_ang_vel_yaw_max)) {
rate_max = MIN(rate_max, get_ang_vel_yaw_max_rads());
}
return rate_max;
}

bool AC_AttitudeControl::pre_arm_checks(const char *param_prefix,
Expand Down
12 changes: 0 additions & 12 deletions libraries/AC_AutoTune/AC_AutoTune.cpp
View file
Open in desktop
Expand Up @@ -529,10 +529,6 @@ void AC_AutoTune::control_attitude()
switch (axis) {
case ROLL: {
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
const float roll_rate_max_cds = degrees(attitude_control->get_ang_vel_roll_max_rads()) * 100;
if (is_positive(roll_rate_max_cds)) {
target_max_rate = MIN(target_max_rate, roll_rate_max_cds);
}
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
Expand All @@ -543,10 +539,6 @@ void AC_AutoTune::control_attitude()
}
case PITCH: {
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
const float pitch_rate_max_cds = degrees(attitude_control->get_ang_vel_pitch_max_rads()) * 100;
if (is_positive(pitch_rate_max_cds)) {
target_max_rate = MIN(target_max_rate, pitch_rate_max_cds);
}
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
Expand All @@ -557,10 +549,6 @@ void AC_AutoTune::control_attitude()
}
case YAW: {
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS);
const float yaw_rate_max_cds = degrees(attitude_control->get_ang_vel_yaw_max_rads()) * 100;
if (is_positive(yaw_rate_max_cds)) {
target_max_rate = MIN(target_max_rate, yaw_rate_max_cds);
}
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD);
abort_angle = AUTOTUNE_TARGET_ANGLE_YAW_CD;
Expand Down