# include <config.h>
#endif
+#include "autopilot.hxx"
+
+#include <simgear/structure/StateMachine.hxx>
+#include <simgear/sg_inlines.h>
+
+#include "component.hxx"
#include "functor.hxx"
#include "predictor.hxx"
#include "digitalfilter.hxx"
#include "pisimplecontroller.hxx"
#include "pidcontroller.hxx"
-#include "autopilot.hxx"
#include "logic.hxx"
#include "flipflop.hxx"
using namespace FGXMLAutopilot;
+class StateMachineComponent : public Component
+{
+public:
+ StateMachineComponent( SGPropertyNode& props_root,
+ SGPropertyNode& cfg )
+ {
+ inner = simgear::StateMachine::createFromPlist(&cfg, &props_root);
+ }
+
+ virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr config)
+ {
+ return false;
+ }
+
+ virtual void update( bool firstTime, double dt )
+ {
+ SG_UNUSED(firstTime);
+ inner->update(dt);
+ }
+
+private:
+ simgear::StateMachine_ptr inner;
+};
+
+class StateMachineFunctor : public FunctorBase<Component>
+{
+public:
+ virtual ~StateMachineFunctor() {}
+ virtual Component* operator()( SGPropertyNode& cfg,
+ SGPropertyNode& prop_root )
+ {
+ return new StateMachineComponent(cfg, prop_root);
+ }
+};
+
+
class ComponentForge : public map<string,FunctorBase<Component> *> {
public:
virtual ~ 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);
+ }
+ }
+}
+
static ComponentForge componentForge;
Autopilot::Autopilot( SGPropertyNode_ptr rootNode, SGPropertyNode_ptr configNode ) :
_serviceable(true),
_rootNode(rootNode)
{
+ if (componentForge.empty())
+ {
+ componentForge["pid-controller"] = new CreateAndConfigureFunctor<PIDController,Component>();
+ componentForge["pi-simple-controller"] = new CreateAndConfigureFunctor<PISimpleController,Component>();
+ componentForge["predict-simple"] = new CreateAndConfigureFunctor<Predictor,Component>();
+ componentForge["filter"] = new CreateAndConfigureFunctor<DigitalFilter,Component>();
+ componentForge["logic"] = new CreateAndConfigureFunctor<Logic,Component>();
+ componentForge["flipflop"] = new CreateAndConfigureFunctor<FlipFlop,Component>();
+ componentForge["state-machine"] = new StateMachineFunctor();
+ }
+
+ 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");
- componentForge["pid-controller"] = new CreateAndConfigureFunctor<PIDController,Component>();
- componentForge["pi-simple-controller"] = new CreateAndConfigureFunctor<PISimpleController,Component>();
- componentForge["predict-simple"] = new CreateAndConfigureFunctor<Predictor,Component>();
- componentForge["filter"] = new CreateAndConfigureFunctor<DigitalFilter,Component>();
- componentForge["logic"] = new CreateAndConfigureFunctor<Logic,Component>();
- componentForge["flipflop"] = new CreateAndConfigureFunctor<FlipFlop,Component>();
+ SGPropertyNode_ptr prop_root =
+ fgGetNode(prop_root_node ? prop_root_node->getStringValue() : "/", true);
- if( configNode == NULL ) configNode = rootNode;
+ // 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;
component->set_name( buf.str() );
}
- SG_LOG( SG_AUTOPILOT, SG_INFO, "adding autopilot component \"" << childName << "\" as \"" << component->get_name() << "\"" );
- add_component(component);
+ double updateInterval = node->getDoubleValue( "update-interval-secs", 0.0 );
+
+ SG_LOG( SG_AUTOPILOT, SG_DEBUG, "adding autopilot component \"" << childName << "\" as \"" << component->get_name() << "\" with interval=" << updateInterval );
+ add_component(component,updateInterval);
}
}
_rootNode->untie( "serviceable" );
}
-void Autopilot::add_component( Component * component )
+void Autopilot::add_component( Component * component, double updateInterval )
{
if( component == NULL ) return;
name = buf.str();
}
if( name != component->get_name() )
- SG_LOG( SG_ALL, SG_WARN, "Duplicate autopilot component " << component->get_name() << ", renamed to " << name );
+ SG_LOG( SG_AUTOPILOT, SG_WARN, "Duplicate autopilot component " << component->get_name() << ", renamed to " << name );
- set_subsystem( name.c_str(), component );
+ set_subsystem( name.c_str(), component, updateInterval );
}
void Autopilot::update( double dt )