Add some initial support for reading live data from a serial port (and

passing it along to FlightGear.)  I notice that the serial port read only
seems to work correctly if I read one character at a time.  Multicharacter
reads seem very unreliable.

utils/GPSsmooth/MIDG-II.cxx

@@ -56,13 +56,17 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
 
     c0 += id;
     c1 += c0;
+    // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
 
     c0 += size;
     c1 += c0;
+    // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
 
     for ( uint8_t i = 0; i < size; i++ ) {
         c0 += (uint8_t)buf[i];
         c1 += c0;
+        // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1
+        //      << " [" << (unsigned int)buf[i] << "]" << endl;
     }
 
     // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
@@ -365,33 +369,65 @@ bool MIDGTrack::load( const string &file ) {
 }
 
 
+// attempt to work around some system dependent issues.  Our read can
+// return < data than we want.
+int myread( SGIOChannel *ch, char *buf, int length ) {
+    bool myeof = false;
+    int result = 0;
+    while ( result != length && !myeof ) {
+        result = ch->read( buf, length );
+        if ( ch->get_type() == sgFileType ) {
+            myeof = ((SGFile *)ch)->eof();
+        }
+    }
+
+    return result;
+}
+
 // load the next message of a real time data stream
 int MIDGTrack::next_message( SGIOChannel *ch, MIDGpos *pos, MIDGatt *att ) {
     char tmpbuf[256];
     char savebuf[256];
 
+    // cout << "in next_message()" << endl;
+
+    bool myeof = false;
+
     // scan for sync characters
     uint8_t sync0, sync1;
-    ch->read( tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
+    myread( ch, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
-    ch->read( tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+    myread( ch, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
-    while ( (sync0 != 129 || sync1 != 161) && !ch->eof() ) {
+    while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
         sync0 = sync1;
-        ch->read( tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+        myread( ch, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+        // cout << "scanning for start of message, eof = " << ch->eof() << endl;
+        if ( ch->get_type() == sgFileType ) {
+            myeof = ((SGFile *)ch)->eof();
+        }
     }
 
-    // cout << "start of message ..." << endl;
+    // cout << "found start of message ..." << endl;
 
     // read message id and size
-    ch->read( tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
+    myread( ch, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
-    ch->read( tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
+    myread( ch, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
     // cout << "message = " << (int)id << " size = " << (int)size << endl;
 
     // load message
-    ch->read( savebuf, size );
+    if ( ch->get_type() == sgFileType ) {
+        int count = myread( ch, savebuf, size );
+        if ( count != size ) {
+            cout << "ERROR: didn't read enough bytes!" << endl;
+        }
+    } else {
+        for ( int i = 0; i < size; ++i ) {
+            myread( ch, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
+        }
+    }
 
     // read checksum
-    ch->read( tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
+    myread( ch, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
-    ch->read( tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
+    myread( ch, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
     
     if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
         parse_msg( id, savebuf, pos, att );

utils/GPSsmooth/MIDG_main.cxx

@@ -10,6 +10,7 @@
 
 #include <simgear/constants.h>
 #include <simgear/io/lowlevel.hxx> // endian tests
+#include <simgear/io/sg_serial.hxx>
 #include <simgear/math/sg_geodesy.hxx>
 #include <simgear/timing/timestamp.hxx>
 
@@ -36,6 +37,7 @@ static int ctrls_port = 5506;
 
 // Default path
 static string file = "";
+static string serialdev = "";
 
 // Master time counter
 float sim_time = 0.0f;
@@ -322,6 +324,14 @@ int main( int argc, char **argv ) {
                 usage( argv[0] );
                 exit( -1 );
             }
+        } else if ( strcmp( argv[i], "--serial" ) == 0 ) {
+            ++i;
+            if ( i < argc ) {
+                serialdev = argv[i];
+            } else {
+                usage( argv[0] );
+                exit( -1 );
+            }
         } else if ( strcmp( argv[i], "--host" ) == 0 ) {
             ++i;
             if ( i < argc ) {
@@ -370,15 +380,6 @@ int main( int argc, char **argv ) {
         }
     }
 
-    // Load the track data
-    if ( file == "" ) {
-        cout << "No track file specified" << endl;
-        exit(-1);
-    }
-    track.load( file );
-    cout << "Loaded " << track.pos_size() << " position records." << endl;
-    cout << "Loaded " << track.att_size() << " attitude records." << endl;
-
     // Setup up outgoing network connections
 
     netInit( &argc,argv ); // We must call this before any other net stuff
@@ -418,6 +419,12 @@ int main( int argc, char **argv ) {
     }
     cout << "connected outgoing ctrls socket" << endl;
 
+    if ( file.length() ) {
+        // Load data from a track data
+        track.load( file );
+        cout << "Loaded " << track.pos_size() << " position records." << endl;
+        cout << "Loaded " << track.att_size() << " attitude records." << endl;
+
         int size = track.pos_size();
 
         double current_time = track.get_pospt(0).get_seconds();
@@ -533,7 +540,58 @@ int main( int argc, char **argv ) {
         }
 
         cout << "Processed " << pos_count << " entries in "
-         << (current_time_stamp - start_time) / 1000000 << " seconds." << endl;
+             << (current_time_stamp - start_time) / 1000000 << " seconds."
+             << endl;
+    } else if ( serialdev.length() ) {
+        // process incoming data from the serial port
+
+        int count = 0;
+        double current_time = 0.0;
+
+        MIDGpos pos;
+        MIDGatt att;
+
+        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;
+            return false;
+        }
+
+        while ( ! input.eof() ) {
+            // cout << "looking for next message ..." << endl;
+            int id = track.next_message( &input, &pos, &att );
+            count++;
+
+            if ( id == 10 ) {
+                if ( att.get_msec() > att_time ) {
+                    att_time = att.get_msec();
+                    current_time = att_time;
+                } else {
+                    cout << "oops att back in time" << endl;
+                }
+            } else if ( id == 12 ) {
+                if ( pos.get_msec() > pos_time ) {
+                    pos_time = pos.get_msec();
+                    current_time = pos_time;
+                } else {
+                    cout << "oops pos back in time" << endl;
+                }
+            }
+
+            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 );
+        }
+    }
 
     return 0;
 }

utils/GPSsmooth/Makefile.am

@@ -13,7 +13,7 @@ MIDGsmooth_SOURCES = \
 	MIDG_main.cxx
 
 MIDGsmooth_LDADD = \
-	-lsgio -lsgtiming -lsgmath -lsgbucket -lsgmisc -lsgdebug \
+	-lsgio -lsgserial -lsgtiming -lsgmath -lsgbucket -lsgmisc -lsgdebug \
 	-lplibnet -lplibul \
 	$(joystick_LIBS) $(network_LIBS) $(base_LIBS) -lz