src/Autopilot/digitalfilter.cxx

   1 // digitalfilter.cxx - a selection of digital filters
   2 //
   3 // Written by Torsten Dreyer
   4 // Based heavily on work created by Curtis Olson, started January 2004.
   5 //
   6 // Copyright (C) 2004  Curtis L. Olson  - http://www.flightgear.org/~curt
   7 // Copyright (C) 2010  Torsten Dreyer - Torsten (at) t3r (dot) de
   8 //
   9 // Washout/high-pass filter, lead-lag filter and integrator added.
  10 // low-pass and lag aliases added to Exponential filter,
  11 // rate-limit added.   A J Teeder 2013
  12 //
  13 // This program is free software; you can redistribute it and/or
  14 // modify it under the terms of the GNU General Public License as
  15 // published by the Free Software Foundation; either version 2 of the
  16 // License, or (at your option) any later version.
  17 //
  18 // This program is distributed in the hope that it will be useful, but
  19 // WITHOUT ANY WARRANTY; without even the implied warranty of
  20 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  21 // General Public License for more details.
  22 //
  23 // You should have received a copy of the GNU General Public License
  24 // along with this program; if not, write to the Free Software
  25 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  26 //
  27
  28 #include "digitalfilter.hxx"
  29 #include <deque>
  30
  31 namespace FGXMLAutopilot
  32 {
  33
  34 /**
  35  *
  36  *
  37  */
  38 class DigitalFilterImplementation:
  39   public SGReferenced
  40 {
  41   public:
  42     virtual ~DigitalFilterImplementation() {}
  43     DigitalFilterImplementation();
  44     virtual void   initialize( double initvalue ) {}
  45     virtual double compute( double dt, double input ) = 0;
  46     virtual bool configure( SGPropertyNode& cfg_node,
  47                             const std::string& cfg_name,
  48                             SGPropertyNode& prop_root ) = 0;
  49
  50     void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
  51
  52   protected:
  53     DigitalFilter * _digitalFilter;
  54 };
  55
  56 /* --------------------------------------------------------------------------------- */
  57 /* --------------------------------------------------------------------------------- */
  58 class GainFilterImplementation : public DigitalFilterImplementation {
  59 protected:
  60   InputValueList _gainInput;
  61   bool configure( SGPropertyNode& cfg_node,
  62                   const std::string& cfg_name,
  63                   SGPropertyNode& prop_root );
  64 public:
  65   GainFilterImplementation() : _gainInput(1.0) {}
  66   double compute(  double dt, double input );
  67 };
  68
  69 class ReciprocalFilterImplementation : public GainFilterImplementation {
  70 public:
  71   double compute(  double dt, double input );
  72 };
  73
  74 class DerivativeFilterImplementation : public GainFilterImplementation {
  75   InputValueList _TfInput;
  76   double _input_1;
  77   bool configure( SGPropertyNode& cfg_node,
  78                   const std::string& cfg_name,
  79                   SGPropertyNode& prop_root );
  80 public:
  81   DerivativeFilterImplementation();
  82   double compute(  double dt, double input );
  83   virtual void initialize( double initvalue );
  84 };
  85
  86 class ExponentialFilterImplementation : public GainFilterImplementation {
  87 protected:
  88   InputValueList _TfInput;
  89   bool configure( SGPropertyNode& cfg_node,
  90                   const std::string& cfg_name,
  91                   SGPropertyNode& prop_root );
  92   bool _isSecondOrder;
  93   double _output_1, _output_2;
  94 public:
  95   ExponentialFilterImplementation();
  96   double compute(  double dt, double input );
  97   virtual void initialize( double initvalue );
  98 };
  99
 100 class MovingAverageFilterImplementation : public DigitalFilterImplementation {
 101 protected:
 102   InputValueList _samplesInput;
 103   double _output_1;
 104   std::deque <double> _inputQueue;
 105   bool configure( SGPropertyNode& cfg_node,
 106                   const std::string& cfg_name,
 107                   SGPropertyNode& prop_root );
 108 public:
 109   MovingAverageFilterImplementation();
 110   double compute(  double dt, double input );
 111   virtual void initialize( double initvalue );
 112 };
 113
 114 class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
 115 protected:
 116   double _output_1;
 117   InputValueList _rateOfChangeInput;
 118   bool configure( SGPropertyNode& cfg_node,
 119                   const std::string& cfg_name,
 120                   SGPropertyNode& prop_root );
 121 public:
 122   NoiseSpikeFilterImplementation();
 123   double compute(  double dt, double input );
 124   virtual void initialize( double initvalue );
 125 };
 126
 127 class RateLimitFilterImplementation : public DigitalFilterImplementation {
 128 protected:
 129   double _output_1;
 130   InputValueList _rateOfChangeMax;
 131   InputValueList _rateOfChangeMin ;
 132   bool configure( SGPropertyNode& cfg_node,
 133                   const std::string& cfg_name,
 134                   SGPropertyNode& prop_root );
 135 public:
 136   RateLimitFilterImplementation();
 137   double compute(  double dt, double input );
 138   virtual void initialize( double initvalue );
 139 };
 140
 141 class IntegratorFilterImplementation : public GainFilterImplementation {
 142 protected:
 143   InputValueList _TfInput;
 144   InputValueList _minInput;
 145   InputValueList _maxInput;
 146   double _input_1;
 147   double _output_1;
 148   bool configure( SGPropertyNode& cfg_node,
 149                   const std::string& cfg_name,
 150                   SGPropertyNode& prop_root );
 151 public:
 152   IntegratorFilterImplementation();
 153   double compute(  double dt, double input );
 154   virtual void initialize( double initvalue );
 155 };
 156
 157 class HighPassFilterImplementation : public GainFilterImplementation {
 158 protected:
 159   InputValueList _TfInput;
 160   double _input_1;
 161   double _output_1;
 162   bool configure( SGPropertyNode& cfg_node,
 163                   const std::string& cfg_name,
 164                   SGPropertyNode& prop_root );
 165 public:
 166   HighPassFilterImplementation();
 167   double compute(  double dt, double input );
 168   virtual void initialize( double initvalue );
 169 };
 170 class LeadLagFilterImplementation : public GainFilterImplementation {
 171 protected:
 172   InputValueList _TfaInput;
 173   InputValueList _TfbInput;
 174   double _input_1;
 175   double _output_1;
 176   bool configure( SGPropertyNode& cfg_node,
 177                   const std::string& cfg_name,
 178                   SGPropertyNode& prop_root );
 179 public:
 180   LeadLagFilterImplementation();
 181   double compute(  double dt, double input );
 182   virtual void initialize( double initvalue );
 183 };
 184 /* --------------------------------------------------------------------------------- */
 185 /* --------------------------------------------------------------------------------- */
 186
 187 } // namespace FGXMLAutopilot
 188
 189 using namespace FGXMLAutopilot;
 190
 191 //------------------------------------------------------------------------------
 192 DigitalFilterImplementation::DigitalFilterImplementation() :
 193   _digitalFilter(NULL)
 194 {
 195
 196 }
 197
 198 //------------------------------------------------------------------------------
 199 double GainFilterImplementation::compute(  double dt, double input )
 200 {
 201   return _gainInput.get_value() * input;
 202 }
 203
 204 bool GainFilterImplementation::configure( SGPropertyNode& cfg_node,
 205                                           const std::string& cfg_name,
 206                                           SGPropertyNode& prop_root )
 207 {
 208   if (cfg_name == "gain" ) {
 209     _gainInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 210     return true;
 211   }
 212
 213   return false;
 214 }
 215
 216 /* --------------------------------------------------------------------------------- */
 217 /* --------------------------------------------------------------------------------- */
 218
 219 double ReciprocalFilterImplementation::compute(  double dt, double input )
 220 {
 221   if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
 222     return SGLimitsd::max();
 223
 224   return _gainInput.get_value() / input;
 225
 226 }
 227
 228 /* --------------------------------------------------------------------------------- */
 229 /* --------------------------------------------------------------------------------- */
 230
 231 DerivativeFilterImplementation::DerivativeFilterImplementation() :
 232   _input_1(0.0)
 233 {
 234 }
 235
 236 void DerivativeFilterImplementation::initialize( double initvalue )
 237 {
 238   _input_1 = initvalue;
 239 }
 240
 241 //------------------------------------------------------------------------------
 242 bool DerivativeFilterImplementation::configure( SGPropertyNode& cfg_node,
 243                                                 const std::string& cfg_name,
 244                                                 SGPropertyNode& prop_root )
 245 {
 246   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 247     return true;
 248
 249   if (cfg_name == "filter-time" ) {
 250     _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 251     return true;
 252   }
 253
 254   return false;
 255 }
 256
 257 double DerivativeFilterImplementation::compute(  double dt, double input )
 258 {
 259   double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
 260   _input_1 = input;
 261   return output;
 262
 263 }
 264
 265 /* --------------------------------------------------------------------------------- */
 266 /* --------------------------------------------------------------------------------- */
 267
 268 MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
 269   _output_1(0.0)
 270 {
 271 }
 272
 273 void MovingAverageFilterImplementation::initialize( double initvalue )
 274 {
 275   _output_1 = initvalue;
 276 }
 277
 278 double MovingAverageFilterImplementation::compute(  double dt, double input )
 279 {
 280   typedef std::deque<double>::size_type size_type;
 281   size_type samples = _samplesInput.get_value();
 282
 283   if (_inputQueue.size() != samples) {
 284     // For constant size filters, this code executed once.
 285     bool shrunk = _inputQueue.size() > samples;
 286     _inputQueue.resize(samples, _output_1);
 287     if (shrunk) {
 288       _output_1 = 0.0;
 289       for (size_type ii = 0; ii < samples; ii++)
 290       _output_1 += _inputQueue[ii];
 291       _output_1 /= samples;
 292     }
 293   }
 294
 295   double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
 296
 297   _output_1 = output_0;
 298   _inputQueue.pop_back();
 299   _inputQueue.push_front(input);
 300   return output_0;
 301 }
 302
 303 bool MovingAverageFilterImplementation::configure( SGPropertyNode& cfg_node,
 304                                                    const std::string& cfg_name,
 305                                                    SGPropertyNode& prop_root )
 306 {
 307   if (cfg_name == "samples" ) {
 308     _samplesInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 309     return true;
 310   }
 311
 312   return false;
 313 }
 314
 315 /* --------------------------------------------------------------------------------- */
 316 /* --------------------------------------------------------------------------------- */
 317
 318 NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
 319   _output_1(0.0)
 320 {
 321 }
 322
 323 void NoiseSpikeFilterImplementation::initialize( double initvalue )
 324 {
 325   _output_1 = initvalue;
 326 }
 327
 328 double NoiseSpikeFilterImplementation::compute(  double dt, double input )
 329 {
 330   double delta = input - _output_1;
 331   if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
 332
 333   double maxChange = _rateOfChangeInput.get_value() * dt;
 334   const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
 335   if( periodical ) delta = periodical->normalizeSymmetric( delta );
 336
 337   if( fabs(delta) <= maxChange )
 338     return (_output_1 = input);
 339   else
 340     return (_output_1 = _output_1 + copysign( maxChange, delta ));
 341 }
 342
 343 //------------------------------------------------------------------------------
 344 bool NoiseSpikeFilterImplementation::configure( SGPropertyNode& cfg_node,
 345                                                 const std::string& cfg_name,
 346                                                 SGPropertyNode& prop_root )
 347 {
 348   if (cfg_name == "max-rate-of-change" ) {
 349     _rateOfChangeInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 350     return true;
 351   }
 352
 353   return false;
 354 }
 355
 356 /* --------------------------------------------------------------------------------- */
 357
 358 RateLimitFilterImplementation::RateLimitFilterImplementation() :
 359   _output_1(0.0)
 360 {
 361 }
 362
 363 void RateLimitFilterImplementation::initialize( double initvalue )
 364 {
 365   _output_1 = initvalue;
 366 }
 367
 368 double RateLimitFilterImplementation::compute(  double dt, double input )
 369 {
 370   double delta = input - _output_1;
 371   double output;
 372
 373   if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
 374
 375   double maxChange = _rateOfChangeMax.get_value() * dt;
 376   double minChange = _rateOfChangeMin.get_value() * dt;
 377 //  const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
 378 //  if( periodical ) delta = periodical->normalizeSymmetric( delta );
 379
 380   output = input;
 381   if(delta >= maxChange ) output = _output_1 + maxChange;
 382   if(delta <= minChange ) output = _output_1 + minChange;
 383   _output_1 = output;
 384
 385   return (output);
 386 }
 387
 388 bool RateLimitFilterImplementation::configure( SGPropertyNode& cfg_node,
 389                                                const std::string& cfg_name,
 390                                                SGPropertyNode& prop_root )
 391 {
 392   std::cout << "RateLimitFilterImplementation " << cfg_name << std::endl;
 393   if (cfg_name == "max-rate-of-change" ) {
 394     _rateOfChangeMax.push_back( new InputValue(prop_root, cfg_node, 1) );
 395     return true;
 396   }
 397   if (cfg_name == "min-rate-of-change" ) {
 398     _rateOfChangeMin.push_back( new InputValue(prop_root, cfg_node, 1) );
 399     return true;
 400   }
 401
 402   return false;
 403 }
 404
 405 /* --------------------------------------------------------------------------------- */
 406 /* --------------------------------------------------------------------------------- */
 407
 408 ExponentialFilterImplementation::ExponentialFilterImplementation()
 409   : _isSecondOrder(false),
 410     _output_1(0.0),
 411     _output_2(0.0)
 412 {
 413 }
 414
 415 void ExponentialFilterImplementation::initialize( double initvalue )
 416 {
 417   _output_1 = _output_2 = initvalue;
 418 }
 419
 420 double ExponentialFilterImplementation::compute(  double dt, double input )
 421 {
 422   input = GainFilterImplementation::compute( dt, input );
 423   double tf = _TfInput.get_value();
 424
 425   double output_0;
 426
 427   // avoid negative filter times
 428   // and div by zero if -tf == dt
 429
 430   double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
 431
 432   if(_isSecondOrder) {
 433     output_0 = alpha * alpha * input +
 434                2 * (1 - alpha) * _output_1 -
 435               (1 - alpha) * (1 - alpha) * _output_2;
 436   } else {
 437     output_0 = alpha * input + (1 - alpha) * _output_1;
 438   }
 439   _output_2 = _output_1;
 440   return (_output_1 = output_0);
 441 }
 442
 443 //------------------------------------------------------------------------------
 444 bool ExponentialFilterImplementation::configure( SGPropertyNode& cfg_node,
 445                                                  const std::string& cfg_name,
 446                                                  SGPropertyNode& prop_root )
 447 {
 448   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 449     return true;
 450
 451   if (cfg_name == "filter-time" ) {
 452     _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 453     return true;
 454   }
 455
 456   if (cfg_name == "type" ) {
 457     std::string type(cfg_node.getStringValue());
 458     _isSecondOrder = type == "double-exponential";
 459   }
 460
 461   return false;
 462 }
 463
 464 /* --------------------------------------------------------------------------------- */
 465
 466 IntegratorFilterImplementation::IntegratorFilterImplementation() :
 467   _input_1(0.0),
 468   _output_1(0.0)
 469 {
 470 }
 471
 472 void IntegratorFilterImplementation::initialize( double initvalue )
 473 {
 474   _input_1 = _output_1 = initvalue;
 475 }
 476
 477 //------------------------------------------------------------------------------
 478 bool IntegratorFilterImplementation::configure( SGPropertyNode& cfg_node,
 479                                                 const std::string& cfg_name,
 480                                                 SGPropertyNode& prop_root )
 481 {
 482   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 483     return true;
 484
 485   if (cfg_name == "u_min" ) {
 486     _minInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 487     return true;
 488   }
 489   if (cfg_name == "u_max" ) {
 490     _maxInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 491     return true;
 492   }
 493   return false;
 494 }
 495
 496 double IntegratorFilterImplementation::compute(  double dt, double input )
 497 {
 498   double output = _output_1 + input *  _gainInput.get_value() * dt;
 499   double u_min = _minInput.get_value();
 500   double u_max = _maxInput.get_value();
 501   if (output >= u_max) output = u_max; // clamping inside "::compute" prevents integrator wind-up
 502   if (output <= u_min) output = u_min;
 503   _input_1 = input;
 504   _output_1 = output;
 505   return output;
 506
 507 }
 508
 509 /* --------------------------------------------------------------------------------- */
 510
 511 HighPassFilterImplementation::HighPassFilterImplementation() :
 512   _input_1(0.0),
 513   _output_1(0.0)
 514
 515 {
 516 }
 517
 518 void HighPassFilterImplementation::initialize( double initvalue )
 519 {
 520   _input_1 = initvalue;
 521   _output_1 = initvalue;
 522 }
 523
 524 double HighPassFilterImplementation::compute(  double dt, double input )
 525 {
 526   input = GainFilterImplementation::compute( dt, input );
 527   double tf = _TfInput.get_value();
 528
 529   double output;
 530
 531   // avoid negative filter times
 532   // and div by zero if -tf == dt
 533
 534   double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
 535   output = (1 - alpha) * (input - _input_1 +  _output_1);
 536   _input_1 = input;
 537   _output_1 = output;
 538   return output;
 539 }
 540
 541 //------------------------------------------------------------------------------
 542 bool HighPassFilterImplementation::configure( SGPropertyNode& cfg_node,
 543                                               const std::string& cfg_name,
 544                                               SGPropertyNode& prop_root )
 545 {
 546   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 547     return true;
 548
 549   if (cfg_name == "filter-time" ) {
 550     _TfInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 551     return true;
 552   }
 553
 554   return false;
 555 }
 556
 557 /* --------------------------------------------------------------------------------- */
 558
 559 LeadLagFilterImplementation::LeadLagFilterImplementation() :
 560   _input_1(0.0),
 561   _output_1(0.0)
 562
 563 {
 564 }
 565
 566 void LeadLagFilterImplementation::initialize( double initvalue )
 567 {
 568   _input_1 = initvalue;
 569   _output_1 = initvalue;
 570 }
 571
 572 double LeadLagFilterImplementation::compute(  double dt, double input )
 573 {
 574   input = GainFilterImplementation::compute( dt, input );
 575   double tfa = _TfaInput.get_value();
 576   double tfb = _TfbInput.get_value();
 577
 578   double output;
 579
 580   // avoid negative filter times
 581   // and div by zero if -tf == dt
 582
 583   double alpha = tfa > 0.0 ? 1 / ((tfa/dt) + 1) : 1.0;
 584   double beta = tfb > 0.0 ? 1 / ((tfb/dt) + 1) : 1.0;
 585   output = (1 - beta) * (input / (1 - alpha) - _input_1 + _output_1);
 586   _input_1 = input;
 587   _output_1 = output;
 588   return output;
 589 }
 590
 591 //------------------------------------------------------------------------------
 592 bool LeadLagFilterImplementation::configure( SGPropertyNode& cfg_node,
 593                                              const std::string& cfg_name,
 594                                              SGPropertyNode& prop_root )
 595 {
 596   if( GainFilterImplementation::configure(cfg_node, cfg_name, prop_root) )
 597     return true;
 598
 599   if (cfg_name == "filter-time-a" ) {
 600     _TfaInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 601     return true;
 602   }
 603   if (cfg_name == "filter-time-b" ) {
 604     _TfbInput.push_back( new InputValue(prop_root, cfg_node, 1) );
 605     return true;
 606   }
 607   return false;
 608 }
 609 /* -------------------------------------------------------------------------- */
 610 /* Digital Filter Component Implementation                                    */
 611 /* -------------------------------------------------------------------------- */
 612
 613 DigitalFilter::DigitalFilter() :
 614     AnalogComponent(),
 615     _initializeTo(INITIALIZE_INPUT)
 616 {
 617 }
 618
 619 DigitalFilter::~DigitalFilter()
 620 {
 621 }
 622
 623 //------------------------------------------------------------------------------
 624 template<class DigitalFilterType>
 625 DigitalFilterImplementation* digitalFilterFactory()
 626 {
 627   return new DigitalFilterType();
 628 }
 629
 630 typedef std::map<std::string, DigitalFilterImplementation*(*)()>
 631 DigitalFilterMap;
 632 static DigitalFilterMap componentForge;
 633
 634 //------------------------------------------------------------------------------
 635 bool DigitalFilter::configure( SGPropertyNode& prop_root,
 636                                SGPropertyNode& cfg )
 637 {
 638   if( componentForge.empty() )
 639   {
 640     componentForge["gain"               ] = digitalFilterFactory<GainFilterImplementation>;
 641     componentForge["exponential"        ] = digitalFilterFactory<ExponentialFilterImplementation>;
 642     componentForge["double-exponential" ] = digitalFilterFactory<ExponentialFilterImplementation>;
 643     componentForge["moving-average"     ] = digitalFilterFactory<MovingAverageFilterImplementation>;
 644     componentForge["noise-spike"        ] = digitalFilterFactory<NoiseSpikeFilterImplementation>;
 645     componentForge["rate-limit"         ] = digitalFilterFactory<RateLimitFilterImplementation>;
 646     componentForge["reciprocal"         ] = digitalFilterFactory<ReciprocalFilterImplementation>;
 647     componentForge["derivative"         ] = digitalFilterFactory<DerivativeFilterImplementation>;
 648     componentForge["high-pass"          ] = digitalFilterFactory<HighPassFilterImplementation>;
 649     componentForge["lead-lag"           ] = digitalFilterFactory<LeadLagFilterImplementation>;
 650     componentForge["integrator"         ] = digitalFilterFactory<IntegratorFilterImplementation>;
 651   }
 652
 653   const std::string type = cfg.getStringValue("type");
 654   DigitalFilterMap::iterator component_factory = componentForge.find(type);
 655   if( component_factory == componentForge.end() )
 656   {
 657     SG_LOG(SG_AUTOPILOT, SG_WARN, "unhandled filter type '" << type << "'");
 658     return false;
 659   }
 660
 661   _implementation = (*component_factory->second)();
 662   _implementation->setDigitalFilter( this );
 663
 664   for( int i = 0; i < cfg.nChildren(); ++i )
 665   {
 666     SGPropertyNode_ptr child = cfg.getChild(i);
 667     std::string cname(child->getName());
 668
 669     if(    !_implementation->configure(*child, cname, prop_root)
 670         && !configure(*child, cname, prop_root)
 671         && cname != "type"
 672         && cname != "params" ) // 'params' is usually used to specify parameters
 673                                // in PropertList files.
 674       SG_LOG
 675       (
 676         SG_AUTOPILOT,
 677         SG_WARN,
 678         "DigitalFilter: unknown config node: " << cname
 679       );
 680   }
 681
 682   return true;
 683 }
 684
 685 //------------------------------------------------------------------------------
 686 bool DigitalFilter::configure( SGPropertyNode& cfg_node,
 687                                const std::string& cfg_name,
 688                                SGPropertyNode& prop_root )
 689 {
 690   if( cfg_name == "initialize-to" )
 691   {
 692     std::string s( cfg_node.getStringValue() );
 693     if( s == "input" )
 694       _initializeTo = INITIALIZE_INPUT;
 695     else if( s == "output" )
 696       _initializeTo = INITIALIZE_OUTPUT;
 697     else if( s == "none" )
 698       _initializeTo = INITIALIZE_NONE;
 699     else
 700       SG_LOG
 701       (
 702         SG_AUTOPILOT,
 703         SG_WARN, "DigitalFilter: initialize-to (" << s << ") ignored"
 704       );
 705
 706     return true;
 707   }
 708
 709   return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
 710 }
 711
 712 //------------------------------------------------------------------------------
 713 void DigitalFilter::update( bool firstTime, double dt)
 714 {
 715   if( _implementation == NULL ) return;
 716
 717   if( firstTime ) {
 718     switch( _initializeTo ) {
 719
 720       case INITIALIZE_INPUT:
 721         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
 722         _implementation->initialize( _valueInput.get_value() );
 723         break;
 724
 725       case INITIALIZE_OUTPUT:
 726         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
 727         _implementation->initialize( get_output_value() );
 728         break;
 729
 730       default:
 731         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
 732         break;
 733     }
 734   }
 735
 736   double input = _valueInput.get_value() - _referenceInput.get_value();
 737   double output = _implementation->compute( dt, input );
 738
 739   set_output_value( output );
 740
 741   if(_debug) {
 742     std::cout << "input:" << input
 743               << "\toutput:" << output << std::endl;
 744   }
 745 }