Revision 9:b45feb91ba38, committed 2013-04-19

Comitter:

jekain314

Date:

Fri Apr 19 16:21:27 2013 +0000

Parent:

8:13724ed3f825

Commit message:

update to allow better imu gps data collection

Changed in this revision

--- a/ADIS16488.h	Wed Apr 17 13:50:21 2013 +0000
+++ b/ADIS16488.h	Fri Apr 19 16:21:27 2013 +0000
@@ -15,6 +15,8 @@
 unsigned short LOW_REGISTER[]  = {0x4000, 0x4400, 0x4800, 0x4C00, 0x5000, 0x5400};   
 // X_DELTANG_HIGH, Y_DELTANG_HIGH, X_DETANG_HIGH, X_DELTVEL_HIGH, Y_DELTVEL_HIGH, Z_DELTVEL_HIGH
 unsigned short HIGH_REGISTER[] = {0x4200, 0x4600, 0x4A00, 0x4E00, 0x5200, 0x5600}; 
+
+unsigned long IMUtimeFrom1PPS = 0;
     
 union WD { long dataWord; unsigned short pt[2];} wd;
 
@@ -30,6 +32,8 @@
 
 void IMUDataReadyISR(void)
 {   
+    //IMUtimeFrom1PPS = timeFromPPS.read_us();
+    IMUClockCounter++;
     IMUDataReady = true;
     return;
 } 
@@ -57,7 +61,8 @@
     //change the DECRATE to 98.4 Hz (this is also in page 3)
     //the 8 sets the high bit to 1 indicating a write to a register
     // The C abd D designate the registers for the DECRATE of Page 3
-    // The 17 sets the rate to:  2460/(23+1) = 102.5Hz
+    // The 0x17 sets the rate to:  2460/(23+1) = 102.5Hz
+    // The 0x18 sets the rate to:  2460/(24+1) =  98.4Hz
     spi.write((int)0x8C17);    //write high byte  (only page number can be written in a single byte)
     spi.write((int)0x8D00);    //write the low byte of DECRATE 
     

--- a/OEM615.h	Wed Apr 17 13:50:21 2013 +0000
+++ b/OEM615.h	Fri Apr 19 16:21:27 2013 +0000
@@ -3,24 +3,26 @@
 //set up the GPS message header in a structure to enable easy reading -- see the OEM615 manual (Table 4, page 24)
 struct MESSAGEHEADER   
 {
-    char synchAA;  //1st synch word
-    char synch44;  //2nd synch word
-    char synch12;  //3rd synch word
-    unsigned char headerLength;  //always 28
-    unsigned short messageID;  //42 (BESTPOS) , 43 (RANGE) , or 99 (BESTVEL), 
-    char messageType;  //always = 0 for binary
-    unsigned char portAddress;  //0x20 for COM1
-    unsigned short messageLength; //not including header or CRC
-    unsigned short sequence;  //typically 0
-    unsigned char idleTime;  //Time the processor is idle, in the last second
-    unsigned char timeStatus; //Enum Indicating quality of the GPS reference time
-    unsigned short GPSweek;   //GPS reference week
-    unsigned long GPSTime_msecs; //from beginning of week
-    unsigned long receiverStatus; //32-bits representing the status of hardware and software
+    char synchAA;                   //1st synch word
+    char synch44;                   //2nd synch word
+    char synch12;                   //3rd synch word
+    unsigned char headerLength;     //always 28
+    unsigned short messageID;       //42  (0x2A  BESTPOS) , 43 (2B  RANGE) , or 99 (x63 BESTVEL), 
+    char messageType;               //always = 0 for binary
+    unsigned char portAddress;      //0x20 for COM1
+    // from spec:  The length in bytes of the body of the message, not including the header nor the CRC
+    unsigned short messageLength;   //not including header or CRC
+    unsigned short sequence;        //typically 0
+    unsigned char idleTime;         //Time the processor is idle, in the last second
+    unsigned char timeStatus;       //Enum Indicating quality of the GPS reference time
+    unsigned short GPSweek;         //GPS reference week
+    unsigned long GPSTime_msecs;    //from beginning of week
+    unsigned long receiverStatus;   //32-bits representing the status of hardware and software
     unsigned short reserved;
     unsigned short receiverSWversion;  //receiver software build number
+    //total length in bytes of this header is 28
 };
