this version has all of Jim's fixes for reading the GPS and IMU data synchronously
Dependencies: MODSERIAL SDFileSystem mbed SDShell CRC CommHandler FP LinkedList LogUtil
Diff: PCMessaging.h
- Revision:
- 26:c2208b0ff78b
- Parent:
- 24:353322495742
- Child:
- 29:dead10cce6e9
diff -r 2287bd8c9877 -r c2208b0ff78b PCMessaging.h --- 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';