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
Revision 26:c2208b0ff78b, committed 2013-06-21
- Comitter:
- jekain314
- Date:
- Fri Jun 21 04:47:32 2013 +0000
- Parent:
- 25:2287bd8c9877
- Child:
- 27:94a6f0589993
- Commit message:
- some changes by jek before sg comments
Changed in this revision
PCMessaging.h | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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';
--- a/main.cpp Thu Jun 20 15:33:38 2013 +0000 +++ b/main.cpp Fri Jun 21 04:47:32 2013 +0000 @@ -279,7 +279,7 @@ if(fireTrigger) //comes from a PC request message { - unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000 + timeFromPPS.read_us()/1000.0; + unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000.0 + timeFromPPS.read_us()/1000.0; toPC.printf("WMsg TRIGGERTIME %10d \n", triggerTime); //pre-fire the trigger using the mid-body 2.5mm connection (T2i) pre_fire = 0; //pin30 (midbody of connector) set to zero @@ -311,7 +311,7 @@ //there are three potential messages and all messages have a header if (completeMessageAvailable && !IMUDataReady) { - + //must unpack header first to get the message length msgHdr = *((MESSAGEHEADER*)&msgBuffer[messageLocation[savedMessageCounter-1]]); //these times are used to tag the IMU sample time. PPSTimeOffset increments by 1 exactly at the 1PPS event (in the 1PPS ISR) @@ -320,19 +320,14 @@ //Thus GPSTimemsecs increments by 1 here while GPSTimemsecs effectively decrements by 1. //This handles IMU time tagging between the 1PPS event and the first receipt of a new GPS time. - //message header length is 28 -- right side is pointer to the receiver-computed CRC for this record + //CRC is computed while reading in the GPS bytes unsigned long msgCRC = *((unsigned long*)&msgBuffer[messageLocation[savedMessageCounter-1] + 28 + msgHdr.messageLength]); //toPC.printf("tmeFrom1PPS= %5d ID= %3d Ln = %3d computedCRC= %08x msgCRC= %08x msgCntr = %3d CRCerr=%4d\n", // timeFromPPS.read_us(), msgHdr.messageID, msgHdr.messageLength, computedCRC, msgCRC, savedMessageCounter, TotalBadCRCmatches); - if ( msgCRC != computedCRC) //computedCRC is performed as we read each bvyte - { - toPC.printf("WMsg bad CRC match for messageID %3d total CRC errors = %4d \n", - msgHdr.messageLength, TotalBadCRCmatches++); - } - else + if ( msgCRC == computedCRC) //computedCRC is performed as we read each bvyte { //if the CRC check is valid -- then get the time from the header //we get three messages each sec -- does it matter that we do this three times? @@ -340,34 +335,48 @@ //the PPSTimeOffset accounts for the occurrence where we do not get any GPS messages over a sec -- but PPS is still operative PPSTimeOffset = 0; //incremented by 1 in the PPS ISR - } + + //We need the pos and vel messages to pass back data to the PC -- error cases that can occur: + // (1) missed 42 (POS) -- use last good pos and extrapolate using last good vel + // (2) missed 99 (VEL) -- use the last good vel since likely not changed much + // (3) missed both 42 and 99 -- must use last good position and extrapolae using last good velocity + // GPS time used to time-tag the IMU data and to do the extrapolttion from last good position to send to PC + // in the position extrapolation, we will use the GPS time that is kept in the header of the POS msg (42) + // see the procedure: sendPosVelMessageToPC() - if (msgHdr.messageID == 42) //this is the position message (lat, lon, alt) - { - //map the starting record byte index to the record structure - curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[savedMessageCounter-1]]); - posMsg = curPos; - - if (streamPos) // we no longer use this functionality + if (msgHdr.messageID == 42) //this is the position message (lat, lon, alt) { - toPC.printf("BESTPOS %5d %1d %8.6lf %9.6lf %5.3lf %d %d\n", - curPos.msgHeader.GPSTime_msecs, curPos.solStatus, - curPos.latitude, curPos.longitude, curPos.height, - curPos.numSV, curPos.numSolSV); + //map the starting record byte index to the record structure + curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[savedMessageCounter-1]]); + posMsg = curPos; + + if (streamPos) // we no longer use this functionality + { + toPC.printf("BESTPOS %5d %1d %8.6lf %9.6lf %5.3lf %d %d\n", + curPos.msgHeader.GPSTime_msecs, curPos.solStatus, + curPos.latitude, curPos.longitude, curPos.height, + curPos.numSV, curPos.numSolSV); + } + + } + else if (msgHdr.messageID == 99) //this is the velocity message + { + curVel = *((OEM615BESTVEL*)&msgBuffer[ messageLocation[savedMessageCounter-1] ]); + //toPC.printf("BESTVEL vel: horizontalSpeed= %5.3f heading=%5.1f verticalSpeed=%4.2f \n", + // curVel.horizontalSpeed, curVel.heading, curVel.verticalSpeed ); + velMsg = curVel; } - - } - else if (msgHdr.messageID == 99) //this is the velocity message + + //below is set to true when we detect that we have received a complete GPS message + completeMessageAvailable = false; + } + else // do this if we do not pass the CRC { - curVel = *((OEM615BESTVEL*)&msgBuffer[ messageLocation[savedMessageCounter-1] ]); - //toPC.printf("BESTVEL vel: horizontalSpeed= %5.3f heading=%5.1f verticalSpeed=%4.2f \n", - // curVel.horizontalSpeed, curVel.heading, curVel.verticalSpeed ); - velMsg = curVel; + toPC.printf("WMsg bad CRC match for messageID %3d total CRC errors = %4d \n", + msgHdr.messageLength, TotalBadCRCmatches++); } - - //below is set to true when we detect that we have received a complete GPS message - completeMessageAvailable = false; } + //write the GPS data to the SD card //NOTE: this is valid only for a once-per-sec GPS message @@ -435,6 +444,7 @@ toPC.printf("WMsg totalBytesWritten %5d \n", totalBytesWritten); wait_ms(100); + //send the nav file to the PC transferFile(); //rxMsg = txMsg = 0; // just indicate that we're in here // to exit this function the HOST (ie: computer or PC app) must send "exit" otherwise the mbed will act