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

APM_Control: fixed bug in slew filter

Browse Source
This commit is contained in:
Andrew Tridgell 2020-10-12 11:25:33 +11:00
parent a4716fb9bf
commit d2a4505184
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/APM_Control/AP_PitchController.cpp
View File

@ -211,7 +211,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
// Calculate the slew rate amplitude produced by the unmodified D term // Calculate the slew rate amplitude produced by the unmodified D term
// calculate a low pass filtered slew rate // calculate a low pass filtered slew rate
float Dterm_slew_rate = _slew_rate_filter.apply(((_pid_info.D - _last_pid_info_D)/ delta_time), delta_time); float Dterm_slew_rate = _slew_rate_filter.apply((fabsf(_pid_info.D - _last_pid_info_D)/ delta_time), delta_time);
// rectify and apply a decaying envelope filter // rectify and apply a decaying envelope filter
float alpha = 1.0f - constrain_float(delta_time/_slew_rate_tau, 0.0f , 1.0f); float alpha = 1.0f - constrain_float(delta_time/_slew_rate_tau, 0.0f , 1.0f);

2
libraries/APM_Control/AP_RollController.cpp
View File

@ -178,7 +178,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
// Calculate the slew rate amplitude produced by the unmodified D term // Calculate the slew rate amplitude produced by the unmodified D term
// calculate a low pass filtered slew rate // calculate a low pass filtered slew rate
float Dterm_slew_rate = _slew_rate_filter.apply(((_pid_info.D - _last_pid_info_D)/ delta_time), delta_time); float Dterm_slew_rate = _slew_rate_filter.apply((fabsf(_pid_info.D - _last_pid_info_D)/ delta_time), delta_time);
// rectify and apply a decaying envelope filter // rectify and apply a decaying envelope filter
float alpha = 1.0f - constrain_float(delta_time/_slew_rate_tau, 0.0f , 1.0f); float alpha = 1.0f - constrain_float(delta_time/_slew_rate_tau, 0.0f , 1.0f);