james kain
JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of
GPS_Incremental
by 
Dan Matthews
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
diff -r 13724ed3f825 -r b45feb91ba38 ADIS16488.h
--- 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
diff -r 13724ed3f825 -r b45feb91ba38 OEM615.h
--- 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
};
diff -r 13724ed3f825 -r b45feb91ba38 main.cpp
--- 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