#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 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 )
{
}
+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;
}