james kain
/
Waldo_Embed_V2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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
