Diff: PCMessaging.h

Revision:

26:c2208b0ff78b

Parent:

24:353322495742

Child:

29:dead10cce6e9

--- a/PCMessaging.h	Thu Jun 20 15:33:38 2013 +0000
+++ b/PCMessaging.h	Fri Jun 21 04:47:32 2013 +0000
@@ -223,11 +223,15 @@
 
             // Elapsed time since last known GPS position
             //PPSTimeOffset is a result of possibly missing a prior GPS position message
-            // timeFromPPS.read() is always the time from the moset recent 1PPS
+            // timeFromPPS.read() is always the time from the most recent 1PPS
             double elTime = (double)PPSTimeOffset + timeFromPPS.read();
             
-            // Position time -- GPSTime is the time of the last valid GPS position message
-            double posTime = GPSTimemsecs/1000 + elTime;
+             //this is the best estimate of the GPS time of the requested POSVEL data
+            double posTime = GPSTimemsecs/1000.0 + elTime;
+            
+            // Position time -- posMsgTime is the time of the last valid GPS position message
+            // this time may differ from GPSTimemsecs if the last position message (42) was missed due to CRC error 
+            double posMsgTime = posMsg.msgHeader.GPSTime_msecs/1000.0;
             
             //toPC.printf(" elTime = %6.3f PPSimeOffset = %6.3f \n", elTime, PPSTimeOffset);
             //toPC.printf(" latRateFac = %10.3f  lonRateFac = %10.3f \n", latRateFac, lonRateFac);
@@ -238,8 +242,8 @@
             //double latPos = posMsg.latitude  + (nVel/latMetersPerDeg)*elTime;
             //double lonPos = posMsg.longitude + (eVel/lonMetersPerDeg)*elTime;
             
-            double latPos = posMsg.latitude  + (nVel/latRateFac)*elTime;
-            double lonPos = posMsg.longitude + (eVel/lonRateFac)*elTime;            
+            double latPos = posMsg.latitude  + (nVel/latRateFac)*(posTime - posMsgTime);
+            double lonPos = posMsg.longitude + (eVel/lonRateFac)*(posTime - posMsgTime);            
             double htPos  = posMsg.height    + velMsg.verticalSpeed/(60*1852)*elTime;
             
             char solReady = 'N';