fly/flightgear
1
0
Fork 0
Code Activity

Autopilot: add interface properties and property-root.

Browse source 
- Support interface properties as with JSBSim for easy reuse
   and parametrization of autopilot components.
 - Add property-root property to allow changing property root
   for all relative paths. This allows easy use of multiple
   instances of the same autopilot component at the same time
   by specifiying different property root nodes.
This commit is contained in:
Thomas Geymayer 2014-02-09 17:40:19 +01:00
parent 4ae8cd99a1
commit e600cd3d00
20 changed files with 588 additions and 357 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

87
src/Autopilot/analogcomponent.cxx
View file

@ -49,65 +49,88 @@ double AnalogComponent::clamp( double value ) const
return value;
}
bool AnalogComponent::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
bool AnalogComponent::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
SG_LOG( SG_AUTOPILOT, SG_BULK, "AnalogComponent::configure(" << nodeName << ")" );
if( Component::configure( nodeName, configNode ) )
SG_LOG
(
SG_AUTOPILOT,
SG_BULK,
"AnalogComponent::configure(" << cfg_name << ")"
);
if( Component::configure(cfg_node, cfg_name, prop_root) )
return true;
if ( nodeName == "feedback-if-disabled" ) {
_feedback_if_disabled = configNode->getBoolValue();
if( cfg_name == "feedback-if-disabled" )
{
_feedback_if_disabled = cfg_node.getBoolValue();
return true;
}
if ( nodeName == "output" ) {
// grab all <prop> and <property> childs
int found = 0;
// backwards compatibility: allow <prop> elements
SGPropertyNode_ptr prop;
for( int i = 0; (prop = configNode->getChild("prop", i)) != NULL; i++ ) {
SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
_output_list.push_back( tmp );
found++;
}
for( int i = 0; (prop = configNode->getChild("property", i)) != NULL; i++ ) {
SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
_output_list.push_back( tmp );
found++;
if( cfg_name == "output" )
{
// grab all <prop> and <property> childs.
bool found = false;
for( int i = 0; i < cfg_node.nChildren(); ++i )
{
SGPropertyNode& child = *cfg_node.getChild(i);
const std::string& name = child.getNameString();
// Allow "prop" for backwards compatiblity
if( name != "property" && name != "prop" )
continue;
_output_list.push_back( prop_root.getNode(child.getStringValue(), true) );
found = true;
}
// no <prop> elements, text node of <output> is property name
if( found == 0 )
_output_list.push_back( fgGetNode(configNode->getStringValue(), true ) );
if( !found )
_output_list.push_back
(
prop_root.getNode(cfg_node.getStringValue(), true)
);
return true;
}
if( nodeName == "input" ) {
_valueInput.push_back( new InputValue( configNode ) );
if( cfg_name == "input" )
{
_valueInput.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
if( nodeName == "reference" ) {
_referenceInput.push_back( new InputValue( configNode ) );
if( cfg_name == "reference" )
{
_referenceInput.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
if( nodeName == "min" || nodeName == "u_min" ) {
_minInput.push_back( new InputValue( configNode ) );
if( cfg_name == "min" || cfg_name == "u_min" )
{
_minInput.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
if( nodeName == "max" || nodeName == "u_max" ) {
_maxInput.push_back( new InputValue( configNode ) );
if( cfg_name == "max" || cfg_name == "u_max" )
{
_maxInput.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
if( nodeName == "period" ) {
_periodical = new PeriodicalValue( configNode );
if( cfg_name == "period" )
{
_periodical = new PeriodicalValue(prop_root, cfg_node);
return true;
}
SG_LOG( SG_AUTOPILOT, SG_BULK, "AnalogComponent::configure(" << nodeName << ") [unhandled]" );
SG_LOG
(
SG_AUTOPILOT,
SG_BULK,
"AnalogComponent::configure(" << cfg_name << ") [unhandled]"
);
return false;
}

15
src/Autopilot/analogcomponent.hxx
View file

@ -83,14 +83,17 @@ protected:
AnalogComponent();
/**
* @brief This method configures this analog component from a property node. Gets
* called multiple times from the base class configure method for every
config node.
* @param nodeName the name of the configuration node provided in configNode
* @param configNode the configuration node itself
* @brief This method configures this analog component from a property node.
* Gets called multiple times from the base class configure method
* for every configuration node.
* @param cfg_name Name of the configuration node provided in cfg_node
* @param cfg_node Configuration node itself
* @param prop_root Property root for all relative paths
* @return true if the node was handled, false otherwise.
*/
virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
virtual bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
/**
* @brief clamp the given value if &lt;min&gt; and/or &lt;max&gt; inputs were given

68
src/Autopilot/autopilot.cxx
View file

@ -49,9 +49,10 @@ using namespace FGXMLAutopilot;
class StateMachineComponent : public Component
{
public:
StateMachineComponent(SGPropertyNode_ptr config)
StateMachineComponent( SGPropertyNode& cfg,
SGPropertyNode& props_root )
{
inner = simgear::StateMachine::createFromPlist(config, globals->get_props());
inner = simgear::StateMachine::createFromPlist(&cfg, &props_root);
}
virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr config)
@ -73,9 +74,10 @@ class StateMachineFunctor : public FunctorBase<Component>
{
public:
virtual ~StateMachineFunctor() {}
virtual Component* operator()( SGPropertyNode_ptr configNode )
virtual Component* operator()( SGPropertyNode& cfg,
SGPropertyNode& prop_root )
{
return new StateMachineComponent(configNode);
return new StateMachineComponent(cfg, prop_root);
}
};
@ -91,6 +93,28 @@ ComponentForge::~ComponentForge()
delete it->second;
}
void readInterfaceProperties( SGPropertyNode_ptr prop_root,
SGPropertyNode_ptr cfg )
{
simgear::PropertyList cfg_props = cfg->getChildren("property");
for( simgear::PropertyList::iterator it = cfg_props.begin();
it != cfg_props.end();
++it )
{
SGPropertyNode_ptr prop = prop_root->getNode((*it)->getStringValue(), true);
SGPropertyNode* val = (*it)->getNode("_attr_/value");
if( val )
{
prop->setDoubleValue( val->getDoubleValue() );
// TODO should we keep the _attr_ node, as soon as the property browser is
// able to cope with it?
(*it)->removeChild("_attr_", 0, false);
}
}
}
static ComponentForge componentForge;
Autopilot::Autopilot( SGPropertyNode_ptr rootNode, SGPropertyNode_ptr configNode ) :
@ -109,18 +133,44 @@ Autopilot::Autopilot( SGPropertyNode_ptr rootNode, SGPropertyNode_ptr configNode
componentForge["state-machine"] = new StateMachineFunctor();
}
if( configNode == NULL ) configNode = rootNode;
if( !configNode )
configNode = rootNode;
// property-root can be set in config file and overridden in the local system
// node. This allows using the same autopilot multiple times but with
// different paths (with all relative property paths being relative to the
// node specified with property-root)
SGPropertyNode_ptr prop_root_node = rootNode->getChild("property-root");
if( !prop_root_node )
prop_root_node = configNode->getChild("property-root");
SGPropertyNode_ptr prop_root =
fgGetNode(prop_root_node ? prop_root_node->getStringValue() : "/", true);
// Just like the JSBSim interface properties for systems, create properties
// given in the autopilot file and set to given (default) values.
readInterfaceProperties(prop_root, configNode);
// Afterwards read the properties specified in local system node to allow
// overriding initial or default values. This allows reusing components with
// just different "parameter" values.
readInterfaceProperties(prop_root, rootNode);
int count = configNode->nChildren();
for ( int i = 0; i < count; ++i ) {
for( int i = 0; i < count; ++i )
{
SGPropertyNode_ptr node = configNode->getChild(i);
string childName = node->getName();
if( componentForge.count(childName) == 0 ) {
SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled element <" << childName << ">" << std::endl );
if( childName == "property"
|| childName == "property-root" )
continue;
if( componentForge.count(childName) == 0 )
{
SG_LOG(SG_AUTOPILOT, SG_BULK, "unhandled element <" << childName << ">");
continue;
}
Component * component = (*componentForge[childName])(node);
Component * component = (*componentForge[childName])(*prop_root, *node);
if( component->get_name().length() == 0 ) {
std::ostringstream buf;
buf << "unnamed_component_" << i;

62
src/Autopilot/component.cxx
View file

@ -40,64 +40,82 @@ Component::~Component()
delete _enable_value;
}
bool Component::configure( SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool Component::configure( SGPropertyNode& prop_root,
SGPropertyNode& cfg )
{
for (int i = 0; i < configNode->nChildren(); ++i ) {
SGPropertyNode_ptr prop;
SGPropertyNode_ptr child = configNode->getChild(i);
for( int i = 0; i < cfg.nChildren(); ++i )
{
SGPropertyNode_ptr child = cfg.getChild(i);
std::string cname(child->getName());
if( configure( cname, child ) )
continue;
} // for configNode->nChildren()
if( !configure(*child, cname, prop_root) )
SG_LOG
(
SG_AUTOPILOT,
SG_INFO,
"Component::configure: unknown node: " << cname
);
}
return true;
}
bool Component::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool Component::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
SG_LOG( SG_AUTOPILOT, SG_BULK, "Component::configure(" << nodeName << ")" << std::endl );
SG_LOG
(
SG_AUTOPILOT,
SG_BULK,
"Component::configure(" << cfg_name << ")"
);
if ( nodeName == "name" ) {
_name = configNode->getStringValue();
if ( cfg_name == "name" ) {
_name = cfg_node.getStringValue();
return true;
}
if ( nodeName == "debug" ) {
_debug = configNode->getBoolValue();
if ( cfg_name == "debug" ) {
_debug = cfg_node.getBoolValue();
return true;
}
if ( nodeName == "enable" ) {
if ( cfg_name == "enable" ) {
SGPropertyNode_ptr prop;
if( (prop = configNode->getChild("condition")) != NULL ) {
if( (prop = cfg_node.getChild("condition")) != NULL ) {
_condition = sgReadCondition(fgGetNode("/"), prop);
return true;
}
if ( (prop = configNode->getChild( "property" )) != NULL ) {
if ( (prop = cfg_node.getChild( "property" )) != NULL ) {
_enable_prop = fgGetNode( prop->getStringValue(), true );
}
if ( (prop = configNode->getChild( "prop" )) != NULL ) {
if ( (prop = cfg_node.getChild( "prop" )) != NULL ) {
_enable_prop = fgGetNode( prop->getStringValue(), true );
}
if ( (prop = configNode->getChild( "value" )) != NULL ) {
if ( (prop = cfg_node.getChild( "value" )) != NULL ) {
delete _enable_value;
_enable_value = new std::string(prop->getStringValue());
}
if ( (prop = configNode->getChild( "honor-passive" )) != NULL ) {
if ( (prop = cfg_node.getChild( "honor-passive" )) != NULL ) {
_honor_passive = prop->getBoolValue();
}
return true;
} // enable
SG_LOG( SG_AUTOPILOT, SG_BULK, "Component::configure(" << nodeName << ") [unhandled]" << std::endl );
SG_LOG
(
SG_AUTOPILOT,
SG_BULK,
"Component::configure(" << cfg_name << ") [unhandled]"
);
return false;
}

17
src/Autopilot/component.hxx
View file

@ -47,8 +47,9 @@ private:
protected:
virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
virtual bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
/**
* @brief the implementation of the update() method of the SGSubsystem
@ -97,11 +98,15 @@ public:
virtual ~Component();
/**
* @brief configure this component from a property node. Iterates through all nodes found
* as childs under configNode and calls configure of the derived class for each child.
* @param configNode the property node containing the configuration
* @brief configure this component from a property node. Iterates through
* all nodes found as children under configNode and calls configure
* of the derived class for each child.
*
* @param prop_root Property root for all relative paths
* @param cfg Property node containing the configuration
*/
bool configure( SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& prop_root,
SGPropertyNode& cfg );
/**
* @brief getter for the name property

30
src/Autopilot/digitalcomponent.cxx
View file

@ -56,13 +56,15 @@ bool DigitalComponent::InputMap::get_value( const std::string & name ) const
</output>
<output>/some/property</output>
*/
bool DigitalComponent::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
bool DigitalComponent::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if( Component::configure( nodeName, configNode ) )
if( Component::configure(cfg_node, cfg_name, prop_root) )
return true;
if (nodeName == "input") {
SGPropertyNode_ptr nameNode = configNode->getNode("name");
if (cfg_name == "input") {
SGPropertyNode_ptr nameNode = cfg_node.getNode("name");
string name;
if( nameNode != NULL ) {
name = nameNode->getStringValue();
@ -71,12 +73,12 @@ bool DigitalComponent::configure( const std::string & nodeName, SGPropertyNode_p
buf << "Input" << _input.size();
name = buf.str();
}
_input[name] = sgReadCondition( fgGetNode("/"), configNode );
_input[name] = sgReadCondition(&prop_root, &cfg_node);
return true;
}
if (nodeName == "output") {
SGPropertyNode_ptr n = configNode->getNode("name");
if (cfg_name == "output") {
SGPropertyNode_ptr n = cfg_node.getNode("name");
string name;
if( n != NULL ) {
name = n->getStringValue();
@ -89,20 +91,20 @@ bool DigitalComponent::configure( const std::string & nodeName, SGPropertyNode_p
DigitalOutput_ptr o = new DigitalOutput();
_output[name] = o;
if( (n = configNode->getNode("inverted")) != NULL )
if( (n = cfg_node.getNode("inverted")) != NULL )
o->setInverted( n->getBoolValue() );
if( (n = configNode->getNode("property")) != NULL )
o->setProperty( fgGetNode( n->getStringValue(), true ) );
if( (n = cfg_node.getNode("property")) != NULL )
o->setProperty( prop_root.getNode(n->getStringValue(), true) );
if( configNode->nChildren() == 0 )
o->setProperty( fgGetNode( configNode->getStringValue(), true ) );
if( cfg_node.nChildren() == 0 )
o->setProperty( prop_root.getNode(cfg_node.getStringValue(), true) );
return true;
}
if (nodeName == "inverted") {
_inverted = configNode->getBoolValue();
if (cfg_name == "inverted") {
_inverted = cfg_node.getBoolValue();
return true;
}

9
src/Autopilot/digitalcomponent.hxx
View file

@ -122,11 +122,14 @@ protected:
/**
* @brief Over-rideable hook method to allow derived classes to refine top-level
* node parsing.
* @param aName
* @param aNode
* @param cfg_node
* @param cfg_name
* @param prop_root
* @return true if the node was handled, false otherwise.
*/
virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
virtual bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
};
}

200
src/Autopilot/digitalfilter.cxx
View file

@ -42,13 +42,16 @@ namespace FGXMLAutopilot {
*/
class DigitalFilterImplementation : public SGReferenced {
protected:
virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode) = 0;
virtual bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root ) = 0;
public:
virtual ~DigitalFilterImplementation() {}
DigitalFilterImplementation();
virtual void initialize( double initvalue ) {}
virtual double compute( double dt, double input ) = 0;
bool configure( SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& cfg,
SGPropertyNode& prop_root );
void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
@ -61,7 +64,9 @@ protected:
class GainFilterImplementation : public DigitalFilterImplementation {
protected:
InputValueList _gainInput;
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
public:
GainFilterImplementation() : _gainInput(1.0) {}
double compute( double dt, double input );
@ -75,7 +80,9 @@ public:
class DerivativeFilterImplementation : public GainFilterImplementation {
InputValueList _TfInput;
double _input_1;
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
public:
DerivativeFilterImplementation();
double compute( double dt, double input );
@ -85,7 +92,9 @@ public:
class ExponentialFilterImplementation : public GainFilterImplementation {
protected:
InputValueList _TfInput;
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
bool _isSecondOrder;
double _output_1, _output_2;
public:
@ -99,7 +108,9 @@ protected:
InputValueList _samplesInput;
double _output_1;
std::deque <double> _inputQueue;
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
public:
MovingAverageFilterImplementation();
double compute( double dt, double input );
@ -110,7 +121,9 @@ class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
protected:
double _output_1;
InputValueList _rateOfChangeInput;
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
public:
NoiseSpikeFilterImplementation();
double compute( double dt, double input );
@ -122,7 +135,9 @@ protected:
double _output_1;
InputValueList _rateOfChangeMax;
InputValueList _rateOfChangeMin ;
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
public:
RateLimitFilterImplementation();
double compute( double dt, double input );
@ -136,7 +151,9 @@ protected:
InputValueList _maxInput;
double _input_1;
double _output_1;
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
public:
IntegratorFilterImplementation();
double compute( double dt, double input );
@ -148,7 +165,9 @@ protected:
InputValueList _TfInput;
double _input_1;
double _output_1;
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
public:
HighPassFilterImplementation();
double compute( double dt, double input );
@ -160,7 +179,9 @@ protected:
InputValueList _TfbInput;
double _input_1;
double _output_1;
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
public:
LeadLagFilterImplementation();
double compute( double dt, double input );
@ -180,18 +201,17 @@ DigitalFilterImplementation::DigitalFilterImplementation() :
{
}
bool DigitalFilterImplementation::configure( SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool DigitalFilterImplementation::configure( SGPropertyNode& cfg,
SGPropertyNode& prop_root )
{
for (int i = 0; i < configNode->nChildren(); ++i ) {
SGPropertyNode_ptr prop;
for (int i = 0; i < cfg.nChildren(); ++i )
{
SGPropertyNode_ptr child = cfg.getChild(i);
SGPropertyNode_ptr child = configNode->getChild(i);
string cname(child->getName());
if( configure( cname, child ) )
if( configure(*child, child->getNameString(), prop_root) )
continue;
} // for configNode->nChildren()
}
return true;
}
@ -204,10 +224,12 @@ double GainFilterImplementation::compute( double dt, double input )
return _gainInput.get_value() * input;
}
bool GainFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
bool GainFilterImplementation::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if (nodeName == "gain" ) {
_gainInput.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "gain" ) {
_gainInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
@ -239,14 +261,16 @@ void DerivativeFilterImplementation::initialize( double initvalue )
_input_1 = initvalue;
}
bool DerivativeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool DerivativeFilterImplementation::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if( GainFilterImplementation::configure( nodeName, configNode ) )
if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
return true;
if (nodeName == "filter-time" ) {
_TfInput.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "filter-time" ) {
_TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
@ -299,10 +323,12 @@ double MovingAverageFilterImplementation::compute( double dt, double input )
return output_0;
}
bool MovingAverageFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
bool MovingAverageFilterImplementation::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if (nodeName == "samples" ) {
_samplesInput.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "samples" ) {
_samplesInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
@ -337,10 +363,13 @@ double NoiseSpikeFilterImplementation::compute( double dt, double input )
return (_output_1 = _output_1 + copysign( maxChange, delta ));
}
bool NoiseSpikeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool NoiseSpikeFilterImplementation::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if (nodeName == "max-rate-of-change" ) {
_rateOfChangeInput.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "max-rate-of-change" ) {
_rateOfChangeInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
@ -379,14 +408,16 @@ double RateLimitFilterImplementation::compute( double dt, double input )
return (output);
}
bool RateLimitFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
bool RateLimitFilterImplementation::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if (nodeName == "max-rate-of-change" ) {
_rateOfChangeMax.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "max-rate-of-change" ) {
_rateOfChangeMax.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
if (nodeName == "min-rate-of-change" ) {
_rateOfChangeMin.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "min-rate-of-change" ) {
_rateOfChangeMin.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
@ -431,18 +462,21 @@ double ExponentialFilterImplementation::compute( double dt, double input )
return (_output_1 = output_0);
}
bool ExponentialFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool ExponentialFilterImplementation::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if( GainFilterImplementation::configure( nodeName, configNode ) )
if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
return true;
if (nodeName == "filter-time" ) {
_TfInput.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "filter-time" ) {
_TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
if (nodeName == "type" ) {
string type(configNode->getStringValue());
if (cfg_name == "type" ) {
string type(cfg_node.getStringValue());
_isSecondOrder = type == "double-exponential";
}
@ -462,17 +496,20 @@ void IntegratorFilterImplementation::initialize( double initvalue )
_input_1 = _output_1 = initvalue;
}
bool IntegratorFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool IntegratorFilterImplementation::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if( GainFilterImplementation::configure( nodeName, configNode ) )
if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
return true;
if (nodeName == "u_min" ) {
_minInput.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "u_min" ) {
_minInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
if (nodeName == "u_max" ) {
_maxInput.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "u_max" ) {
_maxInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
return false;
@ -523,13 +560,16 @@ double HighPassFilterImplementation::compute( double dt, double input )
return output;
}
bool HighPassFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool HighPassFilterImplementation::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if( GainFilterImplementation::configure( nodeName, configNode ) )
if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
return true;
if (nodeName == "filter-time" ) {
_TfInput.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "filter-time" ) {
_TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
@ -570,24 +610,27 @@ double LeadLagFilterImplementation::compute( double dt, double input )
return output;
}
bool LeadLagFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool LeadLagFilterImplementation::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if( GainFilterImplementation::configure( nodeName, configNode ) )
if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
return true;
if (nodeName == "filter-time-a" ) {
_TfaInput.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "filter-time-a" ) {
_TfaInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
if (nodeName == "filter-time-b" ) {
_TfbInput.push_back( new InputValue( configNode, 1 ) );
if (cfg_name == "filter-time-b" ) {
_TfbInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
return false;
}
/* --------------------------------------------------------------------------------- */
/* Digital Filter Component Implementation */
/* --------------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* Digital Filter Component Implementation */
/* -------------------------------------------------------------------------- */
DigitalFilter::DigitalFilter() :
AnalogComponent(),
@ -602,7 +645,10 @@ DigitalFilter::~DigitalFilter()
static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configNode)
//------------------------------------------------------------------------------
bool DigitalFilter::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if( componentForge.empty() ) {
componentForge["gain"] = new CreateAndConfigureFunctor<GainFilterImplementation,DigitalFilterImplementation>();
@ -618,23 +664,24 @@ bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configN
componentForge["integrator"] = new CreateAndConfigureFunctor<IntegratorFilterImplementation,DigitalFilterImplementation>();
}
SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ")" << endl );
if( AnalogComponent::configure( nodeName, configNode ) )
SG_LOG(SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << cfg_name << ")");
if( AnalogComponent::configure(cfg_node, cfg_name, prop_root) )
return true;
if (nodeName == "type" ) {
string type( configNode->getStringValue() );
if (cfg_name == "type" ) {
string type( cfg_node.getStringValue() );
if( componentForge.count(type) == 0 ) {
SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled filter type <" << type << ">" << endl );
return true;
}
_implementation = (*componentForge[type])( configNode->getParent() );
_implementation = (*componentForge[type])(*cfg_node.getParent(), prop_root);
_implementation->setDigitalFilter( this );
return true;
}
if( nodeName == "initialize-to" ) {
string s( configNode->getStringValue() );
if( cfg_name == "initialize-to" ) {
string s( cfg_node.getStringValue() );
if( s == "input" ) {
_initializeTo = INITIALIZE_INPUT;
} else if( s == "output" ) {
@ -647,7 +694,12 @@ bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configN
return true;
}
SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ") [unhandled]" << endl );
SG_LOG
(
SG_AUTOPILOT,
SG_BULK,
"DigitalFilter::configure(" << cfg_name << ") [unhandled]"
);
return false; // not handled by us, let the base class try
}

6
src/Autopilot/digitalfilter.hxx
View file

@ -47,8 +47,10 @@ private:
};
protected:
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode);
void update( bool firstTime, double dt);
virtual bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
virtual void update( bool firstTime, double dt);
InputValueList _Tf;
InputValueList _samples;

75
src/Autopilot/flipflop.cxx
View file

@ -264,7 +264,9 @@ public:
*/
class MonoFlopImplementation : public JKFlipFlopImplementation {
protected:
virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
virtual bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
InputValueList _time;
double _t;
public:
@ -288,13 +290,16 @@ public:
using namespace FGXMLAutopilot;
bool MonoFlopImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool MonoFlopImplementation::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if( JKFlipFlopImplementation::configure( nodeName, configNode ) )
if( JKFlipFlopImplementation::configure(cfg_node, cfg_name, prop_root) )
return true;
if (nodeName == "time") {
_time.push_back( new InputValue( configNode ) );
if (cfg_name == "time") {
_time.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
@ -371,18 +376,18 @@ bool JKFlipFlopImplementation::onRaisingEdge( DigitalComponent::InputMap input,
return false; // signal no change
}
bool FlipFlopImplementation::configure( SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool FlipFlopImplementation::configure( SGPropertyNode& prop_root,
SGPropertyNode& cfg )
{
for (int i = 0; i < configNode->nChildren(); ++i ) {
SGPropertyNode_ptr prop;
SGPropertyNode_ptr child = configNode->getChild(i);
for( int i = 0; i < cfg.nChildren(); ++i )
{
SGPropertyNode_ptr child = cfg.getChild(i);
string cname(child->getName());
if( configure( cname, child ) )
if( configure(*child, cname, prop_root) )
continue;
} // for configNode->nChildren()
}
return true;
}
@ -390,7 +395,10 @@ bool FlipFlopImplementation::configure( SGPropertyNode_ptr configNode )
static map<string,FunctorBase<FlipFlopImplementation> *> componentForge;
bool FlipFlop::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool FlipFlop::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if( componentForge.empty() ) {
componentForge["RS"] = new CreateAndConfigureFunctor<RSFlipFlopImplementation,FlipFlopImplementation>();
@ -401,46 +409,51 @@ bool FlipFlop::configure( const std::string & nodeName, SGPropertyNode_ptr confi
componentForge["monostable"] = new CreateAndConfigureFunctor<MonoFlopImplementation, FlipFlopImplementation>();
}
if( DigitalComponent::configure( nodeName, configNode ) )
if( DigitalComponent::configure(cfg_node, cfg_name, prop_root) )
return true;
if( nodeName == "type" ) {
string type(configNode->getStringValue());
if( cfg_name == "type" ) {
string type(cfg_node.getStringValue());
if( componentForge.count(type) == 0 ) {
SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled flip-flop type <" << type << ">" << endl );
SG_LOG
(
SG_AUTOPILOT,
SG_BULK,
"unhandled flip-flop type <" << type << ">"
);
return true;
}
_implementation = (*componentForge[type])( configNode->getParent() );
_implementation = (*componentForge[type])(prop_root, *cfg_node.getParent());
return true;
}
if (nodeName == "set"||nodeName == "S") {
_input["S"] = sgReadCondition( fgGetNode("/"), configNode );
if (cfg_name == "set"||cfg_name == "S") {
_input["S"] = sgReadCondition(&prop_root, &cfg_node);
return true;
}
if (nodeName == "reset" || nodeName == "R" ) {
_input["R"] = sgReadCondition( fgGetNode("/"), configNode );
if (cfg_name == "reset" || cfg_name == "R" ) {
_input["R"] = sgReadCondition(&prop_root, &cfg_node);
return true;
}
if (nodeName == "J") {
_input["J"] = sgReadCondition( fgGetNode("/"), configNode );
if (cfg_name == "J") {
_input["J"] = sgReadCondition(&prop_root, &cfg_node);
return true;
}
if (nodeName == "K") {
_input["K"] = sgReadCondition( fgGetNode("/"), configNode );
if (cfg_name == "K") {
_input["K"] = sgReadCondition(&prop_root, &cfg_node);
return true;
}
if (nodeName == "D") {
_input["D"] = sgReadCondition( fgGetNode("/"), configNode );
if (cfg_name == "D") {
_input["D"] = sgReadCondition(&prop_root, &cfg_node);
return true;
}
if (nodeName == "clock") {
_input["clock"] = sgReadCondition( fgGetNode("/"), configNode );
if (cfg_name == "clock") {
_input["clock"] = sgReadCondition(&prop_root, &cfg_node);
return true;
}

12
src/Autopilot/flipflop.hxx
View file

@ -36,7 +36,10 @@ protected:
* as childs under configNode and calls configure of the derived class for each child.
* @param configNode the property node containing the configuration
*/
virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode ) { return false; }
virtual bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{ return false; }
public:
virtual ~FlipFlopImplementation() {}
/**
@ -53,7 +56,8 @@ public:
* as childs under configNode and calls configure of the derived class for each child.
* @param configNode the property node containing the configuration
*/
bool configure( SGPropertyNode_ptr configNode );
bool configure( SGPropertyNode& prop_root,
SGPropertyNode& cfg );
};
/**
@ -69,7 +73,9 @@ protected:
* @param aNode
* @return true if the node was handled, false otherwise.
*/
virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
virtual bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
/**
* @brief Implementation of the pure virtual function of the Component class. Gets called from

9
src/Autopilot/functor.hxx
View file

@ -32,15 +32,18 @@ namespace FGXMLAutopilot {
template <class TBase> class FunctorBase {
public:
virtual ~FunctorBase() {}
virtual TBase * operator()( SGPropertyNode_ptr configNode ) = 0;
virtual TBase * operator()( SGPropertyNode& prop_root,
SGPropertyNode& cfg ) = 0;
};
template <class TClass,class TBase> class CreateAndConfigureFunctor :
public FunctorBase<TBase> {
public:
virtual TBase * operator()( SGPropertyNode_ptr configNode ) {
virtual TBase * operator()( SGPropertyNode& prop_root,
SGPropertyNode& cfg )
{
TBase * base = new TClass();
base->configure( configNode );
base->configure(prop_root, cfg);
return base;
}
};

195
src/Autopilot/inputvalue.cxx
View file

@ -25,23 +25,37 @@
using namespace FGXMLAutopilot;
PeriodicalValue::PeriodicalValue( SGPropertyNode_ptr root )
//------------------------------------------------------------------------------
PeriodicalValue::PeriodicalValue( SGPropertyNode& prop_root,
SGPropertyNode& cfg )
{
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 <min> and/or <max> tag. Period ignored." );
} else {
minPeriod = new InputValue( minNode );
maxPeriod = new InputValue( maxNode );
SGPropertyNode_ptr minNode = cfg.getChild( "min" );
SGPropertyNode_ptr maxNode = cfg.getChild( "max" );
if( !minNode || !maxNode )
{
SG_LOG
(
SG_AUTOPILOT,
SG_ALERT,
"periodical defined, but no <min> and/or <max> tag. Period ignored."
);
}
else
{
minPeriod = new InputValue(prop_root, *minNode);
maxPeriod = new InputValue(prop_root, *maxNode);
}
}
//------------------------------------------------------------------------------
double PeriodicalValue::normalize( double value ) const
{
return SGMiscd::normalizePeriodic( minPeriod->get_value(), maxPeriod->get_value(), value );
return SGMiscd::normalizePeriodic( minPeriod->get_value(),
maxPeriod->get_value(),
value );
}
//------------------------------------------------------------------------------
double PeriodicalValue::normalizeSymmetric( double value ) const
{
double minValue = minPeriod->get_value();
@ -52,101 +66,100 @@ double PeriodicalValue::normalizeSymmetric( double value ) const
return value > width_2 ? width_2 - value : value;
}
InputValue::InputValue( SGPropertyNode_ptr node, double value, double offset, double scale) :
//------------------------------------------------------------------------------
InputValue::InputValue( SGPropertyNode& prop_root,
SGPropertyNode& cfg,
double value,
double offset,
double scale ):
_value(0.0),
_abs(false)
{
parse( node, value, offset, scale );
parse(prop_root, cfg, value, offset, scale);
}
void InputValue::parse( SGPropertyNode_ptr node, double aValue, double aOffset, double aScale )
//------------------------------------------------------------------------------
void InputValue::parse( SGPropertyNode& prop_root,
SGPropertyNode& cfg,
double aValue,
double aOffset,
double aScale )
{
_value = aValue;
_property = NULL;
_offset = NULL;
_scale = NULL;
_min = NULL;
_max = NULL;
_periodical = NULL;
_value = aValue;
_property = NULL;
_offset = NULL;
_scale = NULL;
_min = NULL;
_max = NULL;
_periodical = NULL;
if( node == NULL )
return;
SGPropertyNode * n;
SGPropertyNode * n;
if( (n = cfg.getChild("condition")) != NULL )
_condition = sgReadCondition(&prop_root, n);
if( (n = node->getChild("condition")) != NULL ) {
_condition = sgReadCondition(fgGetNode("/"), n);
if( (n = cfg.getChild( "scale" )) != NULL )
_scale = new InputValue(prop_root, *n, aScale);
if( (n = cfg.getChild( "offset" )) != NULL )
_offset = new InputValue(prop_root, *n, aOffset);
if( (n = cfg.getChild( "max" )) != NULL )
_max = new InputValue(prop_root, *n);
if( (n = cfg.getChild( "min" )) != NULL )
_min = new InputValue(prop_root, *n);
if( (n = cfg.getChild( "abs" )) != NULL )
_abs = n->getBoolValue();
if( (n = cfg.getChild( "period" )) != NULL )
_periodical = new PeriodicalValue(prop_root, *n);
SGPropertyNode *valueNode = cfg.getChild("value");
if( valueNode != NULL )
_value = valueNode->getDoubleValue();
if( (n = cfg.getChild("expression")) != NULL )
{
_expression = SGReadDoubleExpression(&prop_root, n->getChild(0));
return;
}
// if no <property> element, check for <prop> element for backwards
// compatibility
if( (n = cfg.getChild("property"))
|| (n = cfg.getChild("prop" )) )
{
_property = prop_root.getNode(n->getStringValue(), true);
if( valueNode )
{
// initialize property with given value
// if both <prop> and <value> 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 = node->getChild( "scale" )) != NULL ) {
_scale = new InputValue( n, aScale );
}
if( (n = node->getChild( "offset" )) != NULL ) {
_offset = new InputValue( n, aOffset );
}
if( (n = node->getChild( "max" )) != NULL ) {
_max = new InputValue( n );
}
if( (n = node->getChild( "min" )) != NULL ) {
_min = new InputValue( n );
}
if( (n = node->getChild( "abs" )) != NULL ) {
_abs = n->getBoolValue();
}
if( (n = node->getChild( "period" )) != NULL ) {
_periodical = new PeriodicalValue( n );
}
SGPropertyNode *valueNode = node->getChild( "value" );
if ( valueNode != NULL ) {
_value = valueNode->getDoubleValue();
}
if ((n = node->getChild("expression")) != NULL) {
_expression = SGReadDoubleExpression(fgGetNode("/"), n->getChild(0));
return;
}
n = node->getChild( "property" );
// if no <property> element, check for <prop> 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 <prop> and <value> 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
}
return;
} // of have a <property> or <prop>
return;
} // of have a <property> or <prop>
if (valueNode == NULL) {
// no <value>, <prop> or <expression> 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 );
}
}
if( !valueNode )
{
// no <value>, <prop> or <expression> element, use text node
const char * textnode = cfg.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 = prop_root.getNode(textnode, true);
}
}
void InputValue::set_value( double aValue )

23
src/Autopilot/inputvalue.hxx
View file

@ -44,7 +44,8 @@ private:
InputValue_ptr minPeriod; // The minimum value of the period
InputValue_ptr maxPeriod; // The maximum value of the period
public:
PeriodicalValue( SGPropertyNode_ptr node );
PeriodicalValue( SGPropertyNode& prop_root,
SGPropertyNode& cfg );
double normalize( double value ) const;
double normalizeSymmetric( double value ) const;
};
@ -70,9 +71,25 @@ private:
SGSharedPtr<SGExpressiond> _expression; ///< expression to generate the value
public:
InputValue( SGPropertyNode_ptr node = NULL, double value = 0.0, double offset = 0.0, double scale = 1.0 );
InputValue( SGPropertyNode& prop_root,
SGPropertyNode& node,
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 );
/**
*
* @param prop_root Root node for all properties with relative path
* @param cfg Configuration node
* @param value Default initial value
* @param offset Default initial offset
* @param scale Default initial scale
*/
void parse( SGPropertyNode& prop_root,
SGPropertyNode& cfg,
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() const;

48
src/Autopilot/pidcontroller.cxx
View file

@ -193,54 +193,62 @@ void PIDController::update( bool firstTime, double dt )
}
}
bool PIDController::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
//------------------------------------------------------------------------------
bool PIDController::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ")" << endl );
SG_LOG(SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << cfg_name << ")");
if( AnalogComponent::configure( nodeName, configNode ) )
if( AnalogComponent::configure(cfg_node, cfg_name, prop_root) )
return true;
if( nodeName == "config" ) {
Component::configure( configNode );
if( cfg_name == "config" ) {
Component::configure(prop_root, cfg_node);
return true;
}
if (nodeName == "Ts") {
desiredTs = configNode->getDoubleValue();
if (cfg_name == "Ts") {
desiredTs = cfg_node.getDoubleValue();
return true;
}
if (nodeName == "Kp") {
Kp.push_back( new InputValue(configNode) );
if (cfg_name == "Kp") {
Kp.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
if (nodeName == "Ti") {
Ti.push_back( new InputValue(configNode) );
if (cfg_name == "Ti") {
Ti.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
if (nodeName == "Td") {
Td.push_back( new InputValue(configNode) );
if (cfg_name == "Td") {
Td.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
if (nodeName == "beta") {
beta = configNode->getDoubleValue();
if (cfg_name == "beta") {
beta = cfg_node.getDoubleValue();
return true;
}
if (nodeName == "alpha") {
alpha = configNode->getDoubleValue();
if (cfg_name == "alpha") {
alpha = cfg_node.getDoubleValue();
return true;
}
if (nodeName == "gamma") {
gamma = configNode->getDoubleValue();
if (cfg_name == "gamma") {
gamma = cfg_node.getDoubleValue();
return true;
}
SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ") [unhandled]" << endl );
SG_LOG
(
SG_AUTOPILOT,
SG_BULK,
"PIDController::configure(" << cfg_name << ") [unhandled]"
);
return false;
}

4
src/Autopilot/pidcontroller.hxx
View file

@ -62,7 +62,9 @@ private:
double elapsedTime; // elapsed time (sec)
protected:
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode);
virtual bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
public:
PIDController();
~PIDController() {}

35
src/Autopilot/pisimplecontroller.cxx
View file

@ -25,32 +25,33 @@
using namespace FGXMLAutopilot;
using std::endl;
using std::cout;
//------------------------------------------------------------------------------
PISimpleController::PISimpleController() :
AnalogComponent(),
_int_sum( 0.0 )
{
}
bool PISimpleController::configure( const std::string& nodeName, SGPropertyNode_ptr configNode)
//------------------------------------------------------------------------------
bool PISimpleController::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
if( AnalogComponent::configure( nodeName, configNode ) )
if( AnalogComponent::configure(cfg_node, cfg_name, prop_root) )
return true;
if( nodeName == "config" ) {
Component::configure( configNode );
if( cfg_name == "config" ) {
Component::configure(prop_root, cfg_node);
return true;
}
if (nodeName == "Kp") {
_Kp.push_back( new InputValue(configNode) );
if (cfg_name == "Kp") {
_Kp.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
if (nodeName == "Ki") {
_Ki.push_back( new InputValue(configNode) );
if (cfg_name == "Ki") {
_Ki.push_back( new InputValue(prop_root, cfg_node) );
return true;
}
@ -64,15 +65,15 @@ void PISimpleController::update( bool firstTime, double dt )
_int_sum = 0.0;
}
if ( _debug ) cout << "Updating " << get_name() << endl;
if ( _debug ) std::cout << "Updating " << get_name() << std::endl;
double y_n = _valueInput.get_value();
double r_n = _referenceInput.get_value();
double error = r_n - y_n;
if ( _debug ) cout << "input = " << y_n
if ( _debug ) std::cout << "input = " << y_n
<< " reference = " << r_n
<< " error = " << error
<< endl;
<< std::endl;
double prop_comp = clamp(error * _Kp.get_value());
_int_sum += error * _Ki.get_value() * dt;
@ -83,9 +84,9 @@ void PISimpleController::update( bool firstTime, double dt )
if( output != clamped_output ) // anti-windup
_int_sum = clamped_output - prop_comp;
if ( _debug ) cout << "prop_comp = " << prop_comp
<< " int_sum = " << _int_sum << endl;
if ( _debug ) std::cout << "prop_comp = " << prop_comp
<< " int_sum = " << _int_sum << std::endl;
set_output_value( clamped_output );
if ( _debug ) cout << "output = " << clamped_output << endl;
if ( _debug ) std::cout << "output = " << clamped_output << std::endl;
}

4
src/Autopilot/pisimplecontroller.hxx
View file

@ -49,7 +49,9 @@ private:
double _int_sum;
protected:
bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
virtual bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
public:

40
src/Autopilot/predictor.cxx
View file

@ -25,41 +25,49 @@
using namespace FGXMLAutopilot;
using std::endl;
using std::cout;
//------------------------------------------------------------------------------
Predictor::Predictor () :
AnalogComponent(),
_average(0.0)
AnalogComponent(),
_last_value(0),
_average(0)
{
}
bool Predictor::configure(const std::string& nodeName, SGPropertyNode_ptr configNode)
//------------------------------------------------------------------------------
bool Predictor::configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
SG_LOG( SG_AUTOPILOT, SG_BULK, "Predictor::configure(" << nodeName << ")" << endl );
SG_LOG( SG_AUTOPILOT, SG_BULK, "Predictor::configure(" << cfg_name << ")");
if( AnalogComponent::configure( nodeName, configNode ) )
if( AnalogComponent::configure(cfg_node, cfg_name, prop_root) )
return true;
if( nodeName == "config" ) {
Component::configure( configNode );
if( cfg_name == "config" ) {
Component::configure(prop_root, cfg_node);
return true;
}
if (nodeName == "seconds") {
_seconds.push_back( new InputValue( configNode, 0 ) );
if (cfg_name == "seconds") {
_seconds.push_back( new InputValue(prop_root, cfg_node, 0) );
return true;
}
if (nodeName == "filter-gain") {
_filter_gain.push_back( new InputValue( configNode, 0 ) );
if (cfg_name == "filter-gain") {
_filter_gain.push_back( new InputValue(prop_root, cfg_node, 0) );
return true;
}
SG_LOG( SG_AUTOPILOT, SG_BULK, "Predictor::configure(" << nodeName << ") [unhandled]" << endl );
SG_LOG
(
SG_AUTOPILOT,
SG_BULK,
"Predictor::configure(" << cfg_name << ") [unhandled]"
);
return false;
}
//------------------------------------------------------------------------------
void Predictor::update( bool firstTime, double dt )
{
double ivalue = _valueInput.get_value();
@ -80,5 +88,3 @@ void Predictor::update( bool firstTime, double dt )
_last_value = ivalue;
}

4
src/Autopilot/predictor.hxx
View file

@ -53,7 +53,9 @@ private:
InputValueList _filter_gain;
protected:
bool configure(const std::string& nodeName, SGPropertyNode_ptr configNode );
virtual bool configure( SGPropertyNode& cfg_node,
const std::string& cfg_name,
SGPropertyNode& prop_root );
public:
Predictor();