fly/flightgear
1
0
Fork 0
Code Activity

src/Autopilot/pidcontroller.* added params for control of startup behaviour.

Browse source 
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 2021-08-01 22:42:52 +01:00
parent 8d2205ca2e
commit 333c790086
2 changed files with 150 additions and 85 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

218
src/Autopilot/pidcontroller.cxx
View file

@ -38,7 +38,10 @@ PIDController::PIDController():
edf_n_2( 0.0 ),
u_n_1( 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
*/
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;
if( elapsedTime <= desiredTs ) {
// do nothing if time step is not positive (i.e. no time has
// elapsed)
if (firstTime) {
iteration = 0;
/* 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;
}
double Ts = elapsedTime; // sampling interval (sec)
iteration += 1;
/* We are going to do an iteration so reset elapsedTime. */
double Ts = elapsedTime;
elapsedTime = 0.0;
if( Ts > SGLimitsd::min()) {
if( _debug ) cout << "Updating " << subsystemId()
<< " Ts " << Ts << endl;
/* Read generic things from our AnalogComponent base class. */
double y_n = _valueInput.get_value(); // input.
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();
double r_n = _referenceInput.get_value();
if ( _debug ) cout << " input = " << y_n << " ref = " << r_n << endl;
/* Read things specific to PIDController. */
double td = Td.get_value(); // derivative time.
double ti = Ti.get_value(); // (reciprocal?) integral time.
// Calculates proportional error:
double ep_n = beta * r_n - y_n;
if ( _debug ) cout << " ep_n = " << ep_n;
if ( _debug ) cout << " ep_n_1 = " << ep_n_1;
/*
Now do the PID calculations.
*/
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 e_n = r_n - y_n;
if ( _debug ) cout << " e_n = " << e_n;
double edf_n = 0.0; // derivate error.
if (td > 0.0) {
edf_n = edf_n_1 / (Ts/Tf + 1)
+ ed_n * (Ts/Tf) / (Ts/Tf + 1);
}
double edf_n = 0.0;
double td = Td.get_value();
if ( td > 0.0 ) { // do we need to calcluate derivative error?
// Calculates derivate error:
double ed_n = gamma * r_n - y_n;
if ( _debug ) cout << " ed_n = " << ed_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;
if (firstTime) {
if (startup_current) {
/* Seed our historical state with current values. This avoids spurious
large terms in calculation of delta_u_n below. */
ep_n_1 = ep_n;
edf_n_2 = edf_n;
edf_n_1 = edf_n;
}
// Calculates the incremental output:
double ti = Ti.get_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)) );
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;
else {
// Default behaviour.
ep_n_1 = 0;
edf_n_2 = 0;
// edf_n_1 is already set to zero above.
}
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";
}
// Integrator anti-windup logic:
if ( delta_u_n > (u_max - u_n_1) ) {
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;
else if (u_n < u_min) {
u_n = u_min;
saturation = " min_saturation";
}
// 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 );
}
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();
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);
}

17
src/Autopilot/pidcontroller.hxx
View file

@ -58,8 +58,21 @@ private:
double edf_n_1; // edf[n-1] (derivative error)
double edf_n_2; // edf[n-2] (derivative error)
double u_n_1; // u[n-1] (output)
double desiredTs; // desired sampling interval (sec)
double elapsedTime; // elapsed time (sec)
double desiredTs; // desired sampling interval (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:
virtual bool configure( SGPropertyNode& cfg_node,