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 
00007 #include "MODSERIAL.h"
00008 #include "SDFileSystem.h"      //imported using the import utility    
00009 
00010 //general digital I/O specifications for this application
00011 //SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name);
00012 SDFileSystem sd(p11,p12,p13,p14,"sd");
00013 DigitalIn sd_detect(p27);
00014 DigitalOut ppsled(LED1);        //blink an LED at the 1PPS
00015 DigitalOut trig1led(LED2);      //blink an LED at the camera trigger detection
00016 DigitalOut recordDataled(LED4); //set the led when the record is on
00017 //InterruptIn camera1Int(p30);    // camera interrupt in
00018 DigitalOut camera2Pin(p29);     // We dont use the second camera interrupt
00019 //USB serial data stream back to the PC
00020 Serial toPC(USBTX, USBRX);      //connect the GPS TX, RX to p9 and p10
00021 
00022 bool detectedGPS1PPS = false;       //flag set in the ISR and reset after processing the 1PPS event
00023 int PPSCounter = 0;                 //counts the 1PPS occurrences
00024 int byteCounter = 0;                //byte counter -- zeroed at 1PPS
00025 unsigned short perSecMessageCounter=0; //counts the number of messages in a sec based on the header detection
00026 bool lookingForMessages = true;     //set in the PPS ISR and set false after the message processing in the main
00027 bool messageDetected = false;       //have detected a message header
00028 unsigned long IMUbytesWritten = 0;  //counts the IMU bytes written by the fwrite() to the SD card 
00029 int savedByteCounter = 0;           //save ByteCounter at the 1PPS for display in main
00030 int savedPerSecMessageCounter=0;    //saved PerSecMsgCounter for display in main
00031 int IMUClockCounter = 0;            //counter for IMU samples per sec
00032 int savedIMUClockCounter=0;         //saved at the 1PPS for later diaplay from main
00033 bool camera1EventDetected = false;  //flag from ISR indicating a clock event occurred
00034 double camera1Time;                 //GPS time of the camera event 
00035 int TotalBadCRCmatches = 0;         //counter for the bad CRC matches for all GPS messages
00036 
00037 //////////////////////////////////////////////////////////////////////
00038 // the below should become classes
00039 //////////////////////////////////////////////////////////////////////
00040 #include "OEM615.h"         //OEM615 GPS activities
00041 #include "ADIS16488.h"      //ADIS16488 activities
00042 #include "PCMessaging.h"    //PC messaging activities
00043 
00044 //ISR for detection of the GPS 1PPS
00045 void detect1PPSISR(void)
00046 {
00047     timeFromPPS.reset();                    //reset the 1PPS timer upon 1PPS detection
00048     savedIMUClockCounter = IMUClockCounter; //number of IMU clocks received since last 1PPS
00049     savedByteCounter = byteCounter;         //save byteCounter for display in main
00050     savedPerSecMessageCounter = perSecMessageCounter;   //save for display un main
00051     IMUClockCounter = 0;                    //counts per-sec IMU samples (between 1PPS events)
00052     byteCounter = 0;                        //countes bytes between 1PPS events 
00053     perSecMessageCounter = 0;               //counts GPS messages between 1PPS events
00054     
00055     GPS_COM1.rxBufferFlush();               //flush the GPS serial buffer
00056     
00057     detectedGPS1PPS = true;         //set false in the main when 1PPS actions are complete
00058     lookingForMessages = true;      //set false in main after processing messages
00059     PPSCounter++;                   //count number of 1PPS epoch
00060     
00061     //note -- the below accounts for time information becoming available AFTER the 1PPS event
00062     PPSTimeOffset++;                //counts 1PPS events between matching POS and VEL messages
00063     
00064     ppsled = !ppsled;               //blink an LED at the 1PPS
00065 };
00066 
00067 //ISR for detection of the hotshoe trigger 1
00068 //void camera1ISR(void)
00069 //{
00070     //GPSTime is from POS message header
00071     //PPSTimeOffset is an even sec to account for Time becoming known AFTER the 1PPS
00072     //PPSTimeOffset + timeFromPPS.read() can be as large as 1.02 secs
00073 //    camera1Time = GPSTime + PPSTimeOffset + timeFromPPS.read();    
00074 //    camera1EventDetected = true;  //reset to false in main after processing the image detection
00075 //    trig1led = !trig1led;  //blink an LEWD at the camera event detection
00076 //};
00077 
00078 ///////////////////////////////////////////////////////
00079 //set up the USB port and the GPS COM port
00080 /////////////////////////////////////////////////////// 
00081 FILE *fpNav = NULL;  //file pointer to the nav file on the SD card
00082 void setupCOM(void)
00083 {
00084     //system starts with GPS in reset active
00085     //dis-engage the reset to get the GPS started
00086     GPS_Reset=1; wait_ms(1000); 
00087     
00088     //establish 1PPS ISR 
00089     PPSInt.rise(&detect1PPSISR);
00090     
00091     //set the USB serial data rate -- rate must be matched at the PC end
00092     //This the serial communication back to the the PC host
00093     //Launch the C++ serial port read program there to catch the ASCII characters
00094     //toPC.baud(9600); wait_ms(100);    
00095     toPC.baud(8*115200); wait_ms(100);
00096     //toPC.baud(1*115200); wait_ms(100);
00097     toPC.printf("\n\n released GPS from RESET and set to high baud rate \n\n");
00098     
00099     //just wait to launch the GPS receiver
00100     for (int i=0; i<5; i++) { toPC.printf(" to start: %3d \n", 4-i); wait(1); }
00101 
00102     sd_detect.mode(PullUp);
00103     
00104     if (sd_detect == 0)
00105     {
00106         mkdir("/sd/Data", 0777);
00107     }
00108     else
00109     {
00110         toPC.printf(" SD card not present \n");
00111     }
00112     
00113     //NOTE:  we do not assume that the GPS receiver has been pre-set up for the WALDO_FCS functionality
00114     //we alwsys start with a reset and reprogram the receiver with our data out products
00115     // this prevents failure because of a blown NVRAM as occurred for the older camera systems
00116     
00117     //this is the COM1 port from th GPS receiuver to the mbed
00118     //it should be always started at 9600 baud because thats the default for the GPS receiver 
00119     GPS_COM1.baud(9600); wait_ms(100);
00120    
00121     // this ASCII command sets up the serial data from the GPS receiver on its COM1
00122     char ch7[] = "serialconfig COM1 9600 n 8 1 n off";
00123     // this is a software reset and has the same effect as a hardware reset (why do it?) 
00124     //char ch0[] = "RESET"; 
00125     //this command stops all communication from the GPS receiver on COM1
00126     //logs should still be presented on USB port so the Novatel CDU application can be used on the PC in parallel
00127     char ch1[] = "unlogall COM1";
00128     //set the final baud rate that we will use from here  
00129     //allowable baud rate values: 9600 115200 230400 460800 921600
00130     //char ch2[] = "serialconfig COM1 921600 n 8 1 n off";
00131     char ch2[] = "serialconfig COM1 460800 n 8 1 n off";
00132     
00133     //the below commands request the POS, VEL, RANGE, and TIME messages
00134     char ch3[] = "log COM1 BESTPOSB ONTIME 1";   //messageID = 42 
00135     char ch4[] = "log COM1 BESTVelB ONTIME 1";   //messageID = 99
00136     char ch5[] = "log COM1 RANGEB ONTIME 1";     //messageID = 43
00137     //char ch6[] = "log COM1 TIMEB ONTIME 1";      //messageID = 101
00138     
00139     //set up VARF to be 100Hz with 1X10^4 * 10^-8 = 10^-4 sec (10usec) pulse width
00140     //in fact, we do not use this output but it is available.
00141     //originally planned to use this to command the IMU data
00142     //char ch8[] = "FREQUENCYOUT enable 10000 1000000";
00143     
00144     toPC.printf("set serial config \n");
00145     sendASCII(ch7, sizeof(ch7)); wait_ms(500);
00146     //sendASCII(ch0, sizeof(ch0));  
00147     toPC.printf("unlog all messages \n");
00148     sendASCII(ch1, sizeof(ch1)); wait_ms(500);
00149     toPC.printf("log BESTPOSB on COM1 \n");
00150     sendASCII(ch3, sizeof(ch3)); wait_ms(500);
00151     toPC.printf("log BESTVELB on COM1\n");
00152     sendASCII(ch4, sizeof(ch4)); wait_ms(500);
00153     toPC.printf("log RANGEB on COM1\n");
00154     sendASCII(ch5, sizeof(ch5)); wait_ms(500);
00155     
00156     //toPC.printf("log TIMEB om COM1 \n");
00157     //sendASCII(ch6, sizeof(ch6)); wait_ms(100);
00158     
00159     //toPC.printf("Set up th VARF signal \n"); 
00160     //sendASCII(ch8, sizeof(ch8)); wait_ms(500);
00161        
00162     //set GPS output COM1 to the final high rate
00163     toPC.printf("set the COM ports to high rate\n");
00164     sendASCII(ch2, sizeof(ch2)); wait_ms(500);
00165     
00166     //set the mbed COM port to match the GPS transmit rate
00167     //the below baud rate must match the COM1 rate coming from the GPS receiver 
00168     GPS_COM1.baud(460800); wait_ms(500);  //without this wait -- the baud rate is not detected when using MODSERIAL     
00169     //GPS_COM1.baud(921600); wait_ms(500);  //without this wait -- the baud rate is not detected when using MODSERIAL     
00170 };
00171 
00172 void setupTriggers()
00173 {
00174 //    camera1Int.mode(PullUp);
00175     camera2Pin = 1;
00176     //establish Trigger ISR 
00177 //    camera1Int.rise(&camera1ISR);
00178     
00179 };
00180 
00181 /////////////////////////////////////////////////////////////////////
00182 //  mbed main to support the Waldo_FCS
00183 /////////////////////////////////////////////////////////////////////
00184 int main() {
00185     
00186     //these are structures for the to GPS messages that must be parsed
00187     OEM615BESTPOS posMsg;   //BESTPOS structure in OEMV615.h that has matching time to a BESTVEL message
00188     OEM615BESTPOS curPos;   //BESTPOS structure in OEMV615.h
00189     OEM615BESTVEL velMsg;   //BESTVEL structure in OEMV615.h that has matching time to a BESTPOS message
00190     OEM615BESTVEL curVel;   //BESTVEL structure in OEMV615.h
00191 
00192     //set up the GPS and mbed COM ports
00193     setupCOM(); 
00194     
00195     //set up the ADIS16488 
00196     setupADIS();
00197     
00198     //setup Hotshoe
00199     setupTriggers();
00200 
00201     setUpMessages();  //set up the expected text message commands frm the PC 
00202     
00203     //set up the interrupt to catch the GPS receiver serial bytes as they are presented
00204     GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq);
00205     
00206     timeFromPPS.start();  //start the time for measuring time from 1PPS events
00207     
00208     toPC.printf("\n\n top of the main loop \n\n");
00209     
00210     int totalBytesWritten = 0;
00211     
00212     //while(PPSCounter < 100)
00213     ///////////////////////////////////////////////////////////////////////////
00214     // top of the while loop
00215     ///////////////////////////////////////////////////////////////////////////
00216     while(1)
00217     {        
00218         //read the USB serial data from the PC to check for commands
00219         //in the primary real-time portion, there are no bytes from the PC so this has no impact
00220         readFromPC();
00221                 
00222         processPCmessages(fpNav, posMsg, velMsg);
00223 
00224 //
00225         ////////////////////////////////////////////////////////////////////////////
00226         //below is where we process the complete stored GPS message for the second
00227         //The !IMUDataReady test prevents the IMU and GPS data from being written 
00228         //to disk on the same pass through thi loop  
00229         /////////////////////////////////////////////////////////////////////////////
00230         if (!IMUDataReady && lookingForMessages && (timeFromPPS.read_us() > 20000))  //it takes less than 20msec to receive all messages
00231         {    
00232             //toPC.printf(" num messages = %3d time = %5d \n", perSecMessageCounter, timeFromPPS.read_us());
00233             
00234             //cycle through all the bytes stored this sec (after the 1PPS as set)
00235             // perSecMessageCounter is incremented whenever we detect a new message headet 0xAA44121C sequence
00236             for (int i=0; i<perSecMessageCounter; i++)  
00237             {
00238                 msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]];
00239                 //toPC.printf("WMsg MESSAGEINFO %5d %5d \n", 
00240                 //                                     msgHeader[i]->messageID, 
00241                 //                                     messageLocation[i]);
00242                
00243                 //calculated CRC
00244                 unsigned long CRC1 = CalculateBlockCRC32(28+msgHeader[i]->messageLength, &msgBuffer[messageLocation[i]]);
00245                 unsigned long CRC2 = *((unsigned long*)&msgBuffer[messageLocation[i] + 28 + msgHeader[i]->messageLength]);
00246                  
00247                 if (CRC1 != CRC2)
00248                 { 
00249                     TotalBadCRCmatches++;
00250                     toPC.printf(" bad CRC match for messageID %3d total CRC errors = %4d \n",  
00251                     msgHeader[i]->messageID, TotalBadCRCmatches);
00252                     continue;
00253                 }                
00254           
00255                 //test for a message 42 (BESTPOS)
00256                 if (msgHeader[i]->messageID == 42)
00257                 {
00258                     curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[i]]);
00259                     
00260                     if (streamPos)
00261                     {
00262                             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",
00263                                               curPos.msgHeader.GPSTime_msecs,
00264                                               curPos.solStatus,
00265                                               curPos.posType,
00266                                               curPos.latitude,
00267                                               curPos.longitude,
00268                                               curPos.height,
00269                                               curPos.undulation,
00270                                               curPos.latitudeSTD,
00271                                               curPos.longitudeSTD,
00272                                               curPos.heightSTD,
00273                                               curPos.diffAge,
00274                                               curPos.solutionAge,
00275                                               curPos.numSV,
00276                                               curPos.numSolSV,
00277                                               curPos.numGGL1,
00278                                               curPos.extSolStatus,
00279                                               curPos.sigMask);
00280                     }
00281                 }
00282                     
00283                 //check for a message 99 (BESTVEL)  -- and cast it into its message structure
00284                 else if (msgHeader[i]->messageID == 99)
00285                 {
00286                     curVel = *((OEM615BESTVEL*)&msgBuffer[messageLocation[i]]);
00287                 }
00288                 
00289                 //the below test ensures that the positin and veocity are matched in time
00290                 //not sure the reason for the "250" below    
00291                 if ((curVel.msgHeader.GPSTime_msecs+250)/1000 == 
00292                     (curPos.msgHeader.GPSTime_msecs+250)/1000)
00293                 {
00294                     // update position and velocity used for calculation
00295                     GPSTimemsecs = curPos.msgHeader.GPSTime_msecs;
00296                     GPSTime = (double)GPSTimemsecs/1000.0;
00297                     velMsg = curVel;  //
00298                     posMsg = curPos;
00299                         
00300                     /////////////////////////////////////////////////////////////////////////////////////////
00301                     //IMPORTANT:   we reset the PPSTimeOffset when we have a matching position and velocity 
00302                     PPSTimeOffset = 0;
00303                     /////////////////////////////////////////////////////////////////////////////////////////
00304                 }
00305             }  //end of per message loop
00306             lookingForMessages = false;
00307             
00308             if (recordData && (fpNav != NULL) && (byteCounter > 0))
00309             {
00310                 wait_us(1000);
00311                 totalBytesWritten += fwrite(msgBuffer, 1, byteCounter, fpNav);  // this writes out a complete set of messages for this sec
00312                 wait_us(1000);
00313             }
00314             
00315         }  //end of the GPS message processing
00316         
00317 //
00318         
00319         //the IMU data record is read from the SPI in the ISR and the IMUDataReady is set true
00320         //we write the IMU data here
00321         if (IMUDataReady)  //IMUDataReady is true if we have a recent IMU data record
00322         {
00323             //GPSTime (secs from midnight) is from the header of the position message
00324             //PPSTimeOffset accounts for time becoming known ~20msec AFTER the 1PPS
00325             imuRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + timeFromPPS.read_us()/1000.0;
00326             wait_us(10);
00327             spi.write((int) HIGH_REGISTER[0]); wait_us(10); // next read will return results from HIGH_REGITER[0]
00328             for (int i=0; i<6; i++)  //read the 6 rate and accel variables
00329             {
00330                 wd.pt[1] = (unsigned short)spi.write((int) LOW_REGISTER[i]); wait_us(10) ; 
00331                 if (i<5)  // dont this on the last because this was pre-called
00332                 {   wd.pt[0] = (unsigned short)spi.write((int) HIGH_REGISTER[i+1]); wait_us(10); }
00333                 imuRec.dataWord[i] = wd.dataWord; //data word is a signed long
00334             }
00335             IMURecordCounter++;
00336             //write the IMU data
00337             if (recordData && (fpNav != NULL))
00338             {   
00339                 totalBytesWritten += fwrite(&imuRec, 1, sizeof(IMUREC), fpNav);
00340             }
00341             IMUClockCounter++;
00342             IMUDataReady = false;
00343         }
00344         
00345         /*
00346         if (messageDetected)  //some GPS message header has been detected
00347         {
00348             toPC.printf(" msgTime = %4d \n", timeFromPPS.read_us());
00349             messageDetected = false;
00350         }
00351         */
00352         
00353         if (camera1EventDetected)  //we have detected a camera trigger event
00354         {
00355             camera2Pin=0;
00356             wait(.25);
00357             camera2Pin=1;
00358             wait(.50);
00359             camera2Pin=0;
00360             camera1Time = GPSTime + PPSTimeOffset + timeFromPPS.read();
00361             toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time);
00362             camera2Pin=1;
00363             camera1EventDetected = false;
00364         }
00365         
00366         if (detectedGPS1PPS)  //true if we are exactly at a 1PPS event detection
00367         {   
00368             //toPC.printf(" PPSCounter=%4d byteCounter=%10d Msgs Received=%3d IMUClock=%4d bytesWritten=%8d\n", 
00369             //                PPSCounter, savedByteCounter, savedPerSecMessageCounter, savedIMUClockCounter, totalBytesWritten);
00370 
00371             detectedGPS1PPS = false;
00372         }
00373     }
00374     
00375     fclose(fpNav);
00376     toPC.printf(" normal termination \n");
00377 }