Latest round of changes ... now works.

diff --git a/utils/GPSsmooth/UGear.cxx b/utils/GPSsmooth/UGear.cxx
index 59dabc2ccc48011c314c822257e56b16f2506b77..52b3c34e5e39646bc70eef878bbeb9aaa5d885c6 100644 (file)
--- a/utils/GPSsmooth/UGear.cxx
+++ b/utils/GPSsmooth/UGear.cxx
@@ -69,9 +69,9 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
         //      << " [" << (unsigned int)buf[i] << "]" << endl;
     }
 
-    // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
-    //      << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
-    //      << ")" << endl;
+    cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
+         << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
+         << ")" << endl;
 
     if ( c0 == cksum0 && c1 == cksum1 ) {
         return true;
@@ -87,8 +87,8 @@ void UGEARTrack::parse_msg( const int id, char *buf,
 {
     if ( id == GPS_PACKET ) {
       *gpspacket = *(struct gps *)buf;
-      gpspacket->lon  = sg_swap_double( (uint8_t *)buf, 0 );
-      gpspacket->lat  = sg_swap_double( (uint8_t *)buf, 8 );
+      gpspacket->lat  = sg_swap_double( (uint8_t *)buf, 0 );
+      gpspacket->lon  = sg_swap_double( (uint8_t *)buf, 8 );
       gpspacket->alt  = sg_swap_double( (uint8_t *)buf, 16 );
       gpspacket->vn   = sg_swap_double( (uint8_t *)buf, 24 );
       gpspacket->ve   = sg_swap_double( (uint8_t *)buf, 32 );
@@ -114,6 +114,13 @@ void UGEARTrack::parse_msg( const int id, char *buf,
       // printf("imu.time = %.4f\n", imupacket->time);
     } else if ( id == NAV_PACKET ) {
       *navpacket = *(struct nav *)buf;
+      navpacket->lon  = sg_swap_double( (uint8_t *)buf, 0 );
+      navpacket->lat  = sg_swap_double( (uint8_t *)buf, 8 );
+      navpacket->alt  = sg_swap_double( (uint8_t *)buf, 16 );
+      navpacket->vn   = sg_swap_double( (uint8_t *)buf, 24 );
+      navpacket->ve   = sg_swap_double( (uint8_t *)buf, 32 );
+      navpacket->vd   = sg_swap_double( (uint8_t *)buf, 40 );
+      navpacket->time = sg_swap_double( (uint8_t *)buf, 52 );
     } else if ( id == SERVO_PACKET ) {
       *servopacket = *(struct servo *)buf;
       servopacket->time = sg_swap_double( (uint8_t *)buf, 20 );
@@ -214,13 +221,18 @@ int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
 
 // 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 serial_read( SGSerialPort *serial, SGIOChannel *log,
+                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 );
+      if ( result > 0 && log != NULL ) {
+        log->write( buf, result );
+      }
       bytes_read += result;
       tmp += result;
       // cout << "  read " << bytes_read << " of " << length << endl;
@@ -308,12 +320,12 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
 
     // scan for sync characters
     uint8_t sync0, sync1;
-    result = serial_read( serial, tmpbuf, 2 );
+    result = serial_read( serial, log, tmpbuf, 2 );
     sync0 = (unsigned char)tmpbuf[0];
     sync1 = (unsigned char)tmpbuf[1];
     while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
         sync0 = sync1;
-        serial_read( serial, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+        serial_read( serial, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
         cout << "scanning for start of message "
             << (unsigned int)sync0 << " " << (unsigned int)sync1
             << endl;
@@ -322,16 +334,16 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
     // cout << "found start of message ..." << endl;
 
     // read message id and size
-    serial_read( serial, tmpbuf, 2 );
+    serial_read( serial, log, 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 );
+    serial_read( serial, log, savebuf, size );
 
     // read checksum
-    serial_read( serial, tmpbuf, 2 );
+    serial_read( serial, log, tmpbuf, 2 );
     uint8_t cksum0 = (unsigned char)tmpbuf[0];
     uint8_t cksum1 = (unsigned char)tmpbuf[1];
     // cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
@@ -340,11 +352,6 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
     if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
         parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket );
 
-       //
-       // FIXME
-       // WRITE DATA TO LOG FILE
-       //
-
         return id;
     }
 

diff --git a/utils/GPSsmooth/UGear_main.cxx b/utils/GPSsmooth/UGear_main.cxx
index 046096fefaf97f0ff1a210b589f94f9577e63e56..80b68c71d595beef9ee6e7a48454bb7e83780283 100644 (file)
--- a/utils/GPSsmooth/UGear_main.cxx
+++ b/utils/GPSsmooth/UGear_main.cxx
@@ -114,8 +114,8 @@ static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
     fdm->version = FG_NET_FDM_VERSION;
 
     // Aero parameters
-    fdm->longitude = gpspacket->lon;
-    fdm->latitude = gpspacket->lat;
+    fdm->longitude = gpspacket->lon * SG_DEGREES_TO_RADIANS;
+    fdm->latitude = gpspacket->lat * SG_DEGREES_TO_RADIANS;
     fdm->altitude = gpspacket->alt;
     fdm->agl = -9999.0;
     fdm->psi = imupacket->psi; // heading
@@ -692,10 +692,12 @@ int main( int argc, char **argv ) {
             }
 
            // if ( gpspacket.lat > -500 ) {
-            printf( "%.3f  %.4f %.4f %.1f  %.2f %.2f %.2f\n",
+            printf( "%.2f  %.6f %.6f %.1f  %.2f %.2f %.2f\n",
                     current_time,
-                    gpspacket.lat, gpspacket.lon, gpspacket.alt,
-                    imupacket.phi, imupacket.the, imupacket.psi );
+                    navpacket.lat, navpacket.lon, navpacket.alt,
+                    imupacket.phi*SGD_RADIANS_TO_DEGREES,
+                   imupacket.the*SGD_RADIANS_TO_DEGREES,
+                   imupacket.psi*SGD_RADIANS_TO_DEGREES );
            // }
 
             send_data( &gpspacket, &imupacket, &navpacket, &servopacket );