-MESSAGEHEADER *msgHeader[4];
+MESSAGEHEADER *msgHeader[6];
 
 #pragma pack(1)
 //structure for OEM615 BESTVEL log message  (page 314)
@@ -113,11 +115,23 @@
 const unsigned char maxGPSMessagesPerSec = 12;
 unsigned short messageLocation[maxGPSMessagesPerSec] = {0};  //stores the message location start within the message buffer
 
+unsigned short bytesFromMessageHdrDetect = 0;
+union SWAPBYTES { unsigned char b[2]; unsigned short w; };
+volatile SWAPBYTES messageLength;  //used to swap the bytes
+unsigned long computedCRC = 0;  //final resulting computed CRC for this message
+unsigned long incrementalCRC = 0;  //incrementally-formed CRC over message sequence
+unsigned short endByteForCRCcomputation = 0;
+bool completeMessageAvailable = false;
+
 //this code was taken from the Novatel Firmware document page 35
-#define CRC32_POLYNOMIAL 0xEDB88320L
+//#define CRC32_POLYNOMIAL 0xEDB88320L
 /* --------------------------------------------------------------------------
 Calculate a CRC value to be used by CRC calculation functions.
 -------------------------------------------------------------------------- */
+/*
+////////////////////////////////////////////
+//original code from the OEM615 manual
+////////////////////////////////////////////
 unsigned long CRC32Value(int i)
 {
     int j;
@@ -132,15 +146,53 @@
     }
     return ulCRC;
 } 
+*/
+
+#define CRC32_POLYNOMIAL 0xEDB88320L
+void CRC32Value(unsigned long &CRC, unsigned char c)
+{
+    /////////////////////////////////////////////////////////////////////////////////////
+    //CRC must be initialized as zero 
+    //c is a character from the sequence that is used to form the CRC
+    //this code is a modification of the code from the Novatel OEM615 specification
+    /////////////////////////////////////////////////////////////////////////////////////
+    unsigned long ulTemp1 = ( CRC >> 8 ) & 0x00FFFFFFL;
+    unsigned long ulCRC = ((int) CRC ^ c ) & 0xff ;
+    for (int  j = 8 ; j > 0; j-- )
+    {
+        if ( ulCRC & 1 )
+            ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL;
+        else
+            ulCRC >>= 1;
+    }
+    CRC = ulTemp1 ^ ulCRC;
+} 
 
 /* --------------------------------------------------------------------------
 Calculates the CRC-32 of a block of data all at once
-//the CRC is c from the complete message (header plus data) but excluding (of course) the CRC at the end
+//the CRC is from the complete message (header plus data) 
+//but excluding (of course) the CRC at the end
 -------------------------------------------------------------------------- */
 unsigned long CalculateBlockCRC32(
         unsigned long ulCount,    /* Number of bytes in the data block */
         unsigned char *ucBuffer ) /* Data block */
 {
+    //////////////////////////////////////////////////////////////////////
+    //the below code tests the CRC32Value procedure used in a markov form
+    //////////////////////////////////////////////////////////////////////
+    unsigned long CRC = 0;
+    for (int i = 0; i<ulCount; i++)  CRC32Value( CRC, *ucBuffer++ );
+    return  CRC;
+}
+
+/*
+unsigned long CalculateBlockCRC32(
+        unsigned long ulCount, 
+        unsigned char *ucBuffer )
+{
+////////////////////////////////////////////
+//original code from the OEM615 manual
+////////////////////////////////////////////
     unsigned long ulTemp1;
     unsigned long ulTemp2;
     unsigned long ulCRC = 0;
@@ -152,8 +204,7 @@
     }
     return( ulCRC );
 }
-
-
+*/
 void sendASCII(char* ASCI_message, int numChars)
 {
     /////////////////////////////////////////////////
@@ -178,24 +229,46 @@
 //MODSERIAL is an easy to use library that extends Serial to add fully buffered input and output.
 void readSerialByte(MODSERIAL_IRQ_INFO *q)
 { 
-    MODSERIAL *serial = q->serial;
-    unsigned char synch0 = serial->getc();
+    MODSERIAL *serial = q->serial;      //see example of MODSERIAL usage in cookbook
+    unsigned char synch0 = serial->getc();  //get the next byte
+    
+    //byteCounter is zeroed only at a 1PPA event in the 1PPS ISR
+    //all message bytes stored for a single GPS second 
     msgBuffer[byteCounter % maxGPSbytesPerSec] = synch0;
+    
+     //accumulate the CRC for this message
+     //incrementalCRC re-initialized after message header is is detected
+     CRC32Value( incrementalCRC, synch0); 
 
-    //we need to trap the GPS message header byte-string 0xAA44121C
+    //Trap the GPS message header byte-string per Novatel OEM615 spec: 0xAA44121C
     //generate a 4-byte sliding-window sequence from the input bytes
-    //shift last 4-byte value left 8 bits & push recently read byte (synch0) into low-order byte
-    test = (test<<8) | synch0;  //
+    //shift last 4-byte value left 8 bits & push current-read byte (synch0) into low-order byte
+    test = (test<<8) | synch0; 
    
     if (test == 0xAA44121C) //test for the Receiver message header signature
     {
-        messageLocation[perSecMessageCounter % maxGPSMessagesPerSec] = byteCounter-3; //store the location of this message (with 4 synch words)
-        perSecMessageCounter++;
-        messageDetected = true;
-     }   
+        messageLocation[perSecMessageCounter % maxGPSMessagesPerSec] = byteCounter-3; //store the location of this message (1st of 4 synch word)
+        perSecMessageCounter++;  //counts messages this second
+        bytesFromMessageHdrDetect = 0;  //start byte counter for this message
+        incrementalCRC = 0x39b0f0e1;    //initializes the recursive CRC after the AA44121C header byte sequence 
+     }
+     else if (bytesFromMessageHdrDetect == 5) messageLength.b[0] = synch0;  //first byte of msg length
+     else if (bytesFromMessageHdrDetect == 6)   //second byte of message length
+     {
+        messageLength.b[1] = synch0; 
+        endByteForCRCcomputation = 24 + messageLength.w;  //use union to perform byte-swap from stored bytes
+     }
+     else if (bytesFromMessageHdrDetect == endByteForCRCcomputation)  //stop the CRC recursive computation
+        computedCRC = incrementalCRC;  //store the computed CRC for this message for use in main  
+     else if (bytesFromMessageHdrDetect == (endByteForCRCcomputation + 4) )  //detect the end of the message (end of its CRC)
+     {
+        savedMessageCounter = perSecMessageCounter;  //message counter can be corrupted before use in main
+        completeMessageAvailable = true;  //set flg for the main message processing
+     }
+         
      //byteCounter reset to zero in main after the 1PPS is detected -- its NOT reset in the 1PPS ISR
      byteCounter++;     //total per-sec byte counter (reset to zero in main when 1PPS detected) 
-
+     bytesFromMessageHdrDetect++;  //counts the byes received after a message header
 };
 
 

