fly/flightgear
1
0
Fork 0
Code Activity

A couple tweaks.

Browse source
This commit is contained in:
curt 2007-06-09 22:38:54 +00:00
parent fd71817c3b
commit c951810f92
2 changed files with 22 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
utils/GPSsmooth/UGear.cxx
View file

@ -552,9 +552,9 @@ imu UGEARInterpIMU( const imu A, const imu B, const double percent )
p.hz = interp(A.hz, B.hz, percent); p.hz = interp(A.hz, B.hz, percent);
p.Ps = interp(A.Ps, B.Ps, percent); p.Ps = interp(A.Ps, B.Ps, percent);
p.Pt = interp(A.Pt, B.Pt, percent); p.Pt = interp(A.Pt, B.Pt, percent);
p.phi = interp(A.phi, B.phi, percent); p.phi = interp(A.phi, B.phi, percent, true);
p.the = interp(A.the, B.the, percent); p.the = interp(A.the, B.the, percent, true);
p.psi = interp(A.psi, B.psi, percent); p.psi = interp(A.psi, B.psi, percent, true);
p.err_type = A.err_type; p.err_type = A.err_type;
return p; return p;

22
utils/GPSsmooth/UGear_main.cxx
View file

@ -149,7 +149,7 @@ static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
double vd = navpacket->vd; double vd = navpacket->vd;
// enable to use ground track heading // 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 ); fdm->vcas = sqrt( vn*vn + ve*ve + vd*vd );
// last_lat = pos.lat_deg; // last_lat = pos.lat_deg;
@ -531,6 +531,9 @@ int main( int argc, char **argv ) {
double last_lat = -999.9, last_lon = -999.9; double last_lat = -999.9, last_lon = -999.9;
printf("<gpx>\n");
printf(" <trk>\n");
printf(" <trkseg>\n");
while ( current_time < end_time ) { while ( current_time < end_time ) {
// cout << "current_time = " << current_time << " end_time = " // cout << "current_time = " << current_time << " end_time = "
// << end_time << endl; // << end_time << endl;
@ -667,8 +670,8 @@ int main( int argc, char **argv ) {
double dlat = last_lat - navpacket.lat; double dlat = last_lat - navpacket.lat;
double dlon = last_lon - navpacket.lon; double dlon = last_lon - navpacket.lon;
double dist = sqrt( dlat*dlat + dlon*dlon ); double dist = sqrt( dlat*dlat + dlon*dlon );
if ( dist > 0.0015 ) { if ( dist > 0.01 ) {
printf(" <trkpt lat=\"%.8f\" lon=\"%.8f\"></trkpt>\n", printf(" <trkpt lat=\"%.8f\" lon=\"%.8f\"></trkpt>\n",
navpacket.lat, navpacket.lon ); navpacket.lat, navpacket.lon );
// printf(" </wpt>\n"); // printf(" </wpt>\n");
last_lat = navpacket.lat; last_lat = navpacket.lat;
@ -704,6 +707,19 @@ int main( int argc, char **argv ) {
last_time_stamp = current_time_stamp; 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 " cout << "Processed " << imu_count << " entries in "
<< (current_time_stamp - start_time) / 1000000 << " seconds." << (current_time_stamp - start_time) / 1000000 << " seconds."
<< endl; << endl;