A couple tweaks.

diff --git a/utils/GPSsmooth/UGear.cxx b/utils/GPSsmooth/UGear.cxx
index 242683498d48105929ce2807dbc256ffe62a8a18..bd12a0f5b0fa20d15067d77f70755de5568cb7af 100644 (file)
--- 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 1cc2c90b24a18ecd20492a2a466e6ac984ec93ad..f8e40d97caa43dc4402bae3836a3202b798535cc 100644 (file)
--- 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;