diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx index 8a91a091c..8d07a66bd 100644 --- a/src/Autopilot/xmlauto.cxx +++ b/src/Autopilot/xmlauto.cxx @@ -40,14 +40,42 @@ using std::cout; using std::endl; +FGPeriodicalValue::FGPeriodicalValue( SGPropertyNode_ptr root ) +{ + SGPropertyNode_ptr minNode = root->getChild( "min" ); + SGPropertyNode_ptr maxNode = root->getChild( "max" ); + if( minNode == NULL || maxNode == NULL ) { + SG_LOG(SG_AUTOPILOT, SG_ALERT, "periodical defined, but no and/or tag. Period ignored." ); + } else { + minPeriod = new FGXMLAutoInput( minNode ); + maxPeriod = new FGXMLAutoInput( maxNode ); + } +} + +double FGPeriodicalValue::normalize( double value ) +{ + if( !(minPeriod && maxPeriod )) return value; + + double p1 = minPeriod->get_value(); + double p2 = maxPeriod->get_value(); + + double min = std::min(p1,p2); + double max = std::max(p1,p2); + double phase = fabs(max - min); + + if( phase > SGLimitsd::min() ) { + while( value < min ) value += phase; + while( value > max ) value -= phase; + } else { + value = min; // phase is zero + } + + return value; +} + FGXMLAutoInput::FGXMLAutoInput( SGPropertyNode_ptr node, double value, double offset, double scale) : value(0.0), abs(false), - property(NULL), - offset(NULL), - scale(NULL), - min(NULL), - max(NULL), _condition(NULL) { parse( node, value, offset, scale ); @@ -62,6 +90,7 @@ void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffs scale = NULL; min = NULL; max = NULL; + periodical = NULL; if( node == NULL ) return; @@ -92,6 +121,10 @@ void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffs abs = n->getBoolValue(); } + if( (n = node->getChild( "period" )) != NULL ) { + periodical = new FGPeriodicalValue( n ); + } + SGPropertyNode *valueNode = node->getChild( "value" ); if ( valueNode != NULL ) { value = valueNode->getDoubleValue(); @@ -161,6 +194,10 @@ double FGXMLAutoInput::get_value() if( value > m ) value = m; } + + if( periodical ) { + value = periodical->normalize( value ); + } return abs ? fabs(value) : value; } @@ -246,6 +283,8 @@ void FGXMLAutoComponent::parseNode(SGPropertyNode* aNode) umaxInput.push_back( new FGXMLAutoInput( child ) ); } else if ( cname == "u_max" ) { umaxInput.push_back( new FGXMLAutoInput( child ) ); + } else if ( cname == "period" ) { + periodical = new FGPeriodicalValue( child ); } else { SG_LOG(SG_AUTOPILOT, SG_ALERT, "malformed autopilot definition - unrecognized node:" << cname << " in section " << name); @@ -314,6 +353,11 @@ void FGXMLAutoComponent::do_feedback_if_disabled() double FGXMLAutoComponent::clamp( double value ) { + //If this is a periodical value, normalize it into our domain + // before clamping + if( periodical ) + value = periodical->normalize( value ); + // clamp, if either min or max is defined if( uminInput.size() + umaxInput.size() > 0 ) { double d = umaxInput.get_value( 0.0 ); diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx index 549244c32..efdf2f3ce 100644 --- a/src/Autopilot/xmlauto.hxx +++ b/src/Autopilot/xmlauto.hxx @@ -34,16 +34,28 @@ #include #include +typedef SGSharedPtr FGXMLAutoInput_ptr; +typedef SGSharedPtr FGPeriodicalValue_ptr; + +class FGPeriodicalValue : public SGReferenced { +private: + FGXMLAutoInput_ptr minPeriod; // The minimum value of the period + FGXMLAutoInput_ptr maxPeriod; // The maximum value of the period +public: + FGPeriodicalValue( SGPropertyNode_ptr node ); + double normalize( double value ); +}; class FGXMLAutoInput : public SGReferenced { private: double value; // The value as a constant or initializer for the property bool abs; // return absolute value SGPropertyNode_ptr property; // The name of the property containing the value - SGSharedPtr offset; // A fixed offset, defaults to zero - SGSharedPtr scale; // A constant scaling factor defaults to one - SGSharedPtr min; // A minimum clip defaults to no clipping - SGSharedPtr max; // A maximum clip defaults to no clipping + FGXMLAutoInput_ptr offset; // A fixed offset, defaults to zero + FGXMLAutoInput_ptr scale; // A constant scaling factor defaults to one + FGXMLAutoInput_ptr min; // A minimum clip defaults to no clipping + FGXMLAutoInput_ptr max; // A maximum clip defaults to no clipping + FGPeriodicalValue_ptr periodical; // SGSharedPtr _condition; public: @@ -71,9 +83,9 @@ public: }; -class FGXMLAutoInputList : public std::vector > { +class FGXMLAutoInputList : public std::vector { public: - FGXMLAutoInput * get_active() { + FGXMLAutoInput_ptr get_active() { for (iterator it = begin(); it != end(); ++it) { if( (*it)->is_enabled() ) return *it; @@ -82,7 +94,7 @@ class FGXMLAutoInputList : public std::vector > { } double get_value( double def = 0.0 ) { - FGXMLAutoInput * input = get_active(); + FGXMLAutoInput_ptr input = get_active(); return input == NULL ? def : input->get_value(); } @@ -147,6 +159,7 @@ protected: FGXMLAutoInputList referenceInput; FGXMLAutoInputList uminInput; FGXMLAutoInputList umaxInput; + FGPeriodicalValue_ptr periodical; // debug flag bool debug; bool enabled; @@ -221,6 +234,8 @@ public: bool isPropertyEnabled(); }; +typedef SGSharedPtr FGXMLAutoComponent_ptr; + /** * Roy Ovesen's PID controller @@ -384,7 +399,7 @@ public: bool build( SGPropertyNode_ptr ); protected: - typedef std::vector > comp_list; + typedef std::vector comp_list; private: