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

140
src/Autopilot/xmlauto.hxx
View file

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