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