fly/flightgear
1
0
Fork 0
Code Activity

New autopilot subsystem implementation.

Browse source 
Split the former single-file-implementation in xmlauto.?xx into multiple
files and use some OO techniques. Started with documentation to be used
with doxygen.
This commit is contained in:
Torsten Dreyer 2010-06-24 17:09:33 +02:00
parent 721ae71cd7
commit d558b52cb6
30 changed files with 3156 additions and 1878 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

92
projects/VC90/FlightGear/FlightGear.vcproj
View file

@ -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>

13
src/Autopilot/Makefile.am
View file

@ -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

113
src/Autopilot/analogcomponent.cxx Normal file
View file

@ -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;
}

148
src/Autopilot/analogcomponent.hxx Normal file
View file

@ -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 &lt;feedback-if-disabled&gt; 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 &lt;min&gt; and/or &lt;max&gt; 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 &lt;min&gt; and/or &lt;max&gt;.
*/
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

109
src/Autopilot/autopilot.cxx Normal file
View file

@ -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 );
}

69
src/Autopilot/autopilot.hxx Normal file
View file

@ -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

210
src/Autopilot/autopilotgroup.cxx Normal file
View file

@ -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();
}

49
src/Autopilot/autopilotgroup.hxx Normal file
View file

@ -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

130
src/Autopilot/component.cxx Normal file
View file

@ -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 );
}

141
src/Autopilot/component.hxx Normal file
View file

@ -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
* &lt;enable&gt; section
* @return true if the enable-condition is true.
*
* If a &lt;condition&gt; is defined, this condition is evaluated,
* &lt;prop&gt; and &lt;value&gt; tags are ignored.
*
* If a &lt;prop&gt; is defined and no &lt;value&gt; is defined, the property
* named in the &lt;prop&gt;&lt;prop&gt; tags is evaluated as boolean.
*
* If a &lt;prop&gt; is defined and a &lt;value&gt; is defined, the property named
* in &lt;prop&gt;&lt;/prop&gt; is compared (as a string) to the value defined in
* &lt;value&gt;&lt;/value&gt;
*
* Returns true, if neither &lt;condition&gt; nor &lt;prop&gt; exists
*/
bool isPropertyEnabled();
};
}
#endif // COMPONENT_HXX

109
src/Autopilot/digitalcomponent.cxx Normal file
View file

@ -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;
}

130
src/Autopilot/digitalcomponent.hxx Normal file
View file

@ -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

358
src/Autopilot/digitalfilter.cxx Normal file
View file

@ -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;
}
}

72
src/Autopilot/digitalfilter.hxx Normal file
View file

@ -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

267
src/Autopilot/flipflop.cxx Normal file
View file

@ -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;
}
}
}

58
src/Autopilot/flipflop.hxx Normal file
View file

@ -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

50
src/Autopilot/functor.hxx Normal file
View file

@ -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

202
src/Autopilot/inputvalue.cxx Normal file
View file

@ -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;
}

122
src/Autopilot/inputvalue.hxx Normal file
View file

@ -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

54
src/Autopilot/logic.cxx Normal file
View file

@ -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() );
}

42
src/Autopilot/logic.hxx Normal file
View file

@ -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 &lt;condition&gt; 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

246
src/Autopilot/pidcontroller.cxx Normal file
View file

@ -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;
}

74
src/Autopilot/pidcontroller.hxx Normal file
View file

@ -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

88
src/Autopilot/pisimplecontroller.cxx Normal file
View file

@ -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;
}

64
src/Autopilot/pisimplecontroller.hxx Normal file
View file

@ -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

81
src/Autopilot/predictor.cxx Normal file
View file

@ -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;
}

67
src/Autopilot/predictor.hxx Normal file
View file

@ -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

1360
src/Autopilot/xmlauto.cxx
View file

File diff suppressed because it is too large Load diff

514
src/Autopilot/xmlauto.hxx
View file

@ -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

2
src/Main/fg_init.cxx
View file

@ -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>