1 // autopilot.cxx - an even more flexible, generic way to build autopilots
3 // Written by Torsten Dreyer
4 // Based heavily on work created by Curtis Olson, started January 2004.
6 // Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
7 // Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
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.
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.
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.
28 #include "functor.hxx"
29 #include "predictor.hxx"
30 #include "digitalfilter.hxx"
31 #include "pisimplecontroller.hxx"
32 #include "pidcontroller.hxx"
33 #include "autopilot.hxx"
35 #include "flipflop.hxx"
37 #include "Main/fg_props.hxx"
42 using namespace FGXMLAutopilot;
44 class ComponentForge : public map<string,FunctorBase<Component> *> {
46 virtual ~ ComponentForge();
49 ComponentForge::~ComponentForge()
51 for( iterator it = begin(); it != end(); ++it )
55 static ComponentForge componentForge;
57 Autopilot::Autopilot( SGPropertyNode_ptr rootNode, SGPropertyNode_ptr configNode ) :
58 _name("unnamed autopilot"),
63 componentForge["pid-controller"] = new CreateAndConfigureFunctor<PIDController,Component>();
64 componentForge["pi-simple-controller"] = new CreateAndConfigureFunctor<PISimpleController,Component>();
65 componentForge["predict-simple"] = new CreateAndConfigureFunctor<Predictor,Component>();
66 componentForge["filter"] = new CreateAndConfigureFunctor<DigitalFilter,Component>();
67 componentForge["logic"] = new CreateAndConfigureFunctor<Logic,Component>();
68 componentForge["flipflop"] = new CreateAndConfigureFunctor<FlipFlop,Component>();
70 if( configNode == NULL ) configNode = rootNode;
72 int count = configNode->nChildren();
73 for ( int i = 0; i < count; ++i ) {
74 SGPropertyNode_ptr node = configNode->getChild(i);
75 string childName = node->getName();
76 if( componentForge.count(childName) == 0 ) {
77 SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled element <" << childName << ">" << std::endl );
81 Component * component = (*componentForge[childName])(node);
82 if( component->get_name().length() == 0 ) {
83 std::ostringstream buf;
84 buf << "unnamed_component_" << i;
85 component->set_name( buf.str() );
88 double updateInterval = node->getDoubleValue( "update-interval-secs", 0.0 );
90 SG_LOG( SG_AUTOPILOT, SG_INFO, "adding autopilot component \"" << childName << "\" as \"" << component->get_name() << "\" with interval=" << updateInterval );
91 add_component(component,updateInterval);
95 Autopilot::~Autopilot()
99 void Autopilot::bind()
101 fgTie( _rootNode->getNode("serviceable", true)->getPath().c_str(), this,
102 &Autopilot::is_serviceable, &Autopilot::set_serviceable );
105 void Autopilot::unbind()
107 _rootNode->untie( "serviceable" );
110 void Autopilot::add_component( Component * component, double updateInterval )
112 if( component == NULL ) return;
114 // check for duplicate name
115 std::string name = component->get_name();
116 for( unsigned i = 0; get_subsystem( name.c_str() ) != NULL; i++ ) {
117 std::ostringstream buf;
118 buf << component->get_name() << "_" << i;
121 if( name != component->get_name() )
122 SG_LOG( SG_ALL, SG_WARN, "Duplicate autopilot component " << component->get_name() << ", renamed to " << name );
124 set_subsystem( name.c_str(), component, updateInterval );
127 void Autopilot::update( double dt )
129 if( !_serviceable || dt <= SGLimitsd::min() )
131 SGSubsystemGroup::update( dt );