X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Fxmlauto.cxx;h=6761cddf7d30a88330064d7e088b9447b089a75d;hb=11d15b451347674fba77648700d23c5aaec3c6c2;hp=bc0d40c3cb3b92e5a1391ed75ae8f463bcebf9da;hpb=fa7f2468c29640326b87d4934b774c031b19baec;p=flightgear.git diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx index bc0d40c3c..6761cddf7 100644 --- a/src/Autopilot/xmlauto.cxx +++ b/src/Autopilot/xmlauto.cxx @@ -2,7 +2,7 @@ // // Written by Curtis Olson, started January 2004. // -// Copyright (C) 2004 Curtis L. Olson - curt@flightgear.org +// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License as @@ -16,14 +16,20 @@ // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software -// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. // // $Id$ +#ifdef HAVE_CONFIG_H +# include +#endif + +#include #include #include #include +#include #include
#include
@@ -31,125 +37,304 @@ #include "xmlauto.hxx" +using std::cout; +using std::endl; -FGPIDController::FGPIDController( SGPropertyNode *node ): - debug( false ), - y_n( 0.0 ), - r_n( 0.0 ), - y_scale( 1.0 ), - r_scale( 1.0 ), - Kp( 0.0 ), - alpha( 0.1 ), - beta( 1.0 ), - gamma( 0.0 ), - Ti( 0.0 ), - Td( 0.0 ), - u_min( 0.0 ), - u_max( 0.0 ), - ep_n_1( 0.0 ), - edf_n_1( 0.0 ), - edf_n_2( 0.0 ), - u_n_1( 0.0 ) +void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffset, double aScale ) +{ + value = aValue; + property = NULL; + offset = NULL; + scale = NULL; + min = NULL; + max = NULL; + + if( node == NULL ) + return; + + SGPropertyNode * n; + + if( (n = node->getChild("condition")) != NULL ) { + _condition = sgReadCondition(node, n); + } + + 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 ); + } + + if( (n = node->getChild( "abs" )) != NULL ) { + abs = n->getBoolValue(); + } + + SGPropertyNode *valueNode = node->getChild( "value" ); + if ( valueNode != NULL ) { + value = valueNode->getDoubleValue(); + } + + n = node->getChild( "property" ); + // if no element, check for element for backwards + // compatibility + if( n == NULL ) + n = node->getChild( "prop" ); + + if ( n != NULL ) { + property = fgGetNode( n->getStringValue(), true ); + if ( valueNode != NULL ) { + // initialize property with given value + // if both and exist + 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 + } + } + + if ( n == NULL && valueNode == NULL ) { + // no element and no element, use text node + const char * textnode = node->getStringValue(); + char * endp = NULL; + // try to convert to a double value. If the textnode does not start with a number + // endp will point to the beginning of the string. We assume this should be + // a property name + value = strtod( textnode, &endp ); + if( endp == textnode ) { + property = fgGetNode( textnode, true ); + } + } +} + +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 abs ? fabs(value) : value; +} + +FGXMLAutoComponent::FGXMLAutoComponent( SGPropertyNode * node ) : + _condition( NULL ), + enable_prop( NULL ), + enable_value( NULL ), + passive_mode( fgGetNode("/autopilot/locks/passive-mode", true) ), + honor_passive( false ), + name(""), + feedback_if_disabled( false ), + debug(false), + enabled( false ) { int i; + SGPropertyNode *prop; + for ( i = 0; i < node->nChildren(); ++i ) { SGPropertyNode *child = node->getChild(i); string cname = child->getName(); string cval = child->getStringValue(); if ( cname == "name" ) { name = cval; + + } else if ( cname == "feedback-if-disabled" ) { + feedback_if_disabled = child->getBoolValue(); + } else if ( cname == "debug" ) { debug = child->getBoolValue(); + } else if ( cname == "enable" ) { - // cout << "parsing enable" << endl; - SGPropertyNode *prop = child->getChild( "prop" ); - if ( prop != NULL ) { - // cout << "prop = " << prop->getStringValue() << endl; - enable_prop = fgGetNode( prop->getStringValue(), true ); + if( (prop = child->getChild("condition")) != NULL ) { + _condition = sgReadCondition(child, prop); } else { - // cout << "no prop child" << endl; + if ( (prop = child->getChild( "prop" )) != NULL ) { + enable_prop = fgGetNode( prop->getStringValue(), true ); + } + + if ( (prop = child->getChild( "value" )) != NULL ) { + delete enable_value; + enable_value = new string(prop->getStringValue()); + } } - SGPropertyNode *val = child->getChild( "value" ); - if ( val != NULL ) { - enable_value = val->getStringValue(); + if ( (prop = child->getChild( "honor-passive" )) != NULL ) { + honor_passive = prop->getBoolValue(); } + } else if ( cname == "input" ) { - SGPropertyNode *prop = child->getChild( "prop" ); - if ( prop != NULL ) { - input_prop = fgGetNode( prop->getStringValue(), true ); - } - prop = child->getChild( "scale" ); - if ( prop != NULL ) { - y_scale = prop->getDoubleValue(); - } + + valueInput.push_back( new FGXMLAutoInput( child ) ); + } else if ( cname == "reference" ) { - SGPropertyNode *prop = child->getChild( "prop" ); - if ( prop != NULL ) { - r_n_prop = fgGetNode( prop->getStringValue(), true ); - } else { - prop = child->getChild( "value" ); - if ( prop != NULL ) { - r_n = prop->getDoubleValue(); - } - } - prop = child->getChild( "scale" ); - if ( prop != NULL ) { - r_scale = prop->getDoubleValue(); - } + + referenceInput.push_back( new FGXMLAutoInput( child ) ); + } else if ( cname == "output" ) { - int i = 0; - SGPropertyNode *prop; - while ( (prop = child->getChild("prop", i)) != NULL ) { + // grab all and childs + int found = 0; + // backwards compatibility: allow elements + for( int i = 0; (prop = child->getChild("prop", i)) != NULL; i++ ) { SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true ); output_list.push_back( tmp ); - i++; + found++; } + for( int i = 0; (prop = child->getChild("property", i)) != NULL; i++ ) { + SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true ); + output_list.push_back( tmp ); + found++; + } + + // no elements, text node of is property name + if( found == 0 ) + output_list.push_back( fgGetNode(child->getStringValue(), true ) ); + } else if ( cname == "config" ) { - SGPropertyNode *prop; + if( (prop = child->getChild("min")) != NULL ) { + uminInput.push_back( new FGXMLAutoInput( prop ) ); + } + if( (prop = child->getChild("u_min")) != NULL ) { + 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.push_back( new FGXMLAutoInput( prop ) ); + } + } else if ( cname == "min" ) { + uminInput.push_back( new FGXMLAutoInput( child ) ); + } else if ( cname == "u_min" ) { + uminInput.push_back( new FGXMLAutoInput( child ) ); + } else if ( cname == "max" ) { + umaxInput.push_back( new FGXMLAutoInput( child ) ); + } else if ( cname == "u_max" ) { + umaxInput.push_back( new FGXMLAutoInput( child ) ); + } + } +} - prop = child->getChild( "Kp" ); - if ( prop != NULL ) { - Kp = prop->getDoubleValue(); - } +FGXMLAutoComponent::~FGXMLAutoComponent() +{ + delete enable_value; +} - prop = child->getChild( "beta" ); - if ( prop != NULL ) { - beta = prop->getDoubleValue(); - } +bool FGXMLAutoComponent::isPropertyEnabled() +{ + if( _condition ) + return _condition->test(); - prop = child->getChild( "alpha" ); - if ( prop != NULL ) { - alpha = prop->getDoubleValue(); - } + if( enable_prop ) { + if( enable_value ) { + return *enable_value == enable_prop->getStringValue(); + } else { + return enable_prop->getBoolValue(); + } + } + return true; +} - prop = child->getChild( "gamma" ); - if ( prop != NULL ) { - gamma = prop->getDoubleValue(); - } +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; +} - prop = child->getChild( "Ti" ); - if ( prop != NULL ) { - Ti = prop->getDoubleValue(); +FGPIDController::FGPIDController( SGPropertyNode *node ): + FGXMLAutoComponent( node ), + alpha( 0.1 ), + beta( 1.0 ), + gamma( 0.0 ), + ep_n_1( 0.0 ), + edf_n_1( 0.0 ), + edf_n_2( 0.0 ), + u_n_1( 0.0 ), + desiredTs( 0.0 ), + elapsedTime( 0.0 ) +{ + int i; + for ( i = 0; i < node->nChildren(); ++i ) { + SGPropertyNode *child = node->getChild(i); + string cname = child->getName(); + string cval = child->getStringValue(); + if ( cname == "config" ) { + SGPropertyNode *config; + + if ( (config = child->getChild( "Ts" )) != NULL ) { + desiredTs = config->getDoubleValue(); } + + Kp.push_back( new FGXMLAutoInput( child->getChild( "Kp" ) ) ); + Ti.push_back( new FGXMLAutoInput( child->getChild( "Ti" ) ) ); + Td.push_back( new FGXMLAutoInput( child->getChild( "Td" ) ) ); - prop = child->getChild( "Td" ); - if ( prop != NULL ) { - Td = prop->getDoubleValue(); + config = child->getChild( "beta" ); + if ( config != NULL ) { + beta = config->getDoubleValue(); } - prop = child->getChild( "u_min" ); - if ( prop != NULL ) { - u_min = prop->getDoubleValue(); + config = child->getChild( "alpha" ); + if ( config != NULL ) { + alpha = config->getDoubleValue(); } - prop = child->getChild( "u_max" ); - if ( prop != NULL ) { - u_max = prop->getDoubleValue(); + config = child->getChild( "gamma" ); + if ( config != NULL ) { + gamma = config->getDoubleValue(); } + } else { SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" ); - if ( name.length() ) { - SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << name ); + if ( get_name().length() ) { + SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << get_name() ); } } } @@ -215,43 +400,39 @@ 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 ) { + double u_min = uminInput.get_value(); + double u_max = umaxInput.get_value(); + + 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 ( isPropertyEnabled() ) { if ( !enabled ) { // first time being enabled, seed u_n with current // property tree value - u_n = output_list[0]->getDoubleValue(); - // and clip - if ( u_n < u_min ) { u_n = u_min; } - if ( u_n > u_max ) { u_n = u_max; } + 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 " << name << endl; - - double y_n = 0.0; - if ( input_prop != NULL ) { - y_n = input_prop->getDoubleValue() * y_scale; - } + if ( debug ) cout << "Updating " << get_name() + << " Ts " << Ts << endl; - double r_n = 0.0; - if ( r_n_prop != NULL ) { - r_n = r_n_prop->getDoubleValue() * r_scale; - } else { - r_n = r_n_value; - } + double y_n = valueInput.get_value(); + double r_n = referenceInput.get_value(); if ( debug ) cout << " input = " << y_n << " ref = " << r_n << endl; @@ -268,9 +449,10 @@ void FGPIDController::update( double dt ) { ed_n = gamma * r_n - y_n; if ( debug ) cout << " ed_n = " << ed_n; - if ( Td > 0.0 ) { + double td = Td.get_value(); + if ( td > 0.0 ) { // Calculates filter time: - Tf = alpha * Td; + Tf = alpha * td; if ( debug ) cout << " Tf = " << Tf; // Filters the derivate error: @@ -282,29 +464,27 @@ void FGPIDController::update( double dt ) { } // Calculates the incremental output: - 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)) ); + double ti = Ti.get_value(); + if ( ti > 0.0 ) { + 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 * (ep_n - ep_n_1) - << " I:" << Kp * ((Ts/Ti) * e_n) - << " D:" << Kp * ((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; } // Integrator anti-windup logic: if ( delta_u_n > (u_max - u_n_1) ) { - delta_u_n = 0; + delta_u_n = u_max - u_n_1; if ( debug ) cout << " max saturation " << endl; } else if ( delta_u_n < (u_min - u_n_1) ) { - delta_u_n = 0; + delta_u_n = u_min - u_n_1; if ( debug ) cout << " min saturation " << endl; } @@ -318,10 +498,7 @@ void FGPIDController::update( double dt ) { edf_n_2 = edf_n_1; edf_n_1 = edf_n; - unsigned int i; - for ( i = 0; i < output_list.size(); ++i ) { - output_list[i]->setDoubleValue( u_n ); - } + set_output_value( u_n ); } else if ( !enabled ) { ep_n = 0.0; edf_n = 0.0; @@ -335,105 +512,21 @@ void FGPIDController::update( double dt ) { FGPISimpleController::FGPISimpleController( SGPropertyNode *node ): - proportional( false ), - Kp( 0.0 ), - offset_prop( NULL ), - offset_value( 0.0 ), - integral( false ), - Ki( 0.0 ), - int_sum( 0.0 ), - clamp( false ), - debug( false ), - y_n( 0.0 ), - r_n( 0.0 ), - y_scale( 1.0 ), - r_scale ( 1.0 ), - u_min( 0.0 ), - u_max( 0.0 ) + FGXMLAutoComponent( node ), + int_sum( 0.0 ) { int i; for ( i = 0; i < node->nChildren(); ++i ) { SGPropertyNode *child = node->getChild(i); string cname = child->getName(); string cval = child->getStringValue(); - if ( cname == "name" ) { - name = cval; - } else if ( cname == "debug" ) { - debug = child->getBoolValue(); - } else if ( cname == "enable" ) { - // cout << "parsing enable" << endl; - SGPropertyNode *prop = child->getChild( "prop" ); - if ( prop != NULL ) { - // cout << "prop = " << prop->getStringValue() << endl; - enable_prop = fgGetNode( prop->getStringValue(), true ); - } else { - // cout << "no prop child" << endl; - } - SGPropertyNode *val = child->getChild( "value" ); - if ( val != NULL ) { - enable_value = val->getStringValue(); - } - } else if ( cname == "input" ) { - SGPropertyNode *prop = child->getChild( "prop" ); - if ( prop != NULL ) { - input_prop = fgGetNode( prop->getStringValue(), true ); - } - prop = child->getChild( "scale" ); - if ( prop != NULL ) { - y_scale = prop->getDoubleValue(); - } - } else if ( cname == "reference" ) { - SGPropertyNode *prop = child->getChild( "prop" ); - if ( prop != NULL ) { - r_n_prop = fgGetNode( prop->getStringValue(), true ); - } else { - prop = child->getChild( "value" ); - if ( prop != NULL ) { - r_n = prop->getDoubleValue(); - } - } - prop = child->getChild( "scale" ); - if ( prop != NULL ) { - r_scale = prop->getDoubleValue(); - } - } else if ( cname == "output" ) { - int i = 0; - SGPropertyNode *prop; - while ( (prop = child->getChild("prop", i)) != NULL ) { - SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true ); - output_list.push_back( tmp ); - i++; - } - } else if ( cname == "config" ) { - SGPropertyNode *prop; - - prop = child->getChild( "Kp" ); - if ( prop != NULL ) { - Kp = prop->getDoubleValue(); - proportional = true; - } - - prop = child->getChild( "Ki" ); - if ( prop != NULL ) { - Ki = prop->getDoubleValue(); - integral = true; - } - - prop = child->getChild( "u_min" ); - if ( prop != NULL ) { - u_min = prop->getDoubleValue(); - clamp = true; - } - - prop = child->getChild( "u_max" ); - if ( prop != NULL ) { - u_max = prop->getDoubleValue(); - clamp = true; - } + if ( cname == "config" ) { + 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 ( name.length() ) { - SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << name ); + if ( get_name().length() ) { + SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << get_name() ); } } } @@ -441,7 +534,8 @@ FGPISimpleController::FGPISimpleController( SGPropertyNode *node ): void FGPISimpleController::update( double dt ) { - if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) { + + if ( isPropertyEnabled() ) { if ( !enabled ) { // we have just been enabled, zero out int_sum int_sum = 0.0; @@ -449,96 +543,47 @@ void FGPISimpleController::update( double dt ) { enabled = true; } else { enabled = false; + do_feedback(); } if ( enabled ) { - if ( debug ) cout << "Updating " << name << endl; - double input = 0.0; - if ( input_prop != NULL ) { - input = input_prop->getDoubleValue() * y_scale; - } - - double r_n = 0.0; - if ( r_n_prop != NULL ) { - r_n = r_n_prop->getDoubleValue() * r_scale; - } else { - r_n = r_n_value; - } + if ( debug ) cout << "Updating " << get_name() << endl; + double y_n = valueInput.get_value(); + double r_n = referenceInput.get_value(); - double error = r_n - input; - if ( debug ) cout << "input = " << input + double error = r_n - y_n; + if ( debug ) cout << "input = " << y_n << " reference = " << r_n << " error = " << error << endl; - double prop_comp = 0.0; - double offset = 0.0; - if ( offset_prop != NULL ) { - offset = offset_prop->getDoubleValue(); - if ( debug ) cout << "offset = " << offset << endl; - } else { - offset = offset_value; - } + double prop_comp = error * Kp.get_value(); + int_sum += error * Ki.get_value() * dt; - if ( proportional ) { - prop_comp = error * Kp + offset; - } - - if ( integral ) { - int_sum += error * Ki * dt; - } else { - int_sum = 0.0; - } if ( debug ) cout << "prop_comp = " << prop_comp << " int_sum = " << int_sum << endl; double output = prop_comp + int_sum; - - if ( clamp ) { - if ( output < u_min ) { - output = u_min; - } - if ( output > u_max ) { - output = u_max; - } - } + output = clamp( output ); + set_output_value( output ); if ( debug ) cout << "output = " << output << endl; - - unsigned int i; - for ( i = 0; i < output_list.size(); ++i ) { - output_list[i]->setDoubleValue( output ); - } } } FGPredictor::FGPredictor ( SGPropertyNode *node ): - last_value ( 999999999.9 ), - average ( 0.0 ), - seconds( 0.0 ), - filter_gain( 0.0 ), - debug( false ), - ivalue( 0.0 ) + FGXMLAutoComponent( node ), + average(0.0) { int i; for ( i = 0; i < node->nChildren(); ++i ) { SGPropertyNode *child = node->getChild(i); string cname = child->getName(); - string cval = child->getStringValue(); - if ( cname == "name" ) { - name = cval; - } else if ( cname == "debug" ) { - debug = child->getBoolValue(); - } else if ( cname == "input" ) { - input_prop = fgGetNode( child->getStringValue(), true ); - } else if ( cname == "seconds" ) { - seconds = child->getDoubleValue(); + if ( cname == "seconds" ) { + seconds.push_back( new FGXMLAutoInput( child, 0 ) ); } else if ( cname == "filter-gain" ) { - filter_gain = child->getDoubleValue(); - } else if ( cname == "output" ) { - SGPropertyNode *tmp = fgGetNode( child->getStringValue(), true ); - output_list.push_back( tmp ); + filter_gain.push_back( new FGXMLAutoInput( child, 0 ) ); } } } @@ -557,84 +602,96 @@ void FGPredictor::update( double dt ) { */ - if ( input_prop != NULL ) { - ivalue = input_prop->getDoubleValue(); - // no sense if there isn't an input :-) + double ivalue = valueInput.get_value(); + + if ( isPropertyEnabled() ) { + if ( !enabled ) { + // first time being enabled + last_value = ivalue; + } enabled = true; } else { enabled = false; + do_feedback(); } if ( enabled ) { - // first time initialize average - if (last_value >= 999999999.0) { - last_value = ivalue; - } - 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; - } + 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); - - unsigned int i; - for ( i = 0; i < output_list.size(); ++i ) { - output_list[i]->setDoubleValue( 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; } } -FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node) +FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node): + FGXMLAutoComponent( node ), + filterType(none) { - samples = 1; - int i; for ( i = 0; i < node->nChildren(); ++i ) { SGPropertyNode *child = node->getChild(i); string cname = child->getName(); string cval = child->getStringValue(); - if ( cname == "name" ) { - name = cval; - } else if ( cname == "debug" ) { - debug = child->getBoolValue(); - } else if ( cname == "type" ) { - filterType = cval; - } else if ( cname == "input" ) { - input_prop = fgGetNode( child->getStringValue(), true ); + if ( cname == "type" ) { + if ( cval == "exponential" ) { + filterType = exponential; + } else if (cval == "double-exponential") { + filterType = doubleExponential; + } else if (cval == "moving-average") { + filterType = movingAverage; + } else if (cval == "noise-spike") { + filterType = noiseSpike; + } else if (cval == "gain") { + filterType = gain; + } else if (cval == "reciprocal") { + filterType = reciprocal; + } } else if ( cname == "filter-time" ) { - Tf = child->getDoubleValue(); + TfInput.push_back( new FGXMLAutoInput( child, 1.0 ) ); + if( filterType == none ) filterType = exponential; } else if ( cname == "samples" ) { - samples = child->getIntValue(); + samplesInput.push_back( new FGXMLAutoInput( child, 1 ) ); + if( filterType == none ) filterType = movingAverage; } else if ( cname == "max-rate-of-change" ) { - rateOfChange = child->getDoubleValue(); - } else if ( cname == "output" ) { - SGPropertyNode *tmp = fgGetNode( child->getStringValue(), true ); - output_list.push_back( tmp ); + rateOfChangeInput.push_back( new FGXMLAutoInput( child, 1 ) ); + if( filterType == none ) filterType = noiseSpike; + } else if ( cname == "gain" ) { + gainInput.push_back( new FGXMLAutoInput( child, 1 ) ); + if( filterType == none ) filterType = gain; } } output.resize(2, 0.0); - input.resize(samples, 0.0); + input.resize(samplesInput.get_value() + 1, 0.0); } void FGDigitalFilter::update(double dt) { - if ( input_prop != NULL ) { - input.push_front(input_prop->getDoubleValue()); - input.resize(samples, 0.0); - // no sense if there isn't an input :-) + if ( isPropertyEnabled() ) { + + 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(get_output_value()); + } + enabled = true; } else { enabled = false; + do_feedback(); } if ( enabled && dt > 0.0 ) { @@ -644,43 +701,30 @@ void FGDigitalFilter::update(double dt) * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1] * */ + if( debug ) cout << "Updating " << get_name() + << " dt " << dt << endl; - if (filterType == "exponential") + if (filterType == exponential) { - double alpha = 1 / ((Tf/dt) + 1); + double alpha = 1 / ((TfInput.get_value()/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 == "double-exponential") + else if (filterType == doubleExponential) { - double alpha = 1 / ((Tf/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]); - unsigned int i; - for ( i = 0; i < output_list.size(); ++i ) { - output_list[i]->setDoubleValue( output[0] ); - } - output.resize(2); } - else if (filterType == "moving-average") + 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); + (input[0] - input.back()) / samplesInput.get_value()); } - else if (filterType == "noise-spike") + else if (filterType == noiseSpike) { - double maxChange = rateOfChange * dt; + double maxChange = rateOfChangeInput.get_value() * dt; if ((output[0] - input[0]) > maxChange) { @@ -694,13 +738,23 @@ 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] ); + } + else if (filterType == gain) + { + output[0] = gainInput.get_value() * input[0]; + } + else if (filterType == reciprocal) + { + if (input[0] != 0.0) { + output[0] = gainInput.get_value() / input[0]; } - output.resize(1); } + + output[0] = clamp(output[0]) ; + set_output_value( output[0] ); + + output.resize(2); + if (debug) { cout << "input:" << input[0] @@ -734,14 +788,14 @@ void FGXMLAutopilot::init() { if ( ! build() ) { SG_LOG( SG_ALL, SG_ALERT, - "Detected an internal inconsistancy in the autopilot"); + "Detected an internal inconsistency in the autopilot"); SG_LOG( SG_ALL, SG_ALERT, " configuration. See earlier errors for" ); SG_LOG( SG_ALL, SG_ALERT, " details."); exit(-1); } - } catch (const sg_exception& exc) { + } catch (const sg_exception&) { SG_LOG( SG_ALL, SG_ALERT, "Failed to load autopilot configuration: " << config.str() ); } @@ -756,7 +810,6 @@ void FGXMLAutopilot::init() { void FGXMLAutopilot::reinit() { components.clear(); init(); - build(); } @@ -776,17 +829,13 @@ bool FGXMLAutopilot::build() { string name = node->getName(); // cout << name << endl; if ( name == "pid-controller" ) { - FGXMLAutoComponent *c = new FGPIDController( node ); - components.push_back( c ); + components.push_back( new FGPIDController( node ) ); } else if ( name == "pi-simple-controller" ) { - FGXMLAutoComponent *c = new FGPISimpleController( node ); - components.push_back( c ); + components.push_back( new FGPISimpleController( node ) ); } else if ( name == "predict-simple" ) { - FGXMLAutoComponent *c = new FGPredictor( node ); - components.push_back( c ); + components.push_back( new FGPredictor( node ) ); } else if ( name == "filter" ) { - FGXMLAutoComponent *c = new FGDigitalFilter( node ); - components.push_back( c ); + components.push_back( new FGDigitalFilter( node ) ); } else { SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: " << name ); @@ -803,10 +852,10 @@ bool FGXMLAutopilot::build() { */ static void update_helper( double dt ) { // Estimate speed in 5,10 seconds - static SGPropertyNode *vel = fgGetNode( "/velocities/airspeed-kt", true ); - static SGPropertyNode *lookahead5 + static SGPropertyNode_ptr vel = fgGetNode( "/velocities/airspeed-kt", true ); + static SGPropertyNode_ptr lookahead5 = fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true ); - static SGPropertyNode *lookahead10 + static SGPropertyNode_ptr lookahead10 = fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true ); static double average = 0.0; // average/filtered prediction @@ -830,12 +879,12 @@ static void update_helper( double dt ) { // Calculate heading bug error normalized to +/- 180.0 (based on // DG indicated heading) - static SGPropertyNode *bug + static SGPropertyNode_ptr bug = fgGetNode( "/autopilot/settings/heading-bug-deg", true ); - static SGPropertyNode *ind_hdg + static SGPropertyNode_ptr ind_hdg = fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg", true ); - static SGPropertyNode *ind_bug_error + static SGPropertyNode_ptr ind_bug_error = fgGetNode( "/autopilot/internal/heading-bug-error-deg", true ); double diff = bug->getDoubleValue() - ind_hdg->getDoubleValue(); @@ -846,9 +895,9 @@ static void update_helper( double dt ) { // Calculate heading bug error normalized to +/- 180.0 (based on // actual/nodrift magnetic-heading, i.e. a DG slaved to magnetic // compass.) - static SGPropertyNode *mag_hdg + static SGPropertyNode_ptr mag_hdg = fgGetNode( "/orientation/heading-magnetic-deg", true ); - static SGPropertyNode *fdm_bug_error + static SGPropertyNode_ptr fdm_bug_error = fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true ); diff = bug->getDoubleValue() - mag_hdg->getDoubleValue(); @@ -857,13 +906,13 @@ static void update_helper( double dt ) { fdm_bug_error->setDoubleValue( diff ); // Calculate true heading error normalized to +/- 180.0 - static SGPropertyNode *target_true + static SGPropertyNode_ptr target_true = fgGetNode( "/autopilot/settings/true-heading-deg", true ); - static SGPropertyNode *true_hdg + static SGPropertyNode_ptr true_hdg = fgGetNode( "/orientation/heading-deg", true ); - static SGPropertyNode *true_track + static SGPropertyNode_ptr true_track = fgGetNode( "/instrumentation/gps/indicated-track-true-deg", true ); - static SGPropertyNode *true_error + static SGPropertyNode_ptr true_error = fgGetNode( "/autopilot/internal/true-heading-error-deg", true ); diff = target_true->getDoubleValue() - true_hdg->getDoubleValue(); @@ -872,11 +921,11 @@ static void update_helper( double dt ) { true_error->setDoubleValue( diff ); // Calculate nav1 target heading error normalized to +/- 180.0 - static SGPropertyNode *target_nav1 - = fgGetNode( "/radios/nav[0]/radials/target-auto-hdg-deg", true ); - static SGPropertyNode *true_nav1 + static SGPropertyNode_ptr target_nav1 + = fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true ); + static SGPropertyNode_ptr true_nav1 = fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true ); - static SGPropertyNode *true_track_nav1 + static SGPropertyNode_ptr true_track_nav1 = fgGetNode( "/autopilot/internal/nav1-track-error-deg", true ); diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue(); @@ -891,10 +940,10 @@ static void update_helper( double dt ) { // Calculate nav1 selected course error normalized to +/- 180.0 // (based on DG indicated heading) - static SGPropertyNode *nav1_course_error + static SGPropertyNode_ptr nav1_course_error = fgGetNode( "/autopilot/internal/nav1-course-error", true ); - static SGPropertyNode *nav1_selected_course - = fgGetNode( "/radios/nav[0]/radials/selected-deg", true ); + static SGPropertyNode_ptr nav1_selected_course + = fgGetNode( "/instrumentation/nav[0]/radials/selected-deg", true ); diff = nav1_selected_course->getDoubleValue() - ind_hdg->getDoubleValue(); // if ( diff < -180.0 ) { diff += 360.0; } @@ -903,9 +952,9 @@ static void update_helper( double dt ) { nav1_course_error->setDoubleValue( diff ); // Calculate vertical speed in fpm - static SGPropertyNode *vs_fps + static SGPropertyNode_ptr vs_fps = fgGetNode( "/velocities/vertical-speed-fps", true ); - static SGPropertyNode *vs_fpm + static SGPropertyNode_ptr vs_fpm = fgGetNode( "/autopilot/internal/vert-speed-fpm", true ); vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 ); @@ -913,9 +962,9 @@ static void update_helper( double dt ) { // Calculate static port pressure rate in [inhg/s]. // Used to determine vertical speed. - static SGPropertyNode *static_pressure + static SGPropertyNode_ptr static_pressure = fgGetNode( "/systems/static[0]/pressure-inhg", true ); - static SGPropertyNode *pressure_rate + static SGPropertyNode_ptr pressure_rate = fgGetNode( "/autopilot/internal/pressure-rate", true ); static double last_static_pressure = 0.0; @@ -946,3 +995,4 @@ void FGXMLAutopilot::update( double dt ) { components[i]->update( dt ); } } +