GPS: add NS and EW velocity computation, which some real-world devices display.

diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx
index 16714a9c9d7f298ee30a2bd14c944a0b9d8c7dea..4eee22a699f3d4dae411a2d566b08a51e6372595 100644 (file)
--- a/src/Instrumentation/gps.cxx
+++ b/src/Instrumentation/gps.cxx
@@ -249,6 +249,8 @@ GPS::init ()
   _trip_odometer_node = _gpsNode->getChild("trip-odometer", 0, true);
   _true_bug_error_node = _gpsNode->getChild("true-bug-error-deg", 0, true);
   _magnetic_bug_error_node = _gpsNode->getChild("magnetic-bug-error-deg", 0, true);
+  _eastWestVelocity = _gpsNode->getChild("ew-velocity-msec", 0, true);
+  _northSouthVelocity = _gpsNode->getChild("ns-velocity-msec", 0, true);
   
 // waypoints
   SGPropertyNode *wp_node = _gpsNode->getChild("wp", 0, true);
@@ -420,6 +422,7 @@ GPS::clearOutput()
   _indicated_pos = SGGeod();
   _last_vertical_speed = 0.0;
   _last_true_track = 0.0;
+  _lastEWVelocity = _lastNSVelocity = 0.0;
   
   _raim_node->setDoubleValue(0.0);
   _indicated_pos = SGGeod();
@@ -431,6 +434,8 @@ GPS::clearOutput()
   _tracking_bug_node->setDoubleValue(0);
   _true_bug_error_node->setDoubleValue(0);
   _magnetic_bug_error_node->setDoubleValue(0);
+  _northSouthVelocity->setDoubleValue(0.0);
+  _eastWestVelocity->setDoubleValue(0.0);
 }
 
 void
@@ -545,9 +550,28 @@ GPS::updateBasicData(double dt)
   double vertical_speed_mpm = ((_indicated_pos.getElevationM() - _last_pos.getElevationM()) * 60 / dt);
   _last_vertical_speed = vertical_speed_mpm * SG_METER_TO_FEET;
   
-  speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, dt/20.0);
+  speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, dt/10.0);
   _last_speed_kts = speed_kt;
   
+  SGGeod g = _indicated_pos;
+  g.setLongitudeDeg(_last_pos.getLongitudeDeg());
+  double northSouthM = SGGeodesy::distanceM(_last_pos, g);
+  northSouthM = copysign(northSouthM, _indicated_pos.getLatitudeDeg() - _last_pos.getLatitudeDeg());
+  
+  double nsMSec = fgGetLowPass(_lastNSVelocity, northSouthM / dt, dt/2.0);
+  _lastNSVelocity = nsMSec;
+  _northSouthVelocity->setDoubleValue(nsMSec);
+
+
+  g = _indicated_pos;
+  g.setLatitudeDeg(_last_pos.getLatitudeDeg());
+  double eastWestM = SGGeodesy::distanceM(_last_pos, g);
+  eastWestM = copysign(eastWestM, _indicated_pos.getLongitudeDeg() - _last_pos.getLongitudeDeg());
+  
+  double ewMSec = fgGetLowPass(_lastEWVelocity, eastWestM / dt, dt/2.0);
+  _lastEWVelocity = ewMSec;
+  _eastWestVelocity->setDoubleValue(ewMSec);
+  
   double odometer = _odometer_node->getDoubleValue();
   _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
   odometer = _trip_odometer_node->getDoubleValue();

diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx
index 24c8f50fb84d33db15d6bb67a0e38d12b6607e6a..56936974d02351ea8f143590a8f12b682bb02fdb 100644 (file)
--- a/src/Instrumentation/gps.hxx
+++ b/src/Instrumentation/gps.hxx
@@ -333,6 +333,8 @@ private:
     SGPropertyNode_ptr _trip_odometer_node;
     SGPropertyNode_ptr _true_bug_error_node;
     SGPropertyNode_ptr _magnetic_bug_error_node;
+    SGPropertyNode_ptr _eastWestVelocity;
+    SGPropertyNode_ptr _northSouthVelocity;
     
     SGPropertyNode_ptr _ref_navaid_id_node;
     SGPropertyNode_ptr _ref_navaid_bearing_node;
@@ -358,6 +360,8 @@ private:
     double _last_speed_kts;
     double _last_true_track;
     double _last_vertical_speed;
+    double _lastEWVelocity;
+    double _lastNSVelocity;
     
     std::string _mode;
     GPSListener* _listener;