--- a/main.cpp	Wed Apr 17 13:50:21 2013 +0000
+++ b/main.cpp	Fri Apr 19 16:21:27 2013 +0000
@@ -128,7 +128,7 @@
     //set the final baud rate that we will use from here  
     //allowable baud rate values: 9600 115200 230400 460800 921600
     //char ch2[] = "serialconfig COM1 921600 n 8 1 n off";
-    char ch2[] = "serialconfig COM1 460800 n 8 1 n off";
+    char ch2[] = "serialconfig COM1 115200 n 8 1 n off";
     
     //the below commands request the POS, VEL, RANGE, and TIME messages
     char ch3[] = "log COM1 BESTPOSB ONTIME 1";   //messageID = 42 
@@ -165,7 +165,7 @@
     
     //set the mbed COM port to match the GPS transmit rate
     //the below baud rate must match the COM1 rate coming from the GPS receiver 
-    GPS_COM1.baud(460800); wait_ms(500);  //without this wait -- the baud rate is not detected when using MODSERIAL     
+    GPS_COM1.baud(115200); wait_ms(500);  //without this wait -- the baud rate is not detected when using MODSERIAL     
     //GPS_COM1.baud(921600); wait_ms(500);  //without this wait -- the baud rate is not detected when using MODSERIAL     
 };
 
