Merge branch 'next' into durk-atc

[flightgear.git] / utils / GPSsmooth / MIDG-II.cxx

diff --git a/utils/GPSsmooth/MIDG-II.cxx b/utils/GPSsmooth/MIDG-II.cxx
index 3d84d9d2b2c20a12c875229aa4a5bd48d20195d5..5ca224d15ec78fd45c8c32c1aa7d394e6dcb97d4 100644 (file)
--- a/utils/GPSsmooth/MIDG-II.cxx
+++ b/utils/GPSsmooth/MIDG-II.cxx
@@ -1,17 +1,22 @@
-#include <iostream>
+#ifdef HAVE_CONFIG_H
+#  include <config.h>
+#endif 
+
+#include <simgear/compiler.h>
 
-#include <plib/ul.h>
+#include <iostream>
 
 #include <simgear/constants.h>
 #include <simgear/io/sg_file.hxx>
 #include <simgear/math/sg_geodesy.hxx>
 #include <simgear/misc/sgstream.hxx>
 #include <simgear/misc/strutils.hxx>
+#include <simgear/misc/stdint.hxx>
 
 #include "MIDG-II.hxx"
 
-SG_USING_STD(cout);
-SG_USING_STD(endl);
+using std::cout;
+using std::endl;
 
 
 MIDGTrack::MIDGTrack() {};
@@ -26,11 +31,11 @@ static uint32_t read_swab( char *buf, size_t offset, size_t size ) {
     char *ptr = buf + offset;
 
     // MIDG data is big endian so swap if needed.
-    if ( ulIsLittleEndian ) {
+    if ( sgIsLittleEndian() ) {
         if ( size == 4 ) {
-            ulEndianSwap( (uint32_t *)ptr );
+            sgEndianSwap( (uint32_t *)ptr );
         } else if ( size == 2 ) {
-            ulEndianSwap( (uint16_t *)ptr );
+            sgEndianSwap( (uint16_t *)ptr );
         }
     }
 
@@ -235,7 +240,7 @@ void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att )
         // cout << "  pos = " << posx << "," << posy << "," << posz << endl;
 
         double xyz[3];
-        xyz[0] = posx/100; xyz[1] = posy/100; xyz[2] = posz/100;
+        xyz[0] = (double)posx/100; xyz[1] = (double)posy/100; xyz[2] = (double)posz/100;
         double lat, lon, alt;
         sgCartToGeod(xyz, &lat, &lon, &alt);
         pos->lat_deg = lat * 180.0 / SG_PI;
@@ -374,11 +379,12 @@ bool MIDGTrack::load( const string &file ) {
 int myread( SGIOChannel *ch, SGIOChannel *log, 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();
-        }
+    if ( !myeof ) {
+      result = ch->read( buf, length );
+      // cout << "wanted " << length << " read " << result << " bytes" << endl;
+      if ( ch->get_type() == sgFileType ) {
+       myeof = ((SGFile *)ch)->eof();
+      }
     }
 
     if ( result > 0 && log != NULL ) {
@@ -388,6 +394,23 @@ int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
     return result;
 }
 
+// attempt to work around some system dependent issues.  Our read can
+// return < data than we want.
+int serial_read( SGSerialPort *serial, char *buf, int length ) {
+    int result = 0;
+    int bytes_read = 0;
+    char *tmp = buf;
+
+    while ( bytes_read < length ) {
+      result = serial->read_port( tmp, length - bytes_read );
+      bytes_read += result;
+      tmp += result;
+      // cout << "  read " << bytes_read << " of " << length << endl;
+    }
+
+    return bytes_read;
+}
+
 // load the next message of a real time data stream
 int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
                              MIDGpos *pos, MIDGatt *att )
@@ -406,7 +429,9 @@ int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
     while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
         sync0 = sync1;
         myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
-        // cout << "scanning for start of message, eof = " << ch->eof() << endl;
+        // cout << "scanning for start of message "
+       //      << (unsigned int)sync0 << " " << (unsigned int)sync1
+       //      << ", eof = " << ch->eof() << endl;
         if ( ch->get_type() == sgFileType ) {
             myeof = ((SGFile *)ch)->eof();
         }
@@ -426,9 +451,13 @@ int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
             cout << "ERROR: didn't read enough bytes!" << endl;
         }
     } else {
+#ifdef READ_ONE_BY_ONE
         for ( int i = 0; i < size; ++i ) {
             myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
         }
+#else
+       myread( ch, log, savebuf, size );
+#endif
     }
 
     // read checksum
@@ -445,6 +474,65 @@ int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
 }
 
 
+// load the next message of a real time data stream
+int MIDGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
+                             MIDGpos *pos, MIDGatt *att )
+{
+    char tmpbuf[256];
+    char savebuf[256];
+    int result = 0;
+
+    cout << "in next_message()" << endl;
+
+    bool myeof = false;
+
+    // scan for sync characters
+    uint8_t sync0, sync1;
+    result = serial_read( serial, tmpbuf, 2 );
+    sync0 = (unsigned char)tmpbuf[0];
+    sync1 = (unsigned char)tmpbuf[1];
+    while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
+        sync0 = sync1;
+        serial_read( serial, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+        cout << "scanning for start of message "
+            << (unsigned int)sync0 << " " << (unsigned int)sync1
+            << endl;
+    }
+
+    cout << "found start of message ..." << endl;
+
+    // read message id and size
+    serial_read( serial, tmpbuf, 2 );
+    uint8_t id = (unsigned char)tmpbuf[0];
+    uint8_t size = (unsigned char)tmpbuf[1];
+    // cout << "message = " << (int)id << " size = " << (int)size << endl;
+
+    // load message
+    serial_read( serial, savebuf, size );
+
+    // read checksum
+    serial_read( serial, tmpbuf, 2 );
+    uint8_t cksum0 = (unsigned char)tmpbuf[0];
+    uint8_t cksum1 = (unsigned char)tmpbuf[1];
+    
+    if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
+        parse_msg( id, savebuf, pos, att );
+
+       //
+       // FIXME
+       // WRITE DATA TO LOG FILE
+       //
+
+        return id;
+    }
+
+    cout << "Check sum failure!" << endl;
+    return -1;
+
+    
+}
+
+
 static double interp( double a, double b, double p, bool rotational = false ) {
     double diff = b - a;
     if ( rotational ) {