fly/flightgear
1
0
Fork 0
Code Activity

Roy Vegard OVESEN & Lee ELLIOT:

Browse source 
Lee has added two new filter types, "gain" and "reciprocal". These filters can
read their gain factor from a property. In the process we also added minimum
and maximum output clamps that are applicable to all filters.

I added the ability to configure adaptive controllers i.e. the controller gain
can be tied to a property, so that it can be changed at runtime. This
requires a change in the xml structure of the autopilot configuration file:

<Kp>
  <prop>/autopilot/KAP140/settings/ROL/Kp</prop>
  <value>0.10</value>
</Kp>        <!-- proportional gain -->

The old method <Kp>0.10</Kp> still works so as to not break all existing
autopilots, but it will output a warning to use the new method.
This commit is contained in:
mfranz 2008-02-17 09:44:03 +00:00
parent b99a5465b0
commit 7d5c5e4aaf
2 changed files with 123 additions and 61 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

175
src/Autopilot/xmlauto.cxx
View file

@ -124,51 +124,71 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
i++;
}
} else if ( cname == "config" ) {
SGPropertyNode *prop;
SGPropertyNode *config;
prop = child->getChild( "Ts" );
if ( prop != NULL ) {
desiredTs = prop->getDoubleValue();
config = child->getChild( "Ts" );
if ( config != NULL ) {
desiredTs = config->getDoubleValue();
}
prop = child->getChild( "Kp" );
if ( prop != NULL ) {
Kp = prop->getDoubleValue();
config = child->getChild( "Kp" );
if ( config != NULL ) {
SGPropertyNode *val = config->getChild( "value" );
if ( val != NULL ) {
Kp = val->getDoubleValue();
}
SGPropertyNode *prop = config->getChild( "prop" );
if ( prop != NULL ) {
Kp_prop = fgGetNode( prop->getStringValue(), true );
if ( val != NULL ) {
Kp_prop->setDoubleValue(Kp);
}
}
// output deprecated usage warning
if (val == NULL && prop == NULL) {
Kp = config->getDoubleValue();
SG_LOG( SG_AUTOPILOT, SG_WARN, "Deprecated Kp config. Please use <prop> and/or <value> tags." );
if ( name.length() ) {
SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << name );
}
}
}
prop = child->getChild( "beta" );
if ( prop != NULL ) {
beta = prop->getDoubleValue();
config = child->getChild( "beta" );
if ( config != NULL ) {
beta = config->getDoubleValue();
}
prop = child->getChild( "alpha" );
if ( prop != NULL ) {
alpha = prop->getDoubleValue();
config = child->getChild( "alpha" );
if ( config != NULL ) {
alpha = config->getDoubleValue();
}
prop = child->getChild( "gamma" );
if ( prop != NULL ) {
gamma = prop->getDoubleValue();
config = child->getChild( "gamma" );
if ( config != NULL ) {
gamma = config->getDoubleValue();
}
prop = child->getChild( "Ti" );
if ( prop != NULL ) {
Ti = prop->getDoubleValue();
config = child->getChild( "Ti" );
if ( config != NULL ) {
Ti = config->getDoubleValue();
}
prop = child->getChild( "Td" );
if ( prop != NULL ) {
Td = prop->getDoubleValue();
config = child->getChild( "Td" );
if ( config != NULL ) {
Td = config->getDoubleValue();
}
prop = child->getChild( "u_min" );
if ( prop != NULL ) {
u_min = prop->getDoubleValue();
config = child->getChild( "u_min" );
if ( config != NULL ) {
u_min = config->getDoubleValue();
}
prop = child->getChild( "u_max" );
if ( prop != NULL ) {
u_max = prop->getDoubleValue();
config = child->getChild( "u_max" );
if ( config != NULL ) {
u_max = config->getDoubleValue();
}
} else {
SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
@ -311,6 +331,9 @@ void FGPIDController::update( double dt ) {
// Calculates the incremental output:
if ( Ti > 0.0 ) {
if (Kp_prop != NULL) {
Kp = Kp_prop->getDoubleValue();
}
delta_u_n = Kp * ( (ep_n - ep_n_1)
+ ((Ts/Ti) * e_n)
+ ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
@ -627,10 +650,15 @@ void FGPredictor::update( double dt ) {
}
FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node)
FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node):
Tf( 1.0 ),
samples( 1 ),
rateOfChange( 1.0 ),
gainFactor( 1.0 ),
gain_prop( NULL ),
output_min_clamp( -std::numeric_limits<double>::max() ),
output_max_clamp( std::numeric_limits<double>::max() )
{
samples = 1;
int i;
for ( i = 0; i < node->nChildren(); ++i ) {
SGPropertyNode *child = node->getChild(i);
@ -662,6 +690,10 @@ FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node)
filterType = movingAverage;
} else if (cval == "noise-spike") {
filterType = noiseSpike;
} else if (cval == "gain") {
filterType = gain;
} else if (cval == "reciprocal") {
filterType = reciprocal;
}
} else if ( cname == "input" ) {
input_prop = fgGetNode( child->getStringValue(), true );
@ -671,6 +703,20 @@ FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node)
samples = child->getIntValue();
} else if ( cname == "max-rate-of-change" ) {
rateOfChange = child->getDoubleValue();
} else if ( cname == "gain" ) {
SGPropertyNode *val = child->getChild( "value" );
if ( val != NULL ) {
gainFactor = val->getDoubleValue();
}
SGPropertyNode *prop = child->getChild( "prop" );
if ( prop != NULL ) {
gain_prop = fgGetNode( prop->getStringValue(), true );
gain_prop->setDoubleValue(gainFactor);
}
} else if ( cname == "u_min" ) {
output_min_clamp = child->getDoubleValue();
} else if ( cname == "u_max" ) {
output_max_clamp = child->getDoubleValue();
} else if ( cname == "output" ) {
SGPropertyNode *tmp = fgGetNode( child->getStringValue(), true );
output_list.push_back( tmp );
@ -683,11 +729,15 @@ FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node)
void FGDigitalFilter::update(double dt)
{
if ( input_prop != NULL &&
enable_prop != NULL &&
enable_prop->getStringValue() == enable_value) {
if ( (input_prop != NULL &&
enable_prop != NULL &&
enable_prop->getStringValue() == enable_value) ||
(enable_prop == NULL &&
input_prop != NULL) ) {
input.push_front(input_prop->getDoubleValue());
input.resize(samples + 1, 0.0);
if ( !enabled ) {
// first time being enabled, initialize output to the
// value of the output property to avoid bumping.
@ -695,11 +745,6 @@ void FGDigitalFilter::update(double dt)
output.resize(1);
}
enabled = true;
} else if (enable_prop == NULL &&
input_prop != NULL) {
input.push_front(input_prop->getDoubleValue());
input.resize(samples + 1, 0.0);
enabled = true;
} else {
enabled = false;
@ -718,11 +763,6 @@ void FGDigitalFilter::update(double dt)
double alpha = 1 / ((Tf/dt) + 1);
output.push_front(alpha * input[0] +
(1 - alpha) * output[0]);
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output[0] );
}
output.resize(1);
}
else if (filterType == doubleExponential)
{
@ -730,21 +770,11 @@ void FGDigitalFilter::update(double dt)
output.push_front(alpha * alpha * input[0] +
2 * (1 - alpha) * output[0] -
(1 - alpha) * (1 - alpha) * output[1]);
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output[0] );
}
output.resize(2);
}
else if (filterType == movingAverage)
{
output.push_front(output[0] +
(input[0] - input.back()) / samples);
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output[0] );
}
output.resize(1);
}
else if (filterType == noiseSpike)
{
@ -762,13 +792,37 @@ void FGDigitalFilter::update(double dt)
{
output.push_front(input[0]);
}
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output[0] );
}
output.resize(1);
}
else if (filterType == gain)
{
if (gain_prop != NULL) {
gainFactor = gain_prop->getDoubleValue();
}
output[0] = gainFactor * input[0];
}
else if (filterType == reciprocal)
{
if (gain_prop != NULL) {
gainFactor = gain_prop->getDoubleValue();
}
if (input[0] != 0.0) {
output[0] = gainFactor / input[0];
}
}
if (output[0] < output_min_clamp) {
output[0] = output_min_clamp;
}
else if (output[0] > output_max_clamp) {
output[0] = output_max_clamp;
}
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output[0] );
}
output.resize(1);
if (debug)
{
cout << "input:" << input[0]
@ -1009,3 +1063,4 @@ void FGXMLAutopilot::update( double dt ) {
components[i]->update( dt );
}
}

9
src/Autopilot/xmlauto.hxx
View file

@ -111,6 +111,7 @@ private:
// Configuration values
double Kp; // proportional gain
SGPropertyNode_ptr Kp_prop;
double alpha; // low pass filter weighing factor (usually 0.1)
double beta; // process value weighing factor for
@ -238,9 +239,15 @@ private:
double Tf; // Filter time [s]
unsigned int samples; // Number of input samples to average
double rateOfChange; // The maximum allowable rate of change [1/s]
double gainFactor;
double output_min_clamp;
double output_max_clamp;
SGPropertyNode_ptr gain_prop;
deque <double> output;
deque <double> input;
enum filterTypes { exponential, doubleExponential, movingAverage, noiseSpike };
enum filterTypes { exponential, doubleExponential, movingAverage,
noiseSpike, gain, reciprocal };
filterTypes filterType;
bool debug;