fly/flightgear
1
0
Fork 0
Code Activity

Roy Ovesen:

Browse source 
I've added some features to the PID controller:

Ability to set desired sampling interval in seconds. Use <Ts> under <config>
to set the desired sampling interval of the PID controller.

Example:
<config>
     <Ts>0.1</Ts>        <!-- desired sampling interval -->
     <Kp>-0.05</Kp>        <!-- proportional gain -->
     <beta>1.0</beta>    <!-- input value weighing factor -->
     ...
     ...
</config>

Ts defaults to 0.0, so if you don't set it it samples at the highest possible
frequency.


Add an offset to the input variables (input and reference).
Example:
    <reference>
      <prop>/controls/flight/elevator</prop>
      <scale>-1.5</scale>
      <offset>1.0</offset>
    </reference>

Note that <scale> has higher precedence than <offset>, regardless of the order
that they appear in the config file.
This commit is contained in:
curt 2005-02-01 21:00:24 +00:00
parent 5a5b67bab1
commit ec50bcfb5a
2 changed files with 31 additions and 7 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

32
src/Autopilot/xmlauto.cxx
View file

@ -38,6 +38,8 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
r_n( 0.0 ),
y_scale( 1.0 ),
r_scale( 1.0 ),
y_offset( 0.0 ),
r_offset( 0.0 ),
Kp( 0.0 ),
alpha( 0.1 ),
beta( 1.0 ),
@ -49,7 +51,8 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
ep_n_1( 0.0 ),
edf_n_1( 0.0 ),
edf_n_2( 0.0 ),
u_n_1( 0.0 )
u_n_1( 0.0 ),
desiredTs( 0.0 )
{
int i;
for ( i = 0; i < node->nChildren(); ++i ) {
@ -82,6 +85,10 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
if ( prop != NULL ) {
y_scale = prop->getDoubleValue();
}
prop = child->getChild( "offset" );
if ( prop != NULL ) {
y_offset = prop->getDoubleValue();
}
} else if ( cname == "reference" ) {
SGPropertyNode *prop = child->getChild( "prop" );
if ( prop != NULL ) {
@ -96,6 +103,10 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
if ( prop != NULL ) {
r_scale = prop->getDoubleValue();
}
prop = child->getChild( "offset" );
if ( prop != NULL ) {
r_offset = prop->getDoubleValue();
}
} else if ( cname == "output" ) {
int i = 0;
SGPropertyNode *prop;
@ -107,6 +118,11 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
} else if ( cname == "config" ) {
SGPropertyNode *prop;
prop = child->getChild( "Ts" );
if ( prop != NULL ) {
desiredTs = prop->getDoubleValue();
}
prop = child->getChild( "Kp" );
if ( prop != NULL ) {
Kp = prop->getDoubleValue();
@ -215,13 +231,16 @@ void FGPIDController::update( double dt ) {
double Tf; // filter time
double delta_u_n = 0.0; // incremental output
double u_n = 0.0; // absolute output
double Ts = dt; // Sampling interval (sec)
double Ts; // sampling interval (sec)
if ( Ts <= 0.0 ) {
elapsedTime += dt;
if ( elapsedTime <= desiredTs ) {
// do nothing if time step is not positive (i.e. no time has
// elapsed)
return;
}
Ts = elapsedTime;
elapsedTime = 0.0;
if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
if ( !enabled ) {
@ -239,16 +258,17 @@ void FGPIDController::update( double dt ) {
}
if ( enabled && Ts > 0.0) {
if ( debug ) cout << "Updating " << name << endl;
if ( debug ) cout << "Updating " << name
<< " Ts " << Ts << endl;
double y_n = 0.0;
if ( input_prop != NULL ) {
y_n = input_prop->getDoubleValue() * y_scale;
y_n = input_prop->getDoubleValue() * y_scale + y_offset;
}
double r_n = 0.0;
if ( r_n_prop != NULL ) {
r_n = r_n_prop->getDoubleValue() * r_scale;
r_n = r_n_prop->getDoubleValue() * r_scale + r_offset;
} else {
r_n = r_n_value;
}

4
src/Autopilot/xmlauto.hxx
View file

@ -100,6 +100,8 @@ private:
double r_n; // reference (set point) value
double y_scale; // scale process input from property system
double r_scale; // scale reference input from property system
double y_offset;
double r_offset;
// Configuration values
double Kp; // proportional gain
@ -123,6 +125,8 @@ 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)