fly/flightgear
1
0
Fork 0
Code Activity

added new features:

Browse source 
- conditions for InputValues
- multiple InputValues
some more code cleanup
This commit is contained in:
torsten 2009-03-28 13:04:36 +00:00 committed by Tim Moore
parent 57e6d292a0
commit b0dd43e022
2 changed files with 251 additions and 142 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

253
src/Autopilot/xmlauto.cxx
View file

@ -40,36 +40,39 @@
using std::cout; using std::cout;
using std::endl; using std::endl;
/*
parse element with
<node>
<value>1</value>
<prop>/some/property</prop>
</node>
or
<node>123</node>
or
<node>/some/property</node>
*/
void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffset, double aScale ) void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffset, double aScale )
{ {
delete property;
property = NULL;
value = aValue; value = aValue;
offset = aOffset; property = NULL;
scale = aScale; offset = NULL;
scale = NULL;
min = NULL;
max = NULL;
if( node == NULL ) if( node == NULL )
return; return;
SGPropertyNode * n; SGPropertyNode * n;
if( (n = node->getChild( "scale" )) != NULL ) if( (n = node->getChild("condition")) != NULL ) {
scale = n->getDoubleValue(); _condition = sgReadCondition(node, n);
}
if( (n = node->getChild( "offset" )) != NULL ) if( (n = node->getChild( "scale" )) != NULL ) {
offset = n->getDoubleValue(); scale = new FGXMLAutoInput( n, aScale );
}
if( (n = node->getChild( "offset" )) != NULL ) {
offset = new FGXMLAutoInput( n, aOffset );
}
if( (n = node->getChild( "max" )) != NULL ) {
max = new FGXMLAutoInput( n );
}
if( (n = node->getChild( "min" )) != NULL ) {
min = new FGXMLAutoInput( n );
}
SGPropertyNode *valueNode = node->getChild( "value" ); SGPropertyNode *valueNode = node->getChild( "value" );
if ( valueNode != NULL ) { if ( valueNode != NULL ) {
@ -87,8 +90,9 @@ void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffs
if ( valueNode != NULL ) { if ( valueNode != NULL ) {
// initialize property with given value // initialize property with given value
// if both <prop> and <value> exist // if both <prop> and <value> exist
if( scale != 0 ) double s = get_scale();
property->setDoubleValue( (value - offset)/scale ); if( s != 0 )
property->setDoubleValue( (value - get_offset())/s );
else else
property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
} }
@ -108,6 +112,41 @@ void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffs
} }
} }
void FGXMLAutoInput::set_value( double aValue )
{
double s = get_scale();
if( s != 0 )
property->setDoubleValue( (aValue - get_offset())/s );
else
property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
}
double FGXMLAutoInput::get_value()
{
if( property != NULL )
value = property->getDoubleValue();
if( scale )
value *= scale->get_value();
if( offset )
value += offset->get_value();
if( min ) {
double m = min->get_value();
if( value < m )
value = m;
}
if( max ) {
double m = max->get_value();
if( value > m )
value = m;
}
return value;
}
FGXMLAutoComponent::FGXMLAutoComponent( SGPropertyNode * node ) : FGXMLAutoComponent::FGXMLAutoComponent( SGPropertyNode * node ) :
debug(false), debug(false),
name(""), name(""),
@ -116,8 +155,8 @@ FGXMLAutoComponent::FGXMLAutoComponent( SGPropertyNode * node ) :
enable_value( NULL ), enable_value( NULL ),
honor_passive( false ), honor_passive( false ),
enabled( false ), enabled( false ),
clamp( false ), _condition( NULL ),
_condition( NULL ) feedback_if_disabled( false )
{ {
int i; int i;
SGPropertyNode *prop; SGPropertyNode *prop;
@ -129,6 +168,9 @@ FGXMLAutoComponent::FGXMLAutoComponent( SGPropertyNode * node ) :
if ( cname == "name" ) { if ( cname == "name" ) {
name = cval; name = cval;
} else if ( cname == "feedback-if-disabled" ) {
feedback_if_disabled = child->getBoolValue();
} else if ( cname == "debug" ) { } else if ( cname == "debug" ) {
debug = child->getBoolValue(); debug = child->getBoolValue();
@ -151,11 +193,11 @@ FGXMLAutoComponent::FGXMLAutoComponent( SGPropertyNode * node ) :
} else if ( cname == "input" ) { } else if ( cname == "input" ) {
valueInput.parse( child ); valueInput.push_back( new FGXMLAutoInput( child ) );
} else if ( cname == "reference" ) { } else if ( cname == "reference" ) {
referenceInput.parse( child ); referenceInput.push_back( new FGXMLAutoInput( child ) );
} else if ( cname == "output" ) { } else if ( cname == "output" ) {
// grab all <prop> and <property> childs // grab all <prop> and <property> childs
@ -177,20 +219,26 @@ FGXMLAutoComponent::FGXMLAutoComponent( SGPropertyNode * node ) :
output_list.push_back( fgGetNode(child->getStringValue(), true ) ); output_list.push_back( fgGetNode(child->getStringValue(), true ) );
} else if ( cname == "config" ) { } else if ( cname == "config" ) {
if( (prop = child->getChild("min")) != NULL ) {
uminInput.push_back( new FGXMLAutoInput( prop ) );
}
if( (prop = child->getChild("u_min")) != NULL ) { if( (prop = child->getChild("u_min")) != NULL ) {
uminInput.parse( prop ); uminInput.push_back( new FGXMLAutoInput( prop ) );
clamp = true; }
if( (prop = child->getChild("max")) != NULL ) {
umaxInput.push_back( new FGXMLAutoInput( prop ) );
} }
if( (prop = child->getChild("u_max")) != NULL ) { if( (prop = child->getChild("u_max")) != NULL ) {
umaxInput.parse( prop ); umaxInput.push_back( new FGXMLAutoInput( prop ) );
clamp = true;
} }
} else if ( cname == "min" ) {
uminInput.push_back( new FGXMLAutoInput( child ) );
} else if ( cname == "u_min" ) { } else if ( cname == "u_min" ) {
uminInput.parse( child ); uminInput.push_back( new FGXMLAutoInput( child ) );
clamp = true; } else if ( cname == "max" ) {
umaxInput.push_back( new FGXMLAutoInput( child ) );
} else if ( cname == "u_max" ) { } else if ( cname == "u_max" ) {
umaxInput.parse( child ); umaxInput.push_back( new FGXMLAutoInput( child ) );
clamp = true;
} }
} }
} }
@ -215,6 +263,27 @@ bool FGXMLAutoComponent::isPropertyEnabled()
return true; return true;
} }
void FGXMLAutoComponent::do_feedback_if_disabled()
{
if( output_list.size() > 0 ) {
FGXMLAutoInput * input = valueInput.get_active();
if( input != NULL )
input->set_value( output_list[0]->getDoubleValue() );
}
}
double FGXMLAutoComponent::clamp( double value )
{
// clamp, if either min or max is defined
if( uminInput.size() + umaxInput.size() > 0 ) {
double d = umaxInput.get_value( 0.0 );
if( value > d ) value = d;
d = uminInput.get_value( 0.0 );
if( value < d ) value = d;
}
return value;
}
FGPIDController::FGPIDController( SGPropertyNode *node ): FGPIDController::FGPIDController( SGPropertyNode *node ):
FGXMLAutoComponent( node ), FGXMLAutoComponent( node ),
alpha( 0.1 ), alpha( 0.1 ),
@ -239,9 +308,9 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
desiredTs = config->getDoubleValue(); desiredTs = config->getDoubleValue();
} }
Kp.parse( child->getChild( "Kp" ) ); Kp.push_back( new FGXMLAutoInput( child->getChild( "Kp" ) ) );
Ti.parse( child->getChild( "Ti" ) ); Ti.push_back( new FGXMLAutoInput( child->getChild( "Ti" ) ) );
Td.parse( child->getChild( "Td" ) ); Td.push_back( new FGXMLAutoInput( child->getChild( "Td" ) ) );
config = child->getChild( "beta" ); config = child->getChild( "beta" );
if ( config != NULL ) { if ( config != NULL ) {
@ -329,8 +398,8 @@ void FGPIDController::update( double dt ) {
double u_n = 0.0; // absolute output double u_n = 0.0; // absolute output
double Ts; // sampling interval (sec) double Ts; // sampling interval (sec)
double u_min = uminInput.getValue(); double u_min = uminInput.get_value();
double u_max = umaxInput.getValue(); double u_max = umaxInput.get_value();
elapsedTime += dt; elapsedTime += dt;
if ( elapsedTime <= desiredTs ) { if ( elapsedTime <= desiredTs ) {
@ -345,20 +414,21 @@ void FGPIDController::update( double dt ) {
if ( !enabled ) { if ( !enabled ) {
// first time being enabled, seed u_n with current // first time being enabled, seed u_n with current
// property tree value // property tree value
u_n = getOutputValue(); u_n = get_output_value();
u_n_1 = u_n; u_n_1 = u_n;
} }
enabled = true; enabled = true;
} else { } else {
enabled = false; enabled = false;
do_feedback();
} }
if ( enabled && Ts > 0.0) { if ( enabled && Ts > 0.0) {
if ( debug ) cout << "Updating " << get_name() if ( debug ) cout << "Updating " << get_name()
<< " Ts " << Ts << endl; << " Ts " << Ts << endl;
double y_n = valueInput.getValue(); double y_n = valueInput.get_value();
double r_n = referenceInput.getValue(); double r_n = referenceInput.get_value();
if ( debug ) cout << " input = " << y_n << " ref = " << r_n << endl; if ( debug ) cout << " input = " << y_n << " ref = " << r_n << endl;
@ -375,7 +445,7 @@ void FGPIDController::update( double dt ) {
ed_n = gamma * r_n - y_n; ed_n = gamma * r_n - y_n;
if ( debug ) cout << " ed_n = " << ed_n; if ( debug ) cout << " ed_n = " << ed_n;
double td = Td.getValue(); double td = Td.get_value();
if ( td > 0.0 ) { if ( td > 0.0 ) {
// Calculates filter time: // Calculates filter time:
Tf = alpha * td; Tf = alpha * td;
@ -390,18 +460,18 @@ void FGPIDController::update( double dt ) {
} }
// Calculates the incremental output: // Calculates the incremental output:
double ti = Ti.getValue(); double ti = Ti.get_value();
if ( ti > 0.0 ) { if ( ti > 0.0 ) {
delta_u_n = Kp.getValue() * ( (ep_n - ep_n_1) delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
+ ((Ts/ti) * e_n) + ((Ts/ti) * e_n)
+ ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) ); + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
} }
if ( debug ) { if ( debug ) {
cout << " delta_u_n = " << delta_u_n << endl; cout << " delta_u_n = " << delta_u_n << endl;
cout << "P:" << Kp.getValue() * (ep_n - ep_n_1) cout << "P:" << Kp.get_value() * (ep_n - ep_n_1)
<< " I:" << Kp.getValue() * ((Ts/ti) * e_n) << " I:" << Kp.get_value() * ((Ts/ti) * e_n)
<< " D:" << Kp.getValue() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) << " D:" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
<< endl; << endl;
} }
@ -424,7 +494,7 @@ void FGPIDController::update( double dt ) {
edf_n_2 = edf_n_1; edf_n_2 = edf_n_1;
edf_n_1 = edf_n; edf_n_1 = edf_n;
setOutputValue( u_n ); set_output_value( u_n );
} else if ( !enabled ) { } else if ( !enabled ) {
ep_n = 0.0; ep_n = 0.0;
edf_n = 0.0; edf_n = 0.0;
@ -447,8 +517,8 @@ FGPISimpleController::FGPISimpleController( SGPropertyNode *node ):
string cname = child->getName(); string cname = child->getName();
string cval = child->getStringValue(); string cval = child->getStringValue();
if ( cname == "config" ) { if ( cname == "config" ) {
Kp.parse( child->getChild( "Kp" ) ); Kp.push_back( new FGXMLAutoInput( child->getChild( "Kp" ) ) );
Ki.parse( child->getChild( "Ki" ) ); Ki.push_back( new FGXMLAutoInput( child->getChild( "Ki" ) ) );
} else { } else {
SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" ); SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
if ( get_name().length() ) { if ( get_name().length() ) {
@ -469,12 +539,13 @@ void FGPISimpleController::update( double dt ) {
enabled = true; enabled = true;
} else { } else {
enabled = false; enabled = false;
do_feedback();
} }
if ( enabled ) { if ( enabled ) {
if ( debug ) cout << "Updating " << get_name() << endl; if ( debug ) cout << "Updating " << get_name() << endl;
double y_n = valueInput.getValue(); double y_n = valueInput.get_value();
double r_n = referenceInput.getValue(); double r_n = referenceInput.get_value();
double error = r_n - y_n; double error = r_n - y_n;
if ( debug ) cout << "input = " << y_n if ( debug ) cout << "input = " << y_n
@ -482,37 +553,32 @@ void FGPISimpleController::update( double dt ) {
<< " error = " << error << " error = " << error
<< endl; << endl;
double prop_comp = error * Kp.getValue(); double prop_comp = error * Kp.get_value();
int_sum += error * Ki.getValue() * dt; int_sum += error * Ki.get_value() * dt;
if ( debug ) cout << "prop_comp = " << prop_comp if ( debug ) cout << "prop_comp = " << prop_comp
<< " int_sum = " << int_sum << endl; << " int_sum = " << int_sum << endl;
double output = prop_comp + int_sum; double output = prop_comp + int_sum;
output = Clamp( output ); output = clamp( output );
setOutputValue( output ); set_output_value( output );
if ( debug ) cout << "output = " << output << endl; if ( debug ) cout << "output = " << output << endl;
} }
} }
FGPredictor::FGPredictor ( SGPropertyNode *node ): FGPredictor::FGPredictor ( SGPropertyNode *node ):
FGXMLAutoComponent( node ), FGXMLAutoComponent( node )
average ( 0.0 ),
seconds( 0.0 ),
filter_gain( 0.0 ),
ivalue( 0.0 )
{ {
int i; int i;
for ( i = 0; i < node->nChildren(); ++i ) { for ( i = 0; i < node->nChildren(); ++i ) {
SGPropertyNode *child = node->getChild(i); SGPropertyNode *child = node->getChild(i);
string cname = child->getName(); string cname = child->getName();
string cval = child->getStringValue();
if ( cname == "seconds" ) { if ( cname == "seconds" ) {
seconds = child->getDoubleValue(); seconds.push_back( new FGXMLAutoInput( child, 0 ) );
} else if ( cname == "filter-gain" ) { } else if ( cname == "filter-gain" ) {
filter_gain = child->getDoubleValue(); filter_gain.push_back( new FGXMLAutoInput( child, 0 ) );
} }
} }
} }
@ -531,7 +597,7 @@ void FGPredictor::update( double dt ) {
*/ */
ivalue = valueInput.getValue(); double ivalue = valueInput.get_value();
if ( isPropertyEnabled() ) { if ( isPropertyEnabled() ) {
if ( !enabled ) { if ( !enabled ) {
@ -541,22 +607,21 @@ void FGPredictor::update( double dt ) {
enabled = true; enabled = true;
} else { } else {
enabled = false; enabled = false;
do_feedback();
} }
if ( enabled ) { if ( enabled ) {
if ( dt > 0.0 ) { if ( dt > 0.0 ) {
double current = (ivalue - last_value)/dt; // calculate current error change (per second) double current = (ivalue - last_value)/dt; // calculate current error change (per second)
if ( dt < 1.0 ) { double average = dt < 1.0 ? ((1.0 - dt) * average + current * dt) : current;
average = (1.0 - dt) * average + current * dt;
} else {
average = current;
}
// calculate output with filter gain adjustment // calculate output with filter gain adjustment
double output = ivalue + (1.0 - filter_gain) * (average * seconds) + filter_gain * (current * seconds); double output = ivalue +
output = Clamp( output ); (1.0 - filter_gain.get_value()) * (average * seconds.get_value()) +
setOutputValue( output ); filter_gain.get_value() * (current * seconds.get_value());
output = clamp( output );
set_output_value( output );
} }
last_value = ivalue; last_value = ivalue;
} }
@ -564,7 +629,8 @@ void FGPredictor::update( double dt ) {
FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node): FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node):
FGXMLAutoComponent( node ) FGXMLAutoComponent( node ),
filterType(none)
{ {
int i; int i;
for ( i = 0; i < node->nChildren(); ++i ) { for ( i = 0; i < node->nChildren(); ++i ) {
@ -586,36 +652,41 @@ FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node):
filterType = reciprocal; filterType = reciprocal;
} }
} else if ( cname == "filter-time" ) { } else if ( cname == "filter-time" ) {
TfInput.parse( child, 1.0 ); TfInput.push_back( new FGXMLAutoInput( child, 1.0 ) );
if( filterType == none ) filterType = exponential;
} else if ( cname == "samples" ) { } else if ( cname == "samples" ) {
samplesInput.parse( child, 1 ); samplesInput.push_back( new FGXMLAutoInput( child, 1 ) );
if( filterType == none ) filterType = movingAverage;
} else if ( cname == "max-rate-of-change" ) { } else if ( cname == "max-rate-of-change" ) {
rateOfChangeInput.parse( child, 1.0 ); rateOfChangeInput.push_back( new FGXMLAutoInput( child, 1 ) );
if( filterType == none ) filterType = noiseSpike;
} else if ( cname == "gain" ) { } else if ( cname == "gain" ) {
gainInput.parse( child ); gainInput.push_back( new FGXMLAutoInput( child, 1 ) );
if( filterType == none ) filterType = gain;
} }
} }
output.resize(2, 0.0); output.resize(2, 0.0);
input.resize(samplesInput.getValue() + 1, 0.0); input.resize(samplesInput.get_value() + 1, 0.0);
} }
void FGDigitalFilter::update(double dt) void FGDigitalFilter::update(double dt)
{ {
if ( isPropertyEnabled() ) { if ( isPropertyEnabled() ) {
input.push_front(valueInput.getValue()); input.push_front(valueInput.get_value());
input.resize(samplesInput.getValue() + 1, 0.0); input.resize(samplesInput.get_value() + 1, 0.0);
if ( !enabled ) { if ( !enabled ) {
// first time being enabled, initialize output to the // first time being enabled, initialize output to the
// value of the output property to avoid bumping. // value of the output property to avoid bumping.
output.push_front(getOutputValue()); output.push_front(get_output_value());
} }
enabled = true; enabled = true;
} else { } else {
enabled = false; enabled = false;
do_feedback();
} }
if ( enabled && dt > 0.0 ) { if ( enabled && dt > 0.0 ) {
@ -630,13 +701,13 @@ void FGDigitalFilter::update(double dt)
if (filterType == exponential) if (filterType == exponential)
{ {
double alpha = 1 / ((TfInput.getValue()/dt) + 1); double alpha = 1 / ((TfInput.get_value()/dt) + 1);
output.push_front(alpha * input[0] + output.push_front(alpha * input[0] +
(1 - alpha) * output[0]); (1 - alpha) * output[0]);
} }
else if (filterType == doubleExponential) else if (filterType == doubleExponential)
{ {
double alpha = 1 / ((TfInput.getValue()/dt) + 1); double alpha = 1 / ((TfInput.get_value()/dt) + 1);
output.push_front(alpha * alpha * input[0] + output.push_front(alpha * alpha * input[0] +
2 * (1 - alpha) * output[0] - 2 * (1 - alpha) * output[0] -
(1 - alpha) * (1 - alpha) * output[1]); (1 - alpha) * (1 - alpha) * output[1]);
@ -644,11 +715,11 @@ void FGDigitalFilter::update(double dt)
else if (filterType == movingAverage) else if (filterType == movingAverage)
{ {
output.push_front(output[0] + output.push_front(output[0] +
(input[0] - input.back()) / samplesInput.getValue()); (input[0] - input.back()) / samplesInput.get_value());
} }
else if (filterType == noiseSpike) else if (filterType == noiseSpike)
{ {
double maxChange = rateOfChangeInput.getValue() * dt; double maxChange = rateOfChangeInput.get_value() * dt;
if ((output[0] - input[0]) > maxChange) if ((output[0] - input[0]) > maxChange)
{ {
@ -665,17 +736,17 @@ void FGDigitalFilter::update(double dt)
} }
else if (filterType == gain) else if (filterType == gain)
{ {
output[0] = gainInput.getValue() * input[0]; output[0] = gainInput.get_value() * input[0];
} }
else if (filterType == reciprocal) else if (filterType == reciprocal)
{ {
if (input[0] != 0.0) { if (input[0] != 0.0) {
output[0] = gainInput.getValue() / input[0]; output[0] = gainInput.get_value() / input[0];
} }
} }
output[0] = Clamp(output[0]) ; output[0] = clamp(output[0]) ;
setOutputValue( output[0] ); set_output_value( output[0] );
output.resize(2); output.resize(2);

138
src/Autopilot/xmlauto.hxx
View file

@ -49,25 +49,65 @@ using std::deque;
#include <Main/fg_props.hxx> #include <Main/fg_props.hxx>
class FGXMLAutoInput { class FGXMLAutoInput : public SGReferenced {
private: private:
SGPropertyNode_ptr property; // The name of the property containing the value
double value; // The value as a constant or initializer for the property double value; // The value as a constant or initializer for the property
double offset; // A fixed offset SGPropertyNode_ptr property; // The name of the property containing the value
double scale; // A constant scaling factor SGSharedPtr<FGXMLAutoInput> offset; // A fixed offset, defaults to zero
SGSharedPtr<FGXMLAutoInput> scale; // A constant scaling factor defaults to one
SGSharedPtr<FGXMLAutoInput> min; // A minimum clip defaults to no clipping
SGSharedPtr<FGXMLAutoInput> max; // A maximum clip defaults to no clipping
SGSharedPtr<const SGCondition> _condition;
public: public:
FGXMLAutoInput() : FGXMLAutoInput( SGPropertyNode_ptr node = NULL, double value = 0.0, double offset = 0.0, double scale = 1.0 ) :
property(NULL), property(NULL),
value(0.0), value(0.0),
offset(0.0), offset(NULL),
scale(1.0) {} scale(NULL),
min(NULL),
max(NULL),
_condition(NULL) {
parse( node, value, offset, scale );
}
void parse( SGPropertyNode_ptr, double value = 0.0, double offset = 0.0, double scale = 1.0 ); void parse( SGPropertyNode_ptr, double value = 0.0, double offset = 0.0, double scale = 1.0 );
inline double getValue() {
if( property != NULL ) value = property->getDoubleValue(); /* get the value of this input, apply scale and offset and clipping */
return value * scale + offset; double get_value();
/* set the input value after applying offset and scale */
void set_value( double value );
inline double get_scale() {
return scale == NULL ? 1.0 : scale->get_value();
} }
inline double get_offset() {
return offset == NULL ? 0.0 : offset->get_value();
}
inline bool is_enabled() {
return _condition == NULL ? true : _condition->test();
}
};
class FGXMLAutoInputList : public vector<SGSharedPtr<FGXMLAutoInput> > {
public:
FGXMLAutoInput * get_active() {
for (iterator it = begin(); it != end(); ++it) {
if( (*it)->is_enabled() )
return *it;
}
return NULL;
}
double get_value( double def = 0.0 ) {
FGXMLAutoInput * input = get_active();
return input == NULL ? def : input->get_value();
}
}; };
/** /**
@ -77,7 +117,6 @@ public:
class FGXMLAutoComponent : public SGReferenced { class FGXMLAutoComponent : public SGReferenced {
private: private:
bool clamp;
vector <SGPropertyNode_ptr> output_list; vector <SGPropertyNode_ptr> output_list;
SGSharedPtr<const SGCondition> _condition; SGSharedPtr<const SGCondition> _condition;
@ -88,16 +127,32 @@ private:
bool honor_passive; bool honor_passive;
string name; string name;
/* Feed back output property to input property if
this filter is disabled. This is for multi-stage
filter where one filter sits behind a pid-controller
to provide changes of the overall output to the pid-
controller.
feedback is disabled by default.
*/
bool feedback_if_disabled;
void do_feedback_if_disabled();
protected: protected:
FGXMLAutoInput valueInput; FGXMLAutoInputList valueInput;
FGXMLAutoInput referenceInput; FGXMLAutoInputList referenceInput;
FGXMLAutoInput uminInput; FGXMLAutoInputList uminInput;
FGXMLAutoInput umaxInput; FGXMLAutoInputList umaxInput;
// debug flag // debug flag
bool debug; bool debug;
bool enabled; bool enabled;
inline void do_feedback() {
if( feedback_if_disabled ) do_feedback_if_disabled();
}
public: public:
FGXMLAutoComponent( SGPropertyNode *node); FGXMLAutoComponent( SGPropertyNode *node);
@ -107,30 +162,21 @@ public:
inline const string& get_name() { return name; } inline const string& get_name() { return name; }
inline double Clamp( double value ) { double clamp( double value );
if( clamp ) {
double d = umaxInput.getValue();
if( value > d ) value = d;
d = uminInput.getValue();
if( value < d ) value = d;
}
return value;
}
inline void setOutputValue( double value ) { inline void set_output_value( double value ) {
// passive_ignore == true means that we go through all the // passive_ignore == true means that we go through all the
// motions, but drive the outputs. This is analogous to // motions, but drive the outputs. This is analogous to
// running the autopilot with the "servos" off. This is // running the autopilot with the "servos" off. This is
// helpful for things like flight directors which position // helpful for things like flight directors which position
// their vbars from the autopilot computations. // their vbars from the autopilot computations.
if ( honor_passive && passive_mode->getBoolValue() ) return; if ( honor_passive && passive_mode->getBoolValue() ) return;
for ( unsigned i = 0; i < output_list.size(); ++i ) { for( vector <SGPropertyNode_ptr>::iterator it = output_list.begin(); it != output_list.end(); ++it)
output_list[i]->setDoubleValue( Clamp(value) ); (*it)->setDoubleValue( clamp( value ) );
}
} }
inline double getOutputValue() { inline double get_output_value() {
return output_list.size() == 0 ? 0.0 : Clamp(output_list[0]->getDoubleValue()); return output_list.size() == 0 ? 0.0 : clamp(output_list[0]->getDoubleValue());
} }
/* /*
@ -184,9 +230,9 @@ private:
// Configuration values // Configuration values
FGXMLAutoInput Kp; // proportional gain FGXMLAutoInputList Kp; // proportional gain
FGXMLAutoInput Ti; // Integrator time (sec) FGXMLAutoInputList Ti; // Integrator time (sec)
FGXMLAutoInput Td; // Derivator time (sec) FGXMLAutoInputList Td; // Derivator time (sec)
double alpha; // low pass filter weighing factor (usually 0.1) double alpha; // low pass filter weighing factor (usually 0.1)
double beta; // process value weighing factor for double beta; // process value weighing factor for
@ -212,7 +258,6 @@ public:
FGPIDController( SGPropertyNode *node, bool old ); FGPIDController( SGPropertyNode *node, bool old );
~FGPIDController() {} ~FGPIDController() {}
void update_old( double dt );
void update( double dt ); void update( double dt );
}; };
@ -226,10 +271,10 @@ class FGPISimpleController : public FGXMLAutoComponent {
private: private:
// proportional component data // proportional component data
FGXMLAutoInput Kp; FGXMLAutoInputList Kp;
// integral component data // integral component data
FGXMLAutoInput Ki; FGXMLAutoInputList Ki;
double int_sum; double int_sum;
@ -249,18 +294,11 @@ public:
class FGPredictor : public FGXMLAutoComponent { class FGPredictor : public FGXMLAutoComponent {
private: private:
// proportional component data
double last_value; double last_value;
double average; FGXMLAutoInputList seconds;
double seconds; FGXMLAutoInputList filter_gain;
double filter_gain;
// Input values
double ivalue; // input value
public: public:
FGPredictor( SGPropertyNode *node ); FGPredictor( SGPropertyNode *node );
~FGPredictor() {} ~FGPredictor() {}
@ -283,15 +321,15 @@ public:
class FGDigitalFilter : public FGXMLAutoComponent class FGDigitalFilter : public FGXMLAutoComponent
{ {
private: private:
FGXMLAutoInput samplesInput; // Number of input samples to average FGXMLAutoInputList samplesInput; // Number of input samples to average
FGXMLAutoInput rateOfChangeInput; // The maximum allowable rate of change [1/s] FGXMLAutoInputList rateOfChangeInput; // The maximum allowable rate of change [1/s]
FGXMLAutoInput gainInput; // FGXMLAutoInputList gainInput; //
FGXMLAutoInput TfInput; // Filter time [s] FGXMLAutoInputList TfInput; // Filter time [s]
deque <double> output; deque <double> output;
deque <double> input; deque <double> input;
enum filterTypes { exponential, doubleExponential, movingAverage, enum filterTypes { exponential, doubleExponential, movingAverage,
noiseSpike, gain, reciprocal }; noiseSpike, gain, reciprocal, none };
filterTypes filterType; filterTypes filterType;
public: public: