c56aaa8771
The tag <initialize-to> can be used to control the value of the output when the component is first enabled. This controls initialization of the output property and the current value for internal computation Valid values are <initialize-to>input</initialize-to> set the output-property to the input value <initialize-to>output</initialize-to> set the output-property to the output value <initialize-to>none</initialize-to> ignore input and output value
389 lines
12 KiB
C++
389 lines
12 KiB
C++
// 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(),
|
|
_initializeTo(INITIALIZE_INPUT)
|
|
{
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
if( nodeName == "initialize-to" ) {
|
|
string s( configNode->getStringValue() );
|
|
if( s == "input" ) {
|
|
_initializeTo = INITIALIZE_INPUT;
|
|
} else if( s == "output" ) {
|
|
_initializeTo = INITIALIZE_OUTPUT;
|
|
} else if( s == "none" ) {
|
|
_initializeTo = INITIALIZE_NONE;
|
|
} else {
|
|
SG_LOG( SG_AUTOPILOT, SG_WARN, "unhandled initialize-to value '" << s << "' ignored" );
|
|
}
|
|
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 ) {
|
|
switch( _initializeTo ) {
|
|
|
|
case INITIALIZE_INPUT:
|
|
SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
|
|
_implementation->initialize( _valueInput.get_value() );
|
|
break;
|
|
|
|
case INITIALIZE_OUTPUT:
|
|
SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
|
|
_implementation->initialize( get_output_value() );
|
|
break;
|
|
|
|
default:
|
|
SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
|
|
break;
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|