// Copyright (C) 2010 Torsten Dreyer - Torsten (at) t3r (dot) de
//
// Washout/high-pass filter, lead-lag filter and integrator added.
-// low-pass and lag aliases added to Exponential filter,
+// low-pass and lag aliases added to Exponential filter,
// rate-limit added. A J Teeder 2013
//
// This program is free software; you can redistribute it and/or
//
#include "digitalfilter.hxx"
-#include "functor.hxx"
#include <deque>
-using std::map;
-using std::string;
-using std::endl;
-using std::cout;
-
-namespace FGXMLAutopilot {
+namespace FGXMLAutopilot
+{
/**
*
*
*/
-class DigitalFilterImplementation : public SGReferenced {
-protected:
- virtual bool configure( SGPropertyNode& cfg_node,
- const std::string& cfg_name,
- SGPropertyNode& prop_root ) = 0;
-public:
- virtual ~DigitalFilterImplementation() {}
- DigitalFilterImplementation();
- virtual void initialize( double initvalue ) {}
- virtual double compute( double dt, double input ) = 0;
- bool configure( SGPropertyNode& cfg,
- SGPropertyNode& prop_root );
-
- void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
-
-protected:
- DigitalFilter * _digitalFilter;
+class DigitalFilterImplementation:
+ public SGReferenced
+{
+ public:
+ virtual ~DigitalFilterImplementation() {}
+ DigitalFilterImplementation();
+ virtual void initialize( double initvalue ) {}
+ virtual double compute( double dt, double input ) = 0;
+ virtual bool configure( SGPropertyNode& cfg_node,
+ const std::string& cfg_name,
+ SGPropertyNode& prop_root ) = 0;
+
+ void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
+
+ protected:
+ DigitalFilter * _digitalFilter;
};
/* --------------------------------------------------------------------------------- */
virtual void initialize( double initvalue );
};
+// integrates x" + ax' + bx + c = 0
+class DampedOscillationFilterImplementation : public GainFilterImplementation {
+protected:
+ InputValueList _aInput;
+ InputValueList _bInput;
+ InputValueList _cInput;
+ double _x2;
+ double _x1;
+ double _x0;
+ bool configure( SGPropertyNode& cfg_node,
+ const std::string& cfg_name,
+ SGPropertyNode& prop_root );
+public:
+ DampedOscillationFilterImplementation();
+ double compute( double dt, double input );
+ virtual void initialize( double initvalue );
+};
+
class HighPassFilterImplementation : public GainFilterImplementation {
protected:
InputValueList _TfInput;
using namespace FGXMLAutopilot;
-/* --------------------------------------------------------------------------------- */
-/* --------------------------------------------------------------------------------- */
+//------------------------------------------------------------------------------
DigitalFilterImplementation::DigitalFilterImplementation() :
_digitalFilter(NULL)
{
-}
-//------------------------------------------------------------------------------
-bool DigitalFilterImplementation::configure( SGPropertyNode& cfg,
- SGPropertyNode& prop_root )
-{
- for (int i = 0; i < cfg.nChildren(); ++i )
- {
- SGPropertyNode_ptr child = cfg.getChild(i);
-
- if( configure(*child, child->getNameString(), prop_root) )
- continue;
- }
-
- return true;
}
-/* --------------------------------------------------------------------------------- */
-/* --------------------------------------------------------------------------------- */
-
+//------------------------------------------------------------------------------
double GainFilterImplementation::compute( double dt, double input )
{
return _gainInput.get_value() * input;
const std::string& cfg_name,
SGPropertyNode& prop_root )
{
+// std::cout << "RateLimitFilterImplementation " << cfg_name << std::endl;
if (cfg_name == "max-rate-of-change" ) {
_rateOfChangeMax.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
double output_0;
- // avoid negative filter times
+ // avoid negative filter times
// and div by zero if -tf == dt
double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
-
+
if(_isSecondOrder) {
- output_0 = alpha * alpha * input +
+ output_0 = alpha * alpha * input +
2 * (1 - alpha) * _output_1 -
(1 - alpha) * (1 - alpha) * _output_2;
} else {
}
if (cfg_name == "type" ) {
- string type(cfg_node.getStringValue());
+ std::string type(cfg_node.getStringValue());
_isSecondOrder = type == "double-exponential";
}
}
+/* --------------------------------------------------------------------------------- */
+DampedOscillationFilterImplementation::DampedOscillationFilterImplementation() :
+ _x0(0.0)
+{
+}
+
+void DampedOscillationFilterImplementation::initialize( double initvalue )
+{
+ _x2 = _x1 = _x0 = initvalue;
+}
+
+bool DampedOscillationFilterImplementation::configure( SGPropertyNode& cfg_node,
+ const std::string& cfg_name,
+ SGPropertyNode& prop_root )
+{
+ if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
+ return true;
+
+ if (cfg_name == "a" ) {
+ _aInput.push_back( new InputValue(prop_root, cfg_node, 1) );
+ return true;
+ }
+ if (cfg_name == "b" ) {
+ _bInput.push_back( new InputValue(prop_root, cfg_node, 1) );
+ return true;
+ }
+ if (cfg_name == "c" ) {
+ _cInput.push_back( new InputValue(prop_root, cfg_node, 1) );
+ return true;
+ }
+ return false;
+}
+
+double DampedOscillationFilterImplementation::compute( double dt, double input )
+{
+ if (fabs(input) > 1e-15) {
+ double dz = dt * input;
+ _x0 = _x1 - dz;
+ _x2 = _x1 + dz;
+ } else {
+ double a = _aInput.get_value();
+ double b = _bInput.get_value();
+ double c = _cInput.get_value();
+ _x0 = (_x1 * (2. + dt * (a - b * dt)) - _x2 - c * dt * dt) / (1. + a * dt);
+ _x2 = _x1;
+ _x1 = _x0;
+ }
+ return _x0;
+}
+
/* --------------------------------------------------------------------------------- */
HighPassFilterImplementation::HighPassFilterImplementation() :
double output;
- // avoid negative filter times
+ // avoid negative filter times
// and div by zero if -tf == dt
double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
_TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
-
+
return false;
}
double output;
- // avoid negative filter times
+ // avoid negative filter times
// and div by zero if -tf == dt
double alpha = tfa > 0.0 ? 1 / ((tfa/dt) + 1) : 1.0;
{
}
+//------------------------------------------------------------------------------
+template<class DigitalFilterType>
+DigitalFilterImplementation* digitalFilterFactory()
+{
+ return new DigitalFilterType();
+}
-static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
+typedef std::map<std::string, DigitalFilterImplementation*(*)()>
+DigitalFilterMap;
+static DigitalFilterMap componentForge;
//------------------------------------------------------------------------------
-bool DigitalFilter::configure( SGPropertyNode& cfg_node,
- const std::string& cfg_name,
- SGPropertyNode& prop_root )
+bool DigitalFilter::configure( SGPropertyNode& prop_root,
+ SGPropertyNode& cfg )
{
- 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["rate-limit"] = new CreateAndConfigureFunctor<RateLimitFilterImplementation,DigitalFilterImplementation>();
- componentForge["reciprocal"] = new CreateAndConfigureFunctor<ReciprocalFilterImplementation,DigitalFilterImplementation>();
- componentForge["derivative"] = new CreateAndConfigureFunctor<DerivativeFilterImplementation,DigitalFilterImplementation>();
- componentForge["high-pass"] = new CreateAndConfigureFunctor<HighPassFilterImplementation,DigitalFilterImplementation>();
- componentForge["lead-lag"] = new CreateAndConfigureFunctor<LeadLagFilterImplementation,DigitalFilterImplementation>();
- componentForge["integrator"] = new CreateAndConfigureFunctor<IntegratorFilterImplementation,DigitalFilterImplementation>();
+ if( componentForge.empty() )
+ {
+ componentForge["gain" ] = digitalFilterFactory<GainFilterImplementation>;
+ componentForge["exponential" ] = digitalFilterFactory<ExponentialFilterImplementation>;
+ componentForge["double-exponential" ] = digitalFilterFactory<ExponentialFilterImplementation>;
+ componentForge["moving-average" ] = digitalFilterFactory<MovingAverageFilterImplementation>;
+ componentForge["noise-spike" ] = digitalFilterFactory<NoiseSpikeFilterImplementation>;
+ componentForge["rate-limit" ] = digitalFilterFactory<RateLimitFilterImplementation>;
+ componentForge["reciprocal" ] = digitalFilterFactory<ReciprocalFilterImplementation>;
+ componentForge["derivative" ] = digitalFilterFactory<DerivativeFilterImplementation>;
+ componentForge["high-pass" ] = digitalFilterFactory<HighPassFilterImplementation>;
+ componentForge["lead-lag" ] = digitalFilterFactory<LeadLagFilterImplementation>;
+ componentForge["integrator" ] = digitalFilterFactory<IntegratorFilterImplementation>;
+ componentForge["damped-oscillation" ] = digitalFilterFactory<DampedOscillationFilterImplementation>;
}
- SG_LOG(SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << cfg_name << ")");
+ const std::string type = cfg.getStringValue("type");
+ DigitalFilterMap::iterator component_factory = componentForge.find(type);
+ if( component_factory == componentForge.end() )
+ {
+ SG_LOG(SG_AUTOPILOT, SG_WARN, "unhandled filter type '" << type << "'");
+ return false;
+ }
- if( AnalogComponent::configure(cfg_node, cfg_name, prop_root) )
- return true;
+ _implementation = (*component_factory->second)();
+ _implementation->setDigitalFilter( this );
- if (cfg_name == "type" ) {
- string type( cfg_node.getStringValue() );
- if( componentForge.count(type) == 0 ) {
- SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled filter type <" << type << ">" << endl );
- return true;
- }
- _implementation = (*componentForge[type])(*cfg_node.getParent(), prop_root);
- _implementation->setDigitalFilter( this );
- return true;
+ for( int i = 0; i < cfg.nChildren(); ++i )
+ {
+ SGPropertyNode_ptr child = cfg.getChild(i);
+ std::string cname(child->getName());
+
+ if( !_implementation->configure(*child, cname, prop_root)
+ && !configure(*child, cname, prop_root)
+ && cname != "type"
+ && cname != "params" ) // 'params' is usually used to specify parameters
+ // in PropertList files.
+ SG_LOG
+ (
+ SG_AUTOPILOT,
+ SG_WARN,
+ "DigitalFilter: unknown config node: " << cname
+ );
}
- if( cfg_name == "initialize-to" ) {
- string s( cfg_node.getStringValue() );
- if( s == "input" ) {
+ return true;
+}
+
+//------------------------------------------------------------------------------
+bool DigitalFilter::configure( SGPropertyNode& cfg_node,
+ const std::string& cfg_name,
+ SGPropertyNode& prop_root )
+{
+ if( cfg_name == "initialize-to" )
+ {
+ std::string s( cfg_node.getStringValue() );
+ if( s == "input" )
_initializeTo = INITIALIZE_INPUT;
- } else if( s == "output" ) {
+ else if( s == "output" )
_initializeTo = INITIALIZE_OUTPUT;
- } else if( s == "none" ) {
+ else if( s == "none" )
_initializeTo = INITIALIZE_NONE;
- } else {
- SG_LOG( SG_AUTOPILOT, SG_WARN, "unhandled initialize-to value '" << s << "' ignored" );
- }
+ else
+ SG_LOG
+ (
+ SG_AUTOPILOT,
+ SG_WARN, "DigitalFilter: initialize-to (" << s << ") ignored"
+ );
+
return true;
}
- SG_LOG
- (
- SG_AUTOPILOT,
- SG_BULK,
- "DigitalFilter::configure(" << cfg_name << ") [unhandled]"
- );
- return false; // not handled by us, let the base class try
+ return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
}
+//------------------------------------------------------------------------------
void DigitalFilter::update( bool firstTime, double dt)
{
if( _implementation == NULL ) return;
set_output_value( output );
if(_debug) {
- cout << "input:" << input
- << "\toutput:" << output << endl;
+ std::cout << "input:" << input
+ << "\toutput:" << output << std::endl;
}
}
projects / flightgear.git / blobdiff