X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=utils%2FGPSsmooth%2FUGear.cxx;h=2a570e1e9070a9ea8c659463fffc0053f67223d3;hb=4dc76922c88187269957737f7a5d23017f8d51ad;hp=242683498d48105929ce2807dbc256ffe62a8a18;hpb=1722d26b2f8168de07d4b5898796b2d59baa9e84;p=flightgear.git

diff --git a/utils/GPSsmooth/UGear.cxx b/utils/GPSsmooth/UGear.cxx
index 242683498..2a570e1e9 100644
--- a/utils/GPSsmooth/UGear.cxx
+++ b/utils/GPSsmooth/UGear.cxx
@@ -3,6 +3,7 @@
 #endif 
 
 #include <iostream>
+#include <cstdio>
 
 #include <simgear/constants.h>
 #include <simgear/io/sg_file.hxx>
@@ -14,20 +15,20 @@
 
 #include "UGear.hxx"
 
-SG_USING_STD(cout);
-SG_USING_STD(endl);
+using std::cout;
+using std::endl;
 
 
 #define START_OF_MSG0 147
 #define START_OF_MSG1 224
 
 
-UGEARTrack::UGEARTrack():
+UGTrack::UGTrack():
     sg_swap(false)
 {
 };
 
-UGEARTrack::~UGEARTrack() {};
+UGTrack::~UGTrack() {};
 
 
 // swap the 1st 4 bytes with the last 4 bytes of a stargate double so
@@ -91,7 +92,7 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
 }
 
 
-void UGEARTrack::parse_msg( const int id, char *buf,
+void UGTrack::parse_msg( const int id, char *buf,
 			    struct gps *gpspacket, imu *imupacket,
 			    nav *navpacket, servo *servopacket,
 			    health *healthpacket )
@@ -106,6 +107,7 @@ void UGEARTrack::parse_msg( const int id, char *buf,
           gpspacket->vn   = sg_swap_double( (uint8_t *)buf, 32 );
           gpspacket->ve   = sg_swap_double( (uint8_t *)buf, 40 );
           gpspacket->vd   = sg_swap_double( (uint8_t *)buf, 48 );
+          gpspacket->ITOW = sg_swap_double( (uint8_t *)buf, 56 );
       }
     } else if ( id == IMU_PACKET ) {
       *imupacket = *(struct imu *)buf;
@@ -143,7 +145,7 @@ void UGEARTrack::parse_msg( const int id, char *buf,
       if ( sg_swap ) {
           servopacket->time = sg_swap_double( (uint8_t *)buf, 0 );
       }
-      // printf("servo time = %.3f\n", servopacket->time);
+      // printf("servo time = %.3f %d %d\n", servopacket->time, servopacket->chn[0], servopacket->chn[1]);
     } else if ( id == HEALTH_PACKET ) {
       *healthpacket = *(struct health *)buf;
       if ( sg_swap ) {
@@ -156,7 +158,7 @@ void UGEARTrack::parse_msg( const int id, char *buf,
 
 
 // load the named stream log file into internal buffers
-bool UGEARTrack::load_stream( const string &file, bool ignore_checksum ) {
+bool UGTrack::load_stream( const string &file, bool ignore_checksum ) {
     int count = 0;
 
     gps gpspacket;
@@ -235,7 +237,7 @@ bool UGEARTrack::load_stream( const string &file, bool ignore_checksum ) {
 
 
 // load the named stream log file into internal buffers
-bool UGEARTrack::load_flight( const string &path ) {
+bool UGTrack::load_flight( const string &path ) {
     gps gpspacket;
     imu imupacket;
     nav navpacket;
@@ -260,7 +262,7 @@ bool UGEARTrack::load_flight( const string &path ) {
     // open the gps file
     file = path; file.append( "gps.dat.gz" );
     if ( (fgps = gzopen( file.c_str(), "r" )) == NULL ) {
-        printf("Cannont open %s\n", file.c_str());
+        printf("Cannot open %s\n", file.c_str());
         return false;
     }
 
@@ -372,7 +374,7 @@ int serial_read( SGSerialPort *serial, SGIOChannel *log,
 
 
 // load the next message of a real time data stream
-int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
+int UGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
 			      gps *gpspacket, imu *imupacket, nav *navpacket,
 			      servo *servopacket, health *healthpacket,
 			      bool ignore_checksum )
@@ -442,10 +444,10 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
 
 
 // load the next message of a real time data stream
-int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
-			      gps *gpspacket, imu *imupacket, nav *navpacket,
-			      servo *servopacket, health *healthpacket,
-			      bool ignore_checksum )
+int UGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
+                           gps *gpspacket, imu *imupacket, nav *navpacket,
+                           servo *servopacket, health *healthpacket,
+                           bool ignore_checksum )
 {
     char tmpbuf[256];
     char savebuf[256];
@@ -552,9 +554,9 @@ imu UGEARInterpIMU( const imu A, const imu B, const double percent )
     p.hz = interp(A.hz, B.hz, percent);
     p.Ps = interp(A.Ps, B.Ps, percent);
     p.Pt = interp(A.Pt, B.Pt, percent);
-    p.phi = interp(A.phi, B.phi, percent);
-    p.the = interp(A.the, B.the, percent);
-    p.psi = interp(A.psi, B.psi, percent);
+    p.phi = interp(A.phi, B.phi, percent, true);
+    p.the = interp(A.the, B.the, percent, true);
+    p.psi = interp(A.psi, B.psi, percent, true);
     p.err_type = A.err_type;
 
     return p;
@@ -591,9 +593,7 @@ servo UGEARInterpSERVO( const servo A, const servo B, const double percent )
 health UGEARInterpHEALTH( const health A, const health B, const double percent )
 {
     health p;
-    p.volts_raw = interp(A.volts_raw, B.volts_raw, percent);
-    p.volts = interp(A.volts, B.volts, percent);
-    p.est_seconds = (uint16_t)interp(A.est_seconds, B.est_seconds, percent);
+    p.command_sequence = B.command_sequence;
     p.time = interp(A.time, B.time, percent);
 
     return p;