main.cpp

00001 #include "mbed.h" 
00002 #include <string>
00003 
00004 //set up the message buffer to be filled by the GPS read process
00005 #define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 256 
00006 #define CRC32_POLYNOMIAL 0xEDB88320
00007 
00008 #include "MODSERIAL.h"
00009 #include "SDFileSystem.h"      //imported using the import utility    
00010 
00011 //general digital I/O specifications for this application
00012 
00013 //if we dont write to the card we dont need this ... 
00014 //SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name);
00015 SDFileSystem sd(p11,p12,p13,p14,"sd");
00016 
00017 DigitalIn sd_detect(p27);
00018 DigitalOut ppsled(LED1);        //blink an LED at the 1PPS
00019 DigitalOut trig1led(LED2);      //blink an LED at the camera trigger detection
00020 DigitalOut rxMsg(LED3);
00021 DigitalOut txMsg(LED4);
00022 //DigitalOut recordDataled(LED4); //set the led when the record is on
00023 
00024 //hardware trigger mechanization for bulb shutter commands
00025 DigitalInOut fire(p29);     //connected to the tip of 2.5mm connector (T2i)
00026 DigitalInOut pre_fire(p30); //connected to the mid-connection for 2.5mm connector (T2i)
00027 
00028 //USB serial data stream back to the PC
00029 Serial toPC(USBTX, USBRX);      //connect the GPS TX, RX to p9 and p10
00030 
00031 Timer timeFromStart;
00032 Timer timeInMessageRead;
00033 
00034 bool detectedGPS1PPS = false;       //flag set in the ISR and reset after processing the 1PPS event
00035 int PPSCounter = 0;                 //counts the 1PPS occurrences
00036 int byteCounter = 0;                //byte counter -- zeroed at 1PPS
00037 unsigned short perSecMessageCounter=0; //counts the number of messages in a sec based on the header detection
00038 bool messageDetected = false;       //have detected a message header
00039 unsigned long IMUbytesWritten = 0;  //counts the IMU bytes written by the fwrite() to the SD card 
00040 int savedByteCounter = 0;           //save ByteCounter at the 1PPS for display in main
00041 int savedPerSecMessageCounter=0;    //saved PerSecMsgCounter for display in main
00042 int savedIMUClockCounter=0;         //saved at the 1PPS for later diaplay from main
00043 bool camera1EventDetected = false;  //flag from ISR indicating a clock event occurred
00044 double camera1Time;                 //GPS time of the camera event 
00045 volatile int PPSTimeOffset = 0;
00046 int totalGPSBytes = 0;
00047 
00048 bool writeIMUDataToPC = false;      //used in main loop to cause IMU data write to the PC
00049 bool writeGPSDataToPC = false;      //used in main loop to cause GPS data write to the PC
00050 
00051 #include "OEM615.h"         //OEM615 GPS activities
00052 #include "ADIS16488.h"      //ADIS16488 activities
00053 #include "PCMessaging.h"    //PC messaging activities
00054 
00055 //ISR for detection of the GPS 1PPS
00056 void detect1PPSISR(void)
00057 {
00058     timeFromPPS.reset();                    //reset the 1PPS timer upon 1PPS detection
00059     
00060     //note -- the below accounts for GPS receiver time information becoming available AFTER the 1PPS event
00061     PPSTimeOffset++;   //counts 1PPS events -- reset to zero in main after we determine a GPS time
00062     
00063     //covers the case where the PPS ISR interrupts the IMU data ready ISR
00064     if(IMUDataReady) IMUtimeFrom1PPS = 0;
00065             
00066     savedByteCounter = byteCounter;         //save byteCounter for display in main
00067     savedPerSecMessageCounter = perSecMessageCounter;   //save for display un main
00068     byteCounter = 0;                        //countes bytes between 1PPS events 
00069     perSecMessageCounter = 0;               //counts GPS messages between 1PPS events
00070     
00071     GPS_COM1.rxBufferFlush();               //flush the GPS serial buffer
00072     
00073     detectedGPS1PPS = true;         //set false in the main when 1PPS actions are complete
00074     PPSCounter++;                   //count number of 1PPS epoch
00075     
00076     ppsled = !ppsled;               //blink an LED at the 1PPS
00077 };
00078 
00079 
00080 ///////////////////////////////////////////////////////
00081 //set up the USB port and the GPS COM port
00082 /////////////////////////////////////////////////////// 
00083 void setupCOM(void)
00084 {
00085     //system starts with GPS in reset active
00086     //dis-engage the reset to get the GPS started
00087     GPS_Reset=1; wait_ms(1000); 
00088     
00089     //establish 1PPS ISR 
00090     PPSInt.rise(&detect1PPSISR);
00091     
00092     //set the USB serial data rate -- rate must be matched at the PC end
00093     //This the serial communication back to the the PC host
00094     //Launch the C++ serial port read program there to catch the ASCII characters
00095     //toPC.baud(9600); wait_ms(100);    
00096     toPC.baud(8*115200); wait_ms(100);
00097     //toPC.baud(1*115200); wait_ms(100);
00098     //toPC.printf("\n\n released GPS from RESET and set to high baud rate \n\n");
00099     
00100     //just wait some time to launch the GPS receiver
00101     for (int i=0; i<5; i++) { toPC.printf("WMessage start countdown: %3d \n", 4-i); wait(1); }
00102 
00103     //NOTE:  we do not assume that the GPS receiver has been pre-set up for the WALDO_FCS functionality
00104     //we alwsys start with a reset and reprogram the receiver with our data out products
00105     // this prevents failure because of a blown NVRAM as occurred for the older camera systems
00106     
00107     //this is the COM1 port from th GPS receiver to the mbed
00108     //it should be always started at 9600 baud because thats the default for the GPS receiver 
00109     GPS_COM1.baud(9600); wait_ms(100);
00110    
00111     // this ASCII command sets up the serial data from the GPS receiver on its COM1
00112     char ch7[] = "serialconfig COM1 9600 n 8 1 n off";
00113     
00114     // this is a software reset and has the same effect as a hardware reset (why do it?) 
00115     //char ch0[] = "RESET"; 
00116     //toPC.printf("WMsg set RESET command \n");
00117     //sendASCII(ch0, sizeof(ch0)); wait_ms(3000);    
00118     
00119     //this command stops all communication from the GPS receiver on COM1
00120     //logs should still be presented on USB port so the Novatel CDU application can be used on the PC in parallel
00121     char ch1[] = "unlogall COM1";
00122     //set the final baud rate that we will use from here  
00123     //allowable baud rate values: 9600 115200 230400 460800 921600
00124     //char ch2[] = "serialconfig COM1 921600 n 8 1 n off";
00125     char ch2[] = "serialconfig COM1 115200 n 8 1 n off";
00126     
00127     //the below commands request the POS, VEL, RANGE, and TIME messages
00128     //Binary commands are shown below
00129     //char ch3[] = "log COM1 BESTPOSB ONTIME 1";   //messageID = 42 
00130     //char ch4[] = "log COM1 BESTVelB ONTIME 1";   //messageID = 99
00131     //char ch5[] = "log COM1 RANGEB ONTIME 1";     //messageID = 43
00132     //char ch6[] = "log COM1 TIMEB ONTIME 1";      //messageID = 101
00133 
00134     //ASCII commands are shown below
00135     char ch3[] = "log COM1 BESTPOSA ONTIME 1";   //messageID = 42 
00136     char ch4[] = "log COM1 BESTVelA ONTIME 1";   //messageID = 99
00137     char ch5[] = "log COM1 RANGEA ONTIME 1";     //messageID = 43
00138     //char ch6[] = "log COM1 TIMEB ONTIME 1";      //messageID = 101
00139     
00140     //toPC.printf("WMsg set serial config \n");
00141     sendASCII(ch7, sizeof(ch7)); wait_ms(500);
00142     //sendASCII(ch0, sizeof(ch0));  
00143     //toPC.printf("WMsg unlog all messages \n");
00144     sendASCII(ch1, sizeof(ch1)); wait_ms(500);
00145     //toPC.printf("WMsg log BESTPOSB on COM1 \n");
00146     sendASCII(ch3, sizeof(ch3)); wait_ms(500);
00147     //toPC.printf("WMsg log BESTVELB on COM1\n");
00148     sendASCII(ch4, sizeof(ch4)); wait_ms(500);
00149     //toPC.printf("WMsg log RANGEB on COM1\n");
00150     sendASCII(ch5, sizeof(ch5)); wait_ms(500);
00151     
00152     //toPC.printf("log TIMEB om COM1 \n");
00153     //sendASCII(ch6, sizeof(ch6)); wait_ms(100);
00154     
00155     //toPC.printf("Set up th VARF signal \n"); 
00156     //sendASCII(ch8, sizeof(ch8)); wait_ms(500);
00157        
00158     //set GPS output COM1 to the final high rate
00159     //toPC.printf("WMsg set the COM ports to high rate\n");
00160     sendASCII(ch2, sizeof(ch2)); wait_ms(500);
00161     
00162     //set the mbed COM port to match the GPS transmit rate
00163     //the below baud rate must match the COM1 rate coming from the GPS receiver 
00164     GPS_COM1.baud(115200); wait_ms(500);  //without this wait -- the baud rate is not detected when using MODSERIAL     
00165     //GPS_COM1.baud(921600); wait_ms(500);  //without this wait -- the baud rate is not detected when using MODSERIAL     
00166 };
00167 
00168 
00169 void CRC32Value_2(unsigned long &CRC, unsigned char c)
00170 {
00171     /////////////////////////////////////////////////////////////////////////////////////
00172     //CRC must be initialized as zero 
00173     //c is a character from the sequence that is used to form the CRC
00174     //this code is a modification of the code from the Novatel OEM615 specification
00175     /////////////////////////////////////////////////////////////////////////////////////
00176     unsigned long ulTemp1 = ( CRC >> 8 ) & 0x00FFFFFFL;
00177     unsigned long ulCRC = ((int) CRC ^ c ) & 0xff ;
00178     for (int  j = 8 ; j > 0; j-- )
00179     {
00180         if ( ulCRC & 1 )
00181             ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL;
00182         else
00183             ulCRC >>= 1;
00184     }
00185     CRC = ulTemp1 ^ ulCRC;
00186 } 
00187 
00188 /* --------------------------------------------------------------------------
00189 Calculates the CRC-32 of a block of data all at once
00190 //the CRC is from the complete message (header plus data) 
00191 //but excluding (of course) the CRC at the end
00192 -------------------------------------------------------------------------- */
00193 unsigned long CalculateBlockCRC32_2(
00194         unsigned long ulCount,    /* Number of bytes in the data block */
00195         unsigned char *ucBuffer ) /* Data block */
00196 {
00197     //////////////////////////////////////////////////////////////////////
00198     //the below code tests the CRC32Value procedure used in a markov form
00199     //////////////////////////////////////////////////////////////////////
00200     unsigned long CRC = 0;
00201     for (int i = 0; i<ulCount; i++)  CRC32Value_2( CRC, *ucBuffer++ );
00202     return  CRC;
00203 }
00204 
00205 bool checkMessageCRC(char* message, int byteCounter)
00206 {
00207     //calculate the Novatel OEM615 CRC
00208     //length of CRC bytes is string length minus:  8 bytes for long word, 2 bytes for "#" & "*" and 2 bytes for CR, LF 
00209     // start at byte 1 (not zero) to skip the "#" at the start                            
00210     unsigned long computedCRC = CalculateBlockCRC32_2((unsigned long)(byteCounter-12), (unsigned char*)(&message[1]));
00211     
00212     //get the computed CRC into a zero-padded string so we can compare to the string CRC in the message
00213     char CRCCharBuffer [50];
00214     int n=sprintf (CRCCharBuffer, "%08x", computedCRC);  //note the zero-padding
00215     //toPC.printf("n = %d  charCRC = %s \n", n, CRCCharBuffer);
00216     
00217     //get a string reresentation of just the CRC from the message
00218     char CRCString[10];
00219     strncpy(CRCString, &message[byteCounter-10], 8);
00220     //toPC.printf(" %s  %s  \n", CRCCharBuffer, CRCString);
00221     
00222     if (strncmp(CRCCharBuffer, CRCString, 8) != 0)
00223     {
00224             toPC.printf("mbed bad CRC %d  %s  %d  %s  \n", strlen(CRCCharBuffer), CRCCharBuffer, strlen(CRCString), CRCString );
00225             return false;
00226     }
00227     
00228     return true;
00229 }
00230 
00231 
00232 /////////////////////////////////////////////////////////////////////
00233 //  mbed main to support the Waldo_FCS
00234 /////////////////////////////////////////////////////////////////////
00235 int main() 
00236 {
00237     
00238     fire.output();      //set the fire pin as output
00239     pre_fire.output();  //set the pre-fire pin as output
00240     
00241     //fire.mode(OpenDrain);
00242     
00243     //set up for the first trigger
00244     fire = 1;
00245     pre_fire = 1;
00246     
00247     //set up the GPS and mbed COM ports
00248     setupCOM(); 
00249     
00250     //prints to PC above here wont work cause the PC serial link baud rate has not been set
00251     toPC.printf("initiating the mbed app\n");
00252 
00253     toPC.printf("setting up the ADIS\n");
00254     //set up the ADIS16488 IMU
00255     setupADIS();
00256 
00257     //initiate the interrupt to catch the GPS receiver serial bytes as they are presented
00258     GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq);
00259     
00260     timeFromPPS.start();  //start the time for measuring time from 1PPS events
00261     timeFromStart.start();
00262     timeInMessageRead.start();  //used to prevent hanging in the PC message read loop
00263     
00264     unsigned long cyclesPerSec = 0;  //main while() loop cycles per GPS sec
00265 
00266     bool finishTrigger = false;
00267     Timer triggerInterval;
00268     
00269     bool newMission = true;
00270     int workingTime = 0;
00271     
00272     bool triggeringInProcess = false;
00273     
00274     int badRangeMessages = 0;
00275     int badBESPOSMessages = 0;
00276     int badBESVELMessages = 0;
00277     
00278     int VERSION = 1;
00279     double timeAtMessage0 = 0;
00280     
00281     ///////////////////////////////////////////////////////////////////////////
00282     // top of the mission while loop
00283     ///////////////////////////////////////////////////////////////////////////
00284     while(newMission)
00285     {         
00286         readFromPC();   //#1
00287                 
00288         //process GPS data messages (messages 0, 1, 2) 
00289         if (toPC.writeable() && message0Complete && !IMUDataReady )
00290         {
00291             int thisWorkTime = timeFromStart.read_us();
00292             
00293             //get the GPS time at the prior 1PPS from the first header
00294             char msgName [15], port [15], clockStatus [15];
00295             int a1=0, GPSWeek=0;
00296             double b1 = 0.0, GPSTimeFromHeader = 0.0;  
00297             
00298             //note the use of the "C" scanset [^,] to read the comma-delimited ASCII message provided by te Novatel OEM615 receiver
00299             //these are used only for the character variables in order to avoid detecting the "," as another character
00300             //only spaces can be used as delimiters if scanset methodology is not used         
00301             int N = sscanf(msgBuffer0,"%15[^,],%15[^,],%d,%f,%15[^,],%d,%lf", msgName, port, &a1, &b1, clockStatus, &GPSWeek, &GPSTimeFromHeader);
00302             if (N != 7)  toPC.printf("scanf failed for message: %s ", msgBuffer0);
00303 
00304             GPSTimemsecs = GPSTimeFromHeader * 1000;
00305             
00306             //this is used to account for the GPS time from the receiver becoming available a few msecs after the actual 1PPS event
00307             //A precision timer is set at the 1PPS event that is used to tag the IMU data
00308             timeAtMessage0 = timeFromPPS.read_us()/1000.0;
00309             PPSTimeOffset = 0.0;
00310             
00311             if (!checkMessageCRC(msgBuffer0, GPSbyteCounter0)) badRangeMessages++;
00312             
00313             N=0;
00314             //write the GPS message bytes to the PC
00315             for (int i=0; i<GPSbyteCounter0; i++)   N += toPC.printf("%c", msgBuffer0[i]);
00316             if (N != GPSbyteCounter0) toPC.printf(" error writing msgBuffer0: %d of %d bytes written \n", N, GPSbyteCounter0);
00317             
00318             message0Complete = false; //set up for the next message
00319             workingTime += timeFromStart.read_us() - thisWorkTime;
00320             
00321             readFromPC();   //#2
00322         } 
00323         else if (toPC.writeable() && message1Complete && !IMUDataReady)
00324         {
00325             if (!checkMessageCRC(msgBuffer1, GPSbyteCounter1)) badBESPOSMessages++;
00326                         
00327             int thisWorkTime = timeFromStart.read_us();
00328             
00329             int N=0;
00330             for (int i=0; i<GPSbyteCounter1; i++)   N += toPC.printf("%c", msgBuffer1[i]);
00331             if (N != GPSbyteCounter1) toPC.printf(" error writing msgBuffer1: %d of %d bytes written \n", N, GPSbyteCounter1);
00332             
00333             message1Complete = false;
00334             workingTime += timeFromStart.read_us() - thisWorkTime;
00335             
00336             readFromPC();   //#3
00337         }    
00338         else if (toPC.writeable() && message2Complete && !IMUDataReady)
00339         {
00340             if( !checkMessageCRC(msgBuffer2, GPSbyteCounter2)) badBESVELMessages++;
00341             
00342             int thisWorkTime = timeFromStart.read_us();
00343             int N = 0;
00344             
00345             for (int i=0; i<GPSbyteCounter2; i++)   N += toPC.printf("%c", msgBuffer2[i]);
00346             if (N != GPSbyteCounter2) toPC.printf(" error writing msgBuffer2: %d of %d bytes written \n", N, GPSbyteCounter2);
00347             
00348             message2Complete = false;
00349             workingTime += timeFromStart.read_us() - thisWorkTime;
00350             
00351             readFromPC();   //#4
00352         } 
00353         //       
00354         //
00355         if (toPC.writeable() && IMUDataReady)
00356         {
00357             int thisWorkTime = timeFromStart.read_us();
00358             //write the IMU data to the PC
00359             for (int i=0; i<IMUrecArraySize; i++)
00360             {
00361                 //
00362                 if (fillingPingWritingPong)
00363                 {
00364                     toPC.printf("IMU %d13,%d13,%d13,%d13,%d13,%d13,%d13 " , //70  bytes
00365                     imuPong[i].GPSTime, 
00366                     imuPong[i].dataWord[0],                    
00367                     imuPong[i].dataWord[1],
00368                     imuPong[i].dataWord[2],
00369                     imuPong[i].dataWord[3],
00370                     imuPong[i].dataWord[4],
00371                     imuPong[i].dataWord[5]  );
00372                 }
00373                 else
00374                 {
00375                     toPC.printf("IMU %d13,%d13,%d13,%d13,%d13,%d13,%d13 " , //70  bytes
00376                     imuPing[i].GPSTime, 
00377                     imuPing[i].dataWord[0],                    
00378                     imuPing[i].dataWord[1],
00379                     imuPing[i].dataWord[2],
00380                     imuPing[i].dataWord[3],
00381                     imuPing[i].dataWord[4],
00382                     imuPing[i].dataWord[5]  );                
00383                 }
00384                 //
00385                 readFromPC();   //#5
00386             }
00387             toPC.printf("\n");  //total of 10 * 70 + 9 = 709 bytes
00388             IMUDataReady = false;
00389             workingTime += timeFromStart.read_us() - thisWorkTime;
00390         }
00391             
00392         //read the USB serial data from the PC to check for commands
00393         //only message we expect will be the trigger fire command
00394         readFromPC();
00395 
00396         if(toPC.writeable() && fireTrigger)  //comes from a PC request message
00397         {
00398             triggeringInProcess = true;
00399             unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000.0 + timeFromPPS.read_us()/1000.0;
00400             toPC.printf("mbedmessage trig1 %d \n", triggerTime);
00401             //pre-fire the trigger using the mid-body 2.5mm connection (T2i)
00402             pre_fire = 0;  //pin30 (midbody of connector) set to zero
00403             
00404             fire = 0; //fire the trigger using the tip connection
00405             fireTrigger = false;  //finished the setup -- but wait to do the actual fire 
00406             finishTrigger = true; //set to false after firing the trigger
00407             triggerInterval.start();
00408         }
00409         
00410         //the trigger requires a pulse -- the above portion lowers the signal and the below raises it
00411         //this has been tested at 50 msecs and it will not fire at that pulse duration
00412         if(toPC.writeable() && finishTrigger && triggerInterval.read_ms() > 100)
00413         {
00414             unsigned long triggerTime = GPSTimemsecs + PPSTimeOffset*1000.0 + timeFromPPS.read_us()/1000.0;
00415             toPC.printf("mbedmessage trig2 %d \n", triggerTime);
00416             //pre-fire the trigger using 2the mid-body 2.5mm connec
00417             fire = 1;
00418             pre_fire = 1;
00419             triggerInterval.reset();
00420             finishTrigger = false;  //completes the trigger firing pulse definition
00421             triggeringInProcess = false;
00422         }
00423         
00424         cyclesPerSec++;
00425         
00426         if (detectedGPS1PPS)  //true if we are exactly at a 1PPS event detection
00427         {   
00428             if (toPC.writeable() )
00429             {   
00430                 toPC.printf("STATUS %03d %04d %06d %02d %06d %5.3f %03d %03d %03d\n", 
00431                 VERSION, PPSCounter, totalGPSBytes, IMURecordCounter, cyclesPerSec, timeAtMessage0,  /*workingTime/1000000.0,*/ 
00432                 badRangeMessages, badBESVELMessages, badBESVELMessages);
00433             }  
00434             totalGPSBytes=0;
00435                             
00436             cyclesPerSec = 0;
00437             workingTime= 0.0;
00438             
00439             messagePerSecCounter = 0;   //GPS message per second counter
00440 
00441             IMURecordCounter = 0;
00442             detectedGPS1PPS = false;
00443             
00444             rxMsg = !rxMsg;  //flash the lights to make sure the mbed loop is operative
00445             txMsg = !txMsg;
00446             
00447             readFromPC();   //#6
00448         }
00449     ///////////////////////////////////////////
00450     }  //end of the major while() loop 
00451     ///////////////////////////////////////////
00452       
00453 
00454     
00455 }
00456