Diff: main.cpp

Revision:

26:c2208b0ff78b

Parent:

25:2287bd8c9877

Child:

27:94a6f0589993

diff -r 2287bd8c9877 -r c2208b0ff78b main.cpp
--- 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