this version has all of Jim's fixes for reading the GPS and IMU data synchronously
Dependencies: MODSERIAL SDFileSystem mbed SDShell CRC CommHandler FP LinkedList LogUtil
Revision 30:96d133f3008e, committed 2014-03-03
- Comitter:
- jekain314
- Date:
- Mon Mar 03 13:19:31 2014 +0000
- Parent:
- 29:dead10cce6e9
- Commit message:
- commit of RT_Download.
Changed in this revision
--- a/ADIS16488.h Thu Jan 09 14:09:05 2014 +0000 +++ b/ADIS16488.h Mon Mar 03 13:19:31 2014 +0000 @@ -22,7 +22,7 @@ union WD { long dataWord; unsigned short pt[2];} wd; //IMU records are buffered in the IMUDataReady ISR -const unsigned char IMUrecArraySize = 10; +const unsigned char IMUrecArraySize = 5; #pragma pack(1) struct IMUREC @@ -42,16 +42,27 @@ void IMUDataReadyISR(void) { - IMUtimeFrom1PPS = timeFromPPS.read_us(); + IMURecordCounter++; + + IMUtimeFrom1PPS = timeFromPPS.read_us(); //timer reset to zero in the the GPS 1PPS + + // GPSTimemsecs is taken from the GPS message -- but that may follow the 1PPS by several millisecs + // PPSTimeOffset is set to zero after the time becomes available + // so PPSTimeOffset accounts for the possible >1sec between the current IMU time and the time of the last PPS + // this also accounts for a missed GPS time message that holds the GPS time tempRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + IMUtimeFrom1PPS/1000.0; + //tempRec.GPSTime = PPSCounter*1000 + timeFromPPS.read_us()/1000; + + //test to see if we are ready to write the current IMU data buffer and swap the ping-pong buffer if (IMUClockCounter == IMUrecArraySize ) { - IMUDataReady = true; - fillingPingWritingPong = !fillingPingWritingPong; - IMUClockCounter = 0; + IMUDataReady = true; //signals the write in the main loop + fillingPingWritingPong = !fillingPingWritingPong; //swap the ping-pong buffer + IMUClockCounter = 0; //reset the IMU record counter } + // 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 { @@ -61,13 +72,16 @@ if ( fillingPingWritingPong) tempRec.dataWord[i] = wd.dataWord; //data word is a signed long else tempRec.dataWord[i] = wd.dataWord; //data word is a signed long - } + } + // //fill the correct buffer ping or pong if (fillingPingWritingPong) imuPing[IMUClockCounter] = tempRec; else imuPong[IMUClockCounter] = tempRec; + // IMUClockCounter++; + return; } @@ -97,7 +111,8 @@ // The C abd D designate the registers for the DECRATE of Page 3 // The 0x17 sets the rate to: 2460/(23+1) = 102.5Hz // The 0x18 sets the rate to: 2460/(24+1) = 98.4Hz - spi.write((int)0x8C17); //write high byte (only page number can be written in a single byte) + //spi.write((int)0x8C17); //write high byte (only page number can be written in a single byte) + spi.write((int)0x8C30); //write high byte (only page number can be written in a single byte) spi.write((int)0x8D00); //write the low byte of DECRATE //to set the GPS VARF clock as the input synch clock for the IMU
--- a/OEM615.h Thu Jan 09 14:09:05 2014 +0000 +++ b/OEM615.h Mon Mar 03 13:19:31 2014 +0000 @@ -13,12 +13,12 @@ bool loadingMessageBuffer = false; -const unsigned short maxGPSbytesPerSec = 512; +const unsigned short maxGPSbytesPerSec = 2048; int messagePerSecCounter = 0; -unsigned char msgBuffer0[maxGPSbytesPerSec]; //array to contain one full second of GPS bytes -unsigned char msgBuffer1[maxGPSbytesPerSec]; //array to contain one full second of GPS bytes -unsigned char msgBuffer2[maxGPSbytesPerSec]; //array to contain one full second of GPS bytes +char msgBuffer0[2048]; //array to contain one full second of GPS bytes +char msgBuffer1[512]; //array to contain one full second of GPS bytes +char msgBuffer2[512]; //array to contain one full second of GPS bytes int GPSbyteCounter0 = 0; int GPSbyteCounter1 = 0; int GPSbyteCounter2 = 0; @@ -26,6 +26,27 @@ bool message1Complete = false; bool message2Complete = false; +//////////////////////////////////////////////////////////// +//hotstart procedure +//////////////////////////////////////////////////////////// +// Novatel OEM615 startup messages: +// setapproxtime 1605 425384 // GPSWeek & GPSSeconds +// setapproxpos 51.116 -114.038 0 //lat, lon, alt +//////////////////////////////////////////////////////////// +// how to use? +// Assumes a certain power-up sequence: WALDO_FCS must be running on the PC?? +// unlikely this start-up sequence will work!! +// PC MAY generates a startUp message that is checked for before the GPS RX starts +// mbed looks for the startup message for 1 sec. +// startUp message contains: Last PCTime, lastLat, lastLon, LastAlt, LastGPSWeek, LastGPSSeconds +// If it is found, send the startup messages to the receiver +// if it is not found, then just start without the messages (cold start) +// On the PC side, check for a startUp file stored within the .exe folder. +// However, if the PC clock time between now and the lastStart is > 24 hours, ignore startup file +// if the startUp file IS PRESENT, use it to send the startUp message +// After reading the startUp file on the PC, always delete it. +// Every time the system enters "FINESTEERING", save the StartUp file + void sendASCII(char* ASCI_message, int numChars) { ///////////////////////////////////////////////// @@ -62,19 +83,33 @@ if (messagePerSecCounter == 0) GPSbyteCounter0 = 0; else if(messagePerSecCounter == 1) GPSbyteCounter1 = 0; else if(messagePerSecCounter == 2) GPSbyteCounter2 = 0; - loadingMessageBuffer = true; + //loadingMessageBuffer = true; } - if (messagePerSecCounter == 0) { msgBuffer0[GPSbyteCounter0 % maxGPSbytesPerSec] = synch0; GPSbyteCounter0++; } - else if(messagePerSecCounter == 1) { msgBuffer1[GPSbyteCounter1 % maxGPSbytesPerSec] = synch0; GPSbyteCounter1++; } - else if(messagePerSecCounter == 2) { msgBuffer2[GPSbyteCounter2 % maxGPSbytesPerSec] = synch0; GPSbyteCounter2++; } + if (messagePerSecCounter == 0) { msgBuffer0[GPSbyteCounter0 % 1024] = synch0; GPSbyteCounter0++; } + else if(messagePerSecCounter == 1) { msgBuffer1[GPSbyteCounter1 % 512] = synch0; GPSbyteCounter1++; } + else if(messagePerSecCounter == 2) { msgBuffer2[GPSbyteCounter2 % 512] = synch0; GPSbyteCounter2++; } //stop storing the message when we get a LF if (synch0 == 0x0a /* LF*/) //test for line feed { - if (messagePerSecCounter == 0) message0Complete = true; - else if(messagePerSecCounter == 1) message1Complete = true; - else if(messagePerSecCounter == 2) message2Complete = true; + //as further confirmation, we could test the prior byte for a CR = 0x0d; + + if (messagePerSecCounter == 0) + { + if ( msgBuffer0[GPSbyteCounter0 - 2] == 0x0d) //ensure the two byte end of message + message0Complete = true; + } + else if(messagePerSecCounter == 1) + { + if ( msgBuffer0[GPSbyteCounter0 - 2] == 0x0d) //ensure the two byte end of message + message1Complete = true; + } + else if(messagePerSecCounter == 2) + { + if ( msgBuffer0[GPSbyteCounter0 - 2] == 0x0d) //ensure the two byte end of message + message2Complete = true; + } messagePerSecCounter++; //count the messages per second } @@ -87,9 +122,3 @@ }; - - - - - -
--- a/PCMessaging.h Thu Jan 09 14:09:05 2014 +0000 +++ b/PCMessaging.h Mon Mar 03 13:19:31 2014 +0000 @@ -1,40 +1,187 @@ //these are defines for the messages that are sent from the PC across the USB //these messages produce reactions on the mbed +//currently there is only a single message -- the trigger command const unsigned char FIRE_TRIGGER_MSG = 1; -const unsigned short serBuffMax = 18; -char serBuf[serBuffMax]; -int serBufChars=0; +const unsigned short msgBuffSize = 28; +char messageBuffer[msgBuffSize+1]; //the +1 allows for a null-terminated string -//flags to control the PC command actions -bool fireTrigger =false; +//flags to control the mbed actions (in main) in response to the PC command +bool fireTrigger = false; unsigned char CR = 0x0d; //ASCII Carriage Return unsigned char LF = 0x0a; //ASCII Line Feed +int totalNumChars = 0; + bool validMessage = false; - +/* void readFromPC() { - // should this be a while rather than if ??? -- may have multiple bytes in buffer - if (toPC.readable()) //read a PC serial byte and test it for a command + //read all available PC serial bytes and test the sequence for a command string + if (toPC.readable()) + { loopsThroughPCreadable++; //reset to zero when we detect a valid message + //wait_ms(5); + } + while (toPC.readable()) { + /////////////////////////////////////////////////////////////////////////////////// + //PC-to-mbed and mbed-to-PC message format + // 1) preamble: "mbedmessage " (note the space) + // 2) messageType "trig " (note the space + // 3) 10-char dataValue "0000000000" (ASCI integer representation + //0123456789012345678901234567890123456789 + //mbedmessage trig 0000000000 CR/LF + /////////////////////////////////////////////////////////////////////////////////// + // Read in next character - // why not read all available bytes + // we get through the main loop at > 500,000 times a sec unsigned char inChar = 0; inChar = toPC.getc(); //read char from the USB serial link to the PC - //incoming messages will end witb a CR / LF -- disregard these chars - if (inChar == CR || inChar == LF) return; //CR is a 0x0a + totalNumChars++; + + //slide all the characters back by one place -- oldest char in the buffer gets discarded + for (int i=0; i<(msgBuffSize-1); i++) messageBuffer[i] = messageBuffer[i+1]; + //now fill in the last entry to the buffer with the most recent received char + messageBuffer[msgBuffSize-1] = inChar; + + //locate the proper message signature in the saved buffer: "mbedmessage " + if (messageBuffer[0] != 'm') continue; + else if( messageBuffer[1] != 'b') continue; + else if( messageBuffer[2] != 'e') continue; + else if( messageBuffer[3] != 'd') continue; + else if( messageBuffer[4] != 'm') continue; + else if( messageBuffer[5] != 'e') continue; + else if( messageBuffer[6] != 's') continue; + else if( messageBuffer[7] != 's') continue; + else if( messageBuffer[8] != 'a') continue; + else if( messageBuffer[9] != 'g') continue; + else if( messageBuffer[10] != 'e') continue; + else if( messageBuffer[11] != ' ') continue; + //else if( messageBuffer[28] != CR) return; + //else if( messageBuffer[29] != LF) return; - //all incoming messages will start with "mbedMessage", "messageType", numberDataBytes, and will end with CR & LF - //1) look for the mbedMessage and then get the next byte as numberDataBytes; - //2) read the next numberDataBytes and then look for CR & LF - //if 1) & 2) are successful, declare a valid incoming message - //if message is valid, then send a response as a repeat of the original message - // + //from testing: this always activates on inChar multiple of 28 + //if ( inChar == 0x0a ) toPC.printf("%d LF received: %s\n",totalNumChars, messageBuffer); + + //if we get here. we have the proper signature of an incoming message + //now test for the message types + + //why do we do this?? + //messageBuffer[msgBuffSize]= '/0'; //make the message buffer a null-terminated ASCII string + + //if (toPC.writeable()) toPC.printf(messageBuffer); + + if ( messageBuffer[12] == 't' && + messageBuffer[13] == 'r' && + messageBuffer[14] == 'i' && + messageBuffer[15] == 'g' ) + { + //we have detected a trigger command message: "trig" + //send back a response to this message + if (toPC.writeable()) + { + toPC.printf("FromMbed %s \n", messageBuffer); + toPC.printf("FromMbed %d %d \n", loopsThroughPCreadable, totalNumChars); + } + fireTrigger = true; + loopsThroughPCreadable = 0; + } } //end pc.readable }; +*/ +void readFromPC() +{ + + //if there is no chars to read -- just return; + if (!toPC.readable()) return; + + totalNumChars = 0; + + if (toPC.writeable()) + { + toPC.printf("FromMbed testing for Trigger \n"); + } + + bool timeOutInRead = false; + + timeInMessageRead.reset(); + + while (totalNumChars < 16) + { + /////////////////////////////////////////////////////////////////////////////////// + //PC-to-mbed and mbed-to-PC message format + // 1) preamble: "mbedmessage " (note the space) + // 2) messageType "trig " (note the space + // 3) 10-char dataValue "0000000000" (ASCI integer representation + //0123456789012345678901234567890123456789 + //mbedmessage trig 0000000000 CR/LF + /////////////////////////////////////////////////////////////////////////////////// + + // we get through the main loop at > 500,000 times a sec + + if ( toPC.readable() ) + { + messageBuffer[totalNumChars] = toPC.getc(); + totalNumChars++; + } + + if (timeInMessageRead.read_us() > 500) + { + timeOutInRead = true; + break; + } + } + + messageBuffer[28] = '\0'; //make a null-terminated string + + if (timeOutInRead) + { + if (toPC.writeable()) + { + toPC.printf("FromMbed TimeOut %s \n", messageBuffer); + } + } + + + + if (messageBuffer[0] == 'm' && + messageBuffer[1] == 'b' && + messageBuffer[2] == 'e' && + messageBuffer[3] == 'd' && + messageBuffer[4] == 'm' && + messageBuffer[5] == 'e' && + messageBuffer[6] == 's' && + messageBuffer[7] == 's' && + messageBuffer[8] == 'a' && + messageBuffer[9] == 'g' && + messageBuffer[10] == 'e' && + messageBuffer[11] == ' ' && + messageBuffer[12] == 't' && + messageBuffer[13] == 'r' && + messageBuffer[14] == 'i' && + messageBuffer[15] == 'g' ) + { + + + //we have detected a trigger command message: "trig" + //send back a response to this message + if (toPC.writeable()) + { + toPC.printf("FromMbed %s %d\n", messageBuffer, timeInMessageRead.read_us()); + } + + for (int i=0; i<=msgBuffSize; i++) messageBuffer[i] = 0; + fireTrigger = true; + } + else + { + toPC.printf("FromMbed badMsg %s \n", messageBuffer); + for (int i=0; i<=msgBuffSize; i++) messageBuffer[i] = 0; + } +}; +
--- 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 +} +