Fix various route-manager issues reported by Hyde.

[flightgear.git] / src / Autopilot / digitalfilter.cxx

diff --git a/src/Autopilot/digitalfilter.cxx b/src/Autopilot/digitalfilter.cxx
index bdd39397a51b6dc97410e52a65188589ced08b1c..26377e13141b289635ca871e4103579e0b5c66fb 100644 (file)
--- a/src/Autopilot/digitalfilter.cxx
+++ b/src/Autopilot/digitalfilter.cxx
@@ -25,8 +25,33 @@
 #include "functor.hxx"
 #include <deque>
 
+using std::map;
+using std::string;
+using std::endl;
+using std::cout;
+
 namespace FGXMLAutopilot {
 
+/**
+ *
+ *
+ */
+class DigitalFilterImplementation : public SGReferenced {
+protected:
+  virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode) = 0;
+public:
+  virtual ~DigitalFilterImplementation() {}
+  DigitalFilterImplementation();
+  virtual void   initialize( double output ) {}
+  virtual double compute( double dt, double input ) = 0;
+  bool configure( SGPropertyNode_ptr configNode );
+
+  void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; }
+
+protected:
+  DigitalFilter * _digitalFilter;
+};
+
 /* --------------------------------------------------------------------------------- */
 /* --------------------------------------------------------------------------------- */
 class GainFilterImplementation : public DigitalFilterImplementation {
@@ -96,6 +121,10 @@ using namespace FGXMLAutopilot;
 
 /* --------------------------------------------------------------------------------- */
 /* --------------------------------------------------------------------------------- */
+DigitalFilterImplementation::DigitalFilterImplementation() :
+  _digitalFilter(NULL)
+{
+}
 
 bool DigitalFilterImplementation::configure( SGPropertyNode_ptr configNode )
 {
@@ -136,7 +165,7 @@ bool GainFilterImplementation::configure( const std::string & nodeName, SGProper
 
 double ReciprocalFilterImplementation::compute(  double dt, double input )
 {
-  if( input >= -SGLimitsd::min() || input <= SGLimitsd::min() )
+  if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
     return SGLimitsd::max();
 
   return _gainInput.get_value() / input;
@@ -222,19 +251,17 @@ void NoiseSpikeFilterImplementation::initialize( double output )
 
 double NoiseSpikeFilterImplementation::compute(  double dt, double input )
 {
-  double maxChange = _rateOfChangeInput.get_value() * dt;
+  double delta = input - _output_1;
+  if( fabs(delta) <= SGLimitsd::min() ) return input; // trivial
 
-  double output_0 = _output_1;
+  double maxChange = _rateOfChangeInput.get_value() * dt;
+  const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
+  if( periodical ) delta = periodical->normalizeSymmetric( delta );
 
-  if (_output_1 - input > maxChange) {
-    output_0 = _output_1 - maxChange;
-  } else if( _output_1 - input < -maxChange ) {
-    output_0 = _output_1 + maxChange;
-  } else if (fabs(input - _output_1) <= maxChange) {
-    output_0 = input;
-  }
-  _output_1 = output_0;
-  return output_0;
+  if( fabs(delta) <= maxChange )
+    return (_output_1 = input);
+  else
+    return (_output_1 = _output_1 + copysign( maxChange, delta ));
 }
 
 bool NoiseSpikeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
@@ -265,9 +292,14 @@ void ExponentialFilterImplementation::initialize( double output )
 double ExponentialFilterImplementation::compute(  double dt, double input )
 {
   input = GainFilterImplementation::compute( dt, input );
+  double tf = _TfInput.get_value();
 
   double output_0;
-  double alpha = 1 / ((_TfInput.get_value()/dt) + 1);
+
+  // avoid negative filter times 
+  // and div by zero if -tf == dt
+
+  double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
  
   if(_isSecondOrder) {
     output_0 = alpha * alpha * input + 
@@ -303,10 +335,16 @@ bool ExponentialFilterImplementation::configure( const std::string & nodeName, S
 /* --------------------------------------------------------------------------------- */
 
 DigitalFilter::DigitalFilter() :
-    AnalogComponent()
+    AnalogComponent(),
+    _initializeTo(INITIALIZE_INPUT)
 {
 }
 
+DigitalFilter::~DigitalFilter()
+{
+}
+
+
 static map<string,FunctorBase<DigitalFilterImplementation> *> componentForge;
 
 bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configNode)
@@ -332,6 +370,21 @@ bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configN
       return true;
     }
     _implementation = (*componentForge[type])( configNode->getParent() );
+    _implementation->setDigitalFilter( this );
+    return true;
+  }
+
+  if( nodeName == "initialize-to" ) {
+    string s( configNode->getStringValue() );
+    if( s == "input" ) {
+      _initializeTo = INITIALIZE_INPUT;
+    } else if( s == "output" ) {
+      _initializeTo = INITIALIZE_OUTPUT;
+    } else if( s == "none" ) {
+      _initializeTo = INITIALIZE_NONE;
+    } else {
+      SG_LOG( SG_AUTOPILOT, SG_WARN, "unhandled initialize-to value '" << s << "' ignored" );
+    }
     return true;
   }
 
@@ -344,8 +397,22 @@ void DigitalFilter::update( bool firstTime, double dt)
   if( _implementation == NULL ) return;
 
   if( firstTime ) {
-    SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
-    _implementation->initialize( _valueInput.get_value() );
+    switch( _initializeTo ) {
+
+      case INITIALIZE_INPUT:
+        SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
+        _implementation->initialize( _valueInput.get_value() );
+        break;
+
+      case INITIALIZE_OUTPUT:
+        SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
+        _implementation->initialize( get_output_value() );
+        break;
+
+      default:
+        SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
+        break;
+    }
   }
 
   double input = _valueInput.get_value() - _referenceInput.get_value();