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