Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PID_2D: add reset for filter and D term

This commit is contained in:
Leonard Hall 2018-08-21 15:55:40 +09:30 committed by Randy Mackay
parent 3c76ec492b
commit 1098890155
1 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AC_PID/AC_PID_2D.cpp
View File

@ -59,8 +59,8 @@ AC_PID_2D::AC_PID_2D(float initial_p, float initial_i, float initial_d, float in
filt_hz(initial_filt_hz); filt_hz(initial_filt_hz);
filt_d_hz(initial_filt_d_hz); filt_d_hz(initial_filt_d_hz);
// reset input filter to first value received // reset input filter to first value received and derivitive to zero
_flags._reset_filter = true; reset_filter();
} }
// set_dt - set time step in seconds // set_dt - set time step in seconds
@ -129,13 +129,6 @@ void AC_PID_2D::set_input_filter_d(const Vector2f& input_delta)
return; return;
} }
// reset input filter
if (_flags._reset_filter) {
_flags._reset_filter = false;
_derivative.x = 0.0f;
_derivative.y = 0.0f;
}
// update filter and calculate derivative // update filter and calculate derivative
if (is_positive(_dt)) { if (is_positive(_dt)) {
const Vector2f derivative = input_delta / _dt; const Vector2f derivative = input_delta / _dt;
@ -193,6 +186,14 @@ void AC_PID_2D::reset_I()
_integrator.zero(); _integrator.zero();
} }
void AC_PID_2D::reset_filter()
{
_flags._reset_filter = true;
_derivative.x = 0.0f;
_derivative.y = 0.0f;
_integrator.zero();
}
void AC_PID_2D::load_gains() void AC_PID_2D::load_gains()
{ {
_kp.load(); _kp.load();