diff --git a/projects/VC90/FlightGear/FlightGear.vcproj b/projects/VC90/FlightGear/FlightGear.vcproj index aa9d5ccb2..7661650cf 100644 --- a/projects/VC90/FlightGear/FlightGear.vcproj +++ b/projects/VC90/FlightGear/FlightGear.vcproj @@ -694,11 +694,99 @@ > </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> diff --git a/src/Autopilot/Makefile.am b/src/Autopilot/Makefile.am index 05e489b21..fdd591884 100644 --- a/src/Autopilot/Makefile.am +++ b/src/Autopilot/Makefile.am @@ -2,6 +2,17 @@ noinst_LIBRARIES = libAutopilot.a 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 diff --git a/src/Autopilot/analogcomponent.cxx b/src/Autopilot/analogcomponent.cxx new file mode 100644 index 000000000..d32717d22 --- /dev/null +++ b/src/Autopilot/analogcomponent.cxx @@ -0,0 +1,113 @@ +// 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; +} diff --git a/src/Autopilot/analogcomponent.hxx b/src/Autopilot/analogcomponent.hxx new file mode 100644 index 000000000..ccbd099b6 --- /dev/null +++ b/src/Autopilot/analogcomponent.hxx @@ -0,0 +1,148 @@ +// 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 diff --git a/src/Autopilot/autopilot.cxx b/src/Autopilot/autopilot.cxx new file mode 100644 index 000000000..d37c38d7d --- /dev/null +++ b/src/Autopilot/autopilot.cxx @@ -0,0 +1,109 @@ +// 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 ); +} diff --git a/src/Autopilot/autopilot.hxx b/src/Autopilot/autopilot.hxx new file mode 100644 index 000000000..56bb6c32e --- /dev/null +++ b/src/Autopilot/autopilot.hxx @@ -0,0 +1,69 @@ +// 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 diff --git a/src/Autopilot/autopilotgroup.cxx b/src/Autopilot/autopilotgroup.cxx new file mode 100644 index 000000000..be1572a7a --- /dev/null +++ b/src/Autopilot/autopilotgroup.cxx @@ -0,0 +1,210 @@ +// 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(); +} + diff --git a/src/Autopilot/autopilotgroup.hxx b/src/Autopilot/autopilotgroup.hxx new file mode 100644 index 000000000..e27a6b16d --- /dev/null +++ b/src/Autopilot/autopilotgroup.hxx @@ -0,0 +1,49 @@ +// 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 diff --git a/src/Autopilot/component.cxx b/src/Autopilot/component.cxx new file mode 100644 index 000000000..bdf6ecc21 --- /dev/null +++ b/src/Autopilot/component.cxx @@ -0,0 +1,130 @@ +// 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 ); +} diff --git a/src/Autopilot/component.hxx b/src/Autopilot/component.hxx new file mode 100644 index 000000000..795f104a0 --- /dev/null +++ b/src/Autopilot/component.hxx @@ -0,0 +1,141 @@ +// 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 diff --git a/src/Autopilot/digitalcomponent.cxx b/src/Autopilot/digitalcomponent.cxx new file mode 100644 index 000000000..8fed619fb --- /dev/null +++ b/src/Autopilot/digitalcomponent.cxx @@ -0,0 +1,109 @@ +// 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; +} diff --git a/src/Autopilot/digitalcomponent.hxx b/src/Autopilot/digitalcomponent.hxx new file mode 100644 index 000000000..606dd5f8b --- /dev/null +++ b/src/Autopilot/digitalcomponent.hxx @@ -0,0 +1,130 @@ +// 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 + diff --git a/src/Autopilot/digitalfilter.cxx b/src/Autopilot/digitalfilter.cxx new file mode 100644 index 000000000..9edf53b82 --- /dev/null +++ b/src/Autopilot/digitalfilter.cxx @@ -0,0 +1,358 @@ +// 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; + } +} diff --git a/src/Autopilot/digitalfilter.hxx b/src/Autopilot/digitalfilter.hxx new file mode 100644 index 000000000..4bc37a904 --- /dev/null +++ b/src/Autopilot/digitalfilter.hxx @@ -0,0 +1,72 @@ +// 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 diff --git a/src/Autopilot/flipflop.cxx b/src/Autopilot/flipflop.cxx new file mode 100644 index 000000000..bfd9a3af7 --- /dev/null +++ b/src/Autopilot/flipflop.cxx @@ -0,0 +1,267 @@ +// 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; + } + } +} + + diff --git a/src/Autopilot/flipflop.hxx b/src/Autopilot/flipflop.hxx new file mode 100644 index 000000000..2c7970057 --- /dev/null +++ b/src/Autopilot/flipflop.hxx @@ -0,0 +1,58 @@ +// 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 diff --git a/src/Autopilot/functor.hxx b/src/Autopilot/functor.hxx new file mode 100644 index 000000000..b68b683c3 --- /dev/null +++ b/src/Autopilot/functor.hxx @@ -0,0 +1,50 @@ +// 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 diff --git a/src/Autopilot/inputvalue.cxx b/src/Autopilot/inputvalue.cxx new file mode 100644 index 000000000..fb80263c1 --- /dev/null +++ b/src/Autopilot/inputvalue.cxx @@ -0,0 +1,202 @@ +// 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; +} + diff --git a/src/Autopilot/inputvalue.hxx b/src/Autopilot/inputvalue.hxx new file mode 100644 index 000000000..40c0377ad --- /dev/null +++ b/src/Autopilot/inputvalue.hxx @@ -0,0 +1,122 @@ +// 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 diff --git a/src/Autopilot/logic.cxx b/src/Autopilot/logic.cxx new file mode 100644 index 000000000..0b5895511 --- /dev/null +++ b/src/Autopilot/logic.cxx @@ -0,0 +1,54 @@ +// 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() ); +} + diff --git a/src/Autopilot/logic.hxx b/src/Autopilot/logic.hxx new file mode 100644 index 000000000..ac4591458 --- /dev/null +++ b/src/Autopilot/logic.hxx @@ -0,0 +1,42 @@ +// 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 + diff --git a/src/Autopilot/pidcontroller.cxx b/src/Autopilot/pidcontroller.cxx new file mode 100644 index 000000000..1ad027460 --- /dev/null +++ b/src/Autopilot/pidcontroller.cxx @@ -0,0 +1,246 @@ +// 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; +} + diff --git a/src/Autopilot/pidcontroller.hxx b/src/Autopilot/pidcontroller.hxx new file mode 100644 index 000000000..da69a7389 --- /dev/null +++ b/src/Autopilot/pidcontroller.hxx @@ -0,0 +1,74 @@ +// 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 diff --git a/src/Autopilot/pisimplecontroller.cxx b/src/Autopilot/pisimplecontroller.cxx new file mode 100644 index 000000000..b19633adf --- /dev/null +++ b/src/Autopilot/pisimplecontroller.cxx @@ -0,0 +1,88 @@ +// 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; +} diff --git a/src/Autopilot/pisimplecontroller.hxx b/src/Autopilot/pisimplecontroller.hxx new file mode 100644 index 000000000..06dcb01a2 --- /dev/null +++ b/src/Autopilot/pisimplecontroller.hxx @@ -0,0 +1,64 @@ +// 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 diff --git a/src/Autopilot/predictor.cxx b/src/Autopilot/predictor.cxx new file mode 100644 index 000000000..fe16d14db --- /dev/null +++ b/src/Autopilot/predictor.cxx @@ -0,0 +1,81 @@ +// 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; +} + + diff --git a/src/Autopilot/predictor.hxx b/src/Autopilot/predictor.hxx new file mode 100644 index 000000000..5418b337f --- /dev/null +++ b/src/Autopilot/predictor.hxx @@ -0,0 +1,67 @@ +// 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 diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx deleted file mode 100644 index 39fced0a1..000000000 --- a/src/Autopilot/xmlauto.cxx +++ /dev/null @@ -1,1360 +0,0 @@ -// 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 ); - } -} - diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx deleted file mode 100644 index 4aba66fc8..000000000 --- a/src/Autopilot/xmlauto.hxx +++ /dev/null @@ -1,514 +0,0 @@ -// 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 diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index aee06568c..328c8913c 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -83,7 +83,7 @@ #endif #include <Autopilot/route_mgr.hxx> -#include <Autopilot/xmlauto.hxx> +#include <Autopilot/autopilotgroup.hxx> #include <Cockpit/cockpit.hxx> #include <Cockpit/panel.hxx>