JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of GPS_Incremental by
Diff: main.cpp
- Revision:
- 2:7301e63832ee
- Parent:
- 1:cbb9104d826f
- Child:
- 3:5913df46f94a
--- a/main.cpp Tue Mar 19 23:06:15 2013 +0000 +++ b/main.cpp Wed Mar 20 21:52:48 2013 +0000 @@ -17,6 +17,8 @@ #define STOPDATA_MSG 3 #define STARTSTREAM_MSG 4 #define STOPSTREAM_MSG 5 +#define STARTLOGINFO_MSG 6 +#define STOPLOGINFO_MSG 7 #define DEGREES_TO_RADIANS (3.14519/180.0) //general items for this application @@ -42,13 +44,15 @@ int IMUClockCounter = 0; bool camera1EventDetected = false; double camera1Time; -char serBuf[64]; +char serBuf[128]; int serBufChars=0; bool sendPosVel=false; bool sendStatus=false; bool sendRecData=false; bool streamPos=false; bool sendStreamPos=false; +bool logMsgInfo=false; +bool sendLogMsgInfo=false; //ISR for detection of the GPS 1PPS @@ -59,7 +63,6 @@ IMUClockCounter = 0; GPS_COM1.rxBufferFlush(); - //byteCounter=0; detectedGPS1PPS = true; lookingForMessages = true; PPSCounter++; @@ -71,7 +74,6 @@ void camera1ISR(void) { camera1Time = GPSTime + (double)PPSTimeOffset + timeFromPPS.read(); - //detectedTrigger1 = true; trig1led = !trig1led; camera1EventDetected = true; }; @@ -87,18 +89,20 @@ serBuf[serBufChars] = '\0'; // Need to parse message to determine behavior // Need to clean this up - char msgList[6][32]; + char msgList[8][32]; sprintf(msgList[STATUS_MSG], "WMsg STATUS"); sprintf(msgList[POSVEL_MSG], "WMsg POSVEL"); sprintf(msgList[STARTDATA_MSG], "WMsg RECORDDATA Y"); sprintf(msgList[STOPDATA_MSG], "WMsg RECORDDATA N"); sprintf(msgList[STARTSTREAM_MSG], "WMsg POSSTREAM Y"); sprintf(msgList[STOPSTREAM_MSG], "WMsg POSSTREAM N"); + sprintf(msgList[STARTLOGINFO_MSG], "WMsg LOGINFO Y"); + sprintf(msgList[STOPLOGINFO_MSG], "WMsg LOGINFO N"); // assume an invalid message which needs to be reset bool validMessage = false; bool resetMessage = true; // Check for valid message - for (int m = 0; m < 6 && !validMessage; m++) + for (int m = 0; m < 8 && !validMessage; m++) { if (strncmp(serBuf, msgList[m], serBufChars) == 0) { @@ -124,6 +128,11 @@ streamPos = (m == STARTSTREAM_MSG); sendStreamPos = true; break; + case STARTLOGINFO_MSG: + case STOPLOGINFO_MSG: + logMsgInfo = (m == STARTLOGINFO_MSG); + sendLogMsgInfo = true; + break; } } @@ -183,6 +192,7 @@ //Launch the C++ serial port read program there to catch the ASCII characters //toPC.baud(9600); wait_ms(100); toPC.baud(8*115200); wait_ms(100); + //toPC.baud(1*115200); wait_ms(100); toPC.printf("\n\n released GPS from RESET and set to high baud rate \n\n"); //just wait to lauinch the GPS receiver @@ -264,7 +274,7 @@ int test = 0; unsigned short messageCounter = 0; unsigned short savedMessageCounter = 0; -unsigned char msgBuffer[1024]; +unsigned char msgBuffer[1536]; unsigned short messageLocation[6] = {0}; //see the mbed COOKBOOK for MODSERIAL @@ -287,7 +297,7 @@ messageDetected = true; } byteCounter++; //total per-sec byte counter (reset to zero in main when 1PPS detected) - + }; int main() { @@ -302,7 +312,7 @@ int msgLen; int msgEnd; - //set up th GPS and mbed COM ports + //set up the GPS and mbed COM ports setupCOM(); //set up the ADIS16488 @@ -430,6 +440,18 @@ streamChar ); } + if (sendLogMsgInfo) + { + sendLogMsgInfo=false; + char logChar = 'N'; + if (logMsgInfo) + { + logChar = 'Y'; + } + toPC.printf("WMsg LOGINFO %c\n", + logChar + ); + } if (IMUDataReady) { //write the IMU data @@ -440,15 +462,41 @@ } IMUDataReady = false; } - if (lookingForMessages && (timeFromPPS.read() > .015)) //it takes less than 20msec to receive all messages + //if (lookingForMessages && (timeFromPPS.read() > 0.100)) //it takes less than 20msec to receive all messages { //toPC.printf(" num messages = %3d time = %5d \n", perSecMessageCounter, timeFromPPS.read_us()); for (int i=0; i<perSecMessageCounter; i++) { + // ensure at message header has arrived before processing + if (byteCounter < (messageLocation[i] + sizeof(MESSAGEHEADER))) + { + // Complete message header has not been received. + // Clear processed messages and break out of message processing loop + for (int j = 0; j < i; j++) + { + messageLocation[j] = messageLocation[i+j]; + perSecMessageCounter--; + } + break; + } msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]]; // Ensure complete message has been received // Message length is header length + message length + CRC + msgLen = 28; + switch (msgHeader[i]->messageID) + { + case 42: + msgLen = 104; + break; + case 99: + msgLen = 76; + break; + default: + msgLen = msgHeader[i]->headerLength + msgHeader[i]->messageLength + sizeof(unsigned long); + break; + + } msgLen = msgHeader[i]->headerLength + msgHeader[i]->messageLength + sizeof(unsigned long); // Find last byte position of message msgEnd = messageLocation[i] + msgLen; @@ -464,12 +512,23 @@ break; } else if ((i < (perSecMessageCounter-1)) && - (messageLocation[i+i] < msgEnd)) + (messageLocation[i+i] < msgEnd)) // ignore CRC for now { // Next message was started before this mesage was completely received. // Ignore and continue on to the next message continue; } + if (logMsgInfo) + { + toPC.printf("WMsg MESSAGEINFO %5d %5d %5d %5d %5d = %5d (%5d)\n", + msgHeader[i]->messageID, + messageLocation[i], + msgHeader[i]->headerLength, + msgHeader[i]->messageLength, + sizeof(unsigned long), + msgEnd, + byteCounter); + } //toPC.printf(" %5d ", msgHeader[i]->messageID); if ((msgHeader[i]->messageID == 42) || (msgHeader[i]->messageID == 99)) @@ -520,15 +579,26 @@ PPSTimeOffset = 0; } } + if (i == (perSecMessageCounter-1)) + { + if (recordData && (fpNav != NULL)) + { + fwrite(msgBuffer, byteCounter, 1, fpNav); + } + byteCounter = 0; + perSecMessageCounter = 0; + } } //toPC.printf(" %3d %8d \n", msgHeader[0]->timeStatus, GPSTimemsecs); //if (recordData && (fpGPS != NULL)) + + /* if (recordData && (fpNav != NULL)) { fwrite(msgBuffer, byteCounter, 1, fpNav); } - - lookingForMessages = false; + */ + //lookingForMessages = false; } if (messageDetected) { @@ -537,13 +607,17 @@ } if (camera1EventDetected) { - toPC.printf("WMsg TRIGGERTIME Camera1 %5.3lf\n", camera1Time); + toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time); camera1EventDetected = false; } if (detectedGPS1PPS) { //toPC.printf(" PPSCounter = %4d byteCounter = %10d num Messages Received = %3d IMUClock = %4d\n", // PPSCounter, byteCounter, perSecMessageCounter, savedIMUClockCounter); + if (recordData && (fpNav != NULL) && (byteCounter > 0)) + { + fwrite(msgBuffer, byteCounter, 1, fpNav); + } byteCounter = 0; perSecMessageCounter=0; detectedGPS1PPS = false;