Diff: main.cpp

Revision:

30:96d133f3008e

Parent:

29:dead10cce6e9

--- a/main.cpp	Thu Jan 09 14:09:05 2014 +0000
+++ b/main.cpp	Mon Mar 03 13:19:31 2014 +0000
@@ -3,6 +3,7 @@
 
 //set up the message buffer to be filled by the GPS read process
 #define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 256 
+#define CRC32_POLYNOMIAL 0xEDB88320
 
 #include "MODSERIAL.h"
 #include "SDFileSystem.h"      //imported using the import utility    
@@ -28,8 +29,7 @@
 Serial toPC(USBTX, USBRX);      //connect the GPS TX, RX to p9 and p10
 
 Timer timeFromStart;
-
-
+Timer timeInMessageRead;
 
 bool detectedGPS1PPS = false;       //flag set in the ISR and reset after processing the 1PPS event
 int PPSCounter = 0;                 //counts the 1PPS occurrences
@@ -56,8 +56,9 @@
 void detect1PPSISR(void)
 {
     timeFromPPS.reset();                    //reset the 1PPS timer upon 1PPS detection
-    //note -- the below accounts for time information becoming available AFTER the 1PPS event
-    PPSTimeOffset++;                //counts 1PPS events between matching POS and VEL messages
+    
+    //note -- the below accounts for GPS receiver time information becoming available AFTER the 1PPS event
+    PPSTimeOffset++;   //counts 1PPS events -- reset to zero in main after we determine a GPS time
     
     //covers the case where the PPS ISR interrupts the IMU data ready ISR
     if(IMUDataReady) IMUtimeFrom1PPS = 0;
@@ -109,8 +110,12 @@
    
     // this ASCII command sets up the serial data from the GPS receiver on its COM1
     char ch7[] = "serialconfig COM1 9600 n 8 1 n off";
+    
     // this is a software reset and has the same effect as a hardware reset (why do it?) 
     //char ch0[] = "RESET"; 
+    //toPC.printf("WMsg set RESET command \n");
+    //sendASCII(ch0, sizeof(ch0)); wait_ms(3000);    
+    
     //this command stops all communication from the GPS receiver on COM1
     //logs should still be presented on USB port so the Novatel CDU application can be used on the PC in parallel
     char ch1[] = "unlogall COM1";
@@ -161,15 +166,76 @@
 };
 
 
