Interim windows build fix

[flightgear.git] / src / Autopilot / autopilot.cxx

diff --git a/src/Autopilot/autopilot.cxx b/src/Autopilot/autopilot.cxx
index b68ea958ee2f601585b85144bd47ca9bece9b89f..b332647dcb0cbd71159075a04407be7c9df61e2e 100644 (file)
--- a/src/Autopilot/autopilot.cxx
+++ b/src/Autopilot/autopilot.cxx
@@ -25,12 +25,17 @@
 #  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"
 
@@ -41,39 +46,141 @@ using std::string;
 
 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();
+};
+
+ComponentForge::~ComponentForge()
+{
+    for( iterator it = begin(); it != end(); ++it )
+        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 ) :
   _name("unnamed autopilot"),
   _serviceable(true),
   _rootNode(rootNode)
 {
-  map<string,FunctorBase<Component> *> componentForge;
-  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>();
+  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");
+
+  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);
   }
 }
 
@@ -92,7 +199,7 @@ void Autopilot::unbind()
   _rootNode->untie( "serviceable" );
 }
 
-void Autopilot::add_component( Component * component )
+void Autopilot::add_component( Component * component, double updateInterval )
 {
   if( component == NULL ) return;
 
@@ -104,9 +211,9 @@ void Autopilot::add_component( Component * component )
       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 )