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 "functor.hxx"
  30 #include <deque>
  31
  32 using std::map;
  33 using std::string;
  34 using std::endl;
  35 using std::cout;
  36
  37 namespace FGXMLAutopilot {
  38
  39 /**
  40  *
  41  *
  42  */
  43 class DigitalFilterImplementation : public SGReferenced {
  44 protected:
  45   virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode) = 0;
  46 public:
  47   virtual ~DigitalFilterImplementation() {}
  48   DigitalFilterImplementation();
  49   virtual void   initialize( double initvalue ) {}
  50   virtual double compute( double dt, double input ) = 0;
  51   bool configure( SGPropertyNode_ptr configNode );
  52
  53   void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
  54
  55 protected:
  56   DigitalFilter * _digitalFilter;
  57 };
  58
  59 /* --------------------------------------------------------------------------------- */
  60 /* --------------------------------------------------------------------------------- */
  61 class GainFilterImplementation : public DigitalFilterImplementation {
  62 protected:
  63   InputValueList _gainInput;
  64   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
  65 public:
  66   GainFilterImplementation() : _gainInput(1.0) {}
  67   double compute(  double dt, double input );
  68 };
  69
  70 class ReciprocalFilterImplementation : public GainFilterImplementation {
  71 public:
  72   double compute(  double dt, double input );
  73 };
  74
  75 class DerivativeFilterImplementation : public GainFilterImplementation {
  76   InputValueList _TfInput;
  77   double _input_1;
  78   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
  79 public:
  80   DerivativeFilterImplementation();
  81   double compute(  double dt, double input );
  82   virtual void initialize( double initvalue );
  83 };
  84
  85 class ExponentialFilterImplementation : public GainFilterImplementation {
  86 protected:
  87   InputValueList _TfInput;
  88   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
  89   bool _isSecondOrder;
  90   double _output_1, _output_2;
  91 public:
  92   ExponentialFilterImplementation();
  93   double compute(  double dt, double input );
  94   virtual void initialize( double initvalue );
  95 };
  96
  97 class MovingAverageFilterImplementation : public DigitalFilterImplementation {
  98 protected:
  99   InputValueList _samplesInput;
 100   double _output_1;
 101   std::deque <double> _inputQueue;
 102   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
 103 public:
 104   MovingAverageFilterImplementation();
 105   double compute(  double dt, double input );
 106   virtual void initialize( double initvalue );
 107 };
 108
 109 class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
 110 protected:
 111   double _output_1;
 112   InputValueList _rateOfChangeInput;
 113   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
 114 public:
 115   NoiseSpikeFilterImplementation();
 116   double compute(  double dt, double input );
 117   virtual void initialize( double initvalue );
 118 };
 119
 120 class RateLimitFilterImplementation : public DigitalFilterImplementation {
 121 protected:
 122   double _output_1;
 123   InputValueList _rateOfChangeMax;
 124   InputValueList _rateOfChangeMin ;
 125   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
 126 public:
 127   RateLimitFilterImplementation();
 128   double compute(  double dt, double input );
 129   virtual void initialize( double initvalue );
 130 };
 131
 132 class IntegratorFilterImplementation : public GainFilterImplementation {
 133 protected:
 134   InputValueList _TfInput;
 135   InputValueList _minInput;
 136   InputValueList _maxInput;
 137   double _input_1;
 138   double _output_1;
 139   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
 140 public:
 141   IntegratorFilterImplementation();
 142   double compute(  double dt, double input );
 143   virtual void initialize( double initvalue );
 144 };
 145
 146 class HighPassFilterImplementation : public GainFilterImplementation {
 147 protected:
 148   InputValueList _TfInput;
 149   double _input_1;
 150   double _output_1;
 151   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
 152 public:
 153   HighPassFilterImplementation();
 154   double compute(  double dt, double input );
 155   virtual void initialize( double initvalue );
 156 };
 157 class LeadLagFilterImplementation : public GainFilterImplementation {
 158 protected:
 159   InputValueList _TfaInput;
 160   InputValueList _TfbInput;
 161   double _input_1;
 162   double _output_1;
 163   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
 164 public:
 165   LeadLagFilterImplementation();
 166   double compute(  double dt, double input );
 167   virtual void initialize( double initvalue );
 168 };
 169 /* --------------------------------------------------------------------------------- */
 170 /* --------------------------------------------------------------------------------- */
 171
 172 } // namespace FGXMLAutopilot
 173
 174 using namespace FGXMLAutopilot;
 175
 176 /* --------------------------------------------------------------------------------- */
 177 /* --------------------------------------------------------------------------------- */
 178 DigitalFilterImplementation::DigitalFilterImplementation() :
 179   _digitalFilter(NULL)
 180 {
 181 }
 182
 183 bool DigitalFilterImplementation::configure( SGPropertyNode_ptr configNode )
 184 {
 185   for (int i = 0; i < configNode->nChildren(); ++i ) {
 186     SGPropertyNode_ptr prop;
 187
 188     SGPropertyNode_ptr child = configNode->getChild(i);
 189     string cname(child->getName());
 190
 191     if( configure( cname, child ) )
 192       continue;
 193
 194   } // for configNode->nChildren()
 195
 196   return true;
 197 }
 198
 199 /* --------------------------------------------------------------------------------- */
 200 /* --------------------------------------------------------------------------------- */
 201
 202 double GainFilterImplementation::compute(  double dt, double input )
 203 {
 204   return _gainInput.get_value() * input;
 205 }
 206
 207 bool GainFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 208 {
 209   if (nodeName == "gain" ) {
 210     _gainInput.push_back( new InputValue( configNode, 1 ) );
 211     return true;
 212   }
 213
 214   return false;
 215 }
 216
 217 /* --------------------------------------------------------------------------------- */
 218 /* --------------------------------------------------------------------------------- */
 219
 220 double ReciprocalFilterImplementation::compute(  double dt, double input )
 221 {
 222   if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
 223     return SGLimitsd::max();
 224
 225   return _gainInput.get_value() / input;
 226
 227 }
 228
 229 /* --------------------------------------------------------------------------------- */
 230 /* --------------------------------------------------------------------------------- */
 231
 232 DerivativeFilterImplementation::DerivativeFilterImplementation() :
 233   _input_1(0.0)
 234 {
 235 }
 236
 237 void DerivativeFilterImplementation::initialize( double initvalue )
 238 {
 239   _input_1 = initvalue;
 240 }
 241
 242
 243 bool DerivativeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 244 {
 245   if( GainFilterImplementation::configure( nodeName, configNode ) )
 246     return true;
 247
 248   if (nodeName == "filter-time" ) {
 249     _TfInput.push_back( new InputValue( configNode, 1 ) );
 250     return true;
 251   }
 252
 253   return false;
 254 }
 255
 256 double DerivativeFilterImplementation::compute(  double dt, double input )
 257 {
 258   double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
 259   _input_1 = input;
 260   return output;
 261
 262 }
 263
 264 /* --------------------------------------------------------------------------------- */
 265 /* --------------------------------------------------------------------------------- */
 266
 267 MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
 268   _output_1(0.0)
 269 {
 270 }
 271
 272 void MovingAverageFilterImplementation::initialize( double initvalue )
 273 {
 274   _output_1 = initvalue;
 275 }
 276
 277 double MovingAverageFilterImplementation::compute(  double dt, double input )
 278 {
 279   typedef std::deque<double>::size_type size_type;
 280   size_type samples = _samplesInput.get_value();
 281
 282   if (_inputQueue.size() != samples) {
 283     // For constant size filters, this code executed once.
 284     bool shrunk = _inputQueue.size() > samples;
 285     _inputQueue.resize(samples, _output_1);
 286     if (shrunk) {
 287       _output_1 = 0.0;
 288       for (size_type ii = 0; ii < samples; ii++)
 289       _output_1 += _inputQueue[ii];
 290       _output_1 /= samples;
 291     }
 292   }
 293
 294   double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
 295
 296   _output_1 = output_0;
 297   _inputQueue.pop_back();
 298   _inputQueue.push_front(input);
 299   return output_0;
 300 }
 301
 302 bool MovingAverageFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 303 {
 304   if (nodeName == "samples" ) {
 305     _samplesInput.push_back( new InputValue( configNode, 1 ) );
 306     return true;
 307   }
 308
 309   return false;
 310 }
 311
 312 /* --------------------------------------------------------------------------------- */
 313 /* --------------------------------------------------------------------------------- */
 314
 315 NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
 316   _output_1(0.0)
 317 {
 318 }
 319
 320 void NoiseSpikeFilterImplementation::initialize( double initvalue )
 321 {
 322   _output_1 = initvalue;
 323 }
 324
 325 double NoiseSpikeFilterImplementation::compute(  double dt, double input )
 326 {
 327   double delta = input - _output_1;
 328   if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
 329
 330   double maxChange = _rateOfChangeInput.get_value() * dt;
 331   const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
 332   if( periodical ) delta = periodical->normalizeSymmetric( delta );
 333
 334   if( fabs(delta) <= maxChange )
 335     return (_output_1 = input);
 336   else
 337     return (_output_1 = _output_1 + copysign( maxChange, delta ));
 338 }
 339
 340 bool NoiseSpikeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 341 {
 342   if (nodeName == "max-rate-of-change" ) {
 343     _rateOfChangeInput.push_back( new InputValue( configNode, 1 ) );
 344     return true;
 345   }
 346
 347   return false;
 348 }
 349
 350 /* --------------------------------------------------------------------------------- */
 351
 352 RateLimitFilterImplementation::RateLimitFilterImplementation() :
 353   _output_1(0.0)
 354 {
 355 }
 356
 357 void RateLimitFilterImplementation::initialize( double initvalue )
 358 {
 359   _output_1 = initvalue;
 360 }
 361
 362 double RateLimitFilterImplementation::compute(  double dt, double input )
 363 {
 364   double delta = input - _output_1;
 365   double output;
 366
 367   if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
 368
 369   double maxChange = _rateOfChangeMax.get_value() * dt;
 370   double minChange = _rateOfChangeMin.get_value() * dt;
 371 //  const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
 372 //  if( periodical ) delta = periodical->normalizeSymmetric( delta );
 373
 374   output = input;
 375   if(delta >= maxChange ) output = _output_1 + maxChange;
 376   if(delta <= minChange ) output = _output_1 + minChange;
 377   _output_1 = output;
 378
 379   return (output);
 380 }
 381
 382 bool RateLimitFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 383 {
 384   if (nodeName == "max-rate-of-change" ) {
 385     _rateOfChangeMax.push_back( new InputValue( configNode, 1 ) );
 386     return true;
 387   }
 388   if (nodeName == "min-rate-of-change" ) {
 389     _rateOfChangeMin.push_back( new InputValue( configNode, 1 ) );
 390     return true;
 391   }
 392
 393   return false;
 394 }
 395
 396 /* --------------------------------------------------------------------------------- */
 397 /* --------------------------------------------------------------------------------- */
 398
 399 ExponentialFilterImplementation::ExponentialFilterImplementation()
 400   : _isSecondOrder(false),
 401     _output_1(0.0),
 402     _output_2(0.0)
 403 {
 404 }
 405
 406 void ExponentialFilterImplementation::initialize( double initvalue )
 407 {
 408   _output_1 = _output_2 = initvalue;
 409 }
 410
 411 double ExponentialFilterImplementation::compute(  double dt, double input )
 412 {
 413   input = GainFilterImplementation::compute( dt, input );
 414   double tf = _TfInput.get_value();
 415
 416   double output_0;
 417
 418   // avoid negative filter times
 419   // and div by zero if -tf == dt
 420
 421   double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
 422
 423   if(_isSecondOrder) {
 424     output_0 = alpha * alpha * input +
 425                2 * (1 - alpha) * _output_1 -
 426               (1 - alpha) * (1 - alpha) * _output_2;
 427   } else {
 428     output_0 = alpha * input + (1 - alpha) * _output_1;
 429   }
 430   _output_2 = _output_1;
 431   return (_output_1 = output_0);
 432 }
 433
 434 bool ExponentialFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 435 {
 436   if( GainFilterImplementation::configure( nodeName, configNode ) )
 437     return true;
 438
 439   if (nodeName == "filter-time" ) {
 440     _TfInput.push_back( new InputValue( configNode, 1 ) );
 441     return true;
 442   }
 443
 444   if (nodeName == "type" ) {
 445     string type(configNode->getStringValue());
 446     _isSecondOrder = type == "double-exponential";
 447   }
 448
 449   return false;
 450 }
 451
 452 /* --------------------------------------------------------------------------------- */
 453
 454 IntegratorFilterImplementation::IntegratorFilterImplementation() :
 455   _input_1(0.0),
 456   _output_1(0.0)
 457 {
 458 }
 459
 460 void IntegratorFilterImplementation::initialize( double initvalue )
 461 {
 462   _input_1 = _output_1 = initvalue;
 463 }
 464
 465
 466 bool IntegratorFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 467 {
 468   if( GainFilterImplementation::configure( nodeName, configNode ) )
 469     return true;
 470   if (nodeName == "u_min" ) {
 471     _minInput.push_back( new InputValue( configNode, 1 ) );
 472     return true;
 473   }
 474   if (nodeName == "u_max" ) {
 475     _maxInput.push_back( new InputValue( configNode, 1 ) );
 476     return true;
 477   }
 478   return false;
 479 }
 480
 481 double IntegratorFilterImplementation::compute(  double dt, double input )
 482 {
 483   double output = _output_1 + input *  _gainInput.get_value() * dt;
 484   double u_min = _minInput.get_value();
 485   double u_max = _maxInput.get_value();
 486   if (output >= u_max) output = u_max; // clamping inside "::compute" prevents integrator wind-up
 487   if (output <= u_min) output = u_min;
 488   _input_1 = input;
 489   _output_1 = output;
 490   return output;
 491
 492 }
 493
 494 /* --------------------------------------------------------------------------------- */
 495
 496 HighPassFilterImplementation::HighPassFilterImplementation() :
 497   _input_1(0.0),
 498   _output_1(0.0)
 499
 500 {
 501 }
 502
 503 void HighPassFilterImplementation::initialize( double initvalue )
 504 {
 505   _input_1 = initvalue;
 506   _output_1 = initvalue;
 507 }
 508
 509 double HighPassFilterImplementation::compute(  double dt, double input )
 510 {
 511   input = GainFilterImplementation::compute( dt, input );
 512   double tf = _TfInput.get_value();
 513
 514   double output;
 515
 516   // avoid negative filter times
 517   // and div by zero if -tf == dt
 518
 519   double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
 520   output = (1 - alpha) * (input - _input_1 +  _output_1);
 521   _input_1 = input;
 522   _output_1 = output;
 523   return output;
 524 }
 525
 526 bool HighPassFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 527 {
 528   if( GainFilterImplementation::configure( nodeName, configNode ) )
 529     return true;
 530
 531   if (nodeName == "filter-time" ) {
 532     _TfInput.push_back( new InputValue( configNode, 1 ) );
 533     return true;
 534   }
 535
 536   return false;
 537 }
 538
 539 /* --------------------------------------------------------------------------------- */
 540
 541 LeadLagFilterImplementation::LeadLagFilterImplementation() :
 542   _input_1(0.0),
 543   _output_1(0.0)
 544
 545 {
 546 }
 547
 548 void LeadLagFilterImplementation::initialize( double initvalue )
 549 {
 550   _input_1 = initvalue;
 551   _output_1 = initvalue;
 552 }
 553
 554 double LeadLagFilterImplementation::compute(  double dt, double input )
 555 {
 556   input = GainFilterImplementation::compute( dt, input );
 557   double tfa = _TfaInput.get_value();
 558   double tfb = _TfbInput.get_value();
 559
 560   double output;
 561
 562   // avoid negative filter times
 563   // and div by zero if -tf == dt
 564
 565   double alpha = tfa > 0.0 ? 1 / ((tfa/dt) + 1) : 1.0;
 566   double beta = tfb > 0.0 ? 1 / ((tfb/dt) + 1) : 1.0;
 567   output = (1 - beta) * (input / (1 - alpha) - _input_1 + _output_1);
 568   _input_1 = input;
 569   _output_1 = output;
 570   return output;
 571 }
 572
 573 bool LeadLagFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 574 {
 575   if( GainFilterImplementation::configure( nodeName, configNode ) )
 576     return true;
 577
 578   if (nodeName == "filter-time-a" ) {
 579     _TfaInput.push_back( new InputValue( configNode, 1 ) );
 580     return true;
 581   }
 582   if (nodeName == "filter-time-b" ) {
 583     _TfbInput.push_back( new InputValue( configNode, 1 ) );
 584     return true;
 585   }
 586   return false;
 587 }
 588 /* --------------------------------------------------------------------------------- */
 589 /* Digital Filter Component Implementation                                           */
 590 /* --------------------------------------------------------------------------------- */
 591
 592 DigitalFilter::DigitalFilter() :
 593     AnalogComponent(),
 594     _initializeTo(INITIALIZE_INPUT)
 595 {
 596 }
 597
 598 DigitalFilter::~DigitalFilter()
 599 {
 600 }
 601
 602
 603 static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
 604
 605 bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configNode)
 606 {
 607   if( componentForge.empty() ) {
 608     componentForge["gain"] = new CreateAndConfigureFunctor<GainFilterImplementation,DigitalFilterImplementation>();
 609     componentForge["exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
 610     componentForge["double-exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
 611     componentForge["moving-average"] = new CreateAndConfigureFunctor<MovingAverageFilterImplementation,DigitalFilterImplementation>();
 612     componentForge["noise-spike"] = new CreateAndConfigureFunctor<NoiseSpikeFilterImplementation,DigitalFilterImplementation>();
 613     componentForge["rate-limit"] = new CreateAndConfigureFunctor<RateLimitFilterImplementation,DigitalFilterImplementation>();
 614     componentForge["reciprocal"] = new CreateAndConfigureFunctor<ReciprocalFilterImplementation,DigitalFilterImplementation>();
 615     componentForge["derivative"] = new CreateAndConfigureFunctor<DerivativeFilterImplementation,DigitalFilterImplementation>();
 616     componentForge["high-pass"] = new CreateAndConfigureFunctor<HighPassFilterImplementation,DigitalFilterImplementation>();
 617     componentForge["lead-lag"] = new CreateAndConfigureFunctor<LeadLagFilterImplementation,DigitalFilterImplementation>();
 618     componentForge["integrator"] = new CreateAndConfigureFunctor<IntegratorFilterImplementation,DigitalFilterImplementation>();
 619   }
 620
 621   SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ")" << endl );
 622   if( AnalogComponent::configure( nodeName, configNode ) )
 623     return true;
 624
 625   if (nodeName == "type" ) {
 626     string type( configNode->getStringValue() );
 627     if( componentForge.count(type) == 0 ) {
 628       SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled filter type <" << type << ">" << endl );
 629       return true;
 630     }
 631     _implementation = (*componentForge[type])( configNode->getParent() );
 632     _implementation->setDigitalFilter( this );
 633     return true;
 634   }
 635
 636   if( nodeName == "initialize-to" ) {
 637     string s( configNode->getStringValue() );
 638     if( s == "input" ) {
 639       _initializeTo = INITIALIZE_INPUT;
 640     } else if( s == "output" ) {
 641       _initializeTo = INITIALIZE_OUTPUT;
 642     } else if( s == "none" ) {
 643       _initializeTo = INITIALIZE_NONE;
 644     } else {
 645       SG_LOG( SG_AUTOPILOT, SG_WARN, "unhandled initialize-to value '" << s << "' ignored" );
 646     }
 647     return true;
 648   }
 649
 650   SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ") [unhandled]" << endl );
 651   return false; // not handled by us, let the base class try
 652 }
 653
 654 void DigitalFilter::update( bool firstTime, double dt)
 655 {
 656   if( _implementation == NULL ) return;
 657
 658   if( firstTime ) {
 659     switch( _initializeTo ) {
 660
 661       case INITIALIZE_INPUT:
 662         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
 663         _implementation->initialize( _valueInput.get_value() );
 664         break;
 665
 666       case INITIALIZE_OUTPUT:
 667         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
 668         _implementation->initialize( get_output_value() );
 669         break;
 670
 671       default:
 672         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
 673         break;
 674     }
 675   }
 676
 677   double input = _valueInput.get_value() - _referenceInput.get_value();
 678   double output = _implementation->compute( dt, input );
 679
 680   set_output_value( output );
 681
 682   if(_debug) {
 683     cout << "input:" << input
 684          << "\toutput:" << output << endl;
 685   }
 686 }