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 // This program is free software; you can redistribute it and/or
  10 // modify it under the terms of the GNU General Public License as
  11 // published by the Free Software Foundation; either version 2 of the
  12 // License, or (at your option) any later version.
  13 //
  14 // This program is distributed in the hope that it will be useful, but
  15 // WITHOUT ANY WARRANTY; without even the implied warranty of
  16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17 // General Public License for more details.
  18 //
  19 // You should have received a copy of the GNU General Public License
  20 // along with this program; if not, write to the Free Software
  21 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  22 //
  23
  24 #include "digitalfilter.hxx"
  25 #include "functor.hxx"
  26 #include <deque>
  27
  28 using std::map;
  29 using std::string;
  30 using std::endl;
  31 using std::cout;
  32
  33 namespace FGXMLAutopilot {
  34
  35 /**
  36  *
  37  *
  38  */
  39 class DigitalFilterImplementation : public SGReferenced {
  40 protected:
  41   virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode) = 0;
  42 public:
  43   virtual ~DigitalFilterImplementation() {}
  44   DigitalFilterImplementation();
  45   virtual void   initialize( double initvalue ) {}
  46   virtual double compute( double dt, double input ) = 0;
  47   bool configure( SGPropertyNode_ptr configNode );
  48
  49   void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
  50
  51 protected:
  52   DigitalFilter * _digitalFilter;
  53 };
  54
  55 /* --------------------------------------------------------------------------------- */
  56 /* --------------------------------------------------------------------------------- */
  57 class GainFilterImplementation : public DigitalFilterImplementation {
  58 protected:
  59   InputValueList _gainInput;
  60   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
  61 public:
  62   GainFilterImplementation() : _gainInput(1.0) {}
  63   double compute(  double dt, double input );
  64 };
  65
  66 class ReciprocalFilterImplementation : public GainFilterImplementation {
  67 public:
  68   double compute(  double dt, double input );
  69 };
  70
  71 class DerivativeFilterImplementation : public GainFilterImplementation {
  72   InputValueList _TfInput;
  73   double _input_1;
  74   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
  75 public:
  76   DerivativeFilterImplementation();
  77   double compute(  double dt, double input );
  78   virtual void initialize( double initvalue );
  79 };
  80
  81 class ExponentialFilterImplementation : public GainFilterImplementation {
  82 protected:
  83   InputValueList _TfInput;
  84   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
  85   bool _isSecondOrder;
  86   double output_1, output_2;
  87 public:
  88   ExponentialFilterImplementation();
  89   double compute(  double dt, double input );
  90   virtual void initialize( double initvalue );
  91 };
  92
  93 class MovingAverageFilterImplementation : public DigitalFilterImplementation {
  94 protected:
  95   InputValueList _samplesInput;
  96   double _output_1;
  97   std::deque <double> _inputQueue;
  98   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
  99 public:
 100   MovingAverageFilterImplementation();
 101   double compute(  double dt, double input );
 102   virtual void initialize( double initvalue );
 103 };
 104
 105 class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
 106 protected:
 107   double _output_1;
 108   InputValueList _rateOfChangeInput;
 109   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
 110 public:
 111   NoiseSpikeFilterImplementation();
 112   double compute(  double dt, double input );
 113   virtual void initialize( double initvalue );
 114 };
 115
 116 /* --------------------------------------------------------------------------------- */
 117 /* --------------------------------------------------------------------------------- */
 118
 119 } // namespace FGXMLAutopilot
 120
 121 using namespace FGXMLAutopilot;
 122
 123 /* --------------------------------------------------------------------------------- */
 124 /* --------------------------------------------------------------------------------- */
 125 DigitalFilterImplementation::DigitalFilterImplementation() :
 126   _digitalFilter(NULL)
 127 {
 128 }
 129
 130 bool DigitalFilterImplementation::configure( SGPropertyNode_ptr configNode )
 131 {
 132   for (int i = 0; i < configNode->nChildren(); ++i ) {
 133     SGPropertyNode_ptr prop;
 134
 135     SGPropertyNode_ptr child = configNode->getChild(i);
 136     string cname(child->getName());
 137
 138     if( configure( cname, child ) )
 139       continue;
 140
 141   } // for configNode->nChildren()
 142
 143   return true;
 144 }
 145
 146 /* --------------------------------------------------------------------------------- */
 147 /* --------------------------------------------------------------------------------- */
 148
 149 double GainFilterImplementation::compute(  double dt, double input )
 150 {
 151   return _gainInput.get_value() * input;
 152 }
 153
 154 bool GainFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 155 {
 156   if (nodeName == "gain" ) {
 157     _gainInput.push_back( new InputValue( configNode, 1 ) );
 158     return true;
 159   }
 160
 161   return false;
 162 }
 163
 164 /* --------------------------------------------------------------------------------- */
 165 /* --------------------------------------------------------------------------------- */
 166
 167 double ReciprocalFilterImplementation::compute(  double dt, double input )
 168 {
 169   if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
 170     return SGLimitsd::max();
 171
 172   return _gainInput.get_value() / input;
 173
 174 }
 175
 176 /* --------------------------------------------------------------------------------- */
 177 /* --------------------------------------------------------------------------------- */
 178
 179 DerivativeFilterImplementation::DerivativeFilterImplementation() :
 180   _input_1(0.0)
 181 {
 182 }
 183
 184 void DerivativeFilterImplementation::initialize( double initvalue )
 185 {
 186   _input_1 = initvalue;
 187 }
 188
 189
 190 bool DerivativeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 191 {
 192   if( GainFilterImplementation::configure( nodeName, configNode ) )
 193     return true;
 194
 195   if (nodeName == "filter-time" ) {
 196     _TfInput.push_back( new InputValue( configNode, 1 ) );
 197     return true;
 198   }
 199
 200   return false;
 201 }
 202
 203 double DerivativeFilterImplementation::compute(  double dt, double input )
 204 {
 205   double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
 206   _input_1 = input;
 207   return output;
 208
 209 }
 210
 211 /* --------------------------------------------------------------------------------- */
 212 /* --------------------------------------------------------------------------------- */
 213
 214 MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
 215   _output_1(0.0)
 216 {
 217 }
 218
 219 void MovingAverageFilterImplementation::initialize( double initvalue )
 220 {
 221   _output_1 = initvalue;
 222 }
 223
 224 double MovingAverageFilterImplementation::compute(  double dt, double input )
 225 {
 226   typedef std::deque<double>::size_type size_type;
 227   size_type samples = _samplesInput.get_value();
 228
 229   if (_inputQueue.size() != samples) {
 230     // For constant size filters, this code executed once.
 231     bool shrunk = _inputQueue.size() > samples;
 232     _inputQueue.resize(samples, _output_1);
 233     if (shrunk) {
 234       _output_1 = 0.0;
 235       for (size_type ii = 0; ii < samples; ii++)
 236       _output_1 += _inputQueue[ii];
 237       _output_1 /= samples;
 238     }
 239   }
 240
 241   double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
 242
 243   _output_1 = output_0;
 244   _inputQueue.pop_back();
 245   _inputQueue.push_front(input);
 246   return output_0;
 247 }
 248
 249 bool MovingAverageFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 250 {
 251   if (nodeName == "samples" ) {
 252     _samplesInput.push_back( new InputValue( configNode, 1 ) );
 253     return true;
 254   }
 255
 256   return false;
 257 }
 258
 259 /* --------------------------------------------------------------------------------- */
 260 /* --------------------------------------------------------------------------------- */
 261
 262 NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
 263   _output_1(0.0)
 264 {
 265 }
 266
 267 void NoiseSpikeFilterImplementation::initialize( double initvalue )
 268 {
 269   _output_1 = initvalue;
 270 }
 271
 272 double NoiseSpikeFilterImplementation::compute(  double dt, double input )
 273 {
 274   double delta = input - _output_1;
 275   if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
 276
 277   double maxChange = _rateOfChangeInput.get_value() * dt;
 278   const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
 279   if( periodical ) delta = periodical->normalizeSymmetric( delta );
 280
 281   if( fabs(delta) <= maxChange )
 282     return (_output_1 = input);
 283   else
 284     return (_output_1 = _output_1 + copysign( maxChange, delta ));
 285 }
 286
 287 bool NoiseSpikeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 288 {
 289   if (nodeName == "max-rate-of-change" ) {
 290     _rateOfChangeInput.push_back( new InputValue( configNode, 1 ) );
 291     return true;
 292   }
 293
 294   return false;
 295 }
 296
 297 /* --------------------------------------------------------------------------------- */
 298 /* --------------------------------------------------------------------------------- */
 299
 300 ExponentialFilterImplementation::ExponentialFilterImplementation()
 301   : _isSecondOrder(false),
 302     output_1(0.0),
 303     output_2(0.0)
 304 {
 305 }
 306
 307 void ExponentialFilterImplementation::initialize( double initvalue )
 308 {
 309   output_1 = output_2 = initvalue;
 310 }
 311
 312 double ExponentialFilterImplementation::compute(  double dt, double input )
 313 {
 314   input = GainFilterImplementation::compute( dt, input );
 315   double tf = _TfInput.get_value();
 316
 317   double output_0;
 318
 319   // avoid negative filter times
 320   // and div by zero if -tf == dt
 321
 322   double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
 323
 324   if(_isSecondOrder) {
 325     output_0 = alpha * alpha * input +
 326                2 * (1 - alpha) * output_1 -
 327               (1 - alpha) * (1 - alpha) * output_2;
 328   } else {
 329     output_0 = alpha * input + (1 - alpha) * output_1;
 330   }
 331   output_2 = output_1;
 332   return (output_1 = output_0);
 333 }
 334
 335 bool ExponentialFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 336 {
 337   if( GainFilterImplementation::configure( nodeName, configNode ) )
 338     return true;
 339
 340   if (nodeName == "filter-time" ) {
 341     _TfInput.push_back( new InputValue( configNode, 1 ) );
 342     return true;
 343   }
 344
 345   if (nodeName == "type" ) {
 346     string type(configNode->getStringValue());
 347     _isSecondOrder = type == "double-exponential";
 348   }
 349
 350   return false;
 351 }
 352
 353 /* --------------------------------------------------------------------------------- */
 354 /* Digital Filter Component Implementation                                           */
 355 /* --------------------------------------------------------------------------------- */
 356
 357 DigitalFilter::DigitalFilter() :
 358     AnalogComponent(),
 359     _initializeTo(INITIALIZE_INPUT)
 360 {
 361 }
 362
 363 DigitalFilter::~DigitalFilter()
 364 {
 365 }
 366
 367
 368 static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
 369
 370 bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configNode)
 371 {
 372   if( componentForge.empty() ) {
 373     componentForge["gain"] = new CreateAndConfigureFunctor<GainFilterImplementation,DigitalFilterImplementation>();
 374     componentForge["exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
 375     componentForge["double-exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
 376     componentForge["moving-average"] = new CreateAndConfigureFunctor<MovingAverageFilterImplementation,DigitalFilterImplementation>();
 377     componentForge["noise-spike"] = new CreateAndConfigureFunctor<NoiseSpikeFilterImplementation,DigitalFilterImplementation>();
 378     componentForge["reciprocal"] = new CreateAndConfigureFunctor<ReciprocalFilterImplementation,DigitalFilterImplementation>();
 379     componentForge["derivative"] = new CreateAndConfigureFunctor<DerivativeFilterImplementation,DigitalFilterImplementation>();
 380   }
 381
 382   SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ")" << endl );
 383   if( AnalogComponent::configure( nodeName, configNode ) )
 384     return true;
 385
 386   if (nodeName == "type" ) {
 387     string type( configNode->getStringValue() );
 388     if( componentForge.count(type) == 0 ) {
 389       SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled filter type <" << type << ">" << endl );
 390       return true;
 391     }
 392     _implementation = (*componentForge[type])( configNode->getParent() );
 393     _implementation->setDigitalFilter( this );
 394     return true;
 395   }
 396
 397   if( nodeName == "initialize-to" ) {
 398     string s( configNode->getStringValue() );
 399     if( s == "input" ) {
 400       _initializeTo = INITIALIZE_INPUT;
 401     } else if( s == "output" ) {
 402       _initializeTo = INITIALIZE_OUTPUT;
 403     } else if( s == "none" ) {
 404       _initializeTo = INITIALIZE_NONE;
 405     } else {
 406       SG_LOG( SG_AUTOPILOT, SG_WARN, "unhandled initialize-to value '" << s << "' ignored" );
 407     }
 408     return true;
 409   }
 410
 411   SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ") [unhandled]" << endl );
 412   return false; // not handled by us, let the base class try
 413 }
 414
 415 void DigitalFilter::update( bool firstTime, double dt)
 416 {
 417   if( _implementation == NULL ) return;
 418
 419   if( firstTime ) {
 420     switch( _initializeTo ) {
 421
 422       case INITIALIZE_INPUT:
 423         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
 424         _implementation->initialize( _valueInput.get_value() );
 425         break;
 426
 427       case INITIALIZE_OUTPUT:
 428         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
 429         _implementation->initialize( get_output_value() );
 430         break;
 431
 432       default:
 433         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
 434         break;
 435     }
 436   }
 437
 438   double input = _valueInput.get_value() - _referenceInput.get_value();
 439   double output = _implementation->compute( dt, input );
 440
 441   set_output_value( output );
 442
 443   if(_debug) {
 444     cout << "input:" << input
 445          << "\toutput:" << output << endl;
 446   }
 447 }