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 59211bb9117c7a0cf39a5c5740e64a013ee36234..613f1d39d8c77d40e0677b0397a410c0d7d29a0d 100644 (file)
--- a/utils/GPSsmooth/MIDG_main.cxx
+++ b/utils/GPSsmooth/MIDG_main.cxx
@@ -10,6 +10,7 @@
 
 #include <simgear/constants.h>
 #include <simgear/io/lowlevel.hxx> // endian tests
+#include <simgear/io/sg_file.hxx>
 #include <simgear/io/sg_serial.hxx>
 #include <simgear/math/sg_geodesy.hxx>
 #include <simgear/timing/timestamp.hxx>
@@ -20,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
@@ -36,8 +37,9 @@ static int fdm_port = 5505;
 static int ctrls_port = 5506;
 
 // Default path
-static string file = "";
+static string infile = "";
 static string serialdev = "";
+static string outfile = "";
 
 // Master time counter
 float sim_time = 0.0f;
@@ -287,7 +289,9 @@ static void send_data( const MIDGpos pos, const MIDGatt att ) {
 void usage( const string &argv0 ) {
     cout << "Usage: " << argv0 << endl;
     cout << "\t[ --help ]" << endl;
-    cout << "\t[ --file <file_name>" << endl;
+    cout << "\t[ --infile <infile_name>" << endl;
+    cout << "\t[ --serial <dev_name>" << endl;
+    cout << "\t[ --outfile <outfile_name> (capture the data to a file)" << endl;
     cout << "\t[ --hertz <hertz> ]" << endl;
     cout << "\t[ --host <hostname> ]" << endl;
     cout << "\t[ --broadcast ]" << endl;
@@ -316,10 +320,18 @@ int main( int argc, char **argv ) {
                 usage( argv[0] );
                 exit( -1 );
             }
-        } else if ( strcmp( argv[i], "--file" ) == 0 ) {
+        } else if ( strcmp( argv[i], "--infile" ) == 0 ) {
             ++i;
             if ( i < argc ) {
-                file = argv[i];
+                infile = argv[i];
+            } else {
+                usage( argv[0] );
+                exit( -1 );
+            }
+        } else if ( strcmp( argv[i], "--outfile" ) == 0 ) {
+            ++i;
+            if ( i < argc ) {
+                outfile = argv[i];
             } else {
                 usage( argv[0] );
                 exit( -1 );
@@ -419,9 +431,9 @@ int main( int argc, char **argv ) {
     }
     cout << "connected outgoing ctrls socket" << endl;
 
-    if ( file.length() ) {
+    if ( infile.length() ) {
         // Load data from a track data
-        track.load( file );
+        track.load( infile );
         cout << "Loaded " << track.pos_size() << " position records." << endl;
         cout << "Loaded " << track.att_size() << " attitude records." << endl;
 
@@ -507,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 );
 
@@ -525,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();
             }
 
@@ -540,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
@@ -554,16 +568,29 @@ int main( int argc, char **argv ) {
         uint32_t pos_time = 1;
         uint32_t att_time = 1;
 
-        // open the file
-        SGSerial input( serialdev, "115200" );
-        if ( !input.open( SG_IO_IN ) ) {
-            cout << "Cannot open file: " << file << endl;
+        // open the serial port device
+        SGSerialPort input( serialdev, 115200 );
+        if ( !input.is_enabled() ) {
+            cout << "Cannot open: " << serialdev << endl;
             return false;
         }
 
-        while ( ! input.eof() ) {
+        // open up the data log file if requested
+        if ( !outfile.length() ) {
+            cout << "no --outfile <name> specified, cannot capture data!"
+                 << endl;
+            return false;
+        }
+        SGFile output( outfile );
+        if ( !output.open( SG_IO_OUT ) ) {
+            cout << "Cannot open: " << outfile << endl;
+            return false;
+        }
+
+        while ( input.is_enabled() ) {
             // cout << "looking for next message ..." << endl;
-            int id = track.next_message( &input, &pos, &att );
+            int id = track.next_message( &input, &output, &pos, &att );
+            cout << "message id = " << id << endl;
             count++;
 
             if ( id == 10 ) {
@@ -582,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 );
         }