fly/flightgear
1
0
Fork 0
Code Activity

Roy Vegard Ovesen:

Browse source 
I've done some changes to xmlauto.cxx.

Only calculate the derivate filtering if derivate time Td is greater than
zero. This means that one can set Td=0.0 in the xml file to completely remove
the derivate action. (Setting Td to zero in the current version would lead to
a division by zero and crash.)

Setting the integrator time Ti to zero doesn't make sense, right! I've
modified so that setting Ti to zero results in the integral action being
completely removed.
This commit is contained in:
curt 2004-03-15 19:09:44 +00:00
parent 1347ca930f
commit a5f9927727
1 changed files with 30 additions and 13 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

43
src/Autopilot/xmlauto.cxx
View file

@ -226,7 +226,7 @@ void FGPIDController::update( double dt ) {
enabled = false;
}
if ( enabled ) {
if ( enabled && Ts > 0.0) {
if ( debug ) cout << "Updating " << name << endl;
double y_n = 0.0;
@ -256,26 +256,44 @@ void FGPIDController::update( double dt ) {
ed_n = gamma * r_n - y_n;
if ( debug ) cout << " ed_n = " << ed_n;
// Calculates filter time:
Tf = alpha * Td;
if ( debug ) cout << " Tf = " << Tf;
if ( Td > 0.0 ) {
// Calculates filter time:
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;
// 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 = ed_n;
}
// Calculates the incremental output:
delta_u_n = Kp * ( (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;
if ( Ti > 0.0 ) {
delta_u_n = Kp * ( (ep_n - ep_n_1)
+ ((Ts/Ti) * e_n)
+ ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
} else if ( Ti <= 0.0 ) {
delta_u_n = Kp * ( (ep_n - ep_n_1)
+ ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
}
if ( debug ) {
cout << " delta_u_n = " << delta_u_n << endl;
cout << "P:" << Kp * (ep_n - ep_n_1)
<< " I:" << Kp * ((Ts/Ti) * e_n)
<< " D:" << Kp * ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
<< endl;
}
// Integrator anti-windup logic:
if ( delta_u_n > (u_max - u_n_1) ) {
delta_u_n = 0;
if ( debug ) cout << " max saturation " << endl;
} else if ( delta_u_n < (u_min - u_n_1) ) {
delta_u_n = 0;
if ( debug ) cout << " min saturation " << endl;
}
// Calculates absolute output:
@ -293,7 +311,6 @@ void FGPIDController::update( double dt ) {
output_list[i]->setDoubleValue( u_n );
}
} else if ( !enabled ) {
u_n = 0.0;
ep_n = 0.0;
edf_n = 0.0;
// Updates indexed values;