fly/flightgear
1
0
Fork 0
Code Activity

add DampedOsciFilter

Browse source
This commit is contained in:
Thomas Albrecht 2015-03-28 17:23:01 +11:00 • committed by Torsten Dreyer
parent 659c5b9676
commit e113586090
1 changed files with 76 additions and 7 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

83
src/Autopilot/digitalfilter.cxx
View file

@ -7,7 +7,7 @@
// 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
@ -154,6 +154,24 @@ public:
virtual void initialize( double initvalue );
};
// integrates x" + ax' + bx + c = 0
class DampedOsciFilterImplementation : 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:
DampedOsciFilterImplementation();
double compute( double dt, double input );
virtual void initialize( double initvalue );
};
class HighPassFilterImplementation : public GainFilterImplementation {
protected:
InputValueList _TfInput;
@ -424,13 +442,13 @@ double ExponentialFilterImplementation::compute( double dt, double input )
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 {
@ -506,6 +524,56 @@ double IntegratorFilterImplementation::compute( double dt, double input )
}
/* --------------------------------------------------------------------------------- */
DampedOsciFilterImplementation::DampedOsciFilterImplementation() :
_x0(0.0)
{
}
void DampedOsciFilterImplementation::initialize( double initvalue )
{
_x2 = _x1 = _x0 = initvalue;
}
bool DampedOsciFilterImplementation::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 DampedOsciFilterImplementation::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() :
@ -528,7 +596,7 @@ double HighPassFilterImplementation::compute( double dt, double input )
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;
@ -550,7 +618,7 @@ bool HighPassFilterImplementation::configure( SGPropertyNode& cfg_node,
_TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
return true;
}
return false;
}
@ -577,7 +645,7 @@ double LeadLagFilterImplementation::compute( double dt, double input )
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;
@ -648,6 +716,7 @@ bool DigitalFilter::configure( SGPropertyNode& prop_root,
componentForge["high-pass" ] = digitalFilterFactory<HighPassFilterImplementation>;
componentForge["lead-lag" ] = digitalFilterFactory<LeadLagFilterImplementation>;
componentForge["integrator" ] = digitalFilterFactory<IntegratorFilterImplementation>;
componentForge["damped-osci" ] = digitalFilterFactory<DampedOsciFilterImplementation>;
}
const std::string type = cfg.getStringValue("type");