>
</File>
<File
- RelativePath="..\..\..\src\Autopilot\xmlauto.cxx"
+ RelativePath="..\..\..\src\Autopilot\autopilotgroup.cxx"
>
</File>
<File
- RelativePath="..\..\..\src\Autopilot\xmlauto.hxx"
+ RelativePath="..\..\..\src\Autopilot\autopilotgroup.hxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\autopilot.cxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\autopilot.hxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\inputvalue.cxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\inputvalue.hxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\component.cxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\component.hxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\analogcomponent.cxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\analogcomponent.hxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\pidcontroller.cxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\pidcontroller.hxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\pisimplecontroller.cxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\pisimplecontroller.hxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\predictor.cxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\predictor.hxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\digitalfilter.cxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\digitalfilter.hxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\digitalcomponent.cxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\digitalcomponent.hxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\logic.cxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\logic.hxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\flipflop.cxx"
+ >
+ </File>
+ <File
+ RelativePath="..\..\..\src\Autopilot\flipflop.hxx"
>
</File>
</Filter>
libAutopilot_a_SOURCES = \
route_mgr.cxx route_mgr.hxx \
- xmlauto.cxx xmlauto.hxx
+ autopilotgroup.cxx autopilotgroup.hxx \
+ autopilot.cxx autopilot.hxx \
+ inputvalue.cxx inputvalue.hxx \
+ component.cxx component.hxx \
+ analogcomponent.cxx analogcomponent.hxx \
+ pidcontroller.cxx pidcontroller.hxx \
+ pisimplecontroller.cxx pisimplecontroller.hxx \
+ predictor.cxx predictor.hxx \
+ digitalfilter.cxx digitalfilter.hxx \
+ digitalcomponent.cxx digitalcomponent.hxx \
+ logic.cxx logic.hxx \
+ flipflop.cxx flipflop.hxx
INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src
--- /dev/null
+// analogcomponent.cxx - Base class for analog autopilot components
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#include "analogcomponent.hxx"
+#include <Main/fg_props.hxx>
+
+using namespace FGXMLAutopilot;
+
+AnalogComponent::AnalogComponent() :
+ Component(),
+ _feedback_if_disabled(false),
+ _passive_mode( fgGetNode("/autopilot/locks/passive-mode", true) )
+{
+}
+
+double AnalogComponent::clamp( double value ) const
+{
+ //If this is a periodical value, normalize it into our domain
+ // before clamping
+ if( _periodical )
+ value = _periodical->normalize( value );
+
+ // clamp, if either min or max is defined
+ if( _minInput.size() + _maxInput.size() > 0 ) {
+ double d = _maxInput.get_value();
+ if( value > d ) value = d;
+ d = _minInput.get_value();
+ if( value < d ) value = d;
+ }
+ return value;
+}
+
+bool AnalogComponent::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+{
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "AnalogComponent::configure(" << nodeName << ")" << endl );
+ if( Component::configure( nodeName, configNode ) )
+ return true;
+
+ if ( nodeName == "feedback-if-disabled" ) {
+ _feedback_if_disabled = configNode->getBoolValue();
+ return true;
+ }
+
+ if ( nodeName == "output" ) {
+ // grab all <prop> and <property> childs
+ int found = 0;
+ // backwards compatibility: allow <prop> elements
+ SGPropertyNode_ptr prop;
+ for( int i = 0; (prop = configNode->getChild("prop", i)) != NULL; i++ ) {
+ SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
+ _output_list.push_back( tmp );
+ found++;
+ }
+ for( int i = 0; (prop = configNode->getChild("property", i)) != NULL; i++ ) {
+ SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
+ _output_list.push_back( tmp );
+ found++;
+ }
+
+ // no <prop> elements, text node of <output> is property name
+ if( found == 0 )
+ _output_list.push_back( fgGetNode(configNode->getStringValue(), true ) );
+
+ return true;
+ }
+
+ if( nodeName == "input" ) {
+ _valueInput.push_back( new InputValue( configNode ) );
+ return true;
+ }
+
+ if( nodeName == "reference" ) {
+ _referenceInput.push_back( new InputValue( configNode ) );
+ return true;
+ }
+
+ if( nodeName == "min" || nodeName == "u_min" ) {
+ _minInput.push_back( new InputValue( configNode ) );
+ return true;
+ }
+
+ if( nodeName == "max" || nodeName == "u_max" ) {
+ _maxInput.push_back( new InputValue( configNode ) );
+ return true;
+ }
+
+ if( nodeName == "period" ) {
+ _periodical = new PeriodicalValue( configNode );
+ return true;
+ }
+
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "AnalogComponent::configure(" << nodeName << ") [unhandled]" << endl );
+ return false;
+}
--- /dev/null
+// analogcomponent.hxx - Base class for analog autopilot components
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#ifndef __ANALOGCOMPONENT_HXX
+#define __ANALOGCOMPONENT_HXX 1
+
+#include "inputvalue.hxx"
+#include "component.hxx"
+
+namespace FGXMLAutopilot {
+
+/**
+ * @brief Base class for analog autopilot components
+ *
+ * Each analog component has
+ * <ul>
+ * <li>one value input</li>
+ * <li>one reference input</li>
+ * <li>one minimum clamp input</li>
+ * <li>one maximum clamp input</li>
+ * <li>an optional periodical definition</li>
+ * </ul>
+ */
+class AnalogComponent : public Component {
+
+private:
+ /**
+ * @brief a flag signalling that the output property value shall be fed back
+ * to the active input property if this component is disabled. This flag
+ * reflects the <feedback-if-disabled> boolean property.
+ */
+ bool _feedback_if_disabled;
+
+protected:
+ /**
+ * @brief the value input
+ */
+ InputValueList _valueInput;
+
+ /**
+ * @brief the reference input
+ */
+ InputValueList _referenceInput;
+
+ /**
+ * @brief the minimum output clamp input
+ */
+ InputValueList _minInput;
+
+ /**
+ * @brief the maximum output clamp input
+ */
+ InputValueList _maxInput;
+
+ /**
+ * @brief the configuration for periodical outputs
+ */
+ PeriodicalValue_ptr _periodical;
+
+ /**
+ * @brief A constructor for an analog component. Call configure() to
+ * configure this component from a property node
+ */
+ AnalogComponent();
+
+ /**
+ * @brief This method configures this analog component from a property node. Gets
+ * called multiple times from the base class configure method for every
+ config node.
+ * @param nodeName the name of the configuration node provided in configNode
+ * @param configNode the configuration node itself
+ * @return true if the node was handled, false otherwise.
+ */
+ virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
+
+ /**
+ * @brief clamp the given value if <min> and/or <max> inputs were given
+ * @param the value to clamp
+ * @return the clamped value
+ */
+ double clamp( double value ) const;
+
+ /**
+ * @brief overideable method being called from the update() method if this component
+ * is disabled. Analog components feed back it's output value to the active
+ input value if disabled and feedback-if-disabled is true
+ */
+ virtual void disabled( double dt );
+
+ /**
+ * @brief return the current double value of the output property
+ * @return the current value of the output property
+ * If no output property is configured, a value of zero will be returned.
+ * If more than one output property is configured, the value of the first output property
+ * is returned. The current value of the output property will be clamped to the configured
+ * values of <min> and/or <max>.
+ */
+ inline double get_output_value() const {
+ return _output_list.size() == 0 ? 0.0 : clamp(_output_list[0]->getDoubleValue());
+ }
+
+ simgear::PropertyList _output_list;
+ SGPropertyNode_ptr _passive_mode;
+
+ inline void set_output_value( double value ) {
+ // passive_ignore == true means that we go through all the
+ // motions, but drive the outputs. This is analogous to
+ // running the autopilot with the "servos" off. This is
+ // helpful for things like flight directors which position
+ // their vbars from the autopilot computations.
+ if ( _honor_passive && _passive_mode->getBoolValue() ) return;
+ value = clamp( value );
+ for( simgear::PropertyList::iterator it = _output_list.begin();
+ it != _output_list.end(); ++it)
+ (*it)->setDoubleValue( value );
+ }
+};
+
+inline void AnalogComponent::disabled( double dt )
+{
+ if( _feedback_if_disabled && _output_list.size() > 0 ) {
+ InputValue * input;
+ if( (input = _valueInput.get_active() ) != NULL )
+ input->set_value( _output_list[0]->getDoubleValue() );
+ }
+}
+
+}
+#endif // ANALOGCOMPONENT_HXX
--- /dev/null
+// autopilot.cxx - an even more flexible, generic way to build autopilots
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include "functor.hxx"
+#include "predictor.hxx"
+#include "digitalfilter.hxx"
+#include "pisimplecontroller.hxx"
+#include "pidcontroller.hxx"
+#include "autopilot.hxx"
+#include "logic.hxx"
+#include "flipflop.hxx"
+
+#include "Main/fg_props.hxx"
+
+using namespace FGXMLAutopilot;
+
+Autopilot::Autopilot( SGPropertyNode_ptr rootNode, SGPropertyNode_ptr configNode ) :
+ _name("unnamed autopilot"),
+ _serviceable(true),
+ _rootNode(rootNode)
+{
+ map<string,FunctorBase<Component> *> componentForge;
+ componentForge["pid-controller"] = new CreateAndConfigureFunctor<PIDController,Component>();
+ componentForge["pi-simple-controller"] = new CreateAndConfigureFunctor<PISimpleController,Component>();
+ componentForge["predict-simple"] = new CreateAndConfigureFunctor<Predictor,Component>();
+ componentForge["filter"] = new CreateAndConfigureFunctor<DigitalFilter,Component>();
+ componentForge["logic"] = new CreateAndConfigureFunctor<Logic,Component>();
+ componentForge["flipflop"] = new CreateAndConfigureFunctor<FlipFlop,Component>();
+
+ if( configNode == NULL ) configNode = rootNode;
+
+ int count = configNode->nChildren();
+ for ( int i = 0; i < count; ++i ) {
+ SGPropertyNode_ptr node = configNode->getChild(i);
+ string childName = node->getName();
+ if( componentForge.count(childName) == 0 ) {
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled element <" << childName << ">" << endl );
+ continue;
+ }
+
+ Component * component = (*componentForge[childName])(node);
+ if( component->get_name().length() == 0 ) {
+ ostringstream buf;
+ buf << "unnamed_component_" << i;
+ component->set_name( buf.str() );
+ }
+
+ SG_LOG( SG_AUTOPILOT, SG_INFO, "adding autopilot component \"" << childName << "\" as \"" << component->get_name() << "\"" );
+ add_component(component);
+ }
+}
+
+Autopilot::~Autopilot()
+{
+}
+
+void Autopilot::bind()
+{
+ fgTie( _rootNode->getNode("serviceable", true)->getPath().c_str(), this,
+ &Autopilot::is_serviceable, &Autopilot::set_serviceable );
+}
+
+void Autopilot::unbind()
+{
+ _rootNode->untie( "serviceable" );
+}
+
+void Autopilot::add_component( Component * component )
+{
+ if( component == NULL ) return;
+
+ std::string name = component->get_name();
+ if( get_subsystem( name.c_str() ) != NULL ) {
+ SG_LOG( SG_ALL, SG_ALERT, "Duplicate autopilot component " << name << " ignored" );
+ return;
+ }
+ set_subsystem( name.c_str(), component );
+}
+
+void Autopilot::update( double dt )
+{
+ if( !_serviceable || dt <= SGLimitsd::min() )
+ return;
+ SGSubsystemGroup::update( dt );
+}
--- /dev/null
+// autopilot.hxx - an even more flexible, generic way to build autopilots
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#ifndef __AUTOPILOT_HXX
+#define __AUTOPILOT_HXX 1
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include "component.hxx"
+
+#include <simgear/props/props.hxx>
+#include <simgear/structure/subsystem_mgr.hxx>
+
+namespace FGXMLAutopilot {
+
+/**
+ * @brief A SGSubsystemGroup implementation to serve as a collection
+ * of Components
+ */
+class Autopilot : public SGSubsystemGroup
+{
+public:
+ Autopilot( SGPropertyNode_ptr rootNode, SGPropertyNode_ptr configNode = NULL );
+ ~Autopilot();
+
+ void bind();
+ void unbind();
+ void update( double dt );
+
+ void set_serviceable( bool value ) { _serviceable = value; }
+ bool is_serviceable() const { return _serviceable; }
+
+ std::string get_name() const { return _name; }
+ void set_name( std::string & name ) { _name = name; }
+
+ void add_component( Component * component );
+
+protected:
+
+private:
+ std::string _name;
+ bool _serviceable;
+ SGPropertyNode_ptr _rootNode;
+};
+
+}
+
+#endif // __AUTOPILOT_HXX 1
--- /dev/null
+// autopilotgroup.cxx - an even more flexible, generic way to build autopilots
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include "autopilot.hxx"
+#include "autopilotgroup.hxx"
+
+#include <Main/fg_props.hxx>
+#include <Main/globals.hxx>
+#include <Main/util.hxx>
+
+#include <simgear/misc/sg_path.hxx>
+#include <simgear/props/props_io.hxx>
+#include <simgear/structure/SGExpression.hxx>
+
+using std::cout;
+using std::endl;
+
+using simgear::PropertyList;
+
+FGXMLAutopilotGroup::FGXMLAutopilotGroup() :
+ SGSubsystemGroup()
+#ifdef XMLAUTO_USEHELPER
+ ,average(0.0), // average/filtered prediction
+ v_last(0.0), // last velocity
+ last_static_pressure(0.0),
+ vel(fgGetNode( "/velocities/airspeed-kt", true )),
+ // Estimate speed in 5,10 seconds
+ lookahead5(fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true )),
+ lookahead10(fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true )),
+ bug(fgGetNode( "/autopilot/settings/heading-bug-deg", true )),
+ mag_hdg(fgGetNode( "/orientation/heading-magnetic-deg", true )),
+ bug_error(fgGetNode( "/autopilot/internal/heading-bug-error-deg", true )),
+ fdm_bug_error(fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true )),
+ target_true(fgGetNode( "/autopilot/settings/true-heading-deg", true )),
+ true_hdg(fgGetNode( "/orientation/heading-deg", true )),
+ true_error(fgGetNode( "/autopilot/internal/true-heading-error-deg", true )),
+ target_nav1(fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true )),
+ true_nav1(fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true )),
+ true_track_nav1(fgGetNode( "/autopilot/internal/nav1-track-error-deg", true )),
+ nav1_course_error(fgGetNode( "/autopilot/internal/nav1-course-error", true )),
+ nav1_selected_course(fgGetNode( "/instrumentation/nav[0]/radials/selected-deg", true )),
+ vs_fps(fgGetNode( "/velocities/vertical-speed-fps", true )),
+ vs_fpm(fgGetNode( "/autopilot/internal/vert-speed-fpm", true )),
+ static_pressure(fgGetNode( "/systems/static[0]/pressure-inhg", true )),
+ pressure_rate(fgGetNode( "/autopilot/internal/pressure-rate", true )),
+ track(fgGetNode( "/orientation/track-deg", true ))
+#endif
+{
+}
+
+void FGXMLAutopilotGroup::update( double dt )
+{
+ // update all configured autopilots
+ SGSubsystemGroup::update( dt );
+#ifdef XMLAUTO_USEHELPER
+ // update helper values
+ double v = vel->getDoubleValue();
+ double a = 0.0;
+ if ( dt > 0.0 ) {
+ a = (v - v_last) / dt;
+
+ if ( dt < 1.0 ) {
+ average = (1.0 - dt) * average + dt * a;
+ } else {
+ average = a;
+ }
+
+ lookahead5->setDoubleValue( v + average * 5.0 );
+ lookahead10->setDoubleValue( v + average * 10.0 );
+ v_last = v;
+ }
+
+ // Calculate heading bug error normalized to +/- 180.0
+ double diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
+ SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
+ bug_error->setDoubleValue( diff );
+
+ fdm_bug_error->setDoubleValue( diff );
+
+ // Calculate true heading error normalized to +/- 180.0
+ diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
+ SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
+ true_error->setDoubleValue( diff );
+
+ // Calculate nav1 target heading error normalized to +/- 180.0
+ diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
+ SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
+ true_nav1->setDoubleValue( diff );
+
+ // Calculate true groundtrack
+ diff = target_nav1->getDoubleValue() - track->getDoubleValue();
+ SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
+ true_track_nav1->setDoubleValue( diff );
+
+ // Calculate nav1 selected course error normalized to +/- 180.0
+ diff = nav1_selected_course->getDoubleValue() - mag_hdg->getDoubleValue();
+ SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
+ nav1_course_error->setDoubleValue( diff );
+
+ // Calculate vertical speed in fpm
+ vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
+
+
+ // Calculate static port pressure rate in [inhg/s].
+ // Used to determine vertical speed.
+ if ( dt > 0.0 ) {
+ double current_static_pressure = static_pressure->getDoubleValue();
+ double current_pressure_rate =
+ ( current_static_pressure - last_static_pressure ) / dt;
+
+ pressure_rate->setDoubleValue(current_pressure_rate);
+ last_static_pressure = current_static_pressure;
+ }
+#endif
+}
+
+void FGXMLAutopilotGroup::reinit()
+{
+ SGSubsystemGroup::unbind();
+
+ for( vector<string>::size_type i = 0; i < _autopilotNames.size(); i++ ) {
+ FGXMLAutopilot::Autopilot * ap = (FGXMLAutopilot::Autopilot*)get_subsystem( _autopilotNames[i] );
+ if( ap == NULL ) continue; // ?
+ remove_subsystem( _autopilotNames[i] );
+ delete ap;
+ }
+ _autopilotNames.clear();
+ init();
+}
+
+void FGXMLAutopilotGroup::init()
+{
+ PropertyList autopilotNodes = fgGetNode( "/sim/systems", true )->getChildren("autopilot");
+ if( autopilotNodes.size() == 0 ) {
+ SG_LOG( SG_ALL, SG_WARN, "No autopilot configuration specified for this model!");
+ return;
+ }
+
+ for( PropertyList::size_type i = 0; i < autopilotNodes.size(); i++ ) {
+ SGPropertyNode_ptr pathNode = autopilotNodes[i]->getNode( "path" );
+ if( pathNode == NULL ) {
+ SG_LOG( SG_ALL, SG_WARN, "No autopilot configuration file specified for this autopilot!");
+ continue;
+ }
+
+ string apName;
+ SGPropertyNode_ptr nameNode = autopilotNodes[i]->getNode( "name" );
+ if( nameNode != NULL ) {
+ apName = nameNode->getStringValue();
+ } else {
+ std::ostringstream buf;
+ buf << "unnamed_autopilot_" << i;
+ apName = buf.str();
+ }
+
+ if( get_subsystem( apName.c_str() ) != NULL ) {
+ SG_LOG( SG_ALL, SG_ALERT, "Duplicate autopilot configuration name " << apName << " ignored" );
+ continue;
+ }
+
+ SGPath config( globals->get_fg_root() );
+ config.append( pathNode->getStringValue() );
+
+ SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from " << config.str() );
+
+ try {
+ SGPropertyNode_ptr root = new SGPropertyNode();
+ readProperties( config.str(), root );
+
+ SG_LOG( SG_AUTOPILOT, SG_INFO, "adding autopilot subsystem " << apName );
+ FGXMLAutopilot::Autopilot * ap = new FGXMLAutopilot::Autopilot( autopilotNodes[i], root );
+ ap->set_name( apName );
+ set_subsystem( apName, ap );
+ _autopilotNames.push_back( apName );
+
+ } catch (const sg_exception& e) {
+ SG_LOG( SG_AUTOPILOT, SG_ALERT, "Failed to load autopilot configuration: "
+ << config.str() << ":" << e.getMessage() );
+ continue;
+ }
+ }
+
+ SGSubsystemGroup::bind();
+ SGSubsystemGroup::init();
+}
+
--- /dev/null
+// autopilotgroup.hxx - an even more flexible, generic way to build autopilots
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifndef _XMLAUTO_HXX
+#define _XMLAUTO_HXX 1
+
+#include <string>
+#include <vector>
+
+#include <simgear/props/props.hxx>
+#include <simgear/structure/subsystem_mgr.hxx>
+
+/**
+ * @brief Model an autopilot system by implementing a SGSubsystemGroup
+ *
+ */
+class FGXMLAutopilotGroup : public SGSubsystemGroup
+{
+public:
+ FGXMLAutopilotGroup();
+ void init();
+ void reinit();
+ void update( double dt );
+private:
+ std::vector<std::string> _autopilotNames;
+
+};
+
+#endif // _XMLAUTO_HXX
--- /dev/null
+// component.cxx - Base class for autopilot components
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#include "component.hxx"
+#include <Main/fg_props.hxx>
+#include <simgear/structure/exception.hxx>
+
+using namespace FGXMLAutopilot;
+
+Component::Component() :
+ _enable_value(NULL),
+ _enabled(false),
+ _debug(false),
+ _honor_passive(false)
+{
+}
+
+Component::~Component()
+{
+ delete _enable_value;
+}
+
+bool Component::configure( SGPropertyNode_ptr configNode )
+{
+ for (int i = 0; i < configNode->nChildren(); ++i ) {
+ SGPropertyNode_ptr prop;
+
+ SGPropertyNode_ptr child = configNode->getChild(i);
+ string cname(child->getName());
+
+ if( configure( cname, child ) )
+ continue;
+
+ } // for configNode->nChildren()
+
+ return true;
+}
+
+bool Component::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+{
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "Component::configure(" << nodeName << ")" << endl );
+
+ if ( nodeName == "name" ) {
+ _name = configNode->getStringValue();
+ return true;
+ }
+
+ if ( nodeName == "debug" ) {
+ _debug = configNode->getBoolValue();
+ return true;
+ }
+
+ if ( nodeName == "enable" ) {
+ SGPropertyNode_ptr prop;
+
+ if( (prop = configNode->getChild("condition")) != NULL ) {
+ _condition = sgReadCondition(fgGetNode("/"), prop);
+ return true;
+ }
+ if ( (prop = configNode->getChild( "property" )) != NULL ) {
+ _enable_prop = fgGetNode( prop->getStringValue(), true );
+ }
+
+ if ( (prop = configNode->getChild( "prop" )) != NULL ) {
+ _enable_prop = fgGetNode( prop->getStringValue(), true );
+ }
+
+ if ( (prop = configNode->getChild( "value" )) != NULL ) {
+ delete _enable_value;
+ _enable_value = new string(prop->getStringValue());
+ }
+
+ if ( (prop = configNode->getChild( "honor-passive" )) != NULL ) {
+ _honor_passive = prop->getBoolValue();
+ }
+
+ return true;
+ } // enable
+
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "Component::configure(" << nodeName << ") [unhandled]" << endl );
+ return false;
+}
+
+bool Component::isPropertyEnabled()
+{
+ if( _condition )
+ return _condition->test();
+
+ if( _enable_prop ) {
+ if( _enable_value ) {
+ return *_enable_value == _enable_prop->getStringValue();
+ } else {
+ return _enable_prop->getBoolValue();
+ }
+ }
+ return true;
+}
+
+void Component::update( double dt )
+{
+ bool firstTime = false;
+ if( isPropertyEnabled() ) {
+ firstTime = !_enabled;
+ _enabled = true;
+ } else {
+ _enabled = false;
+ }
+
+ if( _enabled ) update( firstTime, dt );
+ else disabled( dt );
+}
--- /dev/null
+// component.hxx - Base class for autopilot components
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#ifndef __COMPONENT_HXX
+#define __COMPONENT_HXX 1
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include <simgear/props/props.hxx>
+#include <simgear/structure/subsystem_mgr.hxx>
+#include <simgear/props/condition.hxx>
+
+namespace FGXMLAutopilot {
+
+/**
+ * @brief Base class for other autopilot components
+ */
+class Component : public SGSubsystem {
+
+private:
+
+ SGSharedPtr<const SGCondition> _condition;
+ SGPropertyNode_ptr _enable_prop;
+ std::string * _enable_value;
+ std::string _name;
+ bool _enabled;
+
+protected:
+
+ virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
+
+
+ /**
+ * @brief the implementation of the update() method of the SGSubsystem
+ */
+ virtual void update( double dt );
+
+ /**
+ * @brief pure virtual function to be implemented by the derived classes. Gets called from
+ * the update method if it's not disabled with the firstTime parameter set to true it this
+ * is the first call after being enabled
+ * @param firstTime set to true if this is the first update call since this component has
+ been enabled. Set to false for every subsequent call.
+ * @param dt the elapsed time since the last call
+ */
+ virtual void update( bool firstTime, double dt ) = 0;
+
+ /**
+ * @brief overideable method being called from the update() method if this component
+ * is disabled. It's a noop by default.
+ */
+ virtual void disabled( double dt ) {}
+
+ /**
+ * @brief debug flag, true if this component should generate some useful output
+ * on every iteration
+ */
+ bool _debug;
+
+
+ /**
+ * @brief a (historic) flag signalling the derived class that it should compute it's internal
+ * state but shall not set the output properties if /autopilot/locks/passive-mode is true.
+ */
+ bool _honor_passive;
+
+public:
+
+ /**
+ * @brief A constructor for an empty Component.
+ */
+ Component();
+
+ /**
+ * virtual destructor to clean up resources
+ */
+ virtual ~Component();
+
+ /**
+ * @brief configure this component from a property node. Iterates through all nodes found
+ * as childs under configNode and calls configure of the derived class for each child.
+ * @param configNode the property node containing the configuration
+ */
+ bool configure( SGPropertyNode_ptr configNode );
+
+ /**
+ * @brief getter for the name property
+ * @return the name of the component
+ */
+ inline const std::string& get_name() const { return _name; }
+
+ /**
+ * @brief setter for the name property
+ * @param name the name of the component
+ */
+ inline void set_name( const std::string & name ) { _name = name; }
+
+ /**
+ * @brief check if this component is enabled as configured in the
+ * <enable> section
+ * @return true if the enable-condition is true.
+ *
+ * If a <condition> is defined, this condition is evaluated,
+ * <prop> and <value> tags are ignored.
+ *
+ * If a <prop> is defined and no <value> is defined, the property
+ * named in the <prop><prop> tags is evaluated as boolean.
+ *
+ * If a <prop> is defined and a <value> is defined, the property named
+ * in <prop></prop> is compared (as a string) to the value defined in
+ * <value></value>
+ *
+ * Returns true, if neither <condition> nor <prop> exists
+ */
+ bool isPropertyEnabled();
+};
+
+
+}
+#endif // COMPONENT_HXX
--- /dev/null
+// digitalcomponent.cxx - Base class for digital autopilot components
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#include "digitalcomponent.hxx"
+#include <Main/fg_props.hxx>
+
+using namespace FGXMLAutopilot;
+
+DigitalComponent::DigitalComponent() :
+ _inverted(false)
+{
+}
+
+bool DigitalComponent::InputMap::get_value( const std::string & name ) const
+{
+ // can't use map::operator[] here since it's not const
+ const_iterator __i = lower_bound( name );
+ if (__i == end() || key_comp()(name, (*__i).first))
+ return false; // does not exist, return false
+
+ return (*__i).second->test();
+}
+
+/*
+ <input>
+ <name>Foo</name>
+ <condition>
+ <and>...</and>
+ </condition>
+ </input>
+ <output>
+ <name>Bar</name>
+ <property>/foo/bar</property>
+ <inverted>true</inverted>
+ </output>
+ <output>/some/property</output>
+*/
+bool DigitalComponent::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+{
+ if( Component::configure( nodeName, configNode ) )
+ return true;
+
+ if (nodeName == "input") {
+ SGPropertyNode_ptr nameNode = configNode->getNode("name");
+ string name;
+ if( nameNode != NULL ) {
+ name = nameNode->getStringValue();
+ } else {
+ std::ostringstream buf;
+ buf << "Input" << _input.size();
+ name = buf.str();
+ }
+ _input[name] = sgReadCondition( fgGetNode("/"), configNode );
+ return true;
+ }
+
+ if (nodeName == "output") {
+ SGPropertyNode_ptr n = configNode->getNode("name");
+ string name;
+ if( n != NULL ) {
+ name = n->getStringValue();
+ } else {
+ std::ostringstream buf;
+ buf << "Output" << _output.size();
+ name = buf.str();
+ }
+
+ DigitalOutput_ptr o = new DigitalOutput();
+ _output[name] = o;
+
+ if( (n = configNode->getNode("inverted")) != NULL )
+ o->setInverted( n->getBoolValue() );
+
+ if( (n = configNode->getNode("property")) != NULL )
+ o->setProperty( fgGetNode( n->getStringValue(), true ) );
+
+ if( configNode->nChildren() == 0 )
+ o->setProperty( fgGetNode( configNode->getStringValue(), true ) );
+
+ return true;
+ }
+
+ if (nodeName == "inverted") {
+ _inverted = configNode->getBoolValue();
+ return true;
+ }
+
+ return false;
+}
--- /dev/null
+// digitalcomponent.hxx - Base class for digital autopilot components
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#ifndef __DIGITALCOMPONENT_HXX
+#define __DIGITALCOMPONENT_HXX 1
+
+#include "component.hxx"
+
+namespace FGXMLAutopilot {
+
+/**
+ * @brief Models a digital output bound to a property. May be an inverted output.
+ */
+class DigitalOutput : public SGReferenced {
+
+private:
+ bool _inverted;
+ SGPropertyNode_ptr _node;
+
+protected:
+
+public:
+ /**
+ * @brief Constructs an empty, noninverting output
+ */
+ DigitalOutput();
+
+ inline void setProperty( SGPropertyNode_ptr node );
+
+ inline void setInverted( bool value ) { _inverted = value; }
+ inline bool isInverted() const { return _inverted; }
+
+ bool getValue() const;
+ void setValue( bool value );
+};
+
+inline DigitalOutput::DigitalOutput() : _inverted(false)
+{
+}
+
+inline void DigitalOutput::setProperty( SGPropertyNode_ptr node )
+{
+ _node->setBoolValue( (_node = node)->getBoolValue() );
+}
+
+inline bool DigitalOutput::getValue() const
+{
+ if( _node == NULL ) return false;
+ bool nodeState = _node->getBoolValue();
+ return _inverted ? !nodeState : nodeState;
+}
+
+inline void DigitalOutput::setValue( bool value )
+{
+ if( _node == NULL ) return;
+ _node->setBoolValue( _inverted ? !value : value );
+}
+
+typedef SGSharedPtr<DigitalOutput> DigitalOutput_ptr;
+
+/**
+ * @brief Base class for digital autopilot components
+ *
+ * Each digital component has (at least)
+ * <ul>
+ * <li>one value input</li>
+ * <li>any number of output properties</li>
+ * </ul>
+ */
+class DigitalComponent : public Component {
+public:
+ DigitalComponent();
+
+ class InputMap : public std::map<const std::string,SGSharedPtr<const SGCondition> > {
+ public:
+ bool get_value( const std::string & name ) const;
+ };
+
+
+// typedef std::map<const std::string,SGSharedPtr<const SGCondition> > InputMap;
+ typedef std::map<const std::string,DigitalOutput_ptr> OutputMap;
+protected:
+
+ /**
+ * @brief Named input "pins"
+ */
+ InputMap _input;
+
+ /**
+ * @brief Named output "pins"
+ */
+ OutputMap _output;
+
+ /**
+ * @brief Global "inverted" flag for the outputs
+ */
+ bool _inverted;
+
+ /**
+ * @brief Over-rideable hook method to allow derived classes to refine top-level
+ * node parsing.
+ * @param aName
+ * @param aNode
+ * @return true if the node was handled, false otherwise.
+ */
+ virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
+};
+
+}
+#endif // DIGITALCOMPONENT_HXX
+
--- /dev/null
+// digitalfilter.cxx - a selection of digital filters
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#include "digitalfilter.hxx"
+#include "functor.hxx"
+#include <deque>
+
+namespace FGXMLAutopilot {
+
+/* --------------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------------- */
+class GainFilterImplementation : public DigitalFilterImplementation {
+protected:
+ InputValueList _gainInput;
+ bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
+public:
+ GainFilterImplementation() : _gainInput(1.0) {}
+ double compute( double dt, double input );
+};
+
+class ReciprocalFilterImplementation : public GainFilterImplementation {
+public:
+ double compute( double dt, double input );
+};
+
+class DerivativeFilterImplementation : public GainFilterImplementation {
+ InputValueList _TfInput;
+ double _input_1;
+ bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
+public:
+ DerivativeFilterImplementation();
+ double compute( double dt, double input );
+};
+
+class ExponentialFilterImplementation : public GainFilterImplementation {
+protected:
+ InputValueList _TfInput;
+ bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
+ bool _isSecondOrder;
+ double output_1, output_2;
+public:
+ ExponentialFilterImplementation();
+ double compute( double dt, double input );
+ virtual void initialize( double output );
+};
+
+class MovingAverageFilterImplementation : public DigitalFilterImplementation {
+protected:
+ InputValueList _samplesInput;
+ double _output_1;
+ std::deque <double> _inputQueue;
+ bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
+public:
+ MovingAverageFilterImplementation();
+ double compute( double dt, double input );
+ virtual void initialize( double output );
+};
+
+class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
+protected:
+ double _output_1;
+ InputValueList _rateOfChangeInput;
+ bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
+public:
+ NoiseSpikeFilterImplementation();
+ double compute( double dt, double input );
+ virtual void initialize( double output );
+};
+
+/* --------------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------------- */
+
+} // namespace FGXMLAutopilot
+
+using namespace FGXMLAutopilot;
+
+/* --------------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------------- */
+
+bool DigitalFilterImplementation::configure( SGPropertyNode_ptr configNode )
+{
+ for (int i = 0; i < configNode->nChildren(); ++i ) {
+ SGPropertyNode_ptr prop;
+
+ SGPropertyNode_ptr child = configNode->getChild(i);
+ string cname(child->getName());
+
+ if( configure( cname, child ) )
+ continue;
+
+ } // for configNode->nChildren()
+
+ return true;
+}
+
+/* --------------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------------- */
+
+double GainFilterImplementation::compute( double dt, double input )
+{
+ return _gainInput.get_value() * input;
+}
+
+bool GainFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+{
+ if (nodeName == "gain" ) {
+ _gainInput.push_back( new InputValue( configNode, 1 ) );
+ return true;
+ }
+
+ return false;
+}
+
+/* --------------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------------- */
+
+double ReciprocalFilterImplementation::compute( double dt, double input )
+{
+ if( input >= -SGLimitsd::min() || input <= SGLimitsd::min() )
+ return SGLimitsd::max();
+
+ return _gainInput.get_value() / input;
+
+}
+
+/* --------------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------------- */
+
+DerivativeFilterImplementation::DerivativeFilterImplementation() :
+ _input_1(0.0)
+{
+}
+
+bool DerivativeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+{
+ if( GainFilterImplementation::configure( nodeName, configNode ) )
+ return true;
+
+ if (nodeName == "filter-time" ) {
+ _TfInput.push_back( new InputValue( configNode, 1 ) );
+ return true;
+ }
+
+ return false;
+}
+
+double DerivativeFilterImplementation::compute( double dt, double input )
+{
+ double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
+ _input_1 = input;
+ return output;
+
+}
+
+/* --------------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------------- */
+
+MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
+ _output_1(0.0)
+{
+}
+
+void MovingAverageFilterImplementation::initialize( double output )
+{
+ _output_1 = output;
+}
+
+double MovingAverageFilterImplementation::compute( double dt, double input )
+{
+ std::deque<double>::size_type samples = _samplesInput.get_value();
+ _inputQueue.resize(samples+1, 0.0);
+
+ double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
+
+ _output_1 = output_0;
+ _inputQueue.push_front(input);
+ return output_0;
+}
+
+bool MovingAverageFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+{
+ if (nodeName == "samples" ) {
+ _samplesInput.push_back( new InputValue( configNode, 1 ) );
+ return true;
+ }
+
+ return false;
+}
+
+/* --------------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------------- */
+
+NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
+ _output_1(0.0)
+{
+}
+
+void NoiseSpikeFilterImplementation::initialize( double output )
+{
+ _output_1 = output;
+}
+
+double NoiseSpikeFilterImplementation::compute( double dt, double input )
+{
+ double maxChange = _rateOfChangeInput.get_value() * dt;
+
+ double output_0 = _output_1;
+
+ if (_output_1 - input > maxChange) {
+ output_0 = _output_1 - maxChange;
+ } else if( _output_1 - input < -maxChange ) {
+ output_0 = _output_1 + maxChange;
+ } else if (fabs(input - _output_1) <= maxChange) {
+ output_0 = input;
+ }
+ _output_1 = output_0;
+ return output_0;
+}
+
+bool NoiseSpikeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+{
+ if (nodeName == "max-rate-of-change" ) {
+ _rateOfChangeInput.push_back( new InputValue( configNode, 1 ) );
+ return true;
+ }
+
+ return false;
+}
+
+/* --------------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------------- */
+
+ExponentialFilterImplementation::ExponentialFilterImplementation()
+ : _isSecondOrder(false),
+ output_1(0.0),
+ output_2(0.0)
+{
+}
+
+void ExponentialFilterImplementation::initialize( double output )
+{
+ output_1 = output_2 = output;
+}
+
+double ExponentialFilterImplementation::compute( double dt, double input )
+{
+ input = GainFilterImplementation::compute( dt, input );
+
+ double output_0;
+ double alpha = 1 / ((_TfInput.get_value()/dt) + 1);
+
+ if(_isSecondOrder) {
+ output_0 = alpha * alpha * input +
+ 2 * (1 - alpha) * output_1 -
+ (1 - alpha) * (1 - alpha) * output_2;
+ } else {
+ output_0 = alpha * input + (1 - alpha) * output_1;
+ }
+ output_2 = output_1;
+ return (output_1 = output_0);
+}
+
+bool ExponentialFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+{
+ if( GainFilterImplementation::configure( nodeName, configNode ) )
+ return true;
+
+ if (nodeName == "filter-time" ) {
+ _TfInput.push_back( new InputValue( configNode, 1 ) );
+ return true;
+ }
+
+ if (nodeName == "type" ) {
+ string type(configNode->getStringValue());
+ _isSecondOrder = type == "double-exponential";
+ }
+
+ return false;
+}
+
+/* --------------------------------------------------------------------------------- */
+/* Digital Filter Component Implementation */
+/* --------------------------------------------------------------------------------- */
+
+DigitalFilter::DigitalFilter() :
+ AnalogComponent()
+{
+}
+
+static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
+
+bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configNode)
+{
+ if( componentForge.empty() ) {
+ componentForge["gain"] = new CreateAndConfigureFunctor<GainFilterImplementation,DigitalFilterImplementation>();
+ componentForge["exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
+ componentForge["double-exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
+ componentForge["moving-average"] = new CreateAndConfigureFunctor<MovingAverageFilterImplementation,DigitalFilterImplementation>();
+ componentForge["noise-spike"] = new CreateAndConfigureFunctor<NoiseSpikeFilterImplementation,DigitalFilterImplementation>();
+ componentForge["reciprocal"] = new CreateAndConfigureFunctor<ReciprocalFilterImplementation,DigitalFilterImplementation>();
+ componentForge["derivative"] = new CreateAndConfigureFunctor<DerivativeFilterImplementation,DigitalFilterImplementation>();
+ }
+
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ")" << endl );
+ if( AnalogComponent::configure( nodeName, configNode ) )
+ return true;
+
+ if (nodeName == "type" ) {
+ string type( configNode->getStringValue() );
+ if( componentForge.count(type) == 0 ) {
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled filter type <" << type << ">" << endl );
+ return true;
+ }
+ _implementation = (*componentForge[type])( configNode->getParent() );
+ return true;
+ }
+
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ") [unhandled]" << endl );
+ return false; // not handled by us, let the base class try
+}
+
+void DigitalFilter::update( bool firstTime, double dt)
+{
+ if( _implementation == NULL ) return;
+
+ if( firstTime )
+ _implementation->initialize( get_output_value() );
+
+ double input = _valueInput.get_value() - _referenceInput.get_value();
+ double output = _implementation->compute( dt, input );
+
+ set_output_value( output );
+
+ if(_debug) {
+ cout << "input:" << input
+ << "\toutput:" << output << endl;
+ }
+}
--- /dev/null
+// digitalfilter.hxx - a selection of digital filters
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#ifndef __DIGITALFILTER_HXX
+#define __DIGITALFILTER_HXX 1
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include "analogcomponent.hxx"
+
+namespace FGXMLAutopilot {
+
+/**
+ *
+ *
+ */
+class DigitalFilterImplementation : public SGReferenced {
+protected:
+ virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode) = 0;
+public:
+ virtual void initialize( double output ) {}
+ virtual double compute( double dt, double input ) = 0;
+ bool configure( SGPropertyNode_ptr configNode );
+};
+
+/**
+ * brief@ DigitalFilter - a selection of digital filters
+ *
+ */
+class DigitalFilter : public AnalogComponent
+{
+private:
+ SGSharedPtr<DigitalFilterImplementation> _implementation;
+
+protected:
+ bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode);
+ void update( bool firstTime, double dt);
+
+ InputValueList _Tf;
+ InputValueList _samples;
+ InputValueList _rateOfChange;
+ InputValueList _gain;
+
+public:
+ DigitalFilter();
+ ~DigitalFilter() {}
+
+};
+
+} // namespace FGXMLAutopilot
+#endif
--- /dev/null
+// flipflop.hxx - implementation of multiple flip flop types
+//
+// Written by Torsten Dreyer
+//
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#include "flipflop.hxx"
+#include "functor.hxx"
+#include "inputvalue.hxx"
+#include <Main/fg_props.hxx>
+
+using namespace FGXMLAutopilot;
+
+class RSFlipFlopImplementation : public FlipFlopImplementation {
+protected:
+ bool _rIsDominant;
+public:
+ RSFlipFlopImplementation( bool rIsDominant = true ) : _rIsDominant( rIsDominant ) {}
+ virtual bool getState( double dt, DigitalComponent::InputMap input, bool & q );
+};
+
+class SRFlipFlopImplementation : public RSFlipFlopImplementation {
+public:
+ SRFlipFlopImplementation() : RSFlipFlopImplementation( false ) {}
+};
+
+class ClockedFlipFlopImplementation : public RSFlipFlopImplementation {
+protected:
+ bool _clock;
+ virtual bool onRaisingEdge( DigitalComponent::InputMap input, bool & q ) = 0;
+public:
+ ClockedFlipFlopImplementation( bool rIsDominant = true ) : RSFlipFlopImplementation( rIsDominant ), _clock(false) {}
+ virtual bool getState( double dt, DigitalComponent::InputMap input, bool & q );
+};
+
+class JKFlipFlopImplementation : public ClockedFlipFlopImplementation {
+public:
+ JKFlipFlopImplementation( bool rIsDominant = true ) : ClockedFlipFlopImplementation ( rIsDominant ) {}
+ virtual bool onRaisingEdge( DigitalComponent::InputMap input, bool & q );
+};
+
+class DFlipFlopImplementation : public ClockedFlipFlopImplementation {
+public:
+ DFlipFlopImplementation( bool rIsDominant = true ) : ClockedFlipFlopImplementation ( rIsDominant ) {}
+ virtual bool onRaisingEdge( DigitalComponent::InputMap input, bool & q ) {
+ q = input.get_value("D");
+ return true;
+ }
+};
+
+class TFlipFlopImplementation : public ClockedFlipFlopImplementation {
+public:
+ TFlipFlopImplementation( bool rIsDominant = true ) : ClockedFlipFlopImplementation ( rIsDominant ) {}
+ virtual bool onRaisingEdge( DigitalComponent::InputMap input, bool & q ) {
+ q = !q;
+ return true;
+ }
+};
+
+class MonoFlopImplementation : public JKFlipFlopImplementation {
+protected:
+ virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
+ InputValueList _time;
+ double _t;
+public:
+ MonoFlopImplementation( bool rIsDominant = true ) : JKFlipFlopImplementation( rIsDominant ) {}
+ virtual bool getState( double dt, DigitalComponent::InputMap input, bool & q );
+};
+
+bool MonoFlopImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+{
+ if( JKFlipFlopImplementation::configure( nodeName, configNode ) )
+ return true;
+
+ if (nodeName == "time") {
+ _time.push_back( new InputValue( configNode ) );
+ return true;
+ }
+
+ return false;
+}
+
+bool MonoFlopImplementation::getState( double dt, DigitalComponent::InputMap input, bool & q )
+{
+ if( JKFlipFlopImplementation::getState( dt, input, q ) ) {
+ _t = q ? _time.get_value() : 0;
+ return true;
+ }
+
+ _t -= dt;
+ if( _t <= 0.0 ) {
+ q = 0;
+ return true;
+ }
+
+ return false;
+}
+
+
+bool RSFlipFlopImplementation::getState( double dt, DigitalComponent::InputMap input, bool & q )
+{
+ bool s = input.get_value("S");
+ bool r = input.get_value("R");
+
+ // s == false && q == false: no change, keep state
+ if( s || r ) {
+ if( _rIsDominant ) { // RS: reset is dominant
+ if( s ) q = true; // set
+ if( r ) q = false; // reset
+ } else { // SR: set is dominant
+ if( r ) q = false; // reset
+ if( s ) q = true; // set
+ }
+ return true; // signal state changed
+ }
+ return false; // signal state unchagned
+}
+
+bool ClockedFlipFlopImplementation::getState( double dt, DigitalComponent::InputMap input, bool & q )
+{
+ if( RSFlipFlopImplementation::getState( dt, input, q ) )
+ return true;
+
+ bool c = input.get_value("clock");
+ bool raisingEdge = c && !_clock;
+
+ _clock = c;
+
+ if( !raisingEdge ) return false; //signal no change
+ return onRaisingEdge( input, q );
+}
+
+bool JKFlipFlopImplementation::onRaisingEdge( DigitalComponent::InputMap input, bool & q )
+{
+ bool j = input.get_value("J");
+ bool k = input.get_value("K");
+
+ // j == false && k == false: no change, keep state
+ if( (j || k) ) {
+ if( j && k ) {
+ q = !q; // toggle
+ } else {
+ if( j ) q = true; // set
+ if( k ) q = false; // reset
+ }
+ return true; // signal state changed
+ }
+
+ return false; // signal no change
+}
+
+bool FlipFlopImplementation::configure( SGPropertyNode_ptr configNode )
+{
+ for (int i = 0; i < configNode->nChildren(); ++i ) {
+ SGPropertyNode_ptr prop;
+
+ SGPropertyNode_ptr child = configNode->getChild(i);
+ string cname(child->getName());
+
+ if( configure( cname, child ) )
+ continue;
+
+ } // for configNode->nChildren()
+
+ return true;
+}
+
+
+static map<string,FunctorBase<FlipFlopImplementation> *> componentForge;
+
+bool FlipFlop::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+{
+ if( componentForge.empty() ) {
+ componentForge["RS"] = new CreateAndConfigureFunctor<RSFlipFlopImplementation,FlipFlopImplementation>();
+ componentForge["SR"] = new CreateAndConfigureFunctor<SRFlipFlopImplementation,FlipFlopImplementation>();
+ componentForge["JK"] = new CreateAndConfigureFunctor<JKFlipFlopImplementation,FlipFlopImplementation>();
+ componentForge["D"] = new CreateAndConfigureFunctor<DFlipFlopImplementation, FlipFlopImplementation>();
+ componentForge["T"] = new CreateAndConfigureFunctor<TFlipFlopImplementation, FlipFlopImplementation>();
+ componentForge["monostable"] = new CreateAndConfigureFunctor<MonoFlopImplementation, FlipFlopImplementation>();
+ }
+
+ if( DigitalComponent::configure( nodeName, configNode ) )
+ return true;
+
+ if( nodeName == "type" ) {
+ string type(configNode->getStringValue());
+ if( componentForge.count(type) == 0 ) {
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled flip-flop type <" << type << ">" << endl );
+ return true;
+ }
+ _implementation = (*componentForge[type])( configNode->getParent() );
+ return true;
+ }
+
+ if (nodeName == "set"||nodeName == "S") {
+ _input["S"] = sgReadCondition( fgGetNode("/"), configNode );
+ return true;
+ }
+
+ if (nodeName == "reset" || nodeName == "R" ) {
+ _input["R"] = sgReadCondition( fgGetNode("/"), configNode );
+ return true;
+ }
+
+ if (nodeName == "J") {
+ _input["J"] = sgReadCondition( fgGetNode("/"), configNode );
+ return true;
+ }
+
+ if (nodeName == "K") {
+ _input["K"] = sgReadCondition( fgGetNode("/"), configNode );
+ return true;
+ }
+
+ if (nodeName == "D") {
+ _input["D"] = sgReadCondition( fgGetNode("/"), configNode );
+ return true;
+ }
+
+ if (nodeName == "clock") {
+ _input["clock"] = sgReadCondition( fgGetNode("/"), configNode );
+ return true;
+ }
+
+ return false;
+}
+
+void FlipFlop::update( bool firstTime, double dt )
+{
+ if( _implementation == NULL ) {
+ SG_LOG( SG_AUTOPILOT, SG_ALERT, "No flip-flop implementation for " << get_name() << endl );
+ return;
+ }
+
+ bool q0, q;
+
+ q0 = q = get_output();
+
+ if( _implementation->getState( dt, _input, q ) ) {
+ set_output( q );
+
+ if(_debug) {
+ cout << "updating flip-flop \"" << get_name() << "\"" << endl;
+ cout << "prev. Output:" << q0 << endl;
+ for( InputMap::const_iterator it = _input.begin(); it != _input.end(); it++ )
+ cout << "Input \"" << (*it).first << "\":" << (*it).second->test() << endl;
+ cout << "new Output:" << q << endl;
+ }
+ }
+}
+
+
--- /dev/null
+// flipflop.hxx - implementation of multiple flip flop types
+//
+// Written by Torsten Dreyer
+//
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#ifndef __FLIPFLOPCOMPONENT_HXX
+#define __FLIPFLOPCOMPONENT_HXX 1
+
+#include "logic.hxx"
+
+namespace FGXMLAutopilot {
+
+class FlipFlopImplementation : public SGReferenced {
+protected:
+ virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode ) { return false; }
+public:
+ virtual bool getState( double dt, DigitalComponent::InputMap input, bool & q ) { return false; }
+ bool configure( SGPropertyNode_ptr configNode );
+};
+
+/**
+ * @brief A simple flipflop implementation
+ */
+class FlipFlop : public Logic {
+public:
+protected:
+ /**
+ * @brief Over-rideable hook method to allow derived classes to refine top-level
+ * node parsing.
+ * @param aName
+ * @param aNode
+ * @return true if the node was handled, false otherwise.
+ */
+ virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
+ void update( bool firstTime, double dt );
+
+private:
+ SGSharedPtr<FlipFlopImplementation> _implementation;
+
+};
+
+}
+#endif // FLIPFLOPCOMPONENT_HXX
--- /dev/null
+// functor.hxx - a utility to create object based on names
+//
+// Written by Torsten Dreyer
+//
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#ifndef __FUNCTOR_HXX
+#define __FUNCTOR_HXX 1
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include <simgear/props/props.hxx>
+
+namespace FGXMLAutopilot {
+
+template <class TBase> class FunctorBase {
+public:
+ virtual TBase * operator()( SGPropertyNode_ptr configNode ) = 0;
+};
+
+template <class TClass,class TBase> class CreateAndConfigureFunctor :
+ public FunctorBase<TBase>,
+ SGReferenced {
+public:
+ virtual TBase * operator()( SGPropertyNode_ptr configNode ) {
+ TBase * base = new TClass();
+ base->configure( configNode );
+ return base;
+ }
+};
+
+}
+
+#endif // __FUNCTOR_HXX 1
--- /dev/null
+// inputvalue.hxx - provide input to autopilot components
+//
+// Written by Torsten Dreyer
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#include "inputvalue.hxx"
+#include <Main/fg_props.hxx>
+using namespace FGXMLAutopilot;
+
+PeriodicalValue::PeriodicalValue( SGPropertyNode_ptr root )
+{
+ SGPropertyNode_ptr minNode = root->getChild( "min" );
+ SGPropertyNode_ptr maxNode = root->getChild( "max" );
+ if( minNode == NULL || maxNode == NULL ) {
+ SG_LOG(SG_AUTOPILOT, SG_ALERT, "periodical defined, but no <min> and/or <max> tag. Period ignored." );
+ } else {
+ minPeriod = new InputValue( minNode );
+ maxPeriod = new InputValue( maxNode );
+ }
+}
+
+double PeriodicalValue::normalize( double value )
+{
+ if( !(minPeriod && maxPeriod )) return value;
+
+ double p1 = minPeriod->get_value();
+ double p2 = maxPeriod->get_value();
+
+ double min = std::min<double>(p1,p2);
+ double max = std::max<double>(p1,p2);
+ double phase = fabs(max - min);
+
+ if( phase > SGLimitsd::min() ) {
+ while( value < min ) value += phase;
+ while( value >= max ) value -= phase;
+ } else {
+ value = min; // phase is zero
+ }
+
+ return value;
+}
+
+InputValue::InputValue( SGPropertyNode_ptr node, double value, double offset, double scale) :
+ _value(0.0),
+ _abs(false)
+{
+ parse( node, value, offset, scale );
+}
+
+
+void InputValue::parse( SGPropertyNode_ptr node, double aValue, double aOffset, double aScale )
+{
+ _value = aValue;
+ _property = NULL;
+ _offset = NULL;
+ _scale = NULL;
+ _min = NULL;
+ _max = NULL;
+ _periodical = NULL;
+
+ if( node == NULL )
+ return;
+
+ SGPropertyNode * n;
+
+ if( (n = node->getChild("condition")) != NULL ) {
+ _condition = sgReadCondition(fgGetNode("/"), n);
+ }
+
+ if( (n = node->getChild( "scale" )) != NULL ) {
+ _scale = new InputValue( n, aScale );
+ }
+
+ if( (n = node->getChild( "offset" )) != NULL ) {
+ _offset = new InputValue( n, aOffset );
+ }
+
+ if( (n = node->getChild( "max" )) != NULL ) {
+ _max = new InputValue( n );
+ }
+
+ if( (n = node->getChild( "min" )) != NULL ) {
+ _min = new InputValue( n );
+ }
+
+ if( (n = node->getChild( "abs" )) != NULL ) {
+ _abs = n->getBoolValue();
+ }
+
+ if( (n = node->getChild( "period" )) != NULL ) {
+ _periodical = new PeriodicalValue( n );
+ }
+
+ SGPropertyNode *valueNode = node->getChild( "value" );
+ if ( valueNode != NULL ) {
+ _value = valueNode->getDoubleValue();
+ }
+
+ if ((n = node->getChild("expression")) != NULL) {
+ _expression = SGReadDoubleExpression(fgGetNode("/"), n->getChild(0));
+ return;
+ }
+
+ n = node->getChild( "property" );
+ // if no <property> element, check for <prop> element for backwards
+ // compatibility
+ if( n == NULL )
+ n = node->getChild( "prop" );
+
+ if ( n != NULL ) {
+ _property = fgGetNode( n->getStringValue(), true );
+ if ( valueNode != NULL ) {
+ // initialize property with given value
+ // if both <prop> and <value> exist
+ double s = get_scale();
+ if( s != 0 )
+ _property->setDoubleValue( (_value - get_offset())/s );
+ else
+ _property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
+ }
+
+ return;
+ } // of have a <property> or <prop>
+
+
+ if (valueNode == NULL) {
+ // no <value>, <prop> or <expression> element, use text node
+ const char * textnode = node->getStringValue();
+ char * endp = NULL;
+ // try to convert to a double value. If the textnode does not start with a number
+ // endp will point to the beginning of the string. We assume this should be
+ // a property name
+ _value = strtod( textnode, &endp );
+ if( endp == textnode ) {
+ _property = fgGetNode( textnode, true );
+ }
+ }
+}
+
+void InputValue::set_value( double aValue )
+{
+ if (!_property)
+ return;
+
+ double s = get_scale();
+ if( s != 0 )
+ _property->setDoubleValue( (aValue - get_offset())/s );
+ else
+ _property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
+}
+
+double InputValue::get_value() const
+{
+ double value = _value;
+
+ if (_expression) {
+ // compute the expression value
+ value = _expression->getValue(NULL);
+ } else if( _property != NULL ) {
+ value = _property->getDoubleValue();
+ }
+
+ if( _scale )
+ value *= _scale->get_value();
+
+ if( _offset )
+ value += _offset->get_value();
+
+ if( _min ) {
+ double m = _min->get_value();
+ if( value < m )
+ value = m;
+ }
+
+ if( _max ) {
+ double m = _max->get_value();
+ if( value > m )
+ value = m;
+ }
+
+ if( _periodical ) {
+ value = _periodical->normalize( value );
+ }
+
+ return _abs ? fabs(value) : value;
+}
+
--- /dev/null
+// inputvalue.hxx - provide input to autopilot components
+//
+// Written by Torsten Dreyer
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#ifndef _INPUTVALUE_HXX
+#define _INPUTVALUE_HXX 1
+
+#include <simgear/structure/SGExpression.hxx>
+
+namespace FGXMLAutopilot {
+
+typedef SGSharedPtr<class InputValue> InputValue_ptr;
+typedef SGSharedPtr<class PeriodicalValue> PeriodicalValue_ptr;
+
+/**
+ * @brief Model a periodical value like angular values
+ *
+ * Most common use for periodical values are angular values.
+ * If y = f(x) = f(x + n*period), this is a periodical function
+ */
+class PeriodicalValue : public SGReferenced {
+private:
+ InputValue_ptr minPeriod; // The minimum value of the period
+ InputValue_ptr maxPeriod; // The maximum value of the period
+public:
+ PeriodicalValue( SGPropertyNode_ptr node );
+ double normalize( double value );
+};
+
+/**
+ * @brief A input value for analog autopilot components
+ *
+ * Input values may be constants, property values, transformed with a scale
+ * and/or offset, clamped to min/max values, be periodical, bound to
+ * conditions or evaluated from expressions.
+ */
+class InputValue : public SGReferenced {
+private:
+ double _value; // The value as a constant or initializer for the property
+ bool _abs; // return absolute value
+ SGPropertyNode_ptr _property; // The name of the property containing the value
+ InputValue_ptr _offset; // A fixed offset, defaults to zero
+ InputValue_ptr _scale; // A constant scaling factor defaults to one
+ InputValue_ptr _min; // A minimum clip defaults to no clipping
+ InputValue_ptr _max; // A maximum clip defaults to no clipping
+ PeriodicalValue_ptr _periodical; //
+ SGSharedPtr<const SGCondition> _condition;
+ SGSharedPtr<SGExpressiond> _expression; ///< expression to generate the value
+
+public:
+ InputValue( SGPropertyNode_ptr node = NULL, double value = 0.0, double offset = 0.0, double scale = 1.0 );
+
+ void parse( SGPropertyNode_ptr, double value = 0.0, double offset = 0.0, double scale = 1.0 );
+
+ /* get the value of this input, apply scale and offset and clipping */
+ double get_value() const;
+
+ /* set the input value after applying offset and scale */
+ void set_value( double value );
+
+ inline double get_scale() const {
+ return _scale == NULL ? 1.0 : _scale->get_value();
+ }
+
+ inline double get_offset() const {
+ return _offset == NULL ? 0.0 : _offset->get_value();
+ }
+
+ inline bool is_enabled() const {
+ return _condition == NULL ? true : _condition->test();
+ }
+
+};
+
+/**
+ * @brief A chained list of InputValues
+ *
+ * Many compoments support InputValueLists as input. Each InputValue may be bound to
+ * a condition. This list supports the get_value() function to retrieve the value
+ * of the first InputValue in this list that has a condition evaluating to true.
+ */
+class InputValueList : public std::vector<InputValue_ptr> {
+ public:
+ InputValueList( double def = 0.0 ) : _def(def) { }
+
+ InputValue_ptr get_active() const {
+ for (const_iterator it = begin(); it != end(); ++it) {
+ if( (*it)->is_enabled() )
+ return *it;
+ }
+ return NULL;
+ }
+
+ double get_value() const {
+ InputValue_ptr input = get_active();
+ return input == NULL ? _def : input->get_value();
+ }
+ private:
+
+ double _def;
+
+};
+
+}
+
+#endif
--- /dev/null
+// logic.cxx - Base class for logic components
+//
+// Written by Torsten Dreyer
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#include "logic.hxx"
+
+using namespace FGXMLAutopilot;
+
+bool Logic::get_input() const
+{
+ // return state of first configured condition
+ InputMap::const_iterator it = _input.begin();
+ if( it == _input.end() ) return false; // no inputs?
+ return (*it).second->test();
+}
+
+void Logic::set_output( bool value )
+{
+ // respect global inverted flag
+ if( _inverted ) value = !value;
+
+ // set all outputs to the given value
+ for( OutputMap::iterator it = _output.begin(); it != _output.end(); it++ )
+ (*it).second->setValue( value );
+}
+
+bool Logic::get_output() const
+{
+ OutputMap::const_iterator it = _output.begin();
+ return it != _output.end() ? (*it).second->getValue() : false;
+}
+
+void Logic::update( bool firstTime, double dt )
+{
+ set_output( get_input() );
+}
+
--- /dev/null
+// logic.hxx - Base class for logic components
+//
+// Written by Torsten Dreyer
+//
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#ifndef __LOGICCOMPONENT_HXX
+#define __LOGICCOMPONENT_HXX 1
+
+#include "digitalcomponent.hxx"
+
+namespace FGXMLAutopilot {
+
+/**
+ * @brief A simple logic class writing <condition> to a property
+ */
+class Logic : public DigitalComponent {
+public:
+ bool get_input() const;
+ void set_output( bool value );
+ bool get_output() const;
+protected:
+ void update( bool firstTime, double dt );
+};
+
+}
+#endif // LOGICCOMPONENT_HXX
+
--- /dev/null
+// pidcontroller.cxx - implementation of PID controller
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#include "pidcontroller.hxx"
+
+using namespace FGXMLAutopilot;
+
+PIDController::PIDController():
+ AnalogComponent(),
+ alpha( 0.1 ),
+ beta( 1.0 ),
+ gamma( 0.0 ),
+ ep_n_1( 0.0 ),
+ edf_n_1( 0.0 ),
+ edf_n_2( 0.0 ),
+ u_n_1( 0.0 ),
+ desiredTs( 0.0 ),
+ elapsedTime( 0.0 )
+{
+}
+
+/*
+ * Roy Vegard Ovesen:
+ *
+ * Ok! Here is the PID controller algorithm that I would like to see
+ * implemented:
+ *
+ * delta_u_n = Kp * [ (ep_n - ep_n-1) + ((Ts/Ti)*e_n)
+ * + (Td/Ts)*(edf_n - 2*edf_n-1 + edf_n-2) ]
+ *
+ * u_n = u_n-1 + delta_u_n
+ *
+ * where:
+ *
+ * delta_u : The incremental output
+ * Kp : Proportional gain
+ * ep : Proportional error with reference weighing
+ * ep = beta * r - y
+ * where:
+ * beta : Weighing factor
+ * r : Reference (setpoint)
+ * y : Process value, measured
+ * e : Error
+ * e = r - y
+ * Ts : Sampling interval
+ * Ti : Integrator time
+ * Td : Derivator time
+ * edf : Derivate error with reference weighing and filtering
+ * edf_n = edf_n-1 / ((Ts/Tf) + 1) + ed_n * (Ts/Tf) / ((Ts/Tf) + 1)
+ * where:
+ * Tf : Filter time
+ * Tf = alpha * Td , where alpha usually is set to 0.1
+ * ed : Unfiltered derivate error with reference weighing
+ * ed = gamma * r - y
+ * where:
+ * gamma : Weighing factor
+ *
+ * u : absolute output
+ *
+ * Index n means the n'th value.
+ *
+ *
+ * Inputs:
+ * enabled ,
+ * y_n , r_n , beta=1 , gamma=0 , alpha=0.1 ,
+ * Kp , Ti , Td , Ts (is the sampling time available?)
+ * u_min , u_max
+ *
+ * Output:
+ * u_n
+ */
+
+void PIDController::update( bool firstTime, double dt )
+{
+ double edf_n = 0.0;
+ double delta_u_n = 0.0; // incremental output
+ double u_n = 0.0; // absolute output
+
+ double u_min = _minInput.get_value();
+ double u_max = _maxInput.get_value();
+
+ elapsedTime += dt;
+ if( elapsedTime <= desiredTs ) {
+ // do nothing if time step is not positive (i.e. no time has
+ // elapsed)
+ return;
+ }
+ double Ts = elapsedTime; // sampling interval (sec)
+ elapsedTime = 0.0;
+
+ if( firstTime ) {
+ // first time being enabled, seed u_n with current
+ // property tree value
+ ep_n_1 = 0.0;
+ edf_n_2 = edf_n_1 = edf_n = 0.0;
+ u_n = get_output_value();
+ u_n_1 = u_n;
+ }
+
+ if( Ts > SGLimitsd::min()) {
+ if( _debug ) cout << "Updating " << get_name()
+ << " Ts " << Ts << endl;
+
+ double y_n = _valueInput.get_value();
+ double r_n = _referenceInput.get_value();
+
+ if ( _debug ) cout << " input = " << y_n << " ref = " << r_n << endl;
+
+ // Calculates proportional error:
+ double ep_n = beta * r_n - y_n;
+ if ( _debug ) cout << " ep_n = " << ep_n;
+ if ( _debug ) cout << " ep_n_1 = " << ep_n_1;
+
+ // Calculates error:
+ double e_n = r_n - y_n;
+ if ( _debug ) cout << " e_n = " << e_n;
+
+ double td = Td.get_value();
+ if ( td > 0.0 ) { // do we need to calcluate derivative error?
+
+ // Calculates derivate error:
+ double ed_n = gamma * r_n - y_n;
+ if ( _debug ) cout << " ed_n = " << ed_n;
+
+ // Calculates filter time:
+ double Tf = alpha * td;
+ if ( _debug ) cout << " Tf = " << Tf;
+
+ // Filters the derivate error:
+ edf_n = edf_n_1 / (Ts/Tf + 1)
+ + ed_n * (Ts/Tf) / (Ts/Tf + 1);
+ if ( _debug ) cout << " edf_n = " << edf_n;
+ } else {
+ edf_n_2 = edf_n_1 = edf_n = 0.0;
+ }
+
+ // Calculates the incremental output:
+ double ti = Ti.get_value();
+ if ( ti > 0.0 ) {
+ delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
+ + ((Ts/ti) * e_n)
+ + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
+
+ if ( _debug ) {
+ cout << " delta_u_n = " << delta_u_n << endl;
+ cout << "P:" << Kp.get_value() * (ep_n - ep_n_1)
+ << " I:" << Kp.get_value() * ((Ts/ti) * e_n)
+ << " D:" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
+ << endl;
+ }
+ }
+
+ // Integrator anti-windup logic:
+ if ( delta_u_n > (u_max - u_n_1) ) {
+ delta_u_n = u_max - u_n_1;
+ if ( _debug ) cout << " max saturation " << endl;
+ } else if ( delta_u_n < (u_min - u_n_1) ) {
+ delta_u_n = u_min - u_n_1;
+ if ( _debug ) cout << " min saturation " << endl;
+ }
+
+ // Calculates absolute output:
+ u_n = u_n_1 + delta_u_n;
+ if ( _debug ) cout << " output = " << u_n << endl;
+
+ // Updates indexed values;
+ u_n_1 = u_n;
+ ep_n_1 = ep_n;
+ edf_n_2 = edf_n_1;
+ edf_n_1 = edf_n;
+
+ set_output_value( u_n );
+ }
+}
+
+bool PIDController::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+{
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ")" << endl );
+
+ if( AnalogComponent::configure( nodeName, configNode ) )
+ return true;
+
+ if( nodeName == "config" ) {
+ Component::configure( configNode );
+ return true;
+ }
+
+ if (nodeName == "Ts") {
+ desiredTs = configNode->getDoubleValue();
+ return true;
+ }
+
+ if (nodeName == "Kp") {
+ Kp.push_back( new InputValue(configNode) );
+ return true;
+ }
+
+ if (nodeName == "Ti") {
+ Ti.push_back( new InputValue(configNode) );
+ return true;
+ }
+
+ if (nodeName == "Td") {
+ Td.push_back( new InputValue(configNode) );
+ return true;
+ }
+
+ if (nodeName == "beta") {
+ beta = configNode->getDoubleValue();
+ return true;
+ }
+
+ if (nodeName == "alpha") {
+ alpha = configNode->getDoubleValue();
+ return true;
+ }
+
+ if (nodeName == "gamma") {
+ gamma = configNode->getDoubleValue();
+ return true;
+ }
+
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ") [unhandled]" << endl );
+ return false;
+}
+
--- /dev/null
+// pidcontroller.hxx - implementation of PID controller
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#ifndef __PIDCONTROLLER_HXX
+#define __PIDCONTROLLER_HXX 1
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include "analogcomponent.hxx"
+
+#include <simgear/props/props.hxx>
+#include <simgear/structure/subsystem_mgr.hxx>
+
+namespace FGXMLAutopilot {
+
+/**
+ * Roy Ovesen's PID controller
+ */
+class PIDController : public AnalogComponent {
+
+private:
+ // Configuration values
+ InputValueList Kp; // proportional gain
+ InputValueList Ti; // Integrator time (sec)
+ InputValueList Td; // Derivator time (sec)
+
+ double alpha; // low pass filter weighing factor (usually 0.1)
+ double beta; // process value weighing factor for
+ // calculating proportional error
+ // (usually 1.0)
+ double gamma; // process value weighing factor for
+ // calculating derivative error
+ // (usually 0.0)
+
+ // Previous state tracking values
+ double ep_n_1; // ep[n-1] (prop error)
+ double edf_n_1; // edf[n-1] (derivative error)
+ double edf_n_2; // edf[n-2] (derivative error)
+ double u_n_1; // u[n-1] (output)
+ double desiredTs; // desired sampling interval (sec)
+ double elapsedTime; // elapsed time (sec)
+
+protected:
+ bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode);
+public:
+ PIDController();
+ ~PIDController() {}
+
+ void update( bool firstTime, double dt );
+};
+
+}
+#endif // __PIDCONTROLLER_HXX
--- /dev/null
+// pisimplecontroller.cxx - implementation of a simple PI controller
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#include "pisimplecontroller.hxx"
+
+using namespace FGXMLAutopilot;
+
+PISimpleController::PISimpleController() :
+ AnalogComponent(),
+ _int_sum( 0.0 )
+{
+}
+
+bool PISimpleController::configure( const string& nodeName, SGPropertyNode_ptr configNode)
+{
+ if( AnalogComponent::configure( nodeName, configNode ) )
+ return true;
+
+ if( nodeName == "config" ) {
+ Component::configure( configNode );
+ return true;
+ }
+
+ if (nodeName == "Kp") {
+ _Kp.push_back( new InputValue(configNode) );
+ return true;
+ }
+
+ if (nodeName == "Ki") {
+ _Ki.push_back( new InputValue(configNode) );
+ return true;
+ }
+
+ return false;
+}
+
+void PISimpleController::update( bool firstTime, double dt )
+{
+ if ( firstTime ) {
+ // we have just been enabled, zero out int_sum
+ _int_sum = 0.0;
+ }
+
+ if ( _debug ) cout << "Updating " << get_name() << endl;
+ double y_n = _valueInput.get_value();
+ double r_n = _referenceInput.get_value();
+
+ double error = r_n - y_n;
+ if ( _debug ) cout << "input = " << y_n
+ << " reference = " << r_n
+ << " error = " << error
+ << endl;
+
+ double prop_comp = clamp(error * _Kp.get_value());
+ _int_sum += error * _Ki.get_value() * dt;
+
+
+ double output = prop_comp + _int_sum;
+ double clamped_output = clamp( output );
+ if( output != clamped_output ) // anti-windup
+ _int_sum = clamped_output - prop_comp;
+
+ if ( _debug ) cout << "prop_comp = " << prop_comp
+ << " int_sum = " << _int_sum << endl;
+
+ set_output_value( clamped_output );
+ if ( _debug ) cout << "output = " << clamped_output << endl;
+}
--- /dev/null
+// pisimplecontroller.hxx - implementation of a simple PI controller
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#ifndef __PISIMPLECONTROLLER_HXX
+#define __PISIMPLECONTROLLER_HXX 1
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include "analogcomponent.hxx"
+
+#include <simgear/props/props.hxx>
+#include <simgear/structure/subsystem_mgr.hxx>
+
+namespace FGXMLAutopilot {
+
+/**
+ * A simplistic P [ + I ] PI controller
+ */
+class PISimpleController : public AnalogComponent {
+
+private:
+
+ // proportional component data
+ InputValueList _Kp;
+
+ // integral component data
+ InputValueList _Ki;
+ double _int_sum;
+
+protected:
+ bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
+
+public:
+
+ PISimpleController();
+ ~PISimpleController() {}
+
+ void update( bool firstTime, double dt );
+};
+
+}
+
+#endif
--- /dev/null
+// predictor.cxx - predict future values
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+
+#include "predictor.hxx"
+
+#ifdef SG_BULK
+#undef SG_BULK
+#endif
+#define SG_BULK SG_ALERT
+#ifdef SG_INFO
+#undef SG_INFO
+#endif
+#define SG_INFO SG_ALERT
+
+using namespace FGXMLAutopilot;
+
+Predictor::Predictor () :
+ AnalogComponent(),
+ _average(0.0)
+{
+}
+
+bool Predictor::configure(const string& nodeName, SGPropertyNode* configNode)
+{
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "Predictor::configure(" << nodeName << ")" << endl );
+ if (nodeName == "seconds") {
+ _seconds.push_back( new InputValue( configNode, 0 ) );
+ return true;
+ }
+
+ if (nodeName == "filter-gain") {
+ _filter_gain.push_back( new InputValue( configNode, 0 ) );
+ return true;
+ }
+
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "Predictor::configure(" << nodeName << ") [unhandled]" << endl );
+ return false;
+}
+
+void Predictor::update( bool firstTime, double dt )
+{
+ double ivalue = _valueInput.get_value();
+
+ if ( firstTime ) {
+ _last_value = ivalue;
+ }
+
+ double current = (ivalue - _last_value)/dt; // calculate current error change (per second)
+ _average = dt < 1.0 ? ((1.0 - dt) * _average + current * dt) : current;
+
+ // calculate output with filter gain adjustment
+ double output = ivalue +
+ (1.0 - _filter_gain.get_value()) * (_average * _seconds.get_value()) +
+ _filter_gain.get_value() * (current * _seconds.get_value());
+ output = clamp( output );
+ set_output_value( output );
+
+ _last_value = ivalue;
+}
+
+
--- /dev/null
+// predictor.hxx - predict future values
+//
+// Written by Torsten Dreyer
+// Based heavily on work created by Curtis Olson, started January 2004.
+//
+// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
+// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of the
+// License, or (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+//
+#ifndef __PREDICTOR_HXX
+#define __PREDICTOR_HXX 1
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include "analogcomponent.hxx"
+
+#include <simgear/props/props.hxx>
+
+namespace FGXMLAutopilot {
+
+/**
+ * @brief Simple moving average filter converts input value to predicted value "seconds".
+ *
+ * Smoothing as described by Curt Olson:
+ * gain would be valid in the range of 0 - 1.0
+ * 1.0 would mean no filtering.
+ * 0.0 would mean no input.
+ * 0.5 would mean (1 part past value + 1 part current value) / 2
+ * 0.1 would mean (9 parts past value + 1 part current value) / 10
+ * 0.25 would mean (3 parts past value + 1 part current value) / 4
+ */
+class Predictor : public AnalogComponent {
+
+private:
+ double _last_value;
+ double _average;
+ InputValueList _seconds;
+ InputValueList _filter_gain;
+
+protected:
+ bool configure(const std::string& nodeName, SGPropertyNode* configNode );
+
+public:
+ Predictor();
+ ~Predictor() {}
+
+ void update( bool firstTime, double dt );
+};
+
+} // namespace FGXMLAutopilot
+
+#endif
+++ /dev/null
-// xmlauto.cxx - a more flexible, generic way to build autopilots
-//
-// Written by Curtis Olson, started January 2004.
-//
-// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
-//
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of the
-// License, or (at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful, but
-// WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-// General Public License for more details.
-//
-// You should have received a copy of the GNU General Public License
-// along with this program; if not, write to the Free Software
-// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
-//
-// $Id$
-
-#ifdef HAVE_CONFIG_H
-# include <config.h>
-#endif
-
-#include <iostream>
-
-#include <simgear/misc/strutils.hxx>
-#include <simgear/structure/exception.hxx>
-#include <simgear/misc/sg_path.hxx>
-#include <simgear/sg_inlines.h>
-#include <simgear/props/props_io.hxx>
-
-#include <simgear/structure/SGExpression.hxx>
-
-#include <Main/fg_props.hxx>
-#include <Main/globals.hxx>
-#include <Main/util.hxx>
-
-#include "xmlauto.hxx"
-
-using std::cout;
-using std::endl;
-
-using simgear::PropertyList;
-
-
-
-
-FGPeriodicalValue::FGPeriodicalValue( SGPropertyNode_ptr root )
-{
- SGPropertyNode_ptr minNode = root->getChild( "min" );
- SGPropertyNode_ptr maxNode = root->getChild( "max" );
- if( minNode == NULL || maxNode == NULL ) {
- SG_LOG(SG_AUTOPILOT, SG_ALERT, "periodical defined, but no <min> and/or <max> tag. Period ignored." );
- } else {
- minPeriod = new FGXMLAutoInput( minNode );
- maxPeriod = new FGXMLAutoInput( maxNode );
- }
-}
-
-double FGPeriodicalValue::normalize( double value )
-{
- if( !(minPeriod && maxPeriod )) return value;
-
- double p1 = minPeriod->get_value();
- double p2 = maxPeriod->get_value();
-
- double min = std::min<double>(p1,p2);
- double max = std::max<double>(p1,p2);
- double phase = fabs(max - min);
-
- if( phase > SGLimitsd::min() ) {
- while( value < min ) value += phase;
- while( value >= max ) value -= phase;
- } else {
- value = min; // phase is zero
- }
-
- return value;
-}
-
-FGXMLAutoInput::FGXMLAutoInput( SGPropertyNode_ptr node, double value, double offset, double scale) :
- value(0.0),
- abs(false)
-{
- parse( node, value, offset, scale );
-}
-
-
-void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffset, double aScale )
-{
- value = aValue;
- property = NULL;
- offset = NULL;
- scale = NULL;
- min = NULL;
- max = NULL;
- periodical = NULL;
-
- if( node == NULL )
- return;
-
- SGPropertyNode * n;
-
- if( (n = node->getChild("condition")) != NULL ) {
- _condition = sgReadCondition(fgGetNode("/"), n);
- }
-
- if( (n = node->getChild( "scale" )) != NULL ) {
- scale = new FGXMLAutoInput( n, aScale );
- }
-
- if( (n = node->getChild( "offset" )) != NULL ) {
- offset = new FGXMLAutoInput( n, aOffset );
- }
-
- if( (n = node->getChild( "max" )) != NULL ) {
- max = new FGXMLAutoInput( n );
- }
-
- if( (n = node->getChild( "min" )) != NULL ) {
- min = new FGXMLAutoInput( n );
- }
-
- if( (n = node->getChild( "abs" )) != NULL ) {
- abs = n->getBoolValue();
- }
-
- if( (n = node->getChild( "period" )) != NULL ) {
- periodical = new FGPeriodicalValue( n );
- }
-
- SGPropertyNode *valueNode = node->getChild( "value" );
- if ( valueNode != NULL ) {
- value = valueNode->getDoubleValue();
- }
-
- if ((n = node->getChild("expression")) != NULL) {
- _expression = SGReadDoubleExpression(fgGetNode("/"), n->getChild(0));
- return;
- }
-
- n = node->getChild( "property" );
- // if no <property> element, check for <prop> element for backwards
- // compatibility
- if( n == NULL )
- n = node->getChild( "prop" );
-
- if ( n != NULL ) {
- property = fgGetNode( n->getStringValue(), true );
- if ( valueNode != NULL ) {
- // initialize property with given value
- // if both <prop> and <value> exist
- double s = get_scale();
- if( s != 0 )
- property->setDoubleValue( (value - get_offset())/s );
- else
- property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
- }
-
- return;
- } // of have a <property> or <prop>
-
-
- if (valueNode == NULL) {
- // no <value>, <prop> or <expression> element, use text node
- const char * textnode = node->getStringValue();
- char * endp = NULL;
- // try to convert to a double value. If the textnode does not start with a number
- // endp will point to the beginning of the string. We assume this should be
- // a property name
- value = strtod( textnode, &endp );
- if( endp == textnode ) {
- property = fgGetNode( textnode, true );
- }
- }
-}
-
-void FGXMLAutoInput::set_value( double aValue )
-{
- if (!property)
- return;
-
- double s = get_scale();
- if( s != 0 )
- property->setDoubleValue( (aValue - get_offset())/s );
- else
- property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
-}
-
-double FGXMLAutoInput::get_value()
-{
- if (_expression) {
- // compute the expression value
- value = _expression->getValue(NULL);
- } else if( property != NULL ) {
- value = property->getDoubleValue();
- }
-
- if( scale )
- value *= scale->get_value();
-
- if( offset )
- value += offset->get_value();
-
- if( min ) {
- double m = min->get_value();
- if( value < m )
- value = m;
- }
-
- if( max ) {
- double m = max->get_value();
- if( value > m )
- value = m;
- }
-
- if( periodical ) {
- value = periodical->normalize( value );
- }
-
- return abs ? fabs(value) : value;
-}
-
-FGXMLAutoComponent::FGXMLAutoComponent() :
- _condition( NULL ),
- enable_prop( NULL ),
- enable_value( NULL ),
- passive_mode( fgGetNode("/autopilot/locks/passive-mode", true) ),
- honor_passive( false ),
- name(""),
- feedback_if_disabled( false ),
- debug(false),
- enabled( false )
-{
-}
-
-FGXMLAutoComponent::~FGXMLAutoComponent()
-{
- delete enable_value;
-}
-
-void FGXMLAutoComponent::parseNode(SGPropertyNode* aNode)
-{
- SGPropertyNode *prop;
- for (int i = 0; i < aNode->nChildren(); ++i ) {
- SGPropertyNode *child = aNode->getChild(i);
- string cname(child->getName());
-
- if (parseNodeHook(cname, child)) {
- // derived class handled it, fine
- } else if ( cname == "name" ) {
- name = child->getStringValue();
- } else if ( cname == "feedback-if-disabled" ) {
- feedback_if_disabled = child->getBoolValue();
- } else if ( cname == "debug" ) {
- debug = child->getBoolValue();
- } else if ( cname == "enable" ) {
- if( (prop = child->getChild("condition")) != NULL ) {
- _condition = sgReadCondition(fgGetNode("/"), prop);
- } else {
- if ( (prop = child->getChild( "property" )) != NULL ) {
- enable_prop = fgGetNode( prop->getStringValue(), true );
- }
-
- if ( (prop = child->getChild( "prop" )) != NULL ) {
- enable_prop = fgGetNode( prop->getStringValue(), true );
- }
-
- if ( (prop = child->getChild( "value" )) != NULL ) {
- delete enable_value;
- enable_value = new string(prop->getStringValue());
- }
- }
- if ( (prop = child->getChild( "honor-passive" )) != NULL ) {
- honor_passive = prop->getBoolValue();
- }
- } else if ( cname == "input" ) {
- valueInput.push_back( new FGXMLAutoInput( child ) );
- } else if ( cname == "reference" ) {
- referenceInput.push_back( new FGXMLAutoInput( child ) );
- } else if ( cname == "output" ) {
- // grab all <prop> and <property> childs
- int found = 0;
- // backwards compatibility: allow <prop> elements
- for( int i = 0; (prop = child->getChild("prop", i)) != NULL; i++ ) {
- SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
- output_list.push_back( tmp );
- found++;
- }
- for( int i = 0; (prop = child->getChild("property", i)) != NULL; i++ ) {
- SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
- output_list.push_back( tmp );
- found++;
- }
-
- // no <prop> elements, text node of <output> is property name
- if( found == 0 )
- output_list.push_back( fgGetNode(child->getStringValue(), true ) );
- } else if ( cname == "config" ) {
- parseConfig(child);
- } else if ( cname == "min" ) {
- uminInput.push_back( new FGXMLAutoInput( child ) );
- } else if ( cname == "u_min" ) {
- uminInput.push_back( new FGXMLAutoInput( child ) );
- } else if ( cname == "max" ) {
- umaxInput.push_back( new FGXMLAutoInput( child ) );
- } else if ( cname == "u_max" ) {
- umaxInput.push_back( new FGXMLAutoInput( child ) );
- } else if ( cname == "period" ) {
- periodical = new FGPeriodicalValue( child );
- } else {
- SG_LOG(SG_AUTOPILOT, SG_ALERT, "malformed autopilot definition - unrecognized node:"
- << cname << " in section " << name);
- throw sg_io_exception("XMLAuto: unrecognized component node:" + cname, "Section=" + name);
- }
- } // of top-level iteration
-}
-
-void FGXMLAutoComponent::parseConfig(SGPropertyNode* aConfig)
-{
- for (int i = 0; i < aConfig->nChildren(); ++i ) {
- SGPropertyNode *child = aConfig->getChild(i);
- string cname(child->getName());
-
- if (parseConfigHook(cname, child)) {
- // derived class handled it, fine
- } else if ( cname == "min" ) {
- uminInput.push_back( new FGXMLAutoInput( child ) );
- } else if ( cname == "u_min" ) {
- uminInput.push_back( new FGXMLAutoInput( child ) );
- } else if ( cname == "max" ) {
- umaxInput.push_back( new FGXMLAutoInput( child ) );
- } else if ( cname == "u_max" ) {
- umaxInput.push_back( new FGXMLAutoInput( child ) );
- } else {
- SG_LOG(SG_AUTOPILOT, SG_ALERT, "malformed autopilot definition - unrecognized config node:"
- << cname << " in section " << name);
- throw sg_io_exception("XMLAuto: unrecognized config node:" + cname, "Section=" + name);
- }
- } // of config iteration
-}
-
-bool FGXMLAutoComponent::parseNodeHook(const string& aName, SGPropertyNode* aNode)
-{
- return false;
-}
-
-bool FGXMLAutoComponent::parseConfigHook(const string& aName, SGPropertyNode* aNode)
-{
- return false;
-}
-
-bool FGXMLAutoComponent::isPropertyEnabled()
-{
- if( _condition )
- return _condition->test();
-
- if( enable_prop ) {
- if( enable_value ) {
- return *enable_value == enable_prop->getStringValue();
- } else {
- return enable_prop->getBoolValue();
- }
- }
- return true;
-}
-
-void FGXMLAutoComponent::do_feedback_if_disabled()
-{
- if( output_list.size() > 0 ) {
- FGXMLAutoInput * input = valueInput.get_active();
- if( input != NULL )
- input->set_value( output_list[0]->getDoubleValue() );
- }
-}
-
-double FGXMLAutoComponent::clamp( double value )
-{
- //If this is a periodical value, normalize it into our domain
- // before clamping
- if( periodical )
- value = periodical->normalize( value );
-
- // clamp, if either min or max is defined
- if( uminInput.size() + umaxInput.size() > 0 ) {
- double d = umaxInput.get_value( 0.0 );
- if( value > d ) value = d;
- d = uminInput.get_value( 0.0 );
- if( value < d ) value = d;
- }
- return value;
-}
-
-FGPIDController::FGPIDController( SGPropertyNode *node ):
- FGXMLAutoComponent(),
- alpha( 0.1 ),
- beta( 1.0 ),
- gamma( 0.0 ),
- ep_n_1( 0.0 ),
- edf_n_1( 0.0 ),
- edf_n_2( 0.0 ),
- u_n_1( 0.0 ),
- desiredTs( 0.0 ),
- elapsedTime( 0.0 )
-{
- parseNode(node);
-}
-
-bool FGPIDController::parseConfigHook(const string& aName, SGPropertyNode* aNode)
-{
- if (aName == "Ts") {
- desiredTs = aNode->getDoubleValue();
- } else if (aName == "Kp") {
- Kp.push_back( new FGXMLAutoInput(aNode) );
- } else if (aName == "Ti") {
- Ti.push_back( new FGXMLAutoInput(aNode) );
- } else if (aName == "Td") {
- Td.push_back( new FGXMLAutoInput(aNode) );
- } else if (aName == "beta") {
- beta = aNode->getDoubleValue();
- } else if (aName == "alpha") {
- alpha = aNode->getDoubleValue();
- } else if (aName == "gamma") {
- gamma = aNode->getDoubleValue();
- } else {
- // unhandled by us, let the base class try it
- return false;
- }
-
- return true;
-}
-
-/*
- * Roy Vegard Ovesen:
- *
- * Ok! Here is the PID controller algorithm that I would like to see
- * implemented:
- *
- * delta_u_n = Kp * [ (ep_n - ep_n-1) + ((Ts/Ti)*e_n)
- * + (Td/Ts)*(edf_n - 2*edf_n-1 + edf_n-2) ]
- *
- * u_n = u_n-1 + delta_u_n
- *
- * where:
- *
- * delta_u : The incremental output
- * Kp : Proportional gain
- * ep : Proportional error with reference weighing
- * ep = beta * r - y
- * where:
- * beta : Weighing factor
- * r : Reference (setpoint)
- * y : Process value, measured
- * e : Error
- * e = r - y
- * Ts : Sampling interval
- * Ti : Integrator time
- * Td : Derivator time
- * edf : Derivate error with reference weighing and filtering
- * edf_n = edf_n-1 / ((Ts/Tf) + 1) + ed_n * (Ts/Tf) / ((Ts/Tf) + 1)
- * where:
- * Tf : Filter time
- * Tf = alpha * Td , where alpha usually is set to 0.1
- * ed : Unfiltered derivate error with reference weighing
- * ed = gamma * r - y
- * where:
- * gamma : Weighing factor
- *
- * u : absolute output
- *
- * Index n means the n'th value.
- *
- *
- * Inputs:
- * enabled ,
- * y_n , r_n , beta=1 , gamma=0 , alpha=0.1 ,
- * Kp , Ti , Td , Ts (is the sampling time available?)
- * u_min , u_max
- *
- * Output:
- * u_n
- */
-
-void FGPIDController::update( double dt ) {
- double e_n; // error
- double edf_n;
- double delta_u_n = 0.0; // incremental output
- double u_n = 0.0; // absolute output
- double Ts; // sampling interval (sec)
-
- double u_min = uminInput.get_value();
- double u_max = umaxInput.get_value();
-
- elapsedTime += dt;
- if ( elapsedTime <= desiredTs ) {
- // do nothing if time step is not positive (i.e. no time has
- // elapsed)
- return;
- }
- Ts = elapsedTime;
- elapsedTime = 0.0;
-
- if ( isPropertyEnabled() ) {
- if ( !enabled ) {
- // first time being enabled, seed u_n with current
- // property tree value
- u_n = get_output_value();
- u_n_1 = u_n;
- }
- enabled = true;
- } else {
- enabled = false;
- do_feedback();
- }
-
- if ( enabled && Ts > 0.0) {
- if ( debug ) cout << "Updating " << get_name()
- << " Ts " << Ts << endl;
-
- double y_n = valueInput.get_value();
- double r_n = referenceInput.get_value();
-
- if ( debug ) cout << " input = " << y_n << " ref = " << r_n << endl;
-
- // Calculates proportional error:
- double ep_n = beta * r_n - y_n;
- if ( debug ) cout << " ep_n = " << ep_n;
- if ( debug ) cout << " ep_n_1 = " << ep_n_1;
-
- // Calculates error:
- e_n = r_n - y_n;
- if ( debug ) cout << " e_n = " << e_n;
-
- double td = Td.get_value();
- if ( td > 0.0 ) { // do we need to calcluate derivative error?
-
- // Calculates derivate error:
- double ed_n = gamma * r_n - y_n;
- if ( debug ) cout << " ed_n = " << ed_n;
-
- // Calculates filter time:
- double Tf = alpha * td;
- if ( debug ) cout << " Tf = " << Tf;
-
- // Filters the derivate error:
- edf_n = edf_n_1 / (Ts/Tf + 1)
- + ed_n * (Ts/Tf) / (Ts/Tf + 1);
- if ( debug ) cout << " edf_n = " << edf_n;
- } else {
- edf_n_2 = edf_n_1 = edf_n = 0.0;
- }
-
- // Calculates the incremental output:
- double ti = Ti.get_value();
- if ( ti > 0.0 ) {
- delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
- + ((Ts/ti) * e_n)
- + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
-
- if ( debug ) {
- cout << " delta_u_n = " << delta_u_n << endl;
- cout << "P:" << Kp.get_value() * (ep_n - ep_n_1)
- << " I:" << Kp.get_value() * ((Ts/ti) * e_n)
- << " D:" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
- << endl;
- }
- }
-
- // Integrator anti-windup logic:
- if ( delta_u_n > (u_max - u_n_1) ) {
- delta_u_n = u_max - u_n_1;
- if ( debug ) cout << " max saturation " << endl;
- } else if ( delta_u_n < (u_min - u_n_1) ) {
- delta_u_n = u_min - u_n_1;
- if ( debug ) cout << " min saturation " << endl;
- }
-
- // Calculates absolute output:
- u_n = u_n_1 + delta_u_n;
- if ( debug ) cout << " output = " << u_n << endl;
-
- // Updates indexed values;
- u_n_1 = u_n;
- ep_n_1 = ep_n;
- edf_n_2 = edf_n_1;
- edf_n_1 = edf_n;
-
- set_output_value( u_n );
- } else if ( !enabled ) {
- ep_n_1 = 0.0;
- edf_n_2 = edf_n_1 = edf_n = 0.0;
- }
-}
-
-
-FGPISimpleController::FGPISimpleController( SGPropertyNode *node ):
- FGXMLAutoComponent(),
- int_sum( 0.0 )
-{
- parseNode(node);
-}
-
-bool FGPISimpleController::parseConfigHook(const string& aName, SGPropertyNode* aNode)
-{
- if (aName == "Kp") {
- Kp.push_back( new FGXMLAutoInput(aNode) );
- } else if (aName == "Ki") {
- Ki.push_back( new FGXMLAutoInput(aNode) );
- } else {
- // unhandled by us, let the base class try it
- return false;
- }
-
- return true;
-}
-
-void FGPISimpleController::update( double dt ) {
-
- if ( isPropertyEnabled() ) {
- if ( !enabled ) {
- // we have just been enabled, zero out int_sum
- int_sum = 0.0;
- }
- enabled = true;
- } else {
- enabled = false;
- do_feedback();
- }
-
- if ( enabled ) {
- if ( debug ) cout << "Updating " << get_name() << endl;
- double y_n = valueInput.get_value();
- double r_n = referenceInput.get_value();
-
- double error = r_n - y_n;
- if ( debug ) cout << "input = " << y_n
- << " reference = " << r_n
- << " error = " << error
- << endl;
-
- double prop_comp = clamp(error * Kp.get_value());
- int_sum += error * Ki.get_value() * dt;
-
-
- double output = prop_comp + int_sum;
- double clamped_output = clamp( output );
- if( output != clamped_output ) // anti-windup
- int_sum = clamped_output - prop_comp;
-
- if ( debug ) cout << "prop_comp = " << prop_comp
- << " int_sum = " << int_sum << endl;
-
- set_output_value( clamped_output );
- if ( debug ) cout << "output = " << clamped_output << endl;
- }
-}
-
-
-FGPredictor::FGPredictor ( SGPropertyNode *node ):
- FGXMLAutoComponent(),
- average(0.0)
-{
- parseNode(node);
-}
-
-bool FGPredictor::parseNodeHook(const string& aName, SGPropertyNode* aNode)
-{
- if (aName == "seconds") {
- seconds.push_back( new FGXMLAutoInput( aNode, 0 ) );
- } else if (aName == "filter-gain") {
- filter_gain.push_back( new FGXMLAutoInput( aNode, 0 ) );
- } else {
- return false;
- }
-
- return true;
-}
-
-void FGPredictor::update( double dt ) {
- /*
- Simple moving average filter converts input value to predicted value "seconds".
-
- Smoothing as described by Curt Olson:
- gain would be valid in the range of 0 - 1.0
- 1.0 would mean no filtering.
- 0.0 would mean no input.
- 0.5 would mean (1 part past value + 1 part current value) / 2
- 0.1 would mean (9 parts past value + 1 part current value) / 10
- 0.25 would mean (3 parts past value + 1 part current value) / 4
-
- */
-
- double ivalue = valueInput.get_value();
-
- if ( isPropertyEnabled() ) {
- if ( !enabled ) {
- // first time being enabled
- last_value = ivalue;
- }
- enabled = true;
- } else {
- enabled = false;
- do_feedback();
- }
-
- if ( enabled ) {
-
- if ( dt > 0.0 ) {
- double current = (ivalue - last_value)/dt; // calculate current error change (per second)
- average = dt < 1.0 ? ((1.0 - dt) * average + current * dt) : current;
-
- // calculate output with filter gain adjustment
- double output = ivalue +
- (1.0 - filter_gain.get_value()) * (average * seconds.get_value()) +
- filter_gain.get_value() * (current * seconds.get_value());
- output = clamp( output );
- set_output_value( output );
- }
- last_value = ivalue;
- }
-}
-
-
-FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node):
- FGXMLAutoComponent(),
- filterType(none)
-{
- parseNode(node);
-
- output.resize(2, 0.0);
- input.resize(samplesInput.get_value() + 1, 0.0);
-}
-
-
-bool FGDigitalFilter::parseNodeHook(const string& aName, SGPropertyNode* aNode)
-{
- if (aName == "type" ) {
- string val(aNode->getStringValue());
- if ( val == "exponential" ) {
- filterType = exponential;
- } else if (val == "double-exponential") {
- filterType = doubleExponential;
- } else if (val == "moving-average") {
- filterType = movingAverage;
- } else if (val == "noise-spike") {
- filterType = noiseSpike;
- } else if (val == "gain") {
- filterType = gain;
- } else if (val == "reciprocal") {
- filterType = reciprocal;
- } else if (val == "differential") {
- filterType = differential;
- // use a constant of two samples for current and previous input value
- samplesInput.push_back( new FGXMLAutoInput(NULL, 2.0 ) );
- }
- } else if (aName == "filter-time" ) {
- TfInput.push_back( new FGXMLAutoInput( aNode, 1.0 ) );
- if( filterType == none ) filterType = exponential;
- } else if (aName == "samples" ) {
- samplesInput.push_back( new FGXMLAutoInput( aNode, 1 ) );
- if( filterType == none ) filterType = movingAverage;
- } else if (aName == "max-rate-of-change" ) {
- rateOfChangeInput.push_back( new FGXMLAutoInput( aNode, 1 ) );
- if( filterType == none ) filterType = noiseSpike;
- } else if (aName == "gain" ) {
- gainInput.push_back( new FGXMLAutoInput( aNode, 1 ) );
- if( filterType == none ) filterType = gain;
- } else {
- return false; // not handled by us, let the base class try
- }
-
- return true;
-}
-
-void FGDigitalFilter::update(double dt)
-{
- if ( isPropertyEnabled() ) {
-
- input.push_front(valueInput.get_value()-referenceInput.get_value());
- input.resize(samplesInput.get_value() + 1, 0.0);
-
- if ( !enabled ) {
- // first time being enabled, initialize output to the
- // value of the output property to avoid bumping.
- output.push_front(get_output_value());
- }
-
- enabled = true;
- } else {
- enabled = false;
- do_feedback();
- }
-
- if ( !enabled || dt < SGLimitsd::min() )
- return;
-
- /*
- * Exponential filter
- *
- * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1]
- *
- */
- if( debug ) cout << "Updating " << get_name()
- << " dt " << dt << endl;
-
- if (filterType == exponential)
- {
- double alpha = 1 / ((TfInput.get_value()/dt) + 1);
- output.push_front(alpha * input[0] +
- (1 - alpha) * output[0]);
- }
- else if (filterType == doubleExponential)
- {
- double alpha = 1 / ((TfInput.get_value()/dt) + 1);
- output.push_front(alpha * alpha * input[0] +
- 2 * (1 - alpha) * output[0] -
- (1 - alpha) * (1 - alpha) * output[1]);
- }
- else if (filterType == movingAverage)
- {
- output.push_front(output[0] +
- (input[0] - input.back()) / samplesInput.get_value());
- }
- else if (filterType == noiseSpike)
- {
- double maxChange = rateOfChangeInput.get_value() * dt;
-
- if ((output[0] - input[0]) > maxChange)
- {
- output.push_front(output[0] - maxChange);
- }
- else if ((output[0] - input[0]) < -maxChange)
- {
- output.push_front(output[0] + maxChange);
- }
- else if (fabs(input[0] - output[0]) <= maxChange)
- {
- output.push_front(input[0]);
- }
- }
- else if (filterType == gain)
- {
- output[0] = gainInput.get_value() * input[0];
- }
- else if (filterType == reciprocal)
- {
- if (input[0] != 0.0) {
- output[0] = gainInput.get_value() / input[0];
- }
- }
- else if (filterType == differential)
- {
- if( dt > SGLimitsd::min() ) {
- output[0] = (input[0]-input[1]) * TfInput.get_value() / dt;
- }
- }
-
- output[0] = clamp(output[0]) ;
- set_output_value( output[0] );
-
- output.resize(2);
-
- if (debug)
- {
- cout << "input:" << input[0]
- << "\toutput:" << output[0] << endl;
- }
-}
-
-FGXMLAutoLogic::FGXMLAutoLogic(SGPropertyNode * node ) :
- FGXMLAutoComponent(),
- inverted(false)
-{
- parseNode(node);
-}
-
-bool FGXMLAutoLogic::parseNodeHook(const std::string& aName, SGPropertyNode* aNode)
-{
- if (aName == "input") {
- input = sgReadCondition( fgGetNode("/"), aNode );
- } else if (aName == "inverted") {
- inverted = aNode->getBoolValue();
- } else {
- return false;
- }
-
- return true;
-}
-
-void FGXMLAutoLogic::update(double dt)
-{
- if ( isPropertyEnabled() ) {
- if ( !enabled ) {
- // we have just been enabled
- }
- enabled = true;
- } else {
- enabled = false;
- do_feedback();
- }
-
- if ( !enabled || dt < SGLimitsd::min() )
- return;
-
- if( input == NULL ) {
- if ( debug ) cout << "No input for " << get_name() << endl;
- return;
- }
-
- bool q = input->test();
- if( inverted ) q = !q;
-
- if ( debug ) cout << "Updating " << get_name() << ": " << q << endl;
-
- set_output_value( q );
-}
-
-class FGXMLAutoRSFlipFlop : public FGXMLAutoFlipFlop {
-private:
- bool _rIsDominant;
-public:
- FGXMLAutoRSFlipFlop( SGPropertyNode * node, bool rIsDominant = true ) :
- FGXMLAutoFlipFlop( node ) {}
-
- bool getState( bool & q ) {
-
- bool s = sInput ? sInput->test() : false;
- bool r = rInput ? rInput->test() : false;
-
- // s == false && q == false: no change, keep state
- if( s || r ) {
- if( _rIsDominant ) { // RS: reset is dominant
- if( s ) q = true; // set
- if( r ) q = false; // reset
- } else { // SR: set is dominant
- if( r ) q = false; // reset
- if( s ) q = true; // set
- }
- return true; // signal state changed
- }
- return false; // signal state unchagned
- }
-};
-
-/*
- JK flip-flop with set and reset input
- */
-class FGXMLAutoJKFlipFlop : public FGXMLAutoRSFlipFlop {
-private:
- bool clock;
-public:
- FGXMLAutoJKFlipFlop( SGPropertyNode * node ) :
- FGXMLAutoRSFlipFlop( node ),
- clock(false) {}
-
- bool getState( bool & q ) {
-
- if( FGXMLAutoRSFlipFlop::getState(q ) )
- return true;
-
- bool j = jInput ? jInput->test() : false;
- bool k = kInput ? kInput->test() : false;
- /*
- if the user provided a clock input, use it.
- Otherwise use framerate as clock
- This JK operates on the raising edge.
- */
- bool c = clockInput ? clockInput->test() : false;
- bool raisingEdge = clockInput ? (c && !clock) : true;
- clock = c;
-
- if( !raisingEdge ) return false; //signal no change
-
- // j == false && k == false: no change, keep state
- if( (j || k) ) {
- if( j && k ) {
- q = !q; // toggle
- } else {
- if( j ) q = true; // set
- if( k ) q = false; // reset
- }
- }
- return true; // signal state changed
- }
-};
-
-class FGXMLAutoDFlipFlop : public FGXMLAutoRSFlipFlop {
-private:
- bool clock;
-public:
- FGXMLAutoDFlipFlop( SGPropertyNode * node ) :
- FGXMLAutoRSFlipFlop( node ),
- clock(false) {}
-
- bool getState( bool & q ) {
-
- if( FGXMLAutoRSFlipFlop::getState(q ) )
- return true;
-
- if( clockInput == NULL ) {
- if ( debug ) cout << "No (clock) input for " << get_name() << endl;
- return false;
- }
-
- // check the clock - raising edge
- bool c = clockInput->test();
- bool raisingEdge = c && !clock;
- clock = c;
-
- if( !raisingEdge ) return false; // signal state unchanged
- q = dInput ? dInput->test() : false;
- return true;
- }
-};
-
-class FGXMLAutoTFlipFlop : public FGXMLAutoRSFlipFlop {
-private:
- bool clock;
-public:
- FGXMLAutoTFlipFlop( SGPropertyNode * node ) :
- FGXMLAutoRSFlipFlop( node ),
- clock(false) {}
-
- bool getState( bool & q ) {
-
- if( FGXMLAutoRSFlipFlop::getState(q ) )
- return true;
-
- if( clockInput == NULL ) {
- if ( debug ) cout << "No (clock) input for " << get_name() << endl;
- return false;
- }
-
- // check the clock - raising edge
- bool c = clockInput->test();
- bool raisingEdge = c && !clock;
- clock = c;
-
- if( !raisingEdge ) return false; // signal state unchanged;
- q = !q; // toggle
- return true;
- }
-};
-
-FGXMLAutoFlipFlop::FGXMLAutoFlipFlop(SGPropertyNode * node ) :
- FGXMLAutoComponent(),
- inverted(false)
-{
- parseNode(node);
-}
-
-bool FGXMLAutoFlipFlop::parseNodeHook(const std::string& aName, SGPropertyNode* aNode)
-{
- if (aName == "set"||aName == "S") {
- sInput = sgReadCondition( fgGetNode("/"), aNode );
- } else if (aName == "reset" || aName == "R" ) {
- rInput = sgReadCondition( fgGetNode("/"), aNode );
- } else if (aName == "J") {
- jInput = sgReadCondition( fgGetNode("/"), aNode );
- } else if (aName == "K") {
- kInput = sgReadCondition( fgGetNode("/"), aNode );
- } else if (aName == "D") {
- dInput = sgReadCondition( fgGetNode("/"), aNode );
- } else if (aName == "clock") {
- clockInput = sgReadCondition( fgGetNode("/"), aNode );
- } else if (aName == "inverted") {
- inverted = aNode->getBoolValue();
- } else if (aName == "type") {
- // ignore element type, evaluated by loader
- } else {
- return false;
- }
-
- return true;
-}
-
-void FGXMLAutoFlipFlop::update(double dt)
-{
- bool q = get_bool_output_value();
-
- if ( isPropertyEnabled() ) {
- if ( !enabled ) {
- // we have just been enabled
- // initialize to a bool property
- set_output_value( q );
- }
- enabled = true;
- } else {
- enabled = false;
- do_feedback();
- }
-
- if ( !enabled || dt < SGLimitsd::min() )
- return;
-
- if( getState( q ) ) {
- set_output_value( inverted ? !q : q );
- }
-}
-
-
-FGXMLAutopilotGroup::FGXMLAutopilotGroup() :
- SGSubsystemGroup()
-#ifdef XMLAUTO_USEHELPER
- ,average(0.0), // average/filtered prediction
- v_last(0.0), // last velocity
- last_static_pressure(0.0),
- vel(fgGetNode( "/velocities/airspeed-kt", true )),
- // Estimate speed in 5,10 seconds
- lookahead5(fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true )),
- lookahead10(fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true )),
- bug(fgGetNode( "/autopilot/settings/heading-bug-deg", true )),
- mag_hdg(fgGetNode( "/orientation/heading-magnetic-deg", true )),
- bug_error(fgGetNode( "/autopilot/internal/heading-bug-error-deg", true )),
- fdm_bug_error(fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true )),
- target_true(fgGetNode( "/autopilot/settings/true-heading-deg", true )),
- true_hdg(fgGetNode( "/orientation/heading-deg", true )),
- true_error(fgGetNode( "/autopilot/internal/true-heading-error-deg", true )),
- target_nav1(fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true )),
- true_nav1(fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true )),
- true_track_nav1(fgGetNode( "/autopilot/internal/nav1-track-error-deg", true )),
- nav1_course_error(fgGetNode( "/autopilot/internal/nav1-course-error", true )),
- nav1_selected_course(fgGetNode( "/instrumentation/nav[0]/radials/selected-deg", true )),
- vs_fps(fgGetNode( "/velocities/vertical-speed-fps", true )),
- vs_fpm(fgGetNode( "/autopilot/internal/vert-speed-fpm", true )),
- static_pressure(fgGetNode( "/systems/static[0]/pressure-inhg", true )),
- pressure_rate(fgGetNode( "/autopilot/internal/pressure-rate", true )),
- track(fgGetNode( "/orientation/track-deg", true ))
-#endif
-{
-}
-
-void FGXMLAutopilotGroup::update( double dt )
-{
- // update all configured autopilots
- SGSubsystemGroup::update( dt );
-#ifdef XMLAUTO_USEHELPER
- // update helper values
- double v = vel->getDoubleValue();
- double a = 0.0;
- if ( dt > 0.0 ) {
- a = (v - v_last) / dt;
-
- if ( dt < 1.0 ) {
- average = (1.0 - dt) * average + dt * a;
- } else {
- average = a;
- }
-
- lookahead5->setDoubleValue( v + average * 5.0 );
- lookahead10->setDoubleValue( v + average * 10.0 );
- v_last = v;
- }
-
- // Calculate heading bug error normalized to +/- 180.0
- double diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
- SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
- bug_error->setDoubleValue( diff );
-
- fdm_bug_error->setDoubleValue( diff );
-
- // Calculate true heading error normalized to +/- 180.0
- diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
- SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
- true_error->setDoubleValue( diff );
-
- // Calculate nav1 target heading error normalized to +/- 180.0
- diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
- SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
- true_nav1->setDoubleValue( diff );
-
- // Calculate true groundtrack
- diff = target_nav1->getDoubleValue() - track->getDoubleValue();
- SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
- true_track_nav1->setDoubleValue( diff );
-
- // Calculate nav1 selected course error normalized to +/- 180.0
- diff = nav1_selected_course->getDoubleValue() - mag_hdg->getDoubleValue();
- SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
- nav1_course_error->setDoubleValue( diff );
-
- // Calculate vertical speed in fpm
- vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
-
-
- // Calculate static port pressure rate in [inhg/s].
- // Used to determine vertical speed.
- if ( dt > 0.0 ) {
- double current_static_pressure = static_pressure->getDoubleValue();
- double current_pressure_rate =
- ( current_static_pressure - last_static_pressure ) / dt;
-
- pressure_rate->setDoubleValue(current_pressure_rate);
- last_static_pressure = current_static_pressure;
- }
-#endif
-}
-
-void FGXMLAutopilotGroup::reinit()
-{
- for( vector<string>::size_type i = 0; i < _autopilotNames.size(); i++ ) {
- FGXMLAutopilot * ap = (FGXMLAutopilot*)get_subsystem( _autopilotNames[i] );
- if( ap == NULL ) continue; // ?
- remove_subsystem( _autopilotNames[i] );
- delete ap;
- }
- _autopilotNames.clear();
- init();
-}
-
-void FGXMLAutopilotGroup::init()
-{
- PropertyList autopilotNodes = fgGetNode( "/sim/systems", true )->getChildren("autopilot");
- if( autopilotNodes.size() == 0 ) {
- SG_LOG( SG_ALL, SG_WARN, "No autopilot configuration specified for this model!");
- return;
- }
-
- for( PropertyList::size_type i = 0; i < autopilotNodes.size(); i++ ) {
- SGPropertyNode_ptr pathNode = autopilotNodes[i]->getNode( "path" );
- if( pathNode == NULL ) {
- SG_LOG( SG_ALL, SG_WARN, "No autopilot configuration file specified for this autopilot!");
- continue;
- }
-
- string apName;
- SGPropertyNode_ptr nameNode = autopilotNodes[i]->getNode( "name" );
- if( nameNode != NULL ) {
- apName = nameNode->getStringValue();
- } else {
- std::ostringstream buf;
- buf << "unnamed_autopilot_" << i;
- apName = buf.str();
- }
-
- if( get_subsystem( apName.c_str() ) != NULL ) {
- SG_LOG( SG_ALL, SG_ALERT, "Duplicate autopilot configuration name " << apName << " ignored" );
- continue;
- }
-
- SGPath config( globals->get_fg_root() );
- config.append( pathNode->getStringValue() );
-
- SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from " << config.str() );
- // FGXMLAutopilot
- FGXMLAutopilot * ap = new FGXMLAutopilot;
- try {
- SGPropertyNode_ptr root = new SGPropertyNode();
- readProperties( config.str(), root );
-
-
- if ( ! ap->build( root ) ) {
- SG_LOG( SG_ALL, SG_ALERT,
- "Detected an internal inconsistency in the autopilot configuration." << endl << " See earlier errors for details." );
- delete ap;
- continue;
- }
- } catch (const sg_exception& e) {
- SG_LOG( SG_AUTOPILOT, SG_ALERT, "Failed to load autopilot configuration: "
- << config.str() << ":" << e.getMessage() );
- delete ap;
- continue;
- }
-
- SG_LOG( SG_AUTOPILOT, SG_INFO, "adding autopilot subsystem " << apName );
- set_subsystem( apName, ap );
- _autopilotNames.push_back( apName );
- }
-
- SGSubsystemGroup::init();
-}
-
-FGXMLAutopilot::FGXMLAutopilot() {
-}
-
-
-FGXMLAutopilot::~FGXMLAutopilot() {
-}
-
-
-/* read all /sim/systems/autopilot[n]/path properties, try to read the file specified therein
- * and configure/add the digital filters specified in that file
- */
-void FGXMLAutopilot::init()
-{
-}
-
-
-void FGXMLAutopilot::reinit() {
- components.clear();
- init();
-}
-
-
-void FGXMLAutopilot::bind() {
-}
-
-void FGXMLAutopilot::unbind() {
-}
-
-bool FGXMLAutopilot::build( SGPropertyNode_ptr config_props ) {
- SGPropertyNode *node;
- int i;
-
- int count = config_props->nChildren();
- for ( i = 0; i < count; ++i ) {
- node = config_props->getChild(i);
- string name = node->getName();
- // cout << name << endl;
- SG_LOG( SG_AUTOPILOT, SG_BULK, "adding autopilot component " << name );
- if ( name == "pid-controller" ) {
- components.push_back( new FGPIDController( node ) );
- } else if ( name == "pi-simple-controller" ) {
- components.push_back( new FGPISimpleController( node ) );
- } else if ( name == "predict-simple" ) {
- components.push_back( new FGPredictor( node ) );
- } else if ( name == "filter" ) {
- components.push_back( new FGDigitalFilter( node ) );
- } else if ( name == "logic" ) {
- components.push_back( new FGXMLAutoLogic( node ) );
- } else if ( name == "flipflop" ) {
- FGXMLAutoFlipFlop * flipFlop = NULL;
- SGPropertyNode_ptr typeNode = node->getNode( "type" );
- string val;
- if( typeNode != NULL ) val = typeNode->getStringValue();
- val = simgear::strutils::strip(val);
- if( val == "RS" ) flipFlop = new FGXMLAutoRSFlipFlop( node );
- else if( val == "SR" ) flipFlop = new FGXMLAutoRSFlipFlop( node, false );
- else if( val == "JK" ) flipFlop = new FGXMLAutoJKFlipFlop( node );
- else if( val == "T" ) flipFlop = new FGXMLAutoTFlipFlop( node );
- else if( val == "D" ) flipFlop = new FGXMLAutoDFlipFlop( node );
- if( flipFlop == NULL ) {
- SG_LOG(SG_AUTOPILOT, SG_ALERT, "can't create flipflop of type: " << val);
- return false;
- }
- components.push_back( flipFlop );
- } else {
- SG_LOG( SG_AUTOPILOT, SG_WARN, "Unknown top level autopilot section: " << name );
-// return false;
- }
- }
-
- return true;
-}
-
-/*
- * Update the list of autopilot components
- */
-
-void FGXMLAutopilot::update( double dt )
-{
- unsigned int i;
- for ( i = 0; i < components.size(); ++i ) {
- components[i]->update( dt );
- }
-}
-
+++ /dev/null
-// xmlauto.hxx - a more flexible, generic way to build autopilots
-//
-// Written by Curtis Olson, started January 2004.
-//
-// Copyright (C) 2004 Curtis L. Olson - http://www.flightgear.org/~curt
-//
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License as
-// published by the Free Software Foundation; either version 2 of the
-// License, or (at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful, but
-// WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-// General Public License for more details.
-//
-// You should have received a copy of the GNU General Public License
-// along with this program; if not, write to the Free Software
-// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
-//
-// $Id$
-
-
-#ifndef _XMLAUTO_HXX
-#define _XMLAUTO_HXX 1
-
-/*
-Torsten Dreyer:
-I'd like to deprecate the so called autopilot helper function
-(which is now part of the AutopilotGroup::update() method).
-Every property calculated within this helper can be calculated
-using filters defined in an external autopilot definition file.
-The complete set of calculations may be extracted into a separate
-configuration file. The current implementation is able to hande
-multiple config files and autopilots. The helper doubles code
-and writes properties used only by a few aircraft.
-*/
-// FIXME: this should go into config.h and/or configure
-// or removed along with the "helper" one day.
-#define XMLAUTO_USEHELPER
-
-#include <simgear/compiler.h>
-
-#include <string>
-#include <vector>
-#include <deque>
-
-#include <simgear/props/props.hxx>
-#include <simgear/structure/subsystem_mgr.hxx>
-
-template<typename T>
-class SGExpression;
-
-typedef SGExpression<double> SGExpressiond;
-class SGCondition;
-
-typedef SGSharedPtr<class FGXMLAutoInput> FGXMLAutoInput_ptr;
-typedef SGSharedPtr<class FGPeriodicalValue> FGPeriodicalValue_ptr;
-
-class FGPeriodicalValue : public SGReferenced {
-private:
- FGXMLAutoInput_ptr minPeriod; // The minimum value of the period
- FGXMLAutoInput_ptr maxPeriod; // The maximum value of the period
-public:
- FGPeriodicalValue( SGPropertyNode_ptr node );
- double normalize( double value );
-};
-
-class FGXMLAutoInput : public SGReferenced {
-private:
- double value; // The value as a constant or initializer for the property
- bool abs; // return absolute value
- SGPropertyNode_ptr property; // The name of the property containing the value
- FGXMLAutoInput_ptr offset; // A fixed offset, defaults to zero
- FGXMLAutoInput_ptr scale; // A constant scaling factor defaults to one
- FGXMLAutoInput_ptr min; // A minimum clip defaults to no clipping
- FGXMLAutoInput_ptr max; // A maximum clip defaults to no clipping
- FGPeriodicalValue_ptr periodical; //
- SGSharedPtr<const SGCondition> _condition;
- SGSharedPtr<SGExpressiond> _expression; ///< expression to generate the value
-
-public:
- FGXMLAutoInput( SGPropertyNode_ptr node = NULL, double value = 0.0, double offset = 0.0, double scale = 1.0 );
-
- void parse( SGPropertyNode_ptr, double value = 0.0, double offset = 0.0, double scale = 1.0 );
-
- /* get the value of this input, apply scale and offset and clipping */
- double get_value();
-
- /* set the input value after applying offset and scale */
- void set_value( double value );
-
- inline double get_scale() {
- return scale == NULL ? 1.0 : scale->get_value();
- }
-
- inline double get_offset() {
- return offset == NULL ? 0.0 : offset->get_value();
- }
-
- inline bool is_enabled() {
- return _condition == NULL ? true : _condition->test();
- }
-
-};
-
-class FGXMLAutoInputList : public std::vector<FGXMLAutoInput_ptr> {
- public:
- FGXMLAutoInput_ptr get_active() {
- for (iterator it = begin(); it != end(); ++it) {
- if( (*it)->is_enabled() )
- return *it;
- }
- return NULL;
- }
-
- double get_value( double def = 0.0 ) {
- FGXMLAutoInput_ptr input = get_active();
- return input == NULL ? def : input->get_value();
- }
-
-};
-
-/**
- * Base class for other autopilot components
- */
-
-class FGXMLAutoComponent : public SGReferenced {
-
-private:
- simgear::PropertyList output_list;
-
- SGSharedPtr<const SGCondition> _condition;
- SGPropertyNode_ptr enable_prop;
- std::string * enable_value;
-
- SGPropertyNode_ptr passive_mode;
- bool honor_passive;
-
- std::string name;
-
- /* Feed back output property to input property if
- this filter is disabled. This is for multi-stage
- filter where one filter sits behind a pid-controller
- to provide changes of the overall output to the pid-
- controller.
- feedback is disabled by default.
- */
- bool feedback_if_disabled;
- void do_feedback_if_disabled();
-
-protected:
- FGXMLAutoComponent();
-
- /*
- * Parse a component specification read from a property-list.
- * Calls the hook methods below to allow derived classes to
- * specialise parsing bevaiour.
- */
- void parseNode(SGPropertyNode* aNode);
-
- /**
- * Helper to parse the config section
- */
- void parseConfig(SGPropertyNode* aConfig);
-
- /*
- * Over-rideable hook method to allow derived classes to refine top-level
- * node parsing. Return true if the node was handled, false otherwise.
- */
- virtual bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
-
- /**
- * Over-rideable hook method to allow derived classes to refine config
- * node parsing. Return true if the node was handled, false otherwise.
- */
- virtual bool parseConfigHook(const std::string& aName, SGPropertyNode* aNode);
-
- FGXMLAutoInputList valueInput;
- FGXMLAutoInputList referenceInput;
- FGXMLAutoInputList uminInput;
- FGXMLAutoInputList umaxInput;
- FGPeriodicalValue_ptr periodical;
- // debug flag
- bool debug;
- bool enabled;
-
-
- inline void do_feedback() {
- if( feedback_if_disabled ) do_feedback_if_disabled();
- }
-
-public:
-
- virtual ~FGXMLAutoComponent();
-
- virtual void update (double dt)=0;
-
- inline const std::string& get_name() { return name; }
-
- double clamp( double value );
-
- inline void set_output_value( double value ) {
- // passive_ignore == true means that we go through all the
- // motions, but drive the outputs. This is analogous to
- // running the autopilot with the "servos" off. This is
- // helpful for things like flight directors which position
- // their vbars from the autopilot computations.
- if ( honor_passive && passive_mode->getBoolValue() ) return;
- for( simgear::PropertyList::iterator it = output_list.begin();
- it != output_list.end(); ++it)
- (*it)->setDoubleValue( clamp( value ) );
- }
-
- inline void set_output_value( bool value ) {
- // passive_ignore == true means that we go through all the
- // motions, but drive the outputs. This is analogous to
- // running the autopilot with the "servos" off. This is
- // helpful for things like flight directors which position
- // their vbars from the autopilot computations.
- if ( honor_passive && passive_mode->getBoolValue() ) return;
- for( simgear::PropertyList::iterator it = output_list.begin();
- it != output_list.end(); ++it)
- (*it)->setBoolValue( value ); // don't use clamp here, bool is clamped anyway
- }
-
- inline double get_output_value() {
- return output_list.size() == 0 ? 0.0 : clamp(output_list[0]->getDoubleValue());
- }
-
- inline bool get_bool_output_value() {
- return output_list.size() == 0 ? false : output_list[0]->getBoolValue();
- }
-
- /*
- Returns true if the enable-condition is true.
-
- If a <condition> is defined, this condition is evaluated,
- <prop> and <value> tags are ignored.
-
- If a <prop> is defined and no <value> is defined, the property
- named in the <prop></prop> tags is evaluated as boolean.
-
- If a <prop> is defined a a <value> is defined, the property named
- in <prop></prop> is compared (as a string) to the value defined in
- <value></value>
-
- Returns true, if neither <condition> nor <prop> exists
-
- Examples:
- Using a <condition> tag
- <enable>
- <condition>
- <!-- any legal condition goes here and is evaluated -->
- </condition>
- <prop>This is ignored</prop>
- <value>This is also ignored</value>
- </enable>
-
- Using a single boolean property
- <enable>
- <prop>/some/property/that/is/evaluated/as/boolean</prop>
- </enable>
-
- Using <prop> == <value>
- This is the old style behaviour
- <enable>
- <prop>/only/true/if/this/equals/true</prop>
- <value>true<value>
- </enable>
- */
- bool isPropertyEnabled();
-};
-
-typedef SGSharedPtr<FGXMLAutoComponent> FGXMLAutoComponent_ptr;
-
-
-/**
- * Roy Ovesen's PID controller
- */
-
-class FGPIDController : public FGXMLAutoComponent {
-
-private:
-
-
- // Configuration values
- FGXMLAutoInputList Kp; // proportional gain
- FGXMLAutoInputList Ti; // Integrator time (sec)
- FGXMLAutoInputList Td; // Derivator time (sec)
-
- double alpha; // low pass filter weighing factor (usually 0.1)
- double beta; // process value weighing factor for
- // calculating proportional error
- // (usually 1.0)
- double gamma; // process value weighing factor for
- // calculating derivative error
- // (usually 0.0)
-
- // Previous state tracking values
- double ep_n_1; // ep[n-1] (prop error)
- double edf_n_1; // edf[n-1] (derivative error)
- double edf_n_2; // edf[n-2] (derivative error)
- double u_n_1; // u[n-1] (output)
- double desiredTs; // desired sampling interval (sec)
- double elapsedTime; // elapsed time (sec)
-
-
-protected:
- bool parseConfigHook(const std::string& aName, SGPropertyNode* aNode);
-
-public:
-
- FGPIDController( SGPropertyNode *node );
- FGPIDController( SGPropertyNode *node, bool old );
- ~FGPIDController() {}
-
- void update( double dt );
-};
-
-
-/**
- * A simplistic P [ + I ] PID controller
- */
-
-class FGPISimpleController : public FGXMLAutoComponent {
-
-private:
-
- // proportional component data
- FGXMLAutoInputList Kp;
-
- // integral component data
- FGXMLAutoInputList Ki;
- double int_sum;
-
-protected:
- bool parseConfigHook(const std::string& aName, SGPropertyNode* aNode);
-
-public:
-
- FGPISimpleController( SGPropertyNode *node );
- ~FGPISimpleController() {}
-
- void update( double dt );
-};
-
-
-/**
- * Predictor - calculates value in x seconds future.
- */
-
-class FGPredictor : public FGXMLAutoComponent {
-
-private:
- double last_value;
- double average;
- FGXMLAutoInputList seconds;
- FGXMLAutoInputList filter_gain;
-
-protected:
- bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
-
-public:
- FGPredictor( SGPropertyNode *node );
- ~FGPredictor() {}
-
- void update( double dt );
-};
-
-
-/**
- * FGDigitalFilter - a selection of digital filters
- *
- * Exponential filter
- * Double exponential filter
- * Moving average filter
- * Noise spike filter
- *
- * All these filters are low-pass filters.
- *
- */
-
-class FGDigitalFilter : public FGXMLAutoComponent
-{
-private:
- FGXMLAutoInputList samplesInput; // Number of input samples to average
- FGXMLAutoInputList rateOfChangeInput; // The maximum allowable rate of change [1/s]
- FGXMLAutoInputList gainInput; //
- FGXMLAutoInputList TfInput; // Filter time [s]
-
- std::deque <double> output;
- std::deque <double> input;
- enum FilterTypes { exponential, doubleExponential, movingAverage,
- noiseSpike, gain, reciprocal, differential, none };
- FilterTypes filterType;
-
-protected:
- bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
-
-public:
- FGDigitalFilter(SGPropertyNode *node);
- ~FGDigitalFilter() {}
-
- void update(double dt);
-};
-
-class FGXMLAutoLogic : public FGXMLAutoComponent
-{
-private:
- SGSharedPtr<SGCondition> input;
- bool inverted;
-
-protected:
- bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
-
-public:
- FGXMLAutoLogic(SGPropertyNode * node );
- ~FGXMLAutoLogic() {}
-
- void update(double dt);
-};
-
-class FGXMLAutoFlipFlop : public FGXMLAutoComponent
-{
-private:
-protected:
- SGSharedPtr<SGCondition> sInput;
- SGSharedPtr<SGCondition> rInput;
- SGSharedPtr<SGCondition> clockInput;
- SGSharedPtr<SGCondition> jInput;
- SGSharedPtr<SGCondition> kInput;
- SGSharedPtr<SGCondition> dInput;
- bool inverted;
- FGXMLAutoFlipFlop( SGPropertyNode * node );
- bool parseNodeHook(const std::string& aName, SGPropertyNode* aNode);
-
- void update( double dt );
-
-public:
- ~FGXMLAutoFlipFlop() {};
- virtual bool getState( bool & result ) = 0;
-};
-
-/**
- * Model an autopilot system.
- *
- */
-
-class FGXMLAutopilotGroup : public SGSubsystemGroup
-{
-public:
- FGXMLAutopilotGroup();
- void init();
- void reinit();
- void update( double dt );
-private:
- std::vector<std::string> _autopilotNames;
-
-#ifdef XMLAUTO_USEHELPER
- double average;
- double v_last;
- double last_static_pressure;
-
- SGPropertyNode_ptr vel;
- SGPropertyNode_ptr lookahead5;
- SGPropertyNode_ptr lookahead10;
- SGPropertyNode_ptr bug;
- SGPropertyNode_ptr mag_hdg;
- SGPropertyNode_ptr bug_error;
- SGPropertyNode_ptr fdm_bug_error;
- SGPropertyNode_ptr target_true;
- SGPropertyNode_ptr true_hdg;
- SGPropertyNode_ptr true_error;
- SGPropertyNode_ptr target_nav1;
- SGPropertyNode_ptr true_nav1;
- SGPropertyNode_ptr true_track_nav1;
- SGPropertyNode_ptr nav1_course_error;
- SGPropertyNode_ptr nav1_selected_course;
- SGPropertyNode_ptr vs_fps;
- SGPropertyNode_ptr vs_fpm;
- SGPropertyNode_ptr static_pressure;
- SGPropertyNode_ptr pressure_rate;
- SGPropertyNode_ptr track;
-#endif
-};
-
-class FGXMLAutopilot : public SGSubsystem
-{
-
-public:
-
- FGXMLAutopilot();
- ~FGXMLAutopilot();
-
- void init();
- void reinit();
- void bind();
- void unbind();
- void update( double dt );
-
-
- bool build( SGPropertyNode_ptr );
-protected:
- typedef std::vector<FGXMLAutoComponent_ptr> comp_list;
-
-private:
- bool serviceable;
- comp_list components;
-
-};
-
-
-#endif // _XMLAUTO_HXX
#endif
#include <Autopilot/route_mgr.hxx>
-#include <Autopilot/xmlauto.hxx>
+#include <Autopilot/autopilotgroup.hxx>
#include <Cockpit/cockpit.hxx>
#include <Cockpit/panel.hxx>
projects / flightgear.git / commitdiff