From a0e00e1eaf03075519c09db05da9d50b279e967d Mon Sep 17 00:00:00 2001
From: curt <curt>
Date: Tue, 27 Feb 2007 19:45:48 +0000
Subject: [PATCH] Switch to "nav" (cooked) data to drive FlightGear.

---
 utils/GPSsmooth/UGear.cxx      |  4 ++--
 utils/GPSsmooth/UGear_main.cxx | 15 +++++++++------
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/utils/GPSsmooth/UGear.cxx b/utils/GPSsmooth/UGear.cxx
index ae0adc413..7046e8c61 100644
--- a/utils/GPSsmooth/UGear.cxx
+++ b/utils/GPSsmooth/UGear.cxx
@@ -120,8 +120,8 @@ void UGEARTrack::parse_msg( const int id, char *buf,
       // printf("imu.time = %.4f\n", imupacket->time);
     } else if ( id == NAV_PACKET ) {
       *navpacket = *(struct nav *)buf;
-      navpacket->lon  = sg_swap_double( (uint8_t *)buf, 0 );
-      navpacket->lat  = sg_swap_double( (uint8_t *)buf, 8 );
+      navpacket->lat  = sg_swap_double( (uint8_t *)buf, 0 );
+      navpacket->lon  = sg_swap_double( (uint8_t *)buf, 8 );
       navpacket->alt  = sg_swap_double( (uint8_t *)buf, 16 );
       navpacket->vn   = sg_swap_double( (uint8_t *)buf, 24 );
       navpacket->ve   = sg_swap_double( (uint8_t *)buf, 32 );
diff --git a/utils/GPSsmooth/UGear_main.cxx b/utils/GPSsmooth/UGear_main.cxx
index a4513560d..46db91823 100644
--- a/utils/GPSsmooth/UGear_main.cxx
+++ b/utils/GPSsmooth/UGear_main.cxx
@@ -121,9 +121,9 @@ static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
     fdm->version = FG_NET_FDM_VERSION;
 
     // Aero parameters
-    fdm->longitude = gpspacket->lon * SG_DEGREES_TO_RADIANS;
-    fdm->latitude = gpspacket->lat * SG_DEGREES_TO_RADIANS;
-    fdm->altitude = gpspacket->alt + alt_offset;
+    fdm->longitude = navpacket->lon * SG_DEGREES_TO_RADIANS;
+    fdm->latitude = navpacket->lat * SG_DEGREES_TO_RADIANS;
+    fdm->altitude = navpacket->alt + alt_offset;
     fdm->agl = -9999.0;
     fdm->psi = imupacket->psi; // heading
     fdm->phi = imupacket->phi; // roll
@@ -140,9 +140,12 @@ static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
     // double v_ms = dist / (frame_us / 1000000);
     // double v_kts = v_ms * SG_METER_TO_NM * 3600;
     // kts_filter = (0.99 * kts_filter) + (0.01 * v_kts);
-    double vn = gpspacket->vn;
-    double ve = gpspacket->ve;
-    double vd = gpspacket->vd;
+    double vn = navpacket->vn;
+    double ve = navpacket->ve;
+    double vd = navpacket->vd;
+
+    // enable to use ground track heading
+    fdm->psi = atan2(vn, ve); // heading
 
     fdm->vcas = sqrt( vn*vn + ve*ve + vd*vd );
     // last_lat = pos.lat_deg;