#include "functor.hxx"
#include <deque>
+using std::map;
+using std::string;
+using std::endl;
+using std::cout;
+
namespace FGXMLAutopilot {
+/**
+ *
+ *
+ */
+class DigitalFilterImplementation : public SGReferenced {
+protected:
+ virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode) = 0;
+public:
+ virtual ~DigitalFilterImplementation() {}
+ DigitalFilterImplementation();
+ virtual void initialize( double output ) {}
+ virtual double compute( double dt, double input ) = 0;
+ bool configure( SGPropertyNode_ptr configNode );
+
+ void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
+
+protected:
+ DigitalFilter * _digitalFilter;
+};
+
/* --------------------------------------------------------------------------------- */
/* --------------------------------------------------------------------------------- */
class GainFilterImplementation : public DigitalFilterImplementation {
/* --------------------------------------------------------------------------------- */
/* --------------------------------------------------------------------------------- */
+DigitalFilterImplementation::DigitalFilterImplementation() :
+ _digitalFilter(NULL)
+{
+}
bool DigitalFilterImplementation::configure( SGPropertyNode_ptr configNode )
{
double ReciprocalFilterImplementation::compute( double dt, double input )
{
- if( input >= -SGLimitsd::min() || input <= SGLimitsd::min() )
+ if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
return SGLimitsd::max();
return _gainInput.get_value() / input;
double NoiseSpikeFilterImplementation::compute( double dt, double input )
{
- double maxChange = _rateOfChangeInput.get_value() * dt;
+ double delta = input - _output_1;
+ if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
- double output_0 = _output_1;
+ double maxChange = _rateOfChangeInput.get_value() * dt;
+ const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
+ if( periodical ) delta = periodical->normalizeSymmetric( delta );
- 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;
+ if( fabs(delta) <= maxChange )
+ return (_output_1 = input);
+ else
+ return (_output_1 = _output_1 + copysign( maxChange, delta ));
}
bool NoiseSpikeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
double ExponentialFilterImplementation::compute( double dt, double input )
{
input = GainFilterImplementation::compute( dt, input );
+ double tf = _TfInput.get_value();
double output_0;
- double alpha = 1 / ((_TfInput.get_value()/dt) + 1);
+
+ // 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 +
/* --------------------------------------------------------------------------------- */
DigitalFilter::DigitalFilter() :
- AnalogComponent()
+ AnalogComponent(),
+ _initializeTo(INITIALIZE_INPUT)
{
}
+DigitalFilter::~DigitalFilter()
+{
+}
+
+
static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configNode)
return true;
}
_implementation = (*componentForge[type])( configNode->getParent() );
+ _implementation->setDigitalFilter( this );
+ 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;
}
if( _implementation == NULL ) return;
if( firstTime ) {
- SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
- _implementation->initialize( _valueInput.get_value() );
+ 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();