Merge branch 'next' of git://gitorious.org/fg/flightgear into next

[flightgear.git] / utils / GPSsmooth / MIDG_main.cxx

diff --git a/utils/GPSsmooth/MIDG_main.cxx b/utils/GPSsmooth/MIDG_main.cxx
index 963d908e97f3e0b38e77f3097d71c87f03ca0f69..613f1d39d8c77d40e0677b0397a410c0d7d29a0d 100644 (file)
--- a/utils/GPSsmooth/MIDG_main.cxx
+++ b/utils/GPSsmooth/MIDG_main.cxx
@@ -21,9 +21,9 @@
 #include "MIDG-II.hxx"
 
 
-SG_USING_STD(cout);
-SG_USING_STD(endl);
-SG_USING_STD(string);
+using std::cout;
+using std::endl;
+using std::string;
 
 
 // Network channels
@@ -519,12 +519,14 @@ int main( int argc, char **argv ) {
             //      << endl;
             // cout << (double)current_time << " " << pos.lat_deg << ", "
             //      << pos.lon_deg << " " << att.yaw_deg << endl;
+            if ( pos.lat_deg > -500 ) {
             printf( "%.3f  %.4f %.4f %.1f  %.2f %.2f %.2f\n",
                     current_time,
                     pos.lat_deg, pos.lon_deg, pos.altitude_msl,
                     att.yaw_rad * 180.0 / SG_PI,
                     att.pitch_rad * 180.0 / SG_PI,
                     att.roll_rad * 180.0 / SG_PI );
+           }
 
             send_data( pos, att );
 
@@ -537,13 +539,13 @@ int main( int argc, char **argv ) {
 
             current_time_stamp.stamp();
             /* Convert to ms */
-            double elapsed_us = current_time_stamp - last_time_stamp;
+            double elapsed_us = (current_time_stamp - last_time_stamp).toUSecs();
             if ( elapsed_us < (frame_us - 2000) ) {
                 double requested_us = (frame_us - elapsed_us) - 2000 ;
                 ulMilliSecondSleep ( (int)(requested_us / 1000.0) ) ;
             }
             current_time_stamp.stamp();
-            while ( current_time_stamp - last_time_stamp < frame_us ) {
+            while ( (current_time_stamp - last_time_stamp).toUSecs() < frame_us ) {
                 current_time_stamp.stamp();
             }
 
@@ -552,7 +554,7 @@ int main( int argc, char **argv ) {
         }
 
         cout << "Processed " << pos_count << " entries in "
-             << (current_time_stamp - start_time) / 1000000 << " seconds."
+             << current_time_stamp - start_time << " seconds."
              << endl;
     } else if ( serialdev.length() ) {
         // process incoming data from the serial port
@@ -567,8 +569,8 @@ int main( int argc, char **argv ) {
         uint32_t att_time = 1;
 
         // open the serial port device
-        SGSerial input( serialdev, "115200" );
-        if ( !input.open( SG_IO_IN ) ) {
+        SGSerialPort input( serialdev, 115200 );
+        if ( !input.is_enabled() ) {
             cout << "Cannot open: " << serialdev << endl;
             return false;
         }
@@ -585,9 +587,10 @@ int main( int argc, char **argv ) {
             return false;
         }
 
-        while ( ! input.eof() ) {
+        while ( input.is_enabled() ) {
             // cout << "looking for next message ..." << endl;
             int id = track.next_message( &input, &output, &pos, &att );
+            cout << "message id = " << id << endl;
             count++;
 
             if ( id == 10 ) {
@@ -606,12 +609,14 @@ int main( int argc, char **argv ) {
                 }
             }
 
-            printf( "%.3f  %.4f %.4f %.1f  %.2f %.2f %.2f\n",
-                    current_time,
-                    pos.lat_deg, pos.lon_deg, pos.altitude_msl,
-                    att.yaw_rad * 180.0 / SG_PI,
-                    att.pitch_rad * 180.0 / SG_PI,
-                    att.roll_rad * 180.0 / SG_PI );
+           if ( pos.lat_deg > -500 ) {
+            // printf( "%.3f  %.4f %.4f %.1f  %.2f %.2f %.2f\n",
+            //         current_time,
+            //         pos.lat_deg, pos.lon_deg, pos.altitude_msl,
+            //         att.yaw_rad * 180.0 / SG_PI,
+            //         att.pitch_rad * 180.0 / SG_PI,
+            //         att.roll_rad * 180.0 / SG_PI );
+           }
 
             send_data( pos, att );
         }