src/Autopilot/autopilot.cxx

   1 // autopilot.cxx - an even more flexible, generic way to build autopilots
   2 //
   3 // Written by Torsten Dreyer
   4 // Based heavily on work created by Curtis Olson, started January 2004.
   5 //
   6 // Copyright (C) 2004  Curtis L. Olson  - http://www.flightgear.org/~curt
   7 // Copyright (C) 2010  Torsten Dreyer - Torsten (at) t3r (dot) de
   8 //
   9 // This program is free software; you can redistribute it and/or
  10 // modify it under the terms of the GNU General Public License as
  11 // published by the Free Software Foundation; either version 2 of the
  12 // License, or (at your option) any later version.
  13 //
  14 // This program is distributed in the hope that it will be useful, but
  15 // WITHOUT ANY WARRANTY; without even the implied warranty of
  16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17 // General Public License for more details.
  18 //
  19 // You should have received a copy of the GNU General Public License
  20 // along with this program; if not, write to the Free Software
  21 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  22 //
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include <config.h>
  26 #endif
  27
  28 #include "autopilot.hxx"
  29
  30 #include <simgear/structure/StateMachine.hxx>
  31 #include <simgear/sg_inlines.h>
  32
  33 #include "component.hxx"
  34 #include "functor.hxx"
  35 #include "predictor.hxx"
  36 #include "digitalfilter.hxx"
  37 #include "pisimplecontroller.hxx"
  38 #include "pidcontroller.hxx"
  39 #include "logic.hxx"
  40 #include "flipflop.hxx"
  41
  42 #include "Main/fg_props.hxx"
  43
  44 using std::map;
  45 using std::string;
  46
  47 using namespace FGXMLAutopilot;
  48
  49 class StateMachineComponent : public Component
  50 {
  51 public:
  52   StateMachineComponent( SGPropertyNode& props_root,
  53                          SGPropertyNode& cfg )
  54   {
  55     inner = simgear::StateMachine::createFromPlist(&cfg, &props_root);
  56   }
  57
  58   virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr config)
  59   {
  60     return false;
  61   }
  62
  63   virtual void update( bool firstTime, double dt )
  64   {
  65     SG_UNUSED(firstTime);
  66     inner->update(dt);
  67   }
  68
  69 private:
  70   simgear::StateMachine_ptr inner;
  71 };
  72
  73 class StateMachineFunctor : public FunctorBase<Component>
  74 {
  75 public:
  76   virtual ~StateMachineFunctor() {}
  77   virtual Component* operator()( SGPropertyNode& cfg,
  78                                  SGPropertyNode& prop_root )
  79   {
  80     return new StateMachineComponent(cfg, prop_root);
  81   }
  82 };
  83
  84
  85 class ComponentForge : public map<string,FunctorBase<Component> *> {
  86 public:
  87     virtual ~ ComponentForge();
  88 };
  89
  90 ComponentForge::~ComponentForge()
  91 {
  92     for( iterator it = begin(); it != end(); ++it )
  93         delete it->second;
  94 }
  95
  96 void readInterfaceProperties( SGPropertyNode_ptr prop_root,
  97                               SGPropertyNode_ptr cfg )
  98 {
  99   simgear::PropertyList cfg_props = cfg->getChildren("property");
 100   for( simgear::PropertyList::iterator it = cfg_props.begin();
 101                                        it != cfg_props.end();
 102                                      ++it )
 103   {
 104     SGPropertyNode_ptr prop = prop_root->getNode((*it)->getStringValue(), true);
 105     SGPropertyNode* val = (*it)->getNode("_attr_/value");
 106
 107     if( val )
 108     {
 109       prop->setDoubleValue( val->getDoubleValue() );
 110
 111       // TODO should we keep the _attr_ node, as soon as the property browser is
 112       //      able to cope with it?
 113       (*it)->removeChild("_attr_", 0);
 114     }
 115   }
 116 }
 117
 118 static ComponentForge componentForge;
 119
 120 Autopilot::Autopilot( SGPropertyNode_ptr rootNode, SGPropertyNode_ptr configNode ) :
 121   _name("unnamed autopilot"),
 122   _serviceable(true),
 123   _rootNode(rootNode)
 124 {
 125   if (componentForge.empty())
 126   {
 127       componentForge["pid-controller"]       = new CreateAndConfigureFunctor<PIDController,Component>();
 128       componentForge["pi-simple-controller"] = new CreateAndConfigureFunctor<PISimpleController,Component>();
 129       componentForge["predict-simple"]       = new CreateAndConfigureFunctor<Predictor,Component>();
 130       componentForge["filter"]               = new CreateAndConfigureFunctor<DigitalFilter,Component>();
 131       componentForge["logic"]                = new CreateAndConfigureFunctor<Logic,Component>();
 132       componentForge["flipflop"]             = new CreateAndConfigureFunctor<FlipFlop,Component>();
 133       componentForge["state-machine"]        = new StateMachineFunctor();
 134   }
 135
 136   if( !configNode )
 137     configNode = rootNode;
 138
 139   // property-root can be set in config file and overridden in the local system
 140   // node. This allows using the same autopilot multiple times but with
 141   // different paths (with all relative property paths being relative to the
 142   // node specified with property-root)
 143   SGPropertyNode_ptr prop_root_node = rootNode->getChild("property-root");
 144   if( !prop_root_node )
 145     prop_root_node = configNode->getChild("property-root");
 146
 147   SGPropertyNode_ptr prop_root =
 148     fgGetNode(prop_root_node ? prop_root_node->getStringValue() : "/", true);
 149
 150   // Just like the JSBSim interface properties for systems, create properties
 151   // given in the autopilot file and set to given (default) values.
 152   readInterfaceProperties(prop_root, configNode);
 153
 154   // Afterwards read the properties specified in local system node to allow
 155   // overriding initial or default values. This allows reusing components with
 156   // just different "parameter" values.
 157   readInterfaceProperties(prop_root, rootNode);
 158
 159   int count = configNode->nChildren();
 160   for( int i = 0; i < count; ++i )
 161   {
 162     SGPropertyNode_ptr node = configNode->getChild(i);
 163     string childName = node->getName();
 164     if(    childName == "property"
 165         || childName == "property-root" )
 166       continue;
 167     if( componentForge.count(childName) == 0 )
 168     {
 169       SG_LOG(SG_AUTOPILOT, SG_BULK, "unhandled element <" << childName << ">");
 170       continue;
 171     }
 172
 173     Component * component = (*componentForge[childName])(*prop_root, *node);
 174     if( component->get_name().length() == 0 ) {
 175       std::ostringstream buf;
 176       buf <<  "unnamed_component_" << i;
 177       component->set_name( buf.str() );
 178     }
 179
 180     double updateInterval = node->getDoubleValue( "update-interval-secs", 0.0 );
 181
 182     SG_LOG( SG_AUTOPILOT, SG_DEBUG, "adding  autopilot component \"" << childName << "\" as \"" << component->get_name() << "\" with interval=" << updateInterval );
 183     add_component(component,updateInterval);
 184   }
 185 }
 186
 187 Autopilot::~Autopilot()
 188 {
 189 }
 190
 191 void Autopilot::bind()
 192 {
 193   fgTie( _rootNode->getNode("serviceable", true)->getPath().c_str(), this,
 194     &Autopilot::is_serviceable, &Autopilot::set_serviceable );
 195 }
 196
 197 void Autopilot::unbind()
 198 {
 199   _rootNode->untie( "serviceable" );
 200 }
 201
 202 void Autopilot::add_component( Component * component, double updateInterval )
 203 {
 204   if( component == NULL ) return;
 205
 206   // check for duplicate name
 207   std::string name = component->get_name();
 208   for( unsigned i = 0; get_subsystem( name.c_str() ) != NULL; i++ ) {
 209       std::ostringstream buf;
 210       buf <<  component->get_name() << "_" << i;
 211       name = buf.str();
 212   }
 213   if( name != component->get_name() )
 214     SG_LOG( SG_AUTOPILOT, SG_WARN, "Duplicate autopilot component " << component->get_name() << ", renamed to " << name );
 215
 216   set_subsystem( name.c_str(), component, updateInterval );
 217 }
 218
 219 void Autopilot::update( double dt )
 220 {
 221   if( !_serviceable || dt <= SGLimitsd::min() )
 222     return;
 223   SGSubsystemGroup::update( dt );
 224 }