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 output ) {}
  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 };
  79
  80 class ExponentialFilterImplementation : public GainFilterImplementation {
  81 protected:
  82   InputValueList _TfInput;
  83   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
  84   bool _isSecondOrder;
  85   double output_1, output_2;
  86 public:
  87   ExponentialFilterImplementation();
  88   double compute(  double dt, double input );
  89   virtual void initialize( double output );
  90 };
  91
  92 class MovingAverageFilterImplementation : public DigitalFilterImplementation {
  93 protected:
  94   InputValueList _samplesInput;
  95   double _output_1;
  96   std::deque <double> _inputQueue;
  97   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
  98 public:
  99   MovingAverageFilterImplementation();
 100   double compute(  double dt, double input );
 101   virtual void initialize( double output );
 102 };
 103
 104 class NoiseSpikeFilterImplementation : public DigitalFilterImplementation {
 105 protected:
 106   double _output_1;
 107   InputValueList _rateOfChangeInput;
 108   bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode );
 109 public:
 110   NoiseSpikeFilterImplementation();
 111   double compute(  double dt, double input );
 112   virtual void initialize( double output );
 113 };
 114
 115 /* --------------------------------------------------------------------------------- */
 116 /* --------------------------------------------------------------------------------- */
 117
 118 } // namespace FGXMLAutopilot
 119
 120 using namespace FGXMLAutopilot;
 121
 122 /* --------------------------------------------------------------------------------- */
 123 /* --------------------------------------------------------------------------------- */
 124 DigitalFilterImplementation::DigitalFilterImplementation() :
 125   _digitalFilter(NULL)
 126 {
 127 }
 128
 129 bool DigitalFilterImplementation::configure( SGPropertyNode_ptr configNode )
 130 {
 131   for (int i = 0; i < configNode->nChildren(); ++i ) {
 132     SGPropertyNode_ptr prop;
 133
 134     SGPropertyNode_ptr child = configNode->getChild(i);
 135     string cname(child->getName());
 136
 137     if( configure( cname, child ) )
 138       continue;
 139
 140   } // for configNode->nChildren()
 141
 142   return true;
 143 }
 144
 145 /* --------------------------------------------------------------------------------- */
 146 /* --------------------------------------------------------------------------------- */
 147
 148 double GainFilterImplementation::compute(  double dt, double input )
 149 {
 150   return _gainInput.get_value() * input;
 151 }
 152
 153 bool GainFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 154 {
 155   if (nodeName == "gain" ) {
 156     _gainInput.push_back( new InputValue( configNode, 1 ) );
 157     return true;
 158   }
 159
 160   return false;
 161 }
 162
 163 /* --------------------------------------------------------------------------------- */
 164 /* --------------------------------------------------------------------------------- */
 165
 166 double ReciprocalFilterImplementation::compute(  double dt, double input )
 167 {
 168   if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
 169     return SGLimitsd::max();
 170
 171   return _gainInput.get_value() / input;
 172
 173 }
 174
 175 /* --------------------------------------------------------------------------------- */
 176 /* --------------------------------------------------------------------------------- */
 177
 178 DerivativeFilterImplementation::DerivativeFilterImplementation() :
 179   _input_1(0.0)
 180 {
 181 }
 182
 183 bool DerivativeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 184 {
 185   if( GainFilterImplementation::configure( nodeName, configNode ) )
 186     return true;
 187
 188   if (nodeName == "filter-time" ) {
 189     _TfInput.push_back( new InputValue( configNode, 1 ) );
 190     return true;
 191   }
 192
 193   return false;
 194 }
 195
 196 double DerivativeFilterImplementation::compute(  double dt, double input )
 197 {
 198   double output = (input - _input_1) * _TfInput.get_value() * _gainInput.get_value() / dt;
 199   _input_1 = input;
 200   return output;
 201
 202 }
 203
 204 /* --------------------------------------------------------------------------------- */
 205 /* --------------------------------------------------------------------------------- */
 206
 207 MovingAverageFilterImplementation::MovingAverageFilterImplementation() :
 208   _output_1(0.0)
 209 {
 210 }
 211
 212 void MovingAverageFilterImplementation::initialize( double output )
 213 {
 214   _output_1 = output;
 215 }
 216
 217 double MovingAverageFilterImplementation::compute(  double dt, double input )
 218 {
 219   std::deque<double>::size_type samples = _samplesInput.get_value();
 220   _inputQueue.resize(samples+1, 0.0);
 221
 222   double output_0 = _output_1 + (input - _inputQueue.back()) / samples;
 223
 224   _output_1 = output_0;
 225   _inputQueue.push_front(input);
 226   return output_0;
 227 }
 228
 229 bool MovingAverageFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 230 {
 231   if (nodeName == "samples" ) {
 232     _samplesInput.push_back( new InputValue( configNode, 1 ) );
 233     return true;
 234   }
 235
 236   return false;
 237 }
 238
 239 /* --------------------------------------------------------------------------------- */
 240 /* --------------------------------------------------------------------------------- */
 241
 242 NoiseSpikeFilterImplementation::NoiseSpikeFilterImplementation() :
 243   _output_1(0.0)
 244 {
 245 }
 246
 247 void NoiseSpikeFilterImplementation::initialize( double output )
 248 {
 249   _output_1 = output;
 250 }
 251
 252 double NoiseSpikeFilterImplementation::compute(  double dt, double input )
 253 {
 254   double delta = input - _output_1;
 255   if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
 256
 257   double maxChange = _rateOfChangeInput.get_value() * dt;
 258   const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
 259   if( periodical ) delta = periodical->normalizeSymmetric( delta );
 260
 261   if( fabs(delta) <= maxChange )
 262     return (_output_1 = input);
 263   else
 264     return (_output_1 = _output_1 + copysign( maxChange, delta ));
 265 }
 266
 267 bool NoiseSpikeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 268 {
 269   if (nodeName == "max-rate-of-change" ) {
 270     _rateOfChangeInput.push_back( new InputValue( configNode, 1 ) );
 271     return true;
 272   }
 273
 274   return false;
 275 }
 276
 277 /* --------------------------------------------------------------------------------- */
 278 /* --------------------------------------------------------------------------------- */
 279
 280 ExponentialFilterImplementation::ExponentialFilterImplementation()
 281   : _isSecondOrder(false),
 282     output_1(0.0),
 283     output_2(0.0)
 284 {
 285 }
 286
 287 void ExponentialFilterImplementation::initialize( double output )
 288 {
 289   output_1 = output_2 = output;
 290 }
 291
 292 double ExponentialFilterImplementation::compute(  double dt, double input )
 293 {
 294   input = GainFilterImplementation::compute( dt, input );
 295   double tf = _TfInput.get_value();
 296
 297   double output_0;
 298
 299   // avoid negative filter times
 300   // and div by zero if -tf == dt
 301
 302   double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
 303
 304   if(_isSecondOrder) {
 305     output_0 = alpha * alpha * input +
 306                2 * (1 - alpha) * output_1 -
 307               (1 - alpha) * (1 - alpha) * output_2;
 308   } else {
 309     output_0 = alpha * input + (1 - alpha) * output_1;
 310   }
 311   output_2 = output_1;
 312   return (output_1 = output_0);
 313 }
 314
 315 bool ExponentialFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
 316 {
 317   if( GainFilterImplementation::configure( nodeName, configNode ) )
 318     return true;
 319
 320   if (nodeName == "filter-time" ) {
 321     _TfInput.push_back( new InputValue( configNode, 1 ) );
 322     return true;
 323   }
 324
 325   if (nodeName == "type" ) {
 326     string type(configNode->getStringValue());
 327     _isSecondOrder = type == "double-exponential";
 328   }
 329
 330   return false;
 331 }
 332
 333 /* --------------------------------------------------------------------------------- */
 334 /* Digital Filter Component Implementation                                           */
 335 /* --------------------------------------------------------------------------------- */
 336
 337 DigitalFilter::DigitalFilter() :
 338     AnalogComponent(),
 339     _initializeTo(INITIALIZE_INPUT)
 340 {
 341 }
 342
 343 DigitalFilter::~DigitalFilter()
 344 {
 345 }
 346
 347
 348 static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
 349
 350 bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configNode)
 351 {
 352   if( componentForge.empty() ) {
 353     componentForge["gain"] = new CreateAndConfigureFunctor<GainFilterImplementation,DigitalFilterImplementation>();
 354     componentForge["exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
 355     componentForge["double-exponential"] = new CreateAndConfigureFunctor<ExponentialFilterImplementation,DigitalFilterImplementation>();
 356     componentForge["moving-average"] = new CreateAndConfigureFunctor<MovingAverageFilterImplementation,DigitalFilterImplementation>();
 357     componentForge["noise-spike"] = new CreateAndConfigureFunctor<NoiseSpikeFilterImplementation,DigitalFilterImplementation>();
 358     componentForge["reciprocal"] = new CreateAndConfigureFunctor<ReciprocalFilterImplementation,DigitalFilterImplementation>();
 359     componentForge["derivative"] = new CreateAndConfigureFunctor<DerivativeFilterImplementation,DigitalFilterImplementation>();
 360   }
 361
 362   SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ")" << endl );
 363   if( AnalogComponent::configure( nodeName, configNode ) )
 364     return true;
 365
 366   if (nodeName == "type" ) {
 367     string type( configNode->getStringValue() );
 368     if( componentForge.count(type) == 0 ) {
 369       SG_LOG( SG_AUTOPILOT, SG_BULK, "unhandled filter type <" << type << ">" << endl );
 370       return true;
 371     }
 372     _implementation = (*componentForge[type])( configNode->getParent() );
 373     _implementation->setDigitalFilter( this );
 374     return true;
 375   }
 376
 377   if( nodeName == "initialize-to" ) {
 378     string s( configNode->getStringValue() );
 379     if( s == "input" ) {
 380       _initializeTo = INITIALIZE_INPUT;
 381     } else if( s == "output" ) {
 382       _initializeTo = INITIALIZE_OUTPUT;
 383     } else if( s == "none" ) {
 384       _initializeTo = INITIALIZE_NONE;
 385     } else {
 386       SG_LOG( SG_AUTOPILOT, SG_WARN, "unhandled initialize-to value '" << s << "' ignored" );
 387     }
 388     return true;
 389   }
 390
 391   SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ") [unhandled]" << endl );
 392   return false; // not handled by us, let the base class try
 393 }
 394
 395 void DigitalFilter::update( bool firstTime, double dt)
 396 {
 397   if( _implementation == NULL ) return;
 398
 399   if( firstTime ) {
 400     switch( _initializeTo ) {
 401
 402       case INITIALIZE_INPUT:
 403         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
 404         _implementation->initialize( _valueInput.get_value() );
 405         break;
 406
 407       case INITIALIZE_OUTPUT:
 408         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
 409         _implementation->initialize( get_output_value() );
 410         break;
 411
 412       default:
 413         SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
 414         break;
 415     }
 416   }
 417
 418   double input = _valueInput.get_value() - _referenceInput.get_value();
 419   double output = _implementation->compute( dt, input );
 420
 421   set_output_value( output );
 422
 423   if(_debug) {
 424     cout << "input:" << input
 425          << "\toutput:" << output << endl;
 426   }
 427 }