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

AC_PID: suppress the derivative immediately after reset

Browse Source 
use _last_derivative == NAN to flag that the derivative is invalid
This commit is contained in:
Andrew Tridgell 2012-11-27 18:45:49 +11:00
parent 816fe9d35a
commit 5e7ca5b0f8
2 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_PID/AC_PID.cpp
View File

@ -55,7 +55,17 @@ int32_t AC_PID::get_leaky_i(int32_t error, float dt, float leak_rate)
int32_t AC_PID::get_d(int32_t input, float dt) int32_t AC_PID::get_d(int32_t input, float dt)
{ {
if ((_kd != 0) && (dt != 0)) { if ((_kd != 0) && (dt != 0)) {
float derivative = (input - _last_input) / dt; float derivative;
if (isnan(_last_derivative)) {
// we've just done a reset, suppress the first derivative
// term as we don't want a sudden change in input to cause
// a large D output change
derivative = 0;
_last_derivative = 0;
} else {
// calculate instantaneous derivative
derivative = (input - _last_input) / dt;
}
// discrete low pass filter, cuts out the // discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy // high frequency noise that can drive the controller crazy
@ -87,8 +97,8 @@ void
AC_PID::reset_I() AC_PID::reset_I()
{ {
_integrator = 0; _integrator = 0;
_last_input = 0; // mark derivative as invalid
_last_derivative = 0; _last_derivative = NAN;
} }
void void

3
libraries/AC_PID/AC_PID.h
View File

@ -34,6 +34,9 @@ public:
_ki = initial_i; _ki = initial_i;
_kd = initial_d; _kd = initial_d;
_imax = abs(initial_imax); _imax = abs(initial_imax);
// derivative is invalid on startup
_last_derivative = NAN;
} }
/// Iterate the PID, return the new control value /// Iterate the PID, return the new control value