diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx index 191d745a3..217d0e1a8 100644 --- a/src/Autopilot/xmlauto.cxx +++ b/src/Autopilot/xmlauto.cxx @@ -45,7 +45,9 @@ 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 ), + r_scale( 1.0 ), + y_scale( 1.0 ) { int i; for ( i = 0; i < node->nChildren(); ++i ) { @@ -74,6 +76,10 @@ FGPIDController::FGPIDController( SGPropertyNode *node ): 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 ) { @@ -84,6 +90,10 @@ FGPIDController::FGPIDController( SGPropertyNode *node ): r_n = prop->getDoubleValue(); } } + prop = child->getChild( "scale" ); + if ( prop != NULL ) { + r_scale = prop->getDoubleValue(); + } } else if ( cname == "output" ) { int i = 0; SGPropertyNode *prop; @@ -231,12 +241,12 @@ void FGPIDController::update( double dt ) { double y_n = 0.0; if ( input_prop != NULL ) { - y_n = input_prop->getDoubleValue(); + y_n = input_prop->getDoubleValue() * y_scale; } double r_n = 0.0; if ( r_n_prop != NULL ) { - r_n = r_n_prop->getDoubleValue(); + r_n = r_n_prop->getDoubleValue() * r_scale; } else { r_n = r_n_value; } @@ -335,7 +345,9 @@ FGPISimpleController::FGPISimpleController( SGPropertyNode *node ): y_n( 0.0 ), r_n( 0.0 ), u_min( 0.0 ), - u_max( 0.0 ) + u_max( 0.0 ), + y_scale( 1.0 ), + r_scale ( 1.0 ) { int i; for ( i = 0; i < node->nChildren(); ++i ) { @@ -364,6 +376,10 @@ FGPISimpleController::FGPISimpleController( SGPropertyNode *node ): 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 ) { @@ -374,6 +390,10 @@ FGPISimpleController::FGPISimpleController( SGPropertyNode *node ): r_n = prop->getDoubleValue(); } } + prop = child->getChild( "scale" ); + if ( prop != NULL ) { + r_scale = prop->getDoubleValue(); + } } else if ( cname == "output" ) { int i = 0; SGPropertyNode *prop; @@ -433,12 +453,12 @@ void FGPISimpleController::update( double dt ) { if ( debug ) cout << "Updating " << name << endl; double input = 0.0; if ( input_prop != NULL ) { - input = input_prop->getDoubleValue(); + input = input_prop->getDoubleValue() * y_scale; } double r_n = 0.0; if ( r_n_prop != NULL ) { - r_n = r_n_prop->getDoubleValue(); + r_n = r_n_prop->getDoubleValue() * r_scale; } else { r_n = r_n_value; } diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx index 03fd3f232..73cee4906 100644 --- a/src/Autopilot/xmlauto.hxx +++ b/src/Autopilot/xmlauto.hxx @@ -96,6 +96,8 @@ private: // Input values double y_n; // measured process value 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 // Configuration values double Kp; // proportional gain @@ -161,6 +163,8 @@ private: // Input values double y_n; // measured process value 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 u_min; // Minimum output clamp double u_max; // Maximum output clamp