@@ -184,6 +184,7 @@
 int main() {
     
     //these are structures for the to GPS messages that must be parsed
+    MESSAGEHEADER msgHdr;
     OEM615BESTPOS posMsg;   //BESTPOS structure in OEMV615.h that has matching time to a BESTVEL message
     OEM615BESTPOS curPos;   //BESTPOS structure in OEMV615.h
     OEM615BESTVEL velMsg;   //BESTVEL structure in OEMV615.h that has matching time to a BESTPOS message
@@ -209,11 +210,26 @@
     
     int totalBytesWritten = 0;
     
-    //while(PPSCounter < 100)
+    /*
+    unsigned long CRC = 0;
+    CRC32Value(CRC, 0xAA);
+    CRC32Value(CRC, 0x44);
+    CRC32Value(CRC, 0x12);
+    CRC32Value(CRC, 0x1C);
+    toPC.printf(" CRC after AA44121C header: %08x \n", CRC);
+    wait(20);
+    */
+    
+    int CRCerrors = 0;
+    
+    recordData = true;
+    sendRecData = true;                     
+    
+    while(PPSCounter < 300)
     ///////////////////////////////////////////////////////////////////////////
     // top of the while loop
     ///////////////////////////////////////////////////////////////////////////
-    while(1)
+    //while(1)
     {        
         //read the USB serial data from the PC to check for commands
         //in the primary real-time portion, there are no bytes from the PC so this has no impact
@@ -225,96 +241,66 @@
         ////////////////////////////////////////////////////////////////////////////
         //below is where we process the complete stored GPS message for the second
         //The !IMUDataReady test prevents the IMU and GPS data from being written 
-        //to disk on the same pass through thi loop  
+        //to disk on the same pass through this loop  
         /////////////////////////////////////////////////////////////////////////////
-        if (!IMUDataReady && lookingForMessages && (timeFromPPS.read_us() > 20000))  //it takes less than 20msec to receive all messages
-        {    
-            //toPC.printf(" num messages = %3d time = %5d \n", perSecMessageCounter, timeFromPPS.read_us());
+        
+        
+        if (completeMessageAvailable && !IMUDataReady)
+        {
+            
+            msgHdr = *((MESSAGEHEADER*)&msgBuffer[messageLocation[savedMessageCounter-1]]);
             
-            //cycle through all the bytes stored this sec (after the 1PPS as set)
-            // perSecMessageCounter is incremented whenever we detect a new message headet 0xAA44121C sequence
-            for (int i=0; i<perSecMessageCounter; i++)  
+            GPSTimemsecs = msgHdr.GPSTime_msecs;
+            /////////////////////////////////////////////////////////////////////////////////////////
+            //IMPORTANT:   we reset the PPSTimeOffset when we have a matching position and velocity 
+            PPSTimeOffset = 0;
+            /////////////////////////////////////////////////////////////////////////////////////////
+            
+            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)
             {
-                msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]];
-                //toPC.printf("WMsg MESSAGEINFO %5d %5d \n", 
-                //                                     msgHeader[i]->messageID, 
-                //                                     messageLocation[i]);
-               
-                //calculated CRC
-                unsigned long CRC1 = CalculateBlockCRC32(28+msgHeader[i]->messageLength, &msgBuffer[messageLocation[i]]);
-                unsigned long CRC2 = *((unsigned long*)&msgBuffer[messageLocation[i] + 28 + msgHeader[i]->messageLength]);
-                 
-                if (CRC1 != CRC2)
-                { 
-                    TotalBadCRCmatches++;
-                    toPC.printf(" bad CRC match for messageID %3d total CRC errors = %4d \n",  
-                    msgHeader[i]->messageID, TotalBadCRCmatches);
-                    continue;
-                }                
-          
-                //test for a message 42 (BESTPOS)
-                if (msgHeader[i]->messageID == 42)
+                toPC.printf(" bad CRC match for messageID %3d total CRC errors = %4d \n",  
+                    msgHdr.messageLength, TotalBadCRCmatches++);
+            }
+                     
+            if      (msgHdr.messageID == 42)
+            {
+                curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[savedMessageCounter-1]]);
+                posMsg = curPos;
+                
+                if (streamPos)
                 {
-                    curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[i]]);
-                    
-                    if (streamPos)
-                    {
-                            toPC.printf("WMsg BESTPOS %d %d %d %8.5lf %9.5lf %5.3lf %5.3f %5.3f %5.3f %5.3f %5.3f %5.3f %d %d %d %d %d\n",
-                                              curPos.msgHeader.GPSTime_msecs,
-                                              curPos.solStatus,
-                                              curPos.posType,
-                                              curPos.latitude,
-                                              curPos.longitude,
-                                              curPos.height,
-                                              curPos.undulation,
-                                              curPos.latitudeSTD,
-                                              curPos.longitudeSTD,
-                                              curPos.heightSTD,
-                                              curPos.diffAge,
-                                              curPos.solutionAge,
-                                              curPos.numSV,
-                                              curPos.numSolSV,
-                                              curPos.numGGL1,
-                                              curPos.extSolStatus,
-                                              curPos.sigMask);
-                    }
-                }
-                    
-                //check for a message 99 (BESTVEL)  -- and cast it into its message structure
-                else if (msgHeader[i]->messageID == 99)
-                {
-                    curVel = *((OEM615BESTVEL*)&msgBuffer[messageLocation[i]]);
+                        toPC.printf("WMsg BESTPOS %5d %1d %8.5lf %9.5lf %5.3lf %d %d %d\n",
+                                          curPos.msgHeader.GPSTime_msecs,  curPos.solStatus,
+                                          curPos.latitude, curPos.longitude, curPos.height,
+                                          curPos.numSV, curPos.numSolSV, curPos.numGGL1);
                 }
                 
-                //the below test ensures that the positin and veocity are matched in time
-                //not sure the reason for the "250" below    
-                if ((curVel.msgHeader.GPSTime_msecs+250)/1000 == 
-                    (curPos.msgHeader.GPSTime_msecs+250)/1000)
-                {
-                    // update position and velocity used for calculation
-                    GPSTimemsecs = curPos.msgHeader.GPSTime_msecs;
-                    GPSTime = (double)GPSTimemsecs/1000.0;
-                    velMsg = curVel;  //
-                    posMsg = curPos;
-                        
-                    /////////////////////////////////////////////////////////////////////////////////////////
-                    //IMPORTANT:   we reset the PPSTimeOffset when we have a matching position and velocity 
-                    PPSTimeOffset = 0;
-                    /////////////////////////////////////////////////////////////////////////////////////////
-                }
-            }  //end of per message loop
-            lookingForMessages = false;
+                /////////////////////////////////////////////////////////////////////////////////////////
+                //IMPORTANT:   we reset the PPSTimeOffset when we have a matching position and velocity 
+                PPSTimeOffset = 0;
+                /////////////////////////////////////////////////////////////////////////////////////////
+            } 
+            else if (msgHdr.messageID == 99)  
+            {
+                curVel = *((OEM615BESTVEL*)&msgBuffer[ messageLocation[savedMessageCounter-1] ]);
+                velMsg = curVel;
+            }
             
             if (recordData && (fpNav != NULL) && (byteCounter > 0))
             {
-                wait_us(1000);
-                totalBytesWritten += fwrite(msgBuffer, 1, byteCounter, fpNav);  // this writes out a complete set of messages for this sec
-                wait_us(1000);
+                //wait_us(10);
+                int totalMessageLength = 28 + msgHdr.messageLength + 4;  //header length + message Length + CRC word size
+                totalBytesWritten += fwrite(&msgBuffer[messageLocation[savedMessageCounter-1]], 1, totalMessageLength, fpNav);  // this writes out a complete set of messages for this sec
+                //wait_us(10);
             }
-            
-        }  //end of the GPS message processing
-        
-//
+                
+            completeMessageAvailable = false;
+        }
         
         //the IMU data record is read from the SPI in the ISR and the IMUDataReady is set true
         //we write the IMU data here
@@ -322,14 +308,15 @@
         {
             //GPSTime (secs from midnight) is from the header of the position message
             //PPSTimeOffset accounts for time becoming known ~20msec AFTER the 1PPS
-            imuRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + timeFromPPS.read_us()/1000.0;
-            wait_us(10);
-            spi.write((int) HIGH_REGISTER[0]); wait_us(10); // next read will return results from HIGH_REGITER[0]
+            IMUtimeFrom1PPS = timeFromPPS.read_us();
+            imuRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + IMUtimeFrom1PPS/1000.0;
+            //wait_us(1);
+            spi.write((int) HIGH_REGISTER[0]); //wait_us(1); // next read will return results from HIGH_REGITER[0]
             for (int i=0; i<6; i++)  //read the 6 rate and accel variables
             {
-                wd.pt[1] = (unsigned short)spi.write((int) LOW_REGISTER[i]); wait_us(10) ; 
+                wd.pt[1] = (unsigned short)spi.write((int) LOW_REGISTER[i]); //wait_us(1) ; 
                 if (i<5)  // dont this on the last because this was pre-called
-                {   wd.pt[0] = (unsigned short)spi.write((int) HIGH_REGISTER[i+1]); wait_us(10); }
+                {   wd.pt[0] = (unsigned short)spi.write((int) HIGH_REGISTER[i+1]);  }
                 imuRec.dataWord[i] = wd.dataWord; //data word is a signed long
             }
             IMURecordCounter++;
@@ -338,18 +325,9 @@
             {   
                 totalBytesWritten += fwrite(&imuRec, 1, sizeof(IMUREC), fpNav);
             }
-            IMUClockCounter++;
             IMUDataReady = false;
         }
         
-        /*
-        if (messageDetected)  //some GPS message header has been detected
-        {
-            toPC.printf(" msgTime = %4d \n", timeFromPPS.read_us());
-            messageDetected = false;
-        }
-        */
-        
         if (camera1EventDetected)  //we have detected a camera trigger event
         {
             toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time);
@@ -358,13 +336,14 @@
         
         if (detectedGPS1PPS)  //true if we are exactly at a 1PPS event detection
         {   
-            //toPC.printf(" PPSCounter=%4d byteCounter=%10d Msgs Received=%3d IMUClock=%4d bytesWritten=%8d\n", 
-            //                PPSCounter, savedByteCounter, savedPerSecMessageCounter, savedIMUClockCounter, totalBytesWritten);
-
+            toPC.printf("\n PPSCounter=%4d byteCounter=%10d Msgs Received=%3d IMUClock=%4d IMURec = %3d bytesWritten=%8d\n", 
+                            PPSCounter, savedByteCounter, savedPerSecMessageCounter, savedIMUClockCounter, IMURecordCounter, totalBytesWritten);
+            
+            IMURecordCounter = 0;
             detectedGPS1PPS = false;
         }
     }
-    
+      
     fclose(fpNav);
     toPC.printf(" normal termination \n");
 }
\ No newline at end of file