+FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node):
+ FGXMLAutoComponent( node ),
+ filterType(none)
+{
+ int i;
+ for ( i = 0; i < node->nChildren(); ++i ) {
+ SGPropertyNode *child = node->getChild(i);
+ string cname = child->getName();
+ string cval = child->getStringValue();
+ if ( cname == "type" ) {
+ if ( cval == "exponential" ) {
+ filterType = exponential;
+ } else if (cval == "double-exponential") {
+ filterType = doubleExponential;
+ } else if (cval == "moving-average") {
+ filterType = movingAverage;
+ } else if (cval == "noise-spike") {
+ filterType = noiseSpike;
+ } else if (cval == "gain") {
+ filterType = gain;
+ } else if (cval == "reciprocal") {
+ filterType = reciprocal;
+ }
+ } else if ( cname == "filter-time" ) {
+ TfInput.push_back( new FGXMLAutoInput( child, 1.0 ) );
+ if( filterType == none ) filterType = exponential;
+ } else if ( cname == "samples" ) {
+ samplesInput.push_back( new FGXMLAutoInput( child, 1 ) );
+ if( filterType == none ) filterType = movingAverage;
+ } else if ( cname == "max-rate-of-change" ) {
+ rateOfChangeInput.push_back( new FGXMLAutoInput( child, 1 ) );
+ if( filterType == none ) filterType = noiseSpike;
+ } else if ( cname == "gain" ) {
+ gainInput.push_back( new FGXMLAutoInput( child, 1 ) );
+ if( filterType == none ) filterType = gain;
+ }
+ }
+
+ output.resize(2, 0.0);
+ input.resize(samplesInput.get_value() + 1, 0.0);
+}
+
+void FGDigitalFilter::update(double dt)
+{
+ if ( isPropertyEnabled() ) {
+
+ input.push_front(valueInput.get_value());
+ input.resize(samplesInput.get_value() + 1, 0.0);
+
+ if ( !enabled ) {
+ // first time being enabled, initialize output to the
+ // value of the output property to avoid bumping.
+ output.push_front(get_output_value());
+ }
+
+ enabled = true;
+ } else {
+ enabled = false;
+ do_feedback();
+ }
+
+ if ( enabled && dt > 0.0 ) {
+ /*
+ * Exponential filter
+ *
+ * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1]
+ *
+ */
+ if( debug ) cout << "Updating " << get_name()
+ << " dt " << dt << endl;
+
+ if (filterType == exponential)
+ {
+ double alpha = 1 / ((TfInput.get_value()/dt) + 1);
+ output.push_front(alpha * input[0] +
+ (1 - alpha) * output[0]);
+ }
+ else if (filterType == doubleExponential)
+ {
+ double alpha = 1 / ((TfInput.get_value()/dt) + 1);
+ output.push_front(alpha * alpha * input[0] +
+ 2 * (1 - alpha) * output[0] -
+ (1 - alpha) * (1 - alpha) * output[1]);
+ }
+ else if (filterType == movingAverage)
+ {
+ output.push_front(output[0] +
+ (input[0] - input.back()) / samplesInput.get_value());
+ }
+ else if (filterType == noiseSpike)
+ {
+ double maxChange = rateOfChangeInput.get_value() * dt;