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_ptr config)
  53   {
  54     inner = simgear::StateMachine::createFromPlist(config, globals->get_props());
  55   }
  56
  57   virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr config)
  58   {
  59     return false;
  60   }
  61
  62   virtual void update( bool firstTime, double dt )
  63   {
  64     SG_UNUSED(firstTime);
  65     inner->update(dt);
  66   }
  67
  68 private:
  69   simgear::StateMachine_ptr inner;
  70 };
  71
  72 class StateMachineFunctor : public FunctorBase<Component>
  73 {
  74 public:
  75   virtual ~StateMachineFunctor() {}
  76   virtual Component* operator()( SGPropertyNode_ptr configNode )
  77   {
  78     return new StateMachineComponent(configNode);
  79   }
  80 };
  81
  82
  83 class ComponentForge : public map<string,FunctorBase<Component> *> {
  84 public:
  85     virtual ~ ComponentForge();
  86 };
  87
  88 ComponentForge::~ComponentForge()
  89 {
  90     for( iterator it = begin(); it != end(); ++it )
  91         delete it->second;
  92 }
  93
  94 static ComponentForge componentForge;
  95
  96 Autopilot::Autopilot( SGPropertyNode_ptr rootNode, SGPropertyNode_ptr configNode ) :
  97   _name("unnamed autopilot"),
  98   _serviceable(true),
  99   _rootNode(rootNode)
 100 {
 101   if (componentForge.empty())
 102   {
 103       componentForge["pid-controller"]       = new CreateAndConfigureFunctor<PIDController,Component>();
 104       componentForge["pi-simple-controller"] = new CreateAndConfigureFunctor<PISimpleController,Component>();
 105       componentForge["predict-simple"]       = new CreateAndConfigureFunctor<Predictor,Component>();
 106       componentForge["filter"]               = new CreateAndConfigureFunctor<DigitalFilter,Component>();
 107       componentForge["logic"]                = new CreateAndConfigureFunctor<Logic,Component>();
 108       componentForge["flipflop"]             = new CreateAndConfigureFunctor<FlipFlop,Component>();
 109       componentForge["state-machine"]        = new StateMachineFunctor();
 110   }
 111
 112   if( configNode == NULL ) configNode = rootNode;
 113
 114   int count = configNode->nChildren();
 115   for ( int i = 0; i < count; ++i ) {
 116     SGPropertyNode_ptr node = configNode->getChild(i);
 117     string childName = node->getName();
 118     if( componentForge.count(childName) == 0 ) {
 119       SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled element <" << childName << ">" << std::endl );
 120       continue;
 121     }
 122
 123     Component * component = (*componentForge[childName])(node);
 124     if( component->get_name().length() == 0 ) {
 125       std::ostringstream buf;
 126       buf <<  "unnamed_component_" << i;
 127       component->set_name( buf.str() );
 128     }
 129
 130     double updateInterval = node->getDoubleValue( "update-interval-secs", 0.0 );
 131
 132     SG_LOG( SG_AUTOPILOT, SG_DEBUG, "adding  autopilot component \"" << childName << "\" as \"" << component->get_name() << "\" with interval=" << updateInterval );
 133     add_component(component,updateInterval);
 134   }
 135 }
 136
 137 Autopilot::~Autopilot()
 138 {
 139 }
 140
 141 void Autopilot::bind()
 142 {
 143   fgTie( _rootNode->getNode("serviceable", true)->getPath().c_str(), this,
 144     &Autopilot::is_serviceable, &Autopilot::set_serviceable );
 145 }
 146
 147 void Autopilot::unbind()
 148 {
 149   _rootNode->untie( "serviceable" );
 150 }
 151
 152 void Autopilot::add_component( Component * component, double updateInterval )
 153 {
 154   if( component == NULL ) return;
 155
 156   // check for duplicate name
 157   std::string name = component->get_name();
 158   for( unsigned i = 0; get_subsystem( name.c_str() ) != NULL; i++ ) {
 159       std::ostringstream buf;
 160       buf <<  component->get_name() << "_" << i;
 161       name = buf.str();
 162   }
 163   if( name != component->get_name() )
 164     SG_LOG( SG_ALL, SG_WARN, "Duplicate autopilot component " << component->get_name() << ", renamed to " << name );
 165
 166   set_subsystem( name.c_str(), component, updateInterval );
 167 }
 168
 169 void Autopilot::update( double dt )
 170 {
 171   if( !_serviceable || dt <= SGLimitsd::min() )
 172     return;
 173   SGSubsystemGroup::update( dt );
 174 }