From c951810f928ab12e240a84accce862f4e3b0051f Mon Sep 17 00:00:00 2001
From: curt <curt>
Date: Sat, 9 Jun 2007 22:38:54 +0000
Subject: [PATCH] A couple tweaks.

---
 utils/GPSsmooth/UGear.cxx      |  6 +++---
 utils/GPSsmooth/UGear_main.cxx | 22 +++++++++++++++++++---
 2 files changed, 22 insertions(+), 6 deletions(-)

diff --git a/utils/GPSsmooth/UGear.cxx b/utils/GPSsmooth/UGear.cxx
index 242683498..bd12a0f5b 100644
--- a/utils/GPSsmooth/UGear.cxx
+++ b/utils/GPSsmooth/UGear.cxx
@@ -552,9 +552,9 @@ imu UGEARInterpIMU( const imu A, const imu B, const double percent )
     p.hz = interp(A.hz, B.hz, percent);
     p.Ps = interp(A.Ps, B.Ps, percent);
     p.Pt = interp(A.Pt, B.Pt, percent);
-    p.phi = interp(A.phi, B.phi, percent);
-    p.the = interp(A.the, B.the, percent);
-    p.psi = interp(A.psi, B.psi, percent);
+    p.phi = interp(A.phi, B.phi, percent, true);
+    p.the = interp(A.the, B.the, percent, true);
+    p.psi = interp(A.psi, B.psi, percent, true);
     p.err_type = A.err_type;
 
     return p;
diff --git a/utils/GPSsmooth/UGear_main.cxx b/utils/GPSsmooth/UGear_main.cxx
index 1cc2c90b2..f8e40d97c 100644
--- a/utils/GPSsmooth/UGear_main.cxx
+++ b/utils/GPSsmooth/UGear_main.cxx
@@ -149,7 +149,7 @@ static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
     double vd = navpacket->vd;
 
     // enable to use ground track heading
-    fdm->psi = atan2(vn, ve); // heading
+    // fdm->psi = SGD_PI * 0.5 - atan2(vn, ve); // heading
 
     fdm->vcas = sqrt( vn*vn + ve*ve + vd*vd );
     // last_lat = pos.lat_deg;
@@ -531,6 +531,9 @@ int main( int argc, char **argv ) {
 
         double last_lat = -999.9, last_lon = -999.9;
 
+        printf("<gpx>\n");
+        printf(" <trk>\n");
+        printf("  <trkseg>\n");
         while ( current_time < end_time ) {
             // cout << "current_time = " << current_time << " end_time = "
             //      << end_time << endl;
@@ -667,8 +670,8 @@ int main( int argc, char **argv ) {
                 double dlat = last_lat - navpacket.lat;
                 double dlon = last_lon - navpacket.lon;
                 double dist = sqrt( dlat*dlat + dlon*dlon );
-                if ( dist > 0.0015 ) {
-                    printf("  <trkpt lat=\"%.8f\" lon=\"%.8f\"></trkpt>\n",
+                if ( dist > 0.01 ) {
+                    printf("   <trkpt lat=\"%.8f\" lon=\"%.8f\"></trkpt>\n",
                            navpacket.lat, navpacket.lon );
                     // printf(" </wpt>\n");
                     last_lat = navpacket.lat;
@@ -704,6 +707,19 @@ int main( int argc, char **argv ) {
             last_time_stamp = current_time_stamp;
         }
 
+        printf("   <trkpt lat=\"%.8f\" lon=\"%.8f\"></trkpt>\n",
+               nav1.lat, nav1.lon );
+
+        printf("  </trkseg>\n");
+        printf(" </trk>\n");
+        nav0 = track.get_navpt( 0 );
+        nav1 = track.get_navpt( track.nav_size() - 1 );
+        printf(" <wpt lat=\"%.8f\" lon=\"%.8f\"></wpt>\n",
+               nav0.lat, nav0.lon );
+        printf(" <wpt lat=\"%.8f\" lon=\"%.8f\"></wpt>\n",
+               nav1.lat, nav1.lon );
+        printf("<gpx>\n");
+
         cout << "Processed " << imu_count << " entries in "
              << (current_time_stamp - start_time) / 1000000 << " seconds."
              << endl;
-- 
2.39.5