+void CRC32Value_2(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 from the complete message (header plus data) 
+//but excluding (of course) the CRC at the end
+-------------------------------------------------------------------------- */
+unsigned long CalculateBlockCRC32_2(
+        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_2( CRC, *ucBuffer++ );
+    return  CRC;
+}
+
+bool checkMessageCRC(char* message, int byteCounter)
+{
+    //calculate the Novatel OEM615 CRC
+    //length of CRC bytes is string length minus:  8 bytes for long word, 2 bytes for "#" & "*" and 2 bytes for CR, LF 
+    // start at byte 1 (not zero) to skip the "#" at the start                            
+    unsigned long computedCRC = CalculateBlockCRC32_2((unsigned long)(byteCounter-12), (unsigned char*)(&message[1]));
+    
+    //get the computed CRC into a zero-padded string so we can compare to the string CRC in the message
+    char CRCCharBuffer [50];
+    int n=sprintf (CRCCharBuffer, "%08x", computedCRC);  //note the zero-padding
+    //toPC.printf("n = %d  charCRC = %s \n", n, CRCCharBuffer);
+    
+    //get a string reresentation of just the CRC from the message
+    char CRCString[10];
+    strncpy(CRCString, &message[byteCounter-10], 8);
+    //toPC.printf(" %s  %s  \n", CRCCharBuffer, CRCString);
+    
+    if (strncmp(CRCCharBuffer, CRCString, 8) != 0)
+    {
+            toPC.printf("mbed bad CRC %d  %s  %d  %s  \n", strlen(CRCCharBuffer), CRCCharBuffer, strlen(CRCString), CRCString );
+            return false;
+    }
+    
+    return true;
+}
+
+
 /////////////////////////////////////////////////////////////////////
 //  mbed main to support the Waldo_FCS
 /////////////////////////////////////////////////////////////////////
 int main() 
 {
     
-    toPC.printf("initiating the mbed app\n");
-    
-    fire.output();  //set the fire pin as outoput
+    fire.output();      //set the fire pin as output
     pre_fire.output();  //set the pre-fire pin as output
     
     //fire.mode(OpenDrain);
@@ -180,16 +246,20 @@
     
     //set up the GPS and mbed COM ports
     setupCOM(); 
-    toPC.printf("Completed setting up GPS \n");
+    
+    //prints to PC above here wont work cause the PC serial link baud rate has not been set
+    toPC.printf("initiating the mbed app\n");
 
+    toPC.printf("setting up the ADIS\n");
     //set up the ADIS16488 IMU
-    //setupADIS();
+    setupADIS();
 
     //initiate the interrupt to catch the GPS receiver serial bytes as they are presented
     GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq);
     
     timeFromPPS.start();  //start the time for measuring time from 1PPS events
     timeFromStart.start();
+    timeInMessageRead.start();  //used to prevent hanging in the PC message read loop
     
     unsigned long cyclesPerSec = 0;  //main while() loop cycles per GPS sec
 
@@ -197,40 +267,101 @@
     Timer triggerInterval;
     
     bool newMission = true;
+    int workingTime = 0;
+    
+    bool triggeringInProcess = false;
+    
+    int badRangeMessages = 0;
+    int badBESPOSMessages = 0;
+    int badBESVELMessages = 0;
+    
+    int VERSION = 1;
+    double timeAtMessage0 = 0;
     
     ///////////////////////////////////////////////////////////////////////////
     // top of the mission while loop
     ///////////////////////////////////////////////////////////////////////////
     while(newMission)
     {         
-        if (message0Complete)
+        readFromPC();   //#1
+                
+        //process GPS data messages (messages 0, 1, 2) 
+        if (toPC.writeable() && message0Complete && !IMUDataReady )
         {
-            for (int i=0; i<GPSbyteCounter0; i++)   toPC.printf("%c", msgBuffer0[i]);
-            toPC.printf("\n");
-            message0Complete = false;
+            int thisWorkTime = timeFromStart.read_us();
+            
+            //get the GPS time at the prior 1PPS from the first header
+            char msgName [15], port [15], clockStatus [15];
+            int a1=0, GPSWeek=0;
+            double b1 = 0.0, GPSTimeFromHeader = 0.0;  
+            
+            //note the use of the "C" scanset [^,] to read the comma-delimited ASCII message provided by te Novatel OEM615 receiver
+            //these are used only for the character variables in order to avoid detecting the "," as another character
+            //only spaces can be used as delimiters if scanset methodology is not used         
+            int N = sscanf(msgBuffer0,"%15[^,],%15[^,],%d,%f,%15[^,],%d,%lf", msgName, port, &a1, &b1, clockStatus, &GPSWeek, &GPSTimeFromHeader);
+            if (N != 7)  toPC.printf("scanf failed for message: %s ", msgBuffer0);
+
+            GPSTimemsecs = GPSTimeFromHeader * 1000;
+            
+            //this is used to account for the GPS time from the receiver becoming available a few msecs after the actual 1PPS event
+            //A precision timer is set at the 1PPS event that is used to tag the IMU data
+            timeAtMessage0 = timeFromPPS.read_us()/1000.0;
+            PPSTimeOffset = 0.0;
+            
+            if (!checkMessageCRC(msgBuffer0, GPSbyteCounter0)) badRangeMessages++;
+            
+            N=0;
+            //write the GPS message bytes to the PC
+            for (int i=0; i<GPSbyteCounter0; i++)   N += toPC.printf("%c", msgBuffer0[i]);
+            if (N != GPSbyteCounter0) toPC.printf(" error writing msgBuffer0: %d of %d bytes written \n", N, GPSbyteCounter0);
+            
+            message0Complete = false; //set up for the next message
+            workingTime += timeFromStart.read_us() - thisWorkTime;
+            
+            readFromPC();   //#2
         } 
-        else if (message1Complete)
+        else if (toPC.writeable() && message1Complete && !IMUDataReady)
         {
-            for (int i=0; i<GPSbyteCounter1; i++)   toPC.printf("%c", msgBuffer1[i]);
-            toPC.printf("\n");
+            if (!checkMessageCRC(msgBuffer1, GPSbyteCounter1)) badBESPOSMessages++;
+                        
+            int thisWorkTime = timeFromStart.read_us();
+            
+            int N=0;
+            for (int i=0; i<GPSbyteCounter1; i++)   N += toPC.printf("%c", msgBuffer1[i]);
+            if (N != GPSbyteCounter1) toPC.printf(" error writing msgBuffer1: %d of %d bytes written \n", N, GPSbyteCounter1);
+            
             message1Complete = false;
+            workingTime += timeFromStart.read_us() - thisWorkTime;
+            
+            readFromPC();   //#3
         }    
-        else if (message2Complete)
+        else if (toPC.writeable() && message2Complete && !IMUDataReady)
         {
-            for (int i=0; i<GPSbyteCounter2; i++)   toPC.printf("%c", msgBuffer2[i]);
-            toPC.printf("\n");
+            if( !checkMessageCRC(msgBuffer2, GPSbyteCounter2)) badBESVELMessages++;
+            
+            int thisWorkTime = timeFromStart.read_us();
+            int N = 0;
+            
+            for (int i=0; i<GPSbyteCounter2; i++)   N += toPC.printf("%c", msgBuffer2[i]);
+            if (N != GPSbyteCounter2) toPC.printf(" error writing msgBuffer2: %d of %d bytes written \n", N, GPSbyteCounter2);
+            
             message2Complete = false;
-        }         
-        /*
-        if (writeIMUDataToPC)
+            workingTime += timeFromStart.read_us() - thisWorkTime;
+            
+            readFromPC();   //#4
+        } 
+        //       
+        //
+        if (toPC.writeable() && IMUDataReady)
         {
+            int thisWorkTime = timeFromStart.read_us();
             //write the IMU data to the PC
-            toPC.printf("IMURECORD ");  // 9 bytes header
             for (int i=0; i<IMUrecArraySize; i++)
             {
+                //
                 if (fillingPingWritingPong)
                 {
-                    toPC.printf("%d9 %d9 %d9 %d9 %d9 %d9 %d9", //70  bytes
+                    toPC.printf("IMU %d13,%d13,%d13,%d13,%d13,%d13,%d13 " , //70  bytes
                     imuPong[i].GPSTime, 
                     imuPong[i].dataWord[0],                    
                     imuPong[i].dataWord[1],
@@ -241,7 +372,7 @@
                 }
                 else
                 {
-                   toPC.printf("%d9 %d9 %d9 %d9 %d9 %d9 %d9", 
+                    toPC.printf("IMU %d13,%d13,%d13,%d13,%d13,%d13,%d13 " , //70  bytes
                     imuPing[i].GPSTime, 
                     imuPing[i].dataWord[0],                    
                     imuPing[i].dataWord[1],
@@ -250,23 +381,26 @@
                     imuPing[i].dataWord[4],
                     imuPing[i].dataWord[5]  );                
                 }
+                //
+                readFromPC();   //#5
             }
-            toPC.printf("/n");  //total of 10 * 70 + 9 = 709 bytes
-            writeIMUDataToPC = false;
+            toPC.printf("\n");  //total of 10 * 70 + 9 = 709 bytes
+            IMUDataReady = false;
+            workingTime += timeFromStart.read_us() - thisWorkTime;
         }
-        */
             
         //read the USB serial data from the PC to check for commands
-        //only message we expect will be thr trigger command
+        //only message we expect will be the trigger fire command
         readFromPC();
 
-        if(fireTrigger)  //comes from a PC request message
+        if(toPC.writeable() && fireTrigger)  //comes from a PC request message
         {
+            triggeringInProcess = true;
             unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000.0 + timeFromPPS.read_us()/1000.0;
-            toPC.printf("WMsg TRIGGERTIME %10d \n", triggerTime);
+            toPC.printf("mbedmessage trig1 %d \n", triggerTime);
             //pre-fire the trigger using the mid-body 2.5mm connection (T2i)
             pre_fire = 0;  //pin30 (midbody of connector) set to zero
-            wait(.01f);  // not sure what this does
+            
             fire = 0; //fire the trigger using the tip connection
             fireTrigger = false;  //finished the setup -- but wait to do the actual fire 
             finishTrigger = true; //set to false after firing the trigger
@@ -275,31 +409,32 @@
         
         //the trigger requires a pulse -- the above portion lowers the signal and the below raises it
         //this has been tested at 50 msecs and it will not fire at that pulse duration
-        if(finishTrigger && triggerInterval.read_ms() > 100)
+        if(toPC.writeable() && finishTrigger && triggerInterval.read_ms() > 100)
         {
+            unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000.0 + timeFromPPS.read_us()/1000.0;
+            toPC.printf("mbedmessage trig2 %d \n", triggerTime);
+            //pre-fire the trigger using 2the mid-body 2.5mm connec
             fire = 1;
             pre_fire = 1;
             triggerInterval.reset();
             finishTrigger = false;  //completes the trigger firing pulse definition
+            triggeringInProcess = false;
         }
         
         cyclesPerSec++;
-
-        //this is a command from the PC to fire a trigger
-        if (camera1EventDetected)  //we have detected a camera trigger event
-        {
-            toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time);
-            camera1EventDetected = false;
-        }
-        
-        /* managed in GPS class */
         
         if (detectedGPS1PPS)  //true if we are exactly at a 1PPS event detection
         {   
-            toPC.printf("detected GPS 1PPS now  %5d %10d\n", PPSCounter, totalGPSBytes);
+            if (toPC.writeable() )
+            {   
+                toPC.printf("STATUS %03d %04d %06d %02d %06d %5.3f %03d %03d %03d\n", 
+                VERSION, PPSCounter, totalGPSBytes, IMURecordCounter, cyclesPerSec, timeAtMessage0,  /*workingTime/1000000.0,*/ 
+                badRangeMessages, badBESVELMessages, badBESVELMessages);
+            }  
             totalGPSBytes=0;
                             
             cyclesPerSec = 0;
+            workingTime= 0.0;
             
             messagePerSecCounter = 0;   //GPS message per second counter
 
@@ -308,6 +443,8 @@
             
             rxMsg = !rxMsg;  //flash the lights to make sure the mbed loop is operative
             txMsg = !txMsg;
+            
+            readFromPC();   //#6
         }
     ///////////////////////////////////////////
     }  //end of the major while() loop 
@@ -315,4 +452,5 @@
       
 
     
-}
\ No newline at end of file
+}
+