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 115200 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(115200); 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     MESSAGEHEADER msgHdr;
00188     OEM615BESTPOS posMsg;   //BESTPOS structure in OEMV615.h that has matching time to a BESTVEL message
00189     OEM615BESTPOS curPos;   //BESTPOS structure in OEMV615.h
00190     OEM615BESTVEL velMsg;   //BESTVEL structure in OEMV615.h that has matching time to a BESTPOS message
00191     OEM615BESTVEL curVel;   //BESTVEL structure in OEMV615.h
00192 
00193     //set up the GPS and mbed COM ports
00194     setupCOM(); 
00195     
00196     //set up the ADIS16488 
00197     setupADIS();
00198     
00199     //setup Hotshoe
00200     setupTriggers();
00201 
00202     setUpMessages();  //set up the expected text message commands frm the PC 
00203     
00204     //set up the interrupt to catch the GPS receiver serial bytes as they are presented
00205     GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq);
00206     
00207     timeFromPPS.start();  //start the time for measuring time from 1PPS events
00208     
00209     toPC.printf("\n\n top of the main loop \n\n");
00210     
00211     int totalBytesWritten = 0;
00212     
00213     /*
00214     unsigned long CRC = 0;
00215     CRC32Value(CRC, 0xAA);
00216     CRC32Value(CRC, 0x44);
00217     CRC32Value(CRC, 0x12);
00218     CRC32Value(CRC, 0x1C);
00219     toPC.printf(" CRC after AA44121C header: %08x \n", CRC);
00220     wait(20);
00221     */
00222     
00223     int CRCerrors = 0;
00224     
00225     recordData = true;
00226     sendRecData = true;                     
00227     
00228     while(PPSCounter < 300)
00229     ///////////////////////////////////////////////////////////////////////////
00230     // top of the while loop
00231     ///////////////////////////////////////////////////////////////////////////
00232     //while(1)
00233     {        
00234         //read the USB serial data from the PC to check for commands
00235         //in the primary real-time portion, there are no bytes from the PC so this has no impact
00236         readFromPC();
00237                 
00238         processPCmessages(fpNav, posMsg, velMsg);
00239 
00240 //
00241         ////////////////////////////////////////////////////////////////////////////
00242         //below is where we process the complete stored GPS message for the second
00243         //The !IMUDataReady test prevents the IMU and GPS data from being written 
00244         //to disk on the same pass through this loop  
00245         /////////////////////////////////////////////////////////////////////////////
00246         
00247         
00248         if (completeMessageAvailable && !IMUDataReady)
00249         {
00250             
00251             msgHdr = *((MESSAGEHEADER*)&msgBuffer[messageLocation[savedMessageCounter-1]]);
00252             
00253             GPSTimemsecs = msgHdr.GPSTime_msecs;
00254             /////////////////////////////////////////////////////////////////////////////////////////
00255             //IMPORTANT:   we reset the PPSTimeOffset when we have a matching position and velocity 
00256             PPSTimeOffset = 0;
00257             /////////////////////////////////////////////////////////////////////////////////////////
00258             
00259             unsigned long msgCRC = *((unsigned long*)&msgBuffer[messageLocation[savedMessageCounter-1] + 28 + msgHdr.messageLength]);
00260             
00261             //toPC.printf("tmeFrom1PPS= %5d  ID= %3d Ln = %3d computedCRC= %08x msgCRC= %08x msgCntr = %3d CRCerr=%4d\n", 
00262             //    timeFromPPS.read_us(), msgHdr.messageID, msgHdr.messageLength, computedCRC, msgCRC, savedMessageCounter, TotalBadCRCmatches);
00263                 
00264             if ( msgCRC != computedCRC)
00265             {
00266                 toPC.printf(" bad CRC match for messageID %3d total CRC errors = %4d \n",  
00267                     msgHdr.messageLength, TotalBadCRCmatches++);
00268             }
00269                      
00270             if      (msgHdr.messageID == 42)
00271             {
00272                 curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[savedMessageCounter-1]]);
00273                 posMsg = curPos;
00274                 
00275                 if (streamPos)
00276                 {
00277                         toPC.printf("WMsg BESTPOS %5d %1d %8.5lf %9.5lf %5.3lf %d %d %d\n",
00278                                           curPos.msgHeader.GPSTime_msecs,  curPos.solStatus,
00279                                           curPos.latitude, curPos.longitude, curPos.height,
00280                                           curPos.numSV, curPos.numSolSV, curPos.numGGL1);
00281                 }
00282                 
00283                 /////////////////////////////////////////////////////////////////////////////////////////
00284                 //IMPORTANT:   we reset the PPSTimeOffset when we have a matching position and velocity 
00285                 PPSTimeOffset = 0;
00286                 /////////////////////////////////////////////////////////////////////////////////////////
00287             } 
00288             else if (msgHdr.messageID == 99)  
00289             {
00290                 curVel = *((OEM615BESTVEL*)&msgBuffer[ messageLocation[savedMessageCounter-1] ]);
00291                 velMsg = curVel;
00292             }
00293             
00294             if (recordData && (fpNav != NULL) && (byteCounter > 0))
00295             {
00296                 //wait_us(10);
00297                 int totalMessageLength = 28 + msgHdr.messageLength + 4;  //header length + message Length + CRC word size
00298                 totalBytesWritten += fwrite(&msgBuffer[messageLocation[savedMessageCounter-1]], 1, totalMessageLength, fpNav);  // this writes out a complete set of messages for this sec
00299                 //wait_us(10);
00300             }
00301                 
00302             completeMessageAvailable = false;
00303         }
00304         
00305         //the IMU data record is read from the SPI in the ISR and the IMUDataReady is set true
00306         //we write the IMU data here
00307         if (IMUDataReady)  //IMUDataReady is true if we have a recent IMU data record
00308         {
00309             //GPSTime (secs from midnight) is from the header of the position message
00310             //PPSTimeOffset accounts for time becoming known ~20msec AFTER the 1PPS
00311             IMUtimeFrom1PPS = timeFromPPS.read_us();
00312             imuRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + IMUtimeFrom1PPS/1000.0;
00313             //wait_us(1);
00314             spi.write((int) HIGH_REGISTER[0]); //wait_us(1); // next read will return results from HIGH_REGITER[0]
00315             for (int i=0; i<6; i++)  //read the 6 rate and accel variables
00316             {
00317                 wd.pt[1] = (unsigned short)spi.write((int) LOW_REGISTER[i]); //wait_us(1) ; 
00318                 if (i<5)  // dont this on the last because this was pre-called
00319                 {   wd.pt[0] = (unsigned short)spi.write((int) HIGH_REGISTER[i+1]);  }
00320                 imuRec.dataWord[i] = wd.dataWord; //data word is a signed long
00321             }
00322             IMURecordCounter++;
00323             //write the IMU data
00324             if (recordData && (fpNav != NULL))
00325             {   
00326                 totalBytesWritten += fwrite(&imuRec, 1, sizeof(IMUREC), fpNav);
00327             }
00328             IMUDataReady = false;
00329         }
00330         
00331         if (camera1EventDetected)  //we have detected a camera trigger event
00332         {
00333             toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time);
00334             camera1EventDetected = false;
00335         }
00336         
00337         if (detectedGPS1PPS)  //true if we are exactly at a 1PPS event detection
00338         {   
00339             toPC.printf("\n PPSCounter=%4d byteCounter=%10d Msgs Received=%3d IMUClock=%4d IMURec = %3d bytesWritten=%8d\n", 
00340                             PPSCounter, savedByteCounter, savedPerSecMessageCounter, savedIMUClockCounter, IMURecordCounter, totalBytesWritten);
00341             
00342             IMURecordCounter = 0;
00343             detectedGPS1PPS = false;
00344         }
00345     }
00346       
00347     fclose(fpNav);
00348     toPC.printf(" normal termination \n");
00349 }