Diff: main.cpp

Revision:

1:cbb9104d826f

Parent:

0:c746ee34feae

Child:

2:7301e63832ee

--- a/main.cpp	Tue Mar 19 02:17:40 2013 +0000
+++ b/main.cpp	Tue Mar 19 23:06:15 2013 +0000
@@ -26,8 +26,8 @@
 DigitalOut ppsled(LED1);
 DigitalOut trig1led(LED2);
 DigitalOut recordDataled(LED4);
-InterruptIn Camera1Int(p30);
-InterruptIn Camera2Int(p29);
+InterruptIn camera1Int(p30);
+DigitalOut camera2Pin(p29);
 //USB serial data stream back to the PC
 Serial toPC(USBTX, USBRX); //connect the GPS TX, RX to p9 and p10
 
@@ -41,9 +41,7 @@
 int savedIMUClockCounter=0;
 int IMUClockCounter = 0;
 bool camera1EventDetected = false;
-bool camera2EventDetected = false;
 double camera1Time;
-double camera2Time;
 char serBuf[64];
 int serBufChars=0;
 bool sendPosVel=false;
@@ -70,20 +68,13 @@
 };
 
 //ISR for detection of the hotshoe trigger 1
-void Camera1ISR(void)
+void camera1ISR(void)
 {
     camera1Time = GPSTime + (double)PPSTimeOffset + timeFromPPS.read();
     //detectedTrigger1 = true;
     trig1led = !trig1led;
     camera1EventDetected = true;
 };
-//ISR for detection of the hotshoe trigger 2
-void Camera2ISR(void)
-{
-    camera2Time = GPSTime + (double)PPSTimeOffset + timeFromPPS.read();
-    //detectedTrigger1 = true;
-    camera2EventDetected = true;
-};
 
 void readFromPC()
 {
@@ -263,11 +254,10 @@
 
 void setupTriggers()
 {
-    Camera1Int.mode(PullUp);
-    Camera2Int.mode(PullUp);
+    camera1Int.mode(PullUp);
+    camera2Pin = 1;
     //establish Trigger ISR 
-    Camera1Int.rise(&Camera1ISR);
-    Camera2Int.rise(&Camera2ISR);
+    camera1Int.rise(&camera1ISR);
     
 }
 
@@ -275,7 +265,7 @@
 unsigned short messageCounter = 0;
 unsigned short savedMessageCounter = 0;
 unsigned char msgBuffer[1024];
-unsigned short messageLocation[4] = {0};
+unsigned short messageLocation[6] = {0};
 
 //see the mbed COOKBOOK for MODSERIAL
 //MODSERIAL is an easy to use library that extends Serial to add fully buffered input and output.
@@ -292,7 +282,7 @@
    
     if (test == 0xAA44121C) //test for the Receiver message header signature
     {
-        messageLocation[perSecMessageCounter] = byteCounter; //store the location of this message (with 4 synch words)
+        messageLocation[perSecMessageCounter] = byteCounter-3; //store the location of this message (with 4 synch words)
         perSecMessageCounter++;
         messageDetected = true;
      }   
@@ -309,6 +299,8 @@
     OEM615BESTPOS curPos;
     OEM615BESTVEL velMsg;
     OEM615BESTVEL curVel;
+    int msgLen;
+    int msgEnd;
 
     //set up th GPS and mbed COM ports
     setupCOM(); 
@@ -448,13 +440,36 @@
             }
             IMUDataReady = false;
         }
-        if (lookingForMessages && (timeFromPPS.read_us() > 15000))  //it takes less than 20msec to receive all messages
+        if (lookingForMessages && (timeFromPPS.read() > .015))  //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++)
             {
-                msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]-3];
+                msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]];
+                // Ensure complete message has been received
+                // Message length is header length + message length + CRC
+                msgLen = msgHeader[i]->headerLength + msgHeader[i]->messageLength + sizeof(unsigned long);
+                // Find last byte position of message
+                msgEnd = messageLocation[i] + msgLen;
+                if (byteCounter < msgEnd)
+                {
+                    // Complete message 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;
+                }
+                else if ((i < (perSecMessageCounter-1)) &&
+                         (messageLocation[i+i] < msgEnd))
+                {
+                    // Next message was started before this mesage was completely received.
+                    // Ignore and continue on to the next message
+                    continue;
+                }
                 //toPC.printf(" %5d ", msgHeader[i]->messageID);
                 if ((msgHeader[i]->messageID == 42) ||
                     (msgHeader[i]->messageID == 99))
@@ -463,8 +478,8 @@
                     {
                         // Wait until velocity message has also been received before using as
                         // base position
-                        //memcpy(&curPos, &msgBuffer[messageLocation[i]-3], sizeof(OEM615BESTPOS));
-                        curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[i]-3]);
+                        //memcpy(&curPos, &msgBuffer[messageLocation[i]], sizeof(OEM615BESTPOS));
+                        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",
@@ -491,8 +506,8 @@
                     {
                         // Wait until position message has also been received before using as
                         // base position
-                        //memcpy(&curVel, &msgBuffer[messageLocation[i]-3], sizeof(OEM615BESTVEL));
-                        curVel = *((OEM615BESTVEL*)&msgBuffer[messageLocation[i]-3]);
+                        //memcpy(&curVel, &msgBuffer[messageLocation[i]], sizeof(OEM615BESTVEL));
+                        curVel = *((OEM615BESTVEL*)&msgBuffer[messageLocation[i]]);
                     }
                     if ((curVel.msgHeader.GPSTime_msecs+250)/1000 == 
                         (curPos.msgHeader.GPSTime_msecs+250)/1000)
@@ -525,11 +540,6 @@
             toPC.printf("WMsg TRIGGERTIME Camera1 %5.3lf\n", camera1Time);
             camera1EventDetected = false;
         }
-        if (camera2EventDetected)
-        {
-            toPC.printf("WMsg TRIGGERTIME Camera2 %5.3lf\n", camera2Time);
-            camera2EventDetected = false;
-        }
         if (detectedGPS1PPS)
         {   
             //toPC.printf(" PPSCounter = %4d byteCounter = %10d num Messages Received = %3d IMUClock = %4d\n",