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 "autopilot.hxx"
30 #include <simgear/structure/StateMachine.hxx>
31 #include <simgear/sg_inlines.h>
33 #include "component.hxx"
34 #include "functor.hxx"
35 #include "predictor.hxx"
36 #include "digitalfilter.hxx"
37 #include "pisimplecontroller.hxx"
38 #include "pidcontroller.hxx"
40 #include "flipflop.hxx"
42 #include "Main/fg_props.hxx"
47 using namespace FGXMLAutopilot;
49 class StateMachineComponent : public Component
52 StateMachineComponent( SGPropertyNode& props_root,
55 inner = simgear::StateMachine::createFromPlist(&cfg, &props_root);
58 virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr config)
63 virtual void update( bool firstTime, double dt )
70 simgear::StateMachine_ptr inner;
73 class StateMachineFunctor : public FunctorBase<Component>
76 virtual ~StateMachineFunctor() {}
77 virtual Component* operator()( SGPropertyNode& cfg,
78 SGPropertyNode& prop_root )
80 return new StateMachineComponent(cfg, prop_root);
85 class ComponentForge : public map<string,FunctorBase<Component> *> {
87 virtual ~ ComponentForge();
90 ComponentForge::~ComponentForge()
92 for( iterator it = begin(); it != end(); ++it )
96 void readInterfaceProperties( SGPropertyNode_ptr prop_root,
97 SGPropertyNode_ptr cfg )
99 simgear::PropertyList cfg_props = cfg->getChildren("property");
100 for( simgear::PropertyList::iterator it = cfg_props.begin();
101 it != cfg_props.end();
104 SGPropertyNode_ptr prop = prop_root->getNode((*it)->getStringValue(), true);
105 SGPropertyNode* val = (*it)->getNode("_attr_/value");
109 prop->setDoubleValue( val->getDoubleValue() );
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);
118 static ComponentForge componentForge;
120 Autopilot::Autopilot( SGPropertyNode_ptr rootNode, SGPropertyNode_ptr configNode ) :
121 _name("unnamed autopilot"),
125 if (componentForge.empty())
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();
137 configNode = rootNode;
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");
147 SGPropertyNode_ptr prop_root =
148 fgGetNode(prop_root_node ? prop_root_node->getStringValue() : "/", true);
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);
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);
159 int count = configNode->nChildren();
160 for( int i = 0; i < count; ++i )
162 SGPropertyNode_ptr node = configNode->getChild(i);
163 string childName = node->getName();
164 if( childName == "property"
165 || childName == "property-root" )
167 if( componentForge.count(childName) == 0 )
169 SG_LOG(SG_AUTOPILOT, SG_BULK, "unhandled element <" << childName << ">");
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() );
180 double updateInterval = node->getDoubleValue( "update-interval-secs", 0.0 );
182 SG_LOG( SG_AUTOPILOT, SG_DEBUG, "adding autopilot component \"" << childName << "\" as \"" << component->get_name() << "\" with interval=" << updateInterval );
183 add_component(component,updateInterval);
187 Autopilot::~Autopilot()
191 void Autopilot::bind()
193 fgTie( _rootNode->getNode("serviceable", true)->getPath().c_str(), this,
194 &Autopilot::is_serviceable, &Autopilot::set_serviceable );
197 void Autopilot::unbind()
199 _rootNode->untie( "serviceable" );
202 void Autopilot::add_component( Component * component, double updateInterval )
204 if( component == NULL ) return;
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;
213 if( name != component->get_name() )
214 SG_LOG( SG_AUTOPILOT, SG_WARN, "Duplicate autopilot component " << component->get_name() << ", renamed to " << name );
216 set_subsystem( name.c_str(), component, updateInterval );
219 void Autopilot::update( double dt )
221 if( !_serviceable || dt <= SGLimitsd::min() )
223 SGSubsystemGroup::update( dt );