- added a differential filter

- use /orientation/track-deg instead of computing our own track
- some cosmetic changes

src/Autopilot/xmlauto.cxx

@@ -65,7 +65,7 @@ double FGPeriodicalValue::normalize( double value )
 
   if( phase > SGLimitsd::min() ) {
     while( value < min ) value += phase;
-    while( value > max ) value -= phase;
+    while( value >= max ) value -= phase;
   } else {
     value = min; // phase is zero
   }
@@ -729,6 +729,10 @@ bool FGDigitalFilter::parseNodeHook(const string& aName, SGPropertyNode* aNode)
       filterType = gain;
     } else if (val == "reciprocal") {
       filterType = reciprocal;
+    } else if (val == "differential") {
+      filterType = differential;
+      // use a constant of two samples for current and previous input value
+      samplesInput.push_back( new FGXMLAutoInput(NULL, 2.0 ) ); 
     }
   } else if (aName == "filter-time" ) {
     TfInput.push_back( new FGXMLAutoInput( aNode, 1.0 ) );
@@ -768,7 +772,9 @@ void FGDigitalFilter::update(double dt)
         do_feedback();
     }
 
-    if ( enabled && dt > 0.0 ) {
+    if ( !enabled || dt < SGLimitsd::min() ) 
+        return;
+
     /*
      * Exponential filter
      *
@@ -823,6 +829,12 @@ void FGDigitalFilter::update(double dt)
             output[0] = gainInput.get_value() / input[0];
         }
     }
+    else if (filterType == differential)
+    {
+        if( dt > SGLimitsd::min() ) {
+            output[0] = (input[0]-input[1]) * TfInput.get_value() / dt;
+        }
+    }
 
     output[0] = clamp(output[0]) ;
     set_output_value( output[0] );
@@ -835,7 +847,6 @@ void FGDigitalFilter::update(double dt)
              << "\toutput:" << output[0] << endl;
     }
 }
-}
 
 FGXMLAutopilotGroup::FGXMLAutopilotGroup() :
   SGSubsystemGroup(),
@@ -862,8 +873,7 @@ FGXMLAutopilotGroup::FGXMLAutopilotGroup() :
   vs_fpm(fgGetNode( "/autopilot/internal/vert-speed-fpm", true )),
   static_pressure(fgGetNode( "/systems/static[0]/pressure-inhg", true )),
   pressure_rate(fgGetNode( "/autopilot/internal/pressure-rate", true )),
-  latNode(fgGetNode("/position/latitude-deg")),
-  lonNode(fgGetNode("/position/longitude-deg"))
+  track(fgGetNode( "/orientation/track-deg", true ))
 {
 }
 
@@ -907,11 +917,7 @@ void FGXMLAutopilotGroup::update( double dt )
     true_nav1->setDoubleValue( diff );
 
     // Calculate true groundtrack
-    SGGeod currentPosition(SGGeod::fromDeg(
-        lonNode->getDoubleValue(), latNode->getDoubleValue()));
-    double true_track = SGGeodesy::courseDeg(lastPosition, currentPosition);
-    lastPosition = currentPosition;
-    diff = target_nav1->getDoubleValue() - true_track;
+    diff = target_nav1->getDoubleValue() - track->getDoubleValue();
     SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
     true_track_nav1->setDoubleValue( diff );
 

src/Autopilot/xmlauto.hxx

@@ -354,7 +354,7 @@ private:
     std::deque <double> output;
     std::deque <double> input;
     enum filterTypes { exponential, doubleExponential, movingAverage,
-                       noiseSpike, gain, reciprocal, none };
+                       noiseSpike, gain, reciprocal, differential, none };
     filterTypes filterType;
 
 protected:
@@ -405,9 +405,7 @@ private:
     SGPropertyNode_ptr vs_fpm;
     SGPropertyNode_ptr static_pressure;
     SGPropertyNode_ptr pressure_rate;
-
-    SGPropertyNode_ptr latNode, lonNode;
-    SGGeod lastPosition;
+    SGPropertyNode_ptr track;
 };
 
 class FGXMLAutopilot : public SGSubsystem