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

Added lowpass filter on PID lib derivative calc

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@927 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2010-11-25 23:14:37 +00:00
parent dc16a2961d
commit 00e7f9b6d0
2 changed files with 22 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/PID/PID.cpp
View File

@ -20,9 +20,19 @@ PID::get_pid(long err, long dt, float scaler)
if(_kd != 0){
// Compute derivative component
//derivative = (error - previous_error)/dt
float derivative = 1000 * (error - _last_error) / dt;
//derivative = (error - previous_error)/delta_time
float derivative = (error - _last_error) / delta_time;
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
derivative = _last_derivative + delta_time/
(RC + delta_time)*(derivative - _last_derivative);
//Serial.print("d: ");
//Serial.println(derivative,DEC);
_last_error = error;
_last_derivative = derivative;
output += _kd * derivative; // Sum the derivative component
}
@ -30,7 +40,7 @@ PID::get_pid(long err, long dt, float scaler)
// Compute integral component
if(_ki != 0){
_integrator += (error * _ki) * scaler * dt *.001;
_integrator += (error * _ki) * scaler * delta_time;
_integrator = constrain(_integrator, -_imax, _imax);
output += _integrator;
}
@ -44,6 +54,7 @@ PID::reset_I(void)
{
_integrator = 0;
_last_error = 0;
_last_error_derivative = 0;
}
@ -179,4 +190,4 @@ PID::test(void)
address += 8;
Serial.println(eeprom_read_word((uint16_t *) address));
*/
}
}

8
libraries/PID/PID.h
View File

@ -32,6 +32,12 @@ public:
private:
static uint8_t RC = 20; // low pass filter cut frequency
// for derivative calculation,
// set to 20 Hz becasue anything over that
// is probably noise, see
// http://en.wikipedia.org/wiki/Low-pass_filter
};
#endif
#endif