Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Reverted to older PID. Restored the older filter, and removed PI command. I am no longer using PID for the quad and have switched to PIPI loops which perform much better. The D term in this class is not that great.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@3241 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-09-04 17:47:07 +00:00
parent fa1a6d3beb
commit 4ec8805216
2 changed files with 7 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
libraries/PID/PID.cpp
View File

@ -18,30 +18,17 @@ PID::get_pid(int32_t error, uint16_t dt, 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 = 0; float derivative = (error - _last_error) / delta_time;
// add in
_filter[_filter_index] = (error - _last_error) / delta_time;
_filter_index++;
if(_filter_index >= PID_FILTER_SIZE)
_filter_index = 0;
// sum our filter
for(uint8_t i = 0; i < PID_FILTER_SIZE; i++){
derivative += _filter[i];
}
// grab result
derivative /= PID_FILTER_SIZE;
// 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
//float RC = 1/(2*M_PI*_fCut); float RC = 1/(2*M_PI*_fCut);
//derivative = _last_derivative + (delta_time / (RC + delta_time)) * (derivative - _last_derivative); derivative = _last_derivative +
(delta_time / (RC + delta_time)) * (derivative - _last_derivative);
// update state // update state
_last_error = error; _last_error = error;
//_last_derivative = derivative; _last_derivative = derivative;
// add in derivative component // add in derivative component
output += _kd * derivative; output += _kd * derivative;
@ -64,39 +51,12 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
return output; return output;
} }
long
PID::get_pi(int32_t error, uint16_t dt, float scaler)
{
float output = 0;
float delta_time = (float)dt / 1000.0;
// Compute proportional component
output += error * _kp;
// scale the P components
output *= scaler;
// Compute integral component if time has elapsed
if ((fabs(_ki) > 0) && (dt > 0)) {
_integrator += (error * _ki) * scaler * delta_time;
if (_integrator < -_imax) {
_integrator = -_imax;
} else if (_integrator > _imax) {
_integrator = _imax;
}
output += _integrator;
}
return output;
}
void void
PID::reset_I() PID::reset_I()
{ {
_integrator = 0; _integrator = 0;
_last_error = 0; _last_error = 0;
//_last_derivative = 0; _last_derivative = 0;
} }
void void

9
libraries/PID/PID.h
View File

@ -8,7 +8,6 @@
#include <AP_Common.h> #include <AP_Common.h>
#include <math.h> // for fabs() #include <math.h> // for fabs()
#define PID_FILTER_SIZE 6
/// @class PID /// @class PID
/// @brief Object managing one PID control /// @brief Object managing one PID control
@ -86,9 +85,6 @@ public:
/// ///
long get_pid(int32_t error, uint16_t dt, float scaler = 1.0); long get_pid(int32_t error, uint16_t dt, float scaler = 1.0);
long get_pi(int32_t error, uint16_t dt, float scaler = 1.0);
/// Reset the PID integrator /// Reset the PID integrator
/// ///
void reset_I(); void reset_I();
@ -121,7 +117,6 @@ public:
void kI(const float v) { _ki.set(v); } void kI(const float v) { _ki.set(v); }
void kD(const float v) { _kd.set(v); } void kD(const float v) { _kd.set(v); }
void imax(const int16_t v) { _imax.set(abs(v)); } void imax(const int16_t v) { _imax.set(abs(v)); }
//void filter_size(uint8_t v) { _filter_size = min(v, PID_FILTER_SIZE); }
float get_integrator() const { return _integrator; } float get_integrator() const { return _integrator; }
@ -131,12 +126,10 @@ private:
AP_Float16 _ki; AP_Float16 _ki;
AP_Float16 _kd; AP_Float16 _kd;
AP_Int16 _imax; AP_Int16 _imax;
float _filter[PID_FILTER_SIZE];
uint8_t _filter_index;
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
/// Low pass filter cut frequency for derivative calculation. /// Low pass filter cut frequency for derivative calculation.
/// ///