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