JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of GPS_Incremental by
Diff: main.cpp
- Revision:
- 4:68268737ff89
- Parent:
- 3:5913df46f94a
- Child:
- 6:2a8486283198
--- a/main.cpp Fri Mar 22 01:33:24 2013 +0000 +++ b/main.cpp Fri Mar 29 20:52:24 2013 +0000 @@ -11,6 +11,8 @@ #include "ADIS16488.h" #include <string> +//these are defines for the messages that are sent from the PC across the USB +//these messages produce reactions on the mbed #define STATUS_MSG 0 #define POSVEL_MSG 1 #define STARTDATA_MSG 2 @@ -19,33 +21,39 @@ #define STOPSTREAM_MSG 5 #define STARTLOGINFO_MSG 6 #define STOPLOGINFO_MSG 7 + #define DEGREES_TO_RADIANS (3.14519/180.0) -//general items for this application +//general digital I/O specifications for this application //SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name); SDFileSystem sd(p11,p12,p13,p14,"sd"); //Serial debug(USBTX, USBRX); // tx, rx USB communication to the PC for debug purposes -DigitalOut ppsled(LED1); -DigitalOut trig1led(LED2); -DigitalOut recordDataled(LED4); -InterruptIn camera1Int(p30); -DigitalOut camera2Pin(p29); +DigitalOut ppsled(LED1); //blink an LED at the 1PPS +DigitalOut trig1led(LED2); //blink an LED at the camera trigger detection +DigitalOut recordDataled(LED4); //set the led when the record is on +InterruptIn camera1Int(p30); // camera interrupt in +DigitalOut camera2Pin(p29); // i dont believe we use the second camera interrupt //USB serial data stream back to the PC Serial toPC(USBTX, USBRX); //connect the GPS TX, RX to p9 and p10 -bool detectedGPS1PPS = false; -bool recordData = false; -int PPSCounter = 0; -int byteCounter = 0; -unsigned short perSecMessageCounter=0; -bool lookingForMessages = true; -bool messageDetected = false; -int savedIMUClockCounter=0; +bool detectedGPS1PPS = false; //flag set in the ISR and reset after processing the 1PPS event +bool recordData = false; //set to true when commanded from the PC +int PPSCounter = 0; //counts the 1PPS occurrences +int byteCounter = 0; //byte counter -- where used?? +unsigned short perSecMessageCounter=0; //counts the number of messages in a sec based on the header detection +bool lookingForMessages = true; //set in the PPS ISR and set false after the message processing in the main +bool messageDetected = false; //have detected a message header +int savedIMUClockCounter=0; +unsigned long IMUbytesWritten = 0; +int savedByteCounter = 0; +int savedPerSecMessageCounter=0; int IMUClockCounter = 0; bool camera1EventDetected = false; double camera1Time; char serBuf[128]; int serBufChars=0; + +//flags to control the PC command actions bool sendPosVel=false; bool sendStatus=false; bool sendRecData=false; @@ -54,41 +62,69 @@ bool logMsgInfo=false; bool sendLogMsgInfo=false; - //ISR for detection of the GPS 1PPS void detect1PPSISR(void) { - timeFromPPS.reset(); - savedIMUClockCounter = IMUClockCounter; - IMUClockCounter = 0; - GPS_COM1.rxBufferFlush(); + timeFromPPS.reset(); //reset the 1PPS timer upon 1PPS detection + savedIMUClockCounter = IMUClockCounter; //number of IMU clocks received since last 1PPS + savedByteCounter = byteCounter; + savedPerSecMessageCounter = perSecMessageCounter; + IMUClockCounter = 0; //counts number of IMU samples between 1PPS events + GPS_COM1.rxBufferFlush(); //flush the GPS serial buffer -- likely not needed but OK + byteCounter = 0; + perSecMessageCounter = 0; - detectedGPS1PPS = true; - lookingForMessages = true; - PPSCounter++; - PPSTimeOffset++; - ppsled = !ppsled; + detectedGPS1PPS = true; //reset in the main when 1PPS actions are complete + lookingForMessages = true; //means we should begin looking for new GPS messages + PPSCounter++; //count number of 1PPS epochs + PPSTimeOffset++; //counts the 1PPS events between occurrences when we have matching POS and VEL messages + ppsled = !ppsled; //blink an LED at the 1PPS }; //ISR for detection of the hotshoe trigger 1 void camera1ISR(void) { + //PPSTimeOffset keeps track of missed camera1Time = GPSTime + (double)PPSTimeOffset + timeFromPPS.read(); - trig1led = !trig1led; - camera1EventDetected = true; + + trig1led = !trig1led; //blink an LEWD at the camera event detection + camera1EventDetected = true; //reset to false in main after processing the image detection }; void readFromPC() { + + //better solution + //start a timer when we get a char from the PC and set a bool: detectingPCMessage = true + //keep reading bytes until elapsed time from the first byte is : 0.01 secs of numByes > 16 + //the messages are from 10 to 16 bytes -- this will take from 10/921600 to 16/921600 secs at 921600 baud + //8*115200 = 921600 + //this is about 2e-5 or 20usec. + //so we could wait 50usec from the first char reeived and be certain we had all the chars for a message. + //the steps are below .... + //(1) convert this procedures to a ISR per byte + //(2) in the ISR, start the timer on the first byte received when !detectingPCMessage + //(3) set detectingPCMessage = true + //(4) in the main loop, test the timer for >50usec and parse the bytes to test fr a message + //(5) reset the timer to zero in the main loop after parsing the message + //(6) set detectingPCMessage = false; + + //The received commands obnly occur at the initialization stage -- time is not that critical there. + //during the real-time action, we will never pass the followong if test ( no characters received) if (toPC.readable()) //read a PC serial byte and test it for a command { // Read in next character char inChar = toPC.getc(); serBuf[serBufChars++] = inChar; + // Append end of string character + //why do this for every character we read? + //answer: we always assume we have a complete message and test for this below serBuf[serBufChars] = '\0'; + // Need to parse message to determine behavior // Need to clean this up + //need to do this outside the read byte --- char msgList[8][32]; sprintf(msgList[STATUS_MSG], "WMsg STATUS"); sprintf(msgList[POSVEL_MSG], "WMsg POSVEL"); @@ -98,16 +134,19 @@ sprintf(msgList[STOPSTREAM_MSG], "WMsg POSSTREAM N"); sprintf(msgList[STARTLOGINFO_MSG], "WMsg LOGINFO Y"); sprintf(msgList[STOPLOGINFO_MSG], "WMsg LOGINFO N"); + //message length is from 10 to 16 chars + // assume an invalid message which needs to be reset bool validMessage = false; bool resetMessage = true; + // Check for valid message - for (int m = 0; m < 8 && !validMessage; m++) + for (int m = 0; m < 8 && !validMessage; m++) //check for all messages ... { if (strncmp(serBuf, msgList[m], serBufChars) == 0) { validMessage = true; - // buffer length is same as message length + //test that chars in the serial buffer is same as message length if (serBufChars == strlen(msgList[m])) { switch(m) @@ -143,6 +182,7 @@ } } } + // if message should be reset if (resetMessage) { @@ -154,7 +194,7 @@ { // start a new message serBuf[serBufChars++] = inChar; - serBuf[serBufChars] == '\0'; + serBuf[serBufChars] = '\0'; } // Append end of string character serBuf[serBufChars] = '\0'; @@ -164,6 +204,10 @@ void sendASCII(char* ASCI_message, int numChars) { + ///////////////////////////////////////////////// + //send an ASCII command to the GPS receiver + ///////////////////////////////////////////////// + //char ASCI_message[] = "unlogall COM1"; int as = numChars - 1; unsigned char CR = 0x0d; //ASCII Carriage Return @@ -195,7 +239,7 @@ //toPC.baud(1*115200); wait_ms(100); toPC.printf("\n\n released GPS from RESET and set to high baud rate \n\n"); - //just wait to lauinch the GPS receiver + //just wait to launch the GPS receiver for (int i=0; i<5; i++) { toPC.printf(" to start: %3d \n", 4-i); wait(1); } @@ -210,29 +254,36 @@ else toPC.printf(" opened the IMUGPS data file \n"); */ - + + //NOTE: we do not assume that the GPS receiver has been pre-set up for the WALDO_FCS functionality + //we alwsys start with a reset and reprogram the receiver with our data out products + // this prevents failure because of a blown NVRAM as occurred for the older camera systems + //this is the COM1 port from th GPS receiuver to the mbed //it should be always started at 9600 baud because thats the default for the GPS receiver GPS_COM1.baud(9600); wait_ms(100); // 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"; + // this is a software reset and has the same effect as a hardware reset (why do it?) + //char ch0[] = "RESET"; //this command stops all communication from the GPS receiver on COM1 - //logs should still be presented on USB port so the CDU can be used in parallel + //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"; //set the final baud rate that we will use from here //allowable baud rate values: 9600 115200 230400 460800 921600 - char ch2[] = "serialconfig COM1 921600 n 8 1 n off"; + //char ch2[] = "serialconfig COM1 921600 n 8 1 n off"; + char ch2[] = "serialconfig COM1 460800 n 8 1 n off"; //the below commands request the POS, VEL, RANGE, and TIME messages char ch3[] = "log COM1 BESTPOSB ONTIME 1"; //messageID = 42 char ch4[] = "log COM1 BESTVelB ONTIME 1"; //messageID = 99 char ch5[] = "log COM1 RANGEB ONTIME 1"; //messageID = 43 - char ch6[] = "log COM1 TIMEB ONTIME 1"; //messageID = 101 + //char ch6[] = "log COM1 TIMEB ONTIME 1"; //messageID = 101 //set up VARF to be 100Hz with 1X10^4 * 10^-8 = 10^-4 sec (10usec) pulse width + //in fact, we do not use this output but it is available. + //originally planned to use this to command the IMU data char ch8[] = "FREQUENCYOUT enable 10000 1000000"; toPC.printf("set serial config \n"); @@ -259,7 +310,8 @@ //set the mbed COM port to match the GPS transmit rate //the below baud rate must match the COM1 rate coming from the GPS receiver - GPS_COM1.baud(921600); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL + GPS_COM1.baud(460800); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL + //GPS_COM1.baud(921600); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL }; void setupTriggers() @@ -274,8 +326,8 @@ int test = 0; unsigned short messageCounter = 0; unsigned short savedMessageCounter = 0; -unsigned char msgBuffer[1536]; -unsigned short messageLocation[6] = {0}; +unsigned char msgBuffer[1536]; //array to contain one full second of GPS bytes +unsigned short messageLocation[6] = {0}; //stores the message location start within the message buffer //see the mbed COOKBOOK for MODSERIAL //MODSERIAL is an easy to use library that extends Serial to add fully buffered input and output. @@ -296,19 +348,41 @@ perSecMessageCounter++; messageDetected = true; } + //byteCounter reset to zero in main after the 1PPS is detected -- its NOT reset in the 1PPS ISR byteCounter++; //total per-sec byte counter (reset to zero in main when 1PPS detected) }; +void earthCoefficients(double latitudeRad, double longitudeRad, double height, double &latRateFac, double &lonRateFac) +{ + //compute the lat and lon factors for use in the interpolation of the lat and lon between 1 sec epochs + //see this document (page 32) www.fas.org/spp/military/program/nav/basicnav.pdf + double eccen = 0.0818191908426; //WGS84 earth eccentricity + double earthRadius = 6378137; //WGS84 earthRadius in meters + double eccenSinLat = eccen * sin(latitudeRad); + double temp1 = 1.0 - eccenSinLat*eccenSinLat; + double temp2 = sqrt(temp1); + double r_meridian = earthRadius * ( 1.0 - eccen*eccen)/ (temp1 * temp2); + double r_normal = earthRadius / temp2; + + //multiply latRateFac times V-north to get the latitude rate in radians [er sec + latRateFac = 1.0 / (r_meridian + height); + + //multiply lonRatFac by VEast to get the longitude rate in radians per sec + lonRateFac = 1.0 / ( (r_normal + height) * cos(latitudeRad) ); +} + int main() { //FILE *fpIMU = NULL; //FILE *fpGPS = NULL; FILE *fpNav = NULL; - OEM615BESTPOS posMsg; - OEM615BESTPOS curPos; - OEM615BESTVEL velMsg; - OEM615BESTVEL curVel; + + OEM615BESTPOS posMsg; //BESTPOS structure in OEMV615.h + OEM615BESTPOS curPos; //BESTPOS structure in OEMV615.h + OEM615BESTVEL velMsg; //BESTVEL structure in OEMV615.h + OEM615BESTVEL curVel; //BESTVEL structure in OEMV615.h + int msgLen; int msgEnd; @@ -320,45 +394,62 @@ //setup Hotshoe setupTriggers(); + //attempt to use the mbed RTOS library //Thread thread(writeThread); + //this doesnt show up on the PC Rich Text Box because we dont start the init til after this occurs toPC.printf("completed setting up COM ports \n"); - //set up the interrupt to catch the serial byte availability + //set up the interrupt to catch the GPS receiver serial bytes as they are presented GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq); - timeFromPPS.start(); + timeFromPPS.start(); //start the time for measuring time from 1PPS events - //while(PPSCounter < 1000) - while(1) + while(PPSCounter < 100) + /////////////////////////////////////////////////////////////////////////// + // top of the while loop + /////////////////////////////////////////////////////////////////////////// + //while(1) { //read the USB serial data from the PC to check for commands + //in the primary real-time portion, there are no bytes from the PC so this has no impact readFromPC(); - if (sendPosVel) + //we should put the below stuff into the readPC() procedure. + //only do these actions in response to a command so no need for the tests w/o an inoput byte from the PC + //perform the activities as a response to the commands + if (sendPosVel) //true if we want to return a position solution { - sendPosVel=false; + sendPosVel=false; //set to true if a POSVEL is requested from the PC char solReady = 'N'; - if (posMsg.solStatus == 0) + if (posMsg.solStatus == 0) //how is this set?? { solReady = 'Y'; } + + //north and east velocity from the horizontal speed and heading + //velMsg may not be the "current" message --- but is the one also associated with a position message double nVel = velMsg.horizontalSpeed*cos(velMsg.heading*DEGREES_TO_RADIANS); double eVel = velMsg.horizontalSpeed*sin(velMsg.heading*DEGREES_TO_RADIANS); + // For the 1 second deltas with which we are dealing - // this calculation should be close enough for now - // approximately 1 nautical mile / minute latitude, 60 minutes/degree, 1852 meters/nautical mile + // This calculation should be close enough for now + // Approximately 1 nautical mile / minute latitude, 60 minutes/degree, 1852 meters/nautical mile double latMetersPerDeg = 60.0*1852.0; // longitude separation is approximately equal to latitude separation * cosine of latitude double lonMetersPerDeg = latMetersPerDeg*cos(posMsg.latitude*DEGREES_TO_RADIANS); // Elapsed time since last known GPS position + //PPSTimeOffset is a result of possibly missing a prior GPS position message + // timeFromPPS.read() is always the time from the moset recent 1PPS double elTime = (double)PPSTimeOffset + timeFromPPS.read(); - // Position time + + // Position time -- GPSTime is the time of the last valid GPS position message double posTime = GPSTime + elTime; // Estimated position based on previous position and velocity + // posMsg is the last time when the BESTVEL and BESTPOS messages had identical times double latPos = posMsg.latitude + (nVel/latMetersPerDeg)*elTime; double lonPos = posMsg.longitude + (eVel/lonMetersPerDeg)*elTime; double htPos = posMsg.height + velMsg.verticalSpeed/(60*1852)*elTime; @@ -374,7 +465,9 @@ velMsg.verticalSpeed ); } - if (sendStatus) + + //all this does is assess the GPS convergence -- really available in the above + if (sendStatus) //send the status message to the PC { sendStatus=false; char solReady = 'N'; @@ -387,7 +480,9 @@ solReady ); } - if (sendRecData) + + //should just record ALL the data -- can pck over it in the post-processing + if (sendRecData) //begin to (or stop) record the serial data { sendRecData=false; char recChar = 'N'; @@ -427,8 +522,11 @@ recChar ); } - recordDataled = recordData; - if (sendStreamPos) + + //this is called everytime through the loop -- wasteful + //recordDataled = recordData; + + if (sendStreamPos) //stream the position data to the PC { sendStreamPos=false; char streamChar = 'N'; @@ -440,7 +538,9 @@ streamChar ); } - if (sendLogMsgInfo) + + //not sure this is ever used .. + if (sendLogMsgInfo) //send log info to the PC { sendLogMsgInfo=false; char logChar = 'N'; @@ -452,95 +552,32 @@ logChar ); } - if (IMUDataReady) - { - //write the IMU data - //if (recordData && (fpIMU != NULL)) - if (recordData && (fpNav != NULL)) + + + //////////////////////////////////////////////////////////////////////////// + //below is where we process the complete stored GPS message for the second + //The !IMUDataReady test prevents the IMU and GPS data from being written + //to disk on the same pass through thi loop + ///////////////////////////////////////////////////////////////////////////// + if (!IMUDataReady && lookingForMessages && (timeFromPPS.read_us() > 20000)) //it takes less than 20msec to receive all messages + { + toPC.printf(" num messages = %3d time = %5d \n", perSecMessageCounter, timeFromPPS.read_us()); + + //cycle through all the bytes stored this sec (after the 1PPS as set) + // perSecMessageCounter is incremented whenever we detect a new message headet 0xAA44121C sequence + for (int i=0; i<perSecMessageCounter; i++) { - fwrite(imuRec.dataWord, sizeof(IMUREC), 1, fpNav); - } - IMUDataReady = false; - } - //if (lookingForMessages && (timeFromPPS.read() > 0.100)) //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++) - { - // ensure at message header has arrived before processing - if (byteCounter < (messageLocation[i] + sizeof(MESSAGEHEADER))) + msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]]; + toPC.printf("WMsg MESSAGEINFO %5d %5d \n", + msgHeader[i]->messageID, + messageLocation[i]); + //test for a message 42 (BESTPOS) + if (msgHeader[i]->messageID == 42) { - // Complete message header 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; - } - msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]]; - // Ensure complete message has been received - // Message length is header length + message length + CRC - msgLen = 28; - switch (msgHeader[i]->messageID) - { - case 42: - msgLen = 104; - break; - case 99: - msgLen = 76; - break; - default: - msgLen = msgHeader[i]->headerLength + msgHeader[i]->messageLength + sizeof(unsigned long); - break; + curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[i]]); - } - 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++) + if (streamPos) { - messageLocation[j] = messageLocation[i+j]; - perSecMessageCounter--; - } - break; - } - else if ((i < (perSecMessageCounter-1)) && - (messageLocation[i+i] < msgEnd)) // ignore CRC for now - { - // Next message was started before this mesage was completely received. - // Ignore and continue on to the next message - continue; - } - if (logMsgInfo) - { - toPC.printf("WMsg MESSAGEINFO %5d %5d %5d %5d %5d = %5d (%5d)\n", - msgHeader[i]->messageID, - messageLocation[i], - msgHeader[i]->headerLength, - msgHeader[i]->messageLength, - sizeof(unsigned long), - msgEnd, - byteCounter); - } - //toPC.printf(" %5d ", msgHeader[i]->messageID); - if ((msgHeader[i]->messageID == 42) || - (msgHeader[i]->messageID == 99)) - { - if (msgHeader[i]->messageID == 42) - { - // Wait until velocity message has also been received before using as - // base position - //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", curPos.msgHeader.GPSTime_msecs, curPos.solStatus, @@ -559,69 +596,86 @@ curPos.numGGL1, curPos.extSolStatus, curPos.sigMask); - } - } - else if (msgHeader[i]->messageID == 99) - { - // Wait until position message has also been received before using as - // base position - //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) - { - // update position and velocity used for calculation - GPSTimemsecs = curPos.msgHeader.GPSTime_msecs; - GPSTime = (double)GPSTimemsecs/1000.0; - velMsg = curVel; - posMsg = curPos; - PPSTimeOffset = 0; } } - if (i == (perSecMessageCounter-1)) + + //check for a message 99 (BESTVEL) -- and cast it into its message structure + else if (msgHeader[i]->messageID == 99) + { + curVel = *((OEM615BESTVEL*)&msgBuffer[messageLocation[i]]); + } + + //the below test ensures that the positin and veocity are matched in time + //not sure the reason for the "250" below + if ((curVel.msgHeader.GPSTime_msecs+250)/1000 == + (curPos.msgHeader.GPSTime_msecs+250)/1000) { - if (recordData && (fpNav != NULL)) - { - fwrite(msgBuffer, byteCounter, 1, fpNav); - } - byteCounter = 0; - perSecMessageCounter = 0; + // update position and velocity used for calculation + GPSTimemsecs = curPos.msgHeader.GPSTime_msecs; + GPSTime = (double)GPSTimemsecs/1000.0; + velMsg = curVel; // + posMsg = curPos; + + ///////////////////////////////////////////////////////////////////////////////////////// + //IMPORTANT: we reset the PPSTimeOffset when we have a matching position and velocity + PPSTimeOffset = 0; + ///////////////////////////////////////////////////////////////////////////////////////// } - } - //toPC.printf(" %3d %8d \n", msgHeader[0]->timeStatus, GPSTimemsecs); - //if (recordData && (fpGPS != NULL)) - - /* + } + lookingForMessages = false; + + } //end of the GPS message processing + + //the IMU data record is read from the SPI in the ISR and the IMUDataReady is set true + //we write the IMU data here + if (IMUDataReady) //IMUDataReady is true if we have a recent IMU data record + { + imuRec.GPSTime = GPSTimemsecs + timeFromPPS.read_us()/1000000.0; + spi.write((int) HIGH_REGISTER[0]); // next read will return results from HIGH_REGITER[0] + for (int i=0; i<6; i++) //read the 6 rate and accel variables + { + wd.pt[1] = (unsigned short)spi.write((int) LOW_REGISTER[i]) ; + if (i<5) // dont this on the last because this was pre-called + { wd.pt[0] = (unsigned short)spi.write((int) HIGH_REGISTER[i+1]); } + imuRec.dataWord[i] = wd.dataWord; //data word is a signed long + + } + IMURecordCounter++; + //write the IMU data + //if (recordData && (fpIMU != NULL)) if (recordData && (fpNav != NULL)) { - fwrite(msgBuffer, byteCounter, 1, fpNav); + IMUbytesWritten += fwrite(imuRec.dataWord, sizeof(IMUREC), 1, fpNav); } - */ - //lookingForMessages = false; + IMUClockCounter++; + IMUDataReady = false; } - if (messageDetected) + + if (messageDetected) //some GPS message header has been detected { - //toPC.printf(" msgTime = %4d \n", timeFromPPS.read_us()); + toPC.printf(" msgTime = %4d \n", timeFromPPS.read_us()); messageDetected = false; } - if (camera1EventDetected) + + if (camera1EventDetected) //we have detected a camera trigger event { toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time); camera1EventDetected = false; } - if (detectedGPS1PPS) + + if (detectedGPS1PPS) //true if we are exactly at a 1PPS event detection { - //toPC.printf(" PPSCounter = %4d byteCounter = %10d num Messages Received = %3d IMUClock = %4d\n", - // PPSCounter, byteCounter, perSecMessageCounter, savedIMUClockCounter); + toPC.printf(" PPSCounter=%4d byteCounter=%10d Msgs Received=%3d IMUClock=%4d bytesWritten=%8d\n", + PPSCounter, savedByteCounter, savedPerSecMessageCounter, savedIMUClockCounter, IMUbytesWritten); if (recordData && (fpNav != NULL) && (byteCounter > 0)) { - fwrite(msgBuffer, byteCounter, 1, fpNav); + // we know that we are not reading a GPS message at exactly the 1PPS occurrence + fwrite(msgBuffer, byteCounter, 1, fpNav); // this writes out a complete set of messages for ths sec } - byteCounter = 0; - perSecMessageCounter=0; detectedGPS1PPS = false; } } + + fclose(fpNav); toPC.printf(" normal termination \n"); } \ No newline at end of file