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 "functor.hxx"
  29 #include "predictor.hxx"
  30 #include "digitalfilter.hxx"
  31 #include "pisimplecontroller.hxx"
  32 #include "pidcontroller.hxx"
  33 #include "autopilot.hxx"
  34 #include "logic.hxx"
  35 #include "flipflop.hxx"
  36
  37 #include "Main/fg_props.hxx"
  38
  39 using namespace FGXMLAutopilot;
  40
  41 Autopilot::Autopilot( SGPropertyNode_ptr rootNode, SGPropertyNode_ptr configNode ) :
  42   _name("unnamed autopilot"),
  43   _serviceable(true),
  44   _rootNode(rootNode)
  45 {
  46   map<string,FunctorBase<Component> *> componentForge;
  47   componentForge["pid-controller"]       = new CreateAndConfigureFunctor<PIDController,Component>();
  48   componentForge["pi-simple-controller"] = new CreateAndConfigureFunctor<PISimpleController,Component>();
  49   componentForge["predict-simple"]       = new CreateAndConfigureFunctor<Predictor,Component>();
  50   componentForge["filter"]               = new CreateAndConfigureFunctor<DigitalFilter,Component>();
  51   componentForge["logic"]                = new CreateAndConfigureFunctor<Logic,Component>();
  52   componentForge["flipflop"]             = new CreateAndConfigureFunctor<FlipFlop,Component>();
  53
  54   if( configNode == NULL ) configNode = rootNode;
  55
  56   int count = configNode->nChildren();
  57   for ( int i = 0; i < count; ++i ) {
  58     SGPropertyNode_ptr node = configNode->getChild(i);
  59     string childName = node->getName();
  60     if( componentForge.count(childName) == 0 ) {
  61       SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled element <" << childName << ">" << endl );
  62       continue;
  63     }
  64
  65     Component * component = (*componentForge[childName])(node);
  66     if( component->get_name().length() == 0 ) {
  67       ostringstream buf;
  68       buf <<  "unnamed_component_" << i;
  69       component->set_name( buf.str() );
  70     }
  71
  72     SG_LOG( SG_AUTOPILOT, SG_INFO, "adding  autopilot component \"" << childName << "\" as \"" << component->get_name() << "\"" );
  73     add_component(component);
  74   }
  75 }
  76
  77 Autopilot::~Autopilot()
  78 {
  79 }
  80
  81 void Autopilot::bind()
  82 {
  83   fgTie( _rootNode->getNode("serviceable", true)->getPath().c_str(), this,
  84     &Autopilot::is_serviceable, &Autopilot::set_serviceable );
  85 }
  86
  87 void Autopilot::unbind()
  88 {
  89   _rootNode->untie( "serviceable" );
  90 }
  91
  92 void Autopilot::add_component( Component * component )
  93 {
  94   if( component == NULL ) return;
  95
  96   // check for duplicate name
  97   std::string name = component->get_name();
  98   for( unsigned i = 0; get_subsystem( name.c_str() ) != NULL; i++ ) {
  99       ostringstream buf;
 100       buf <<  component->get_name() << "_" << i;
 101       name = buf.str();
 102   }
 103   if( name != component->get_name() )
 104     SG_LOG( SG_ALL, SG_WARN, "Duplicate autopilot component " << component->get_name() << ", renamed to " << name );
 105
 106   set_subsystem( name.c_str(), component );
 107 }
 108
 109 void Autopilot::update( double dt )
 110 {
 111   if( !_serviceable || dt <= SGLimitsd::min() )
 112     return;
 113   SGSubsystemGroup::update( dt );
 114 }