aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeControl: Fix Autotune high hover throttle based limits

Browse Source
This commit is contained in:
Leonard Hall 2018-12-21 19:19:07 +10:30 committed by Andrew Tridgell
parent b25f7a38e8
commit 8d572e8565
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -983,7 +983,9 @@ float AC_AttitudeControl::max_rate_step_bf_roll()
{
float alpha = get_rate_roll_pid().get_filt_alpha();
float alpha_remaining = 1-alpha;
return 2.0f*_motors.get_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());
// 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());
}
// Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
@ -991,7 +993,9 @@ float AC_AttitudeControl::max_rate_step_bf_pitch()
{
float alpha = get_rate_pitch_pid().get_filt_alpha();
float alpha_remaining = 1-alpha;
return 2.0f*_motors.get_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());
// 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());
}
// Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
@ -999,5 +1003,7 @@ float AC_AttitudeControl::max_rate_step_bf_yaw()
{
float alpha = get_rate_yaw_pid().get_filt_alpha();
float alpha_remaining = 1-alpha;
return 2.0f*_motors.get_throttle_hover()*AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_yaw_pid().kD())/_dt + get_rate_yaw_pid().kP());
// 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());
}