Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

PID: use NAN to flag a D reset

Browse Source 
this saves a byte per PID for a flag, and gives us a safe way to
reset_I() without causing a spike in D
This commit is contained in:
Andrew Tridgell 2012-11-27 18:41:31 +11:00
parent eac1ad49d6
commit 816fe9d35a
2 changed files with 19 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/PID/PID.cpp
View File

@ -30,7 +30,7 @@ PID::get_pid(int32_t error, float scaler)
// the intergator term. This prevents I buildup from a // the intergator term. This prevents I buildup from a
// previous fight mode from causing a massive return before // previous fight mode from causing a massive return before
// the integrator gets a chance to correct itself // the integrator gets a chance to correct itself
_integrator = 0; reset_I();
} }
_last_t = tnow; _last_t = tnow;
@ -41,7 +41,17 @@ PID::get_pid(int32_t error, float scaler)
// Compute derivative component if time has elapsed // Compute derivative component if time has elapsed
if ((fabs(_kd) > 0) && (dt > 0)) { if ((fabs(_kd) > 0) && (dt > 0)) {
float derivative = (error - _last_error) / delta_time; 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 {
derivative = (error - _last_error) / delta_time;
}
// 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
@ -78,7 +88,9 @@ void
PID::reset_I() PID::reset_I()
{ {
_integrator = 0; _integrator = 0;
_last_derivative = 0; // we use NAN (Not A Number) to indicate that the last
// derivative value is not valid
_last_derivative = NAN;
} }
void void

5
libraries/PID/PID.h
View File

@ -23,6 +23,9 @@ public:
_ki = initial_i; _ki = initial_i;
_kd = initial_d; _kd = initial_d;
_imax = initial_imax; _imax = initial_imax;
// set _last_derivative as invalid when we startup
_last_derivative = NAN;
} }
/// Iterate the PID, return the new control value /// Iterate the PID, return the new control value
@ -104,7 +107,7 @@ private:
float _integrator; ///< integrator value float _integrator; ///< integrator value
int32_t _last_error; ///< last error for derivative int32_t _last_error; ///< last error for derivative
float _last_derivative; ///< last derivative for low-pass filter float _last_derivative; ///< last derivative for low-pass filter
uint32_t _last_t; uint32_t _last_t; ///< last time get_pid() was called in millis
int32_t _get_pid(int32_t error, uint16_t dt, float scaler); int32_t _get_pid(int32_t error, uint16_t dt, float scaler);