JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of GPS_Incremental by
Diff: main.cpp
- Revision:
- 9:b45feb91ba38
- Parent:
- 8:13724ed3f825
--- 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