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;