src/Autopilot/pidcontroller.* added params for control of startup behaviour.
Added <startup-current> param. Default is false which preserves previous
behaviour where internal state is initialised to zero. Otherwise we initialise
internal state using the current error value, which can reduce oscillations.
Added <startup-its> param, default is zero. For the first <startup-its>
iterations we don't update the output, which can be used to cope with initial
bad data from earlier controllers.
Also refactored the code to hopefully make it clearer and modified debug
logging to output all info in a single statement and on a single line for
clarity.
For example avoid initial oscillations with:
<pid-controller>
..
<startup-current>true</startup-current>
...
</pid-controller>
This commit is contained in:
Julian Smith
parent
8d2205ca2e
commit
333c790086
2 changed files with 150 additions and 85 deletions
|
|
@ -38,7 +38,10 @@ PIDController::PIDController():
|
||||||
edf_n_2( 0.0 ),
|
edf_n_2( 0.0 ),
|
||||||
u_n_1( 0.0 ),
|
u_n_1( 0.0 ),
|
||||||
desiredTs( 0.0 ),
|
desiredTs( 0.0 ),
|
||||||
elapsedTime( 0.0 )
|
elapsedTime( 0.0 ),
|
||||||
|
startup_current( false ),
|
||||||
|
startup_its( 0 ),
|
||||||
|
iteration( 0 )
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -93,104 +96,143 @@ PIDController::PIDController():
|
||||||
* u_n
|
* u_n
|
||||||
*/
|
*/
|
||||||
|
|
||||||
void PIDController::update( bool firstTime, double dt )
|
void PIDController::update( bool firstTime, double dt )
|
||||||
{
|
{
|
||||||
if( firstTime ) {
|
|
||||||
ep_n_1 = 0.0;
|
|
||||||
edf_n_2 = edf_n_1 = 0.0;
|
|
||||||
|
|
||||||
// first time being enabled, seed with current property tree value
|
|
||||||
u_n_1 = get_output_value();
|
|
||||||
}
|
|
||||||
|
|
||||||
double u_min = _minInput.get_value();
|
|
||||||
double u_max = _maxInput.get_value();
|
|
||||||
|
|
||||||
elapsedTime += dt;
|
elapsedTime += dt;
|
||||||
if( elapsedTime <= desiredTs ) {
|
if (firstTime) {
|
||||||
// do nothing if time step is not positive (i.e. no time has
|
iteration = 0;
|
||||||
// elapsed)
|
/* We always initialise edf_n_1 to zero, regardless of startup_its. */
|
||||||
|
edf_n_1 = 0;
|
||||||
|
}
|
||||||
|
else if (elapsedTime <= desiredTs ) {
|
||||||
|
// do nothing if not enough time has passed.
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
double Ts = elapsedTime; // sampling interval (sec)
|
|
||||||
|
iteration += 1;
|
||||||
|
|
||||||
|
/* We are going to do an iteration so reset elapsedTime. */
|
||||||
|
double Ts = elapsedTime;
|
||||||
elapsedTime = 0.0;
|
elapsedTime = 0.0;
|
||||||
|
|
||||||
if( Ts > SGLimitsd::min()) {
|
/* Read generic things from our AnalogComponent base class. */
|
||||||
if( _debug ) cout << "Updating " << subsystemId()
|
double y_n = _valueInput.get_value(); // input.
|
||||||
<< " Ts " << Ts << endl;
|
double r_n = _referenceInput.get_value(); // reference.
|
||||||
|
double u_min = _minInput.get_value(); // minimum output.
|
||||||
|
double u_max = _maxInput.get_value(); // maximum output.
|
||||||
|
|
||||||
double y_n = _valueInput.get_value();
|
/* Read things specific to PIDController. */
|
||||||
double r_n = _referenceInput.get_value();
|
double td = Td.get_value(); // derivative time.
|
||||||
|
double ti = Ti.get_value(); // (reciprocal?) integral time.
|
||||||
if ( _debug ) cout << " input = " << y_n << " ref = " << r_n << endl;
|
|
||||||
|
|
||||||
// Calculates proportional error:
|
/*
|
||||||
double ep_n = beta * r_n - y_n;
|
Now do the PID calculations.
|
||||||
if ( _debug ) cout << " ep_n = " << ep_n;
|
*/
|
||||||
if ( _debug ) cout << " ep_n_1 = " << ep_n_1;
|
|
||||||
|
double ep_n = beta * r_n - y_n; // proportional error.
|
||||||
|
double e_n = r_n - y_n; // error.
|
||||||
|
double ed_n = gamma * r_n - y_n; // derivate error.
|
||||||
|
double Tf = alpha * td; // filter time.
|
||||||
|
|
||||||
// Calculates error:
|
double edf_n = 0.0; // derivate error.
|
||||||
double e_n = r_n - y_n;
|
if (td > 0.0) {
|
||||||
if ( _debug ) cout << " e_n = " << e_n;
|
edf_n = edf_n_1 / (Ts/Tf + 1)
|
||||||
|
+ ed_n * (Ts/Tf) / (Ts/Tf + 1);
|
||||||
|
}
|
||||||
|
|
||||||
double edf_n = 0.0;
|
if (firstTime) {
|
||||||
double td = Td.get_value();
|
if (startup_current) {
|
||||||
if ( td > 0.0 ) { // do we need to calcluate derivative error?
|
/* Seed our historical state with current values. This avoids spurious
|
||||||
|
large terms in calculation of delta_u_n below. */
|
||||||
// Calculates derivate error:
|
ep_n_1 = ep_n;
|
||||||
double ed_n = gamma * r_n - y_n;
|
edf_n_2 = edf_n;
|
||||||
if ( _debug ) cout << " ed_n = " << ed_n;
|
edf_n_1 = edf_n;
|
||||||
|
|
||||||
// Calculates filter time:
|
|
||||||
double Tf = alpha * td;
|
|
||||||
if ( _debug ) cout << " Tf = " << Tf;
|
|
||||||
|
|
||||||
// Filters the derivate error:
|
|
||||||
edf_n = edf_n_1 / (Ts/Tf + 1)
|
|
||||||
+ ed_n * (Ts/Tf) / (Ts/Tf + 1);
|
|
||||||
if ( _debug ) cout << " edf_n = " << edf_n;
|
|
||||||
} else {
|
|
||||||
edf_n_2 = edf_n_1 = edf_n = 0.0;
|
|
||||||
}
|
}
|
||||||
|
else {
|
||||||
// Calculates the incremental output:
|
// Default behaviour.
|
||||||
double ti = Ti.get_value();
|
ep_n_1 = 0;
|
||||||
double delta_u_n = 0.0; // incremental output
|
edf_n_2 = 0;
|
||||||
if ( ti > 0.0 ) {
|
// edf_n_1 is already set to zero above.
|
||||||
delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
|
|
||||||
+ ((Ts/ti) * e_n)
|
|
||||||
+ ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
|
|
||||||
|
|
||||||
if ( _debug ) {
|
|
||||||
cout << " delta_u_n = " << delta_u_n << endl;
|
|
||||||
cout << "P:" << Kp.get_value() * (ep_n - ep_n_1)
|
|
||||||
<< " I:" << Kp.get_value() * ((Ts/ti) * e_n)
|
|
||||||
<< " D:" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
|
|
||||||
<< endl;
|
|
||||||
}
|
}
|
||||||
|
u_n_1 = get_output_value();
|
||||||
|
}
|
||||||
|
|
||||||
|
double delta_u_n = 0.0; // incremental output
|
||||||
|
if ( ti > 0.0 ) {
|
||||||
|
delta_u_n = Kp.get_value() * (
|
||||||
|
(ep_n - ep_n_1)
|
||||||
|
+ ((Ts/ti) * e_n)
|
||||||
|
+ ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
const char* saturation = "";
|
||||||
|
double u_n; // absolute output
|
||||||
|
|
||||||
|
if (iteration > startup_its) {
|
||||||
|
|
||||||
|
/* Update the output, clipping to u_min..u_max. */
|
||||||
|
|
||||||
|
u_n = u_n_1 + delta_u_n;
|
||||||
|
|
||||||
|
if (u_n > u_max) {
|
||||||
|
u_n = u_max;
|
||||||
|
saturation = " max_saturation";
|
||||||
}
|
}
|
||||||
|
else if (u_n < u_min) {
|
||||||
// Integrator anti-windup logic:
|
u_n = u_min;
|
||||||
if ( delta_u_n > (u_max - u_n_1) ) {
|
saturation = " min_saturation";
|
||||||
delta_u_n = u_max - u_n_1;
|
|
||||||
if ( _debug ) cout << " max saturation " << endl;
|
|
||||||
} else if ( delta_u_n < (u_min - u_n_1) ) {
|
|
||||||
delta_u_n = u_min - u_n_1;
|
|
||||||
if ( _debug ) cout << " min saturation " << endl;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculates absolute output:
|
|
||||||
double u_n = u_n_1 + delta_u_n;
|
|
||||||
if ( _debug ) cout << " output = " << u_n << endl;
|
|
||||||
|
|
||||||
// Updates indexed values;
|
|
||||||
u_n_1 = u_n;
|
|
||||||
ep_n_1 = ep_n;
|
|
||||||
edf_n_2 = edf_n_1;
|
|
||||||
edf_n_1 = edf_n;
|
|
||||||
|
|
||||||
set_output_value( u_n );
|
set_output_value( u_n );
|
||||||
}
|
}
|
||||||
|
else {
|
||||||
|
/* Do not change the output. Instead get the current output value for
|
||||||
|
use in setting our historical state below. */
|
||||||
|
if (_debug) {
|
||||||
|
cout << subsystemId()
|
||||||
|
<< ": doing nothing."
|
||||||
|
<< " startup_its=" << startup_its
|
||||||
|
<< " iteration=" << iteration
|
||||||
|
<< std::endl;
|
||||||
|
}
|
||||||
|
u_n = get_output_value();
|
||||||
|
}
|
||||||
|
|
||||||
|
if ( _debug ) {
|
||||||
|
cout
|
||||||
|
<< "Updating " << subsystemId()
|
||||||
|
<< " startup_its=" << startup_its
|
||||||
|
<< " startup_current=" << startup_current
|
||||||
|
<< " firstTime=" << firstTime
|
||||||
|
<< " iteration=" << iteration
|
||||||
|
<< " Ts=" << Ts
|
||||||
|
<< " input=" << y_n
|
||||||
|
<< " ref=" << r_n
|
||||||
|
<< " ep_n=" << ep_n
|
||||||
|
<< " ep_n_1=" << ep_n_1
|
||||||
|
<< " e_n=" << e_n
|
||||||
|
<< " ed_n=" << ed_n
|
||||||
|
<< " Tf=" << Tf
|
||||||
|
<< " edf_n=" << edf_n
|
||||||
|
<< " edf_n_1=" << edf_n_1
|
||||||
|
<< " edf_n_2=" << edf_n_2
|
||||||
|
<< " ti=" << ti
|
||||||
|
<< " delta_u_n=" << delta_u_n
|
||||||
|
<< " P=" << Kp.get_value() * (ep_n - ep_n_1)
|
||||||
|
<< " I=" << Kp.get_value() * ((Ts/ti) * e_n)
|
||||||
|
<< " D=" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
|
||||||
|
<< saturation
|
||||||
|
<< " u_n_1=" << u_n_1
|
||||||
|
<< " delta_u_n=" << delta_u_n
|
||||||
|
<< " output=" << u_n
|
||||||
|
<< std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Updates indexed values;
|
||||||
|
u_n_1 = u_n;
|
||||||
|
ep_n_1 = ep_n;
|
||||||
|
edf_n_2 = edf_n_1;
|
||||||
|
edf_n_1 = edf_n;
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
|
@ -237,6 +279,16 @@ bool PIDController::configure( SGPropertyNode& cfg_node,
|
||||||
gamma = cfg_node.getDoubleValue();
|
gamma = cfg_node.getDoubleValue();
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (cfg_name == "startup-its") {
|
||||||
|
startup_its = cfg_node.getIntValue();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (cfg_name == "startup-current") {
|
||||||
|
startup_current = cfg_node.getBoolValue();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
|
return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -58,8 +58,21 @@ private:
|
||||||
double edf_n_1; // edf[n-1] (derivative error)
|
double edf_n_1; // edf[n-1] (derivative error)
|
||||||
double edf_n_2; // edf[n-2] (derivative error)
|
double edf_n_2; // edf[n-2] (derivative error)
|
||||||
double u_n_1; // u[n-1] (output)
|
double u_n_1; // u[n-1] (output)
|
||||||
double desiredTs; // desired sampling interval (sec)
|
double desiredTs; // desired sampling interval (sec)
|
||||||
double elapsedTime; // elapsed time (sec)
|
double elapsedTime; // elapsed time (sec)
|
||||||
|
|
||||||
|
/* If startup_current is false (the default), we initialise internal state
|
||||||
|
variables to zero.
|
||||||
|
|
||||||
|
Otherwise we initialise internal variables to the current values, which can
|
||||||
|
reduce initial transient behaviour. */
|
||||||
|
bool startup_current;
|
||||||
|
|
||||||
|
/* For the first startup_its iterations we don't modify our output. Default
|
||||||
|
is zero. */
|
||||||
|
unsigned startup_its;
|
||||||
|
|
||||||
|
unsigned iteration;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
virtual bool configure( SGPropertyNode& cfg_node,
|
virtual bool configure( SGPropertyNode& cfg_node,
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue