JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of GPS_Incremental by
main.cpp
- Committer:
- jekain314
- Date:
- 2013-04-06
- Revision:
- 7:2e20e4cf53e6
- Parent:
- 6:2a8486283198
- Child:
- 8:13724ed3f825
File content as of revision 7:2e20e4cf53e6:
#include "mbed.h" #include <string> //set up the message buffer to be filled by the GPS read process #define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 256 #include "MODSERIAL.h" #include "SDFileSystem.h" //imported using the import utility //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"); DigitalIn sd_detect(p27); 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); // We dont 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; //flag set in the ISR and reset after processing the 1PPS event int PPSCounter = 0; //counts the 1PPS occurrences int byteCounter = 0; //byte counter -- zeroed at 1PPS 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 unsigned long IMUbytesWritten = 0; //counts the IMU bytes written by the fwrite() to the SD card int savedByteCounter = 0; //save ByteCounter at the 1PPS for display in main int savedPerSecMessageCounter=0; //saved PerSecMsgCounter for display in main int IMUClockCounter = 0; //counter for IMU samples per sec int savedIMUClockCounter=0; //saved at the 1PPS for later diaplay from main bool camera1EventDetected = false; //flag from ISR indicating a clock event occurred double camera1Time; //GPS time of the camera event int TotalBadCRCmatches = 0; //counter for the bad CRC matches for all GPS messages ////////////////////////////////////////////////////////////////////// // the below should become classes ////////////////////////////////////////////////////////////////////// #include "OEM615.h" //OEM615 GPS activities #include "ADIS16488.h" //ADIS16488 activities #include "PCMessaging.h" //PC messaging activities //ISR for detection of the GPS 1PPS void detect1PPSISR(void) { timeFromPPS.reset(); //reset the 1PPS timer upon 1PPS detection savedIMUClockCounter = IMUClockCounter; //number of IMU clocks received since last 1PPS savedByteCounter = byteCounter; //save byteCounter for display in main savedPerSecMessageCounter = perSecMessageCounter; //save for display un main IMUClockCounter = 0; //counts per-sec IMU samples (between 1PPS events) byteCounter = 0; //countes bytes between 1PPS events perSecMessageCounter = 0; //counts GPS messages between 1PPS events GPS_COM1.rxBufferFlush(); //flush the GPS serial buffer detectedGPS1PPS = true; //set false in the main when 1PPS actions are complete lookingForMessages = true; //set false in main after processing messages PPSCounter++; //count number of 1PPS epoch //note -- the below accounts for time information becoming available AFTER the 1PPS event PPSTimeOffset++; //counts 1PPS events between matching POS and VEL messages ppsled = !ppsled; //blink an LED at the 1PPS }; //ISR for detection of the hotshoe trigger 1 void camera1ISR(void) { //GPSTime is from POS message header //PPSTimeOffset is an even sec to account for Time becoming known AFTER the 1PPS //PPSTimeOffset + timeFromPPS.read() can be as large as 1.02 secs camera1Time = GPSTime + PPSTimeOffset + timeFromPPS.read(); camera1EventDetected = true; //reset to false in main after processing the image detection trig1led = !trig1led; //blink an LEWD at the camera event detection }; /////////////////////////////////////////////////////// //set up the USB port and the GPS COM port /////////////////////////////////////////////////////// FILE *fpNav = NULL; //file pointer to the nav file on the SD card void setupCOM(void) { //system starts with GPS in reset active //dis-engage the reset to get the GPS started GPS_Reset=1; wait_ms(1000); //establish 1PPS ISR PPSInt.rise(&detect1PPSISR); //set the USB serial data rate -- rate must be matched at the PC end //This the serial communication back to the the PC host //Launch the C++ serial port read program there to catch the ASCII characters //toPC.baud(9600); wait_ms(100); toPC.baud(8*115200); wait_ms(100); //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 launch the GPS receiver for (int i=0; i<5; i++) { toPC.printf(" to start: %3d \n", 4-i); wait(1); } sd_detect.mode(PullUp); if (sd_detect == 0) { mkdir("/sd/Data", 0777); } else { toPC.printf(" SD card not present \n"); } fpNav = fopen("/sd/Data/IMUGPS.bin", "wb"); if (fpNav == NULL) { toPC.printf(" cannot open the IMUGPS data file \n"); } 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 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"; //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 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 //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"); sendASCII(ch7, sizeof(ch7)); wait_ms(500); //sendASCII(ch0, sizeof(ch0)); toPC.printf("unlog all messages \n"); sendASCII(ch1, sizeof(ch1)); wait_ms(500); toPC.printf("log BESTPOSB on COM1 \n"); sendASCII(ch3, sizeof(ch3)); wait_ms(500); toPC.printf("log BESTVELB on COM1\n"); sendASCII(ch4, sizeof(ch4)); wait_ms(500); toPC.printf("log RANGEB on COM1\n"); sendASCII(ch5, sizeof(ch5)); wait_ms(500); //toPC.printf("log TIMEB om COM1 \n"); //sendASCII(ch6, sizeof(ch6)); wait_ms(100); //toPC.printf("Set up th VARF signal \n"); //sendASCII(ch8, sizeof(ch8)); wait_ms(500); //set GPS output COM1 to the final high rate toPC.printf("set the COM ports to high rate\n"); sendASCII(ch2, sizeof(ch2)); wait_ms(500); //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(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() { camera1Int.mode(PullUp); camera2Pin = 1; //establish Trigger ISR camera1Int.rise(&camera1ISR); }; ///////////////////////////////////////////////////////////////////// // mbed main to support the Waldo_FCS ///////////////////////////////////////////////////////////////////// int main() { //these are structures for the to GPS messages that must be parsed OEM615BESTPOS posMsg; //BESTPOS structure in OEMV615.h that has matching time to a BESTVEL message OEM615BESTPOS curPos; //BESTPOS structure in OEMV615.h OEM615BESTVEL velMsg; //BESTVEL structure in OEMV615.h that has matching time to a BESTPOS message OEM615BESTVEL curVel; //BESTVEL structure in OEMV615.h //set up the GPS and mbed COM ports setupCOM(); //set up the ADIS16488 setupADIS(); //setup Hotshoe setupTriggers(); setUpMessages(); //set up the expected text message commands frm the PC //set up 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 toPC.printf("\n\n top of the main loop \n\n"); int totalBytesWritten = 0; //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(); processPCmessages(fpNav, posMsg, velMsg); // //////////////////////////////////////////////////////////////////////////// //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++) { msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]]; //toPC.printf("WMsg MESSAGEINFO %5d %5d \n", // msgHeader[i]->messageID, // messageLocation[i]); //calculated CRC unsigned long CRC1 = CalculateBlockCRC32(28+msgHeader[i]->messageLength, &msgBuffer[messageLocation[i]]); unsigned long CRC2 = *((unsigned long*)&msgBuffer[messageLocation[i] + 28 + msgHeader[i]->messageLength]); if (CRC1 != CRC2) { TotalBadCRCmatches++; toPC.printf(" bad CRC match for messageID %3d total CRC errors = %4d \n", msgHeader[i]->messageID, TotalBadCRCmatches); continue; } //test for a message 42 (BESTPOS) if (msgHeader[i]->messageID == 42) { 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, curPos.posType, curPos.latitude, curPos.longitude, curPos.height, curPos.undulation, curPos.latitudeSTD, curPos.longitudeSTD, curPos.heightSTD, curPos.diffAge, curPos.solutionAge, curPos.numSV, curPos.numSolSV, curPos.numGGL1, curPos.extSolStatus, curPos.sigMask); } } //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) { // 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; ///////////////////////////////////////////////////////////////////////////////////////// } } //end of per message loop lookingForMessages = false; if (recordData && (fpNav != NULL) && (byteCounter > 0)) { wait_us(1000); totalBytesWritten += fwrite(msgBuffer, 1, byteCounter, fpNav); // this writes out a complete set of messages for this sec wait_us(1000); } } //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 { //GPSTime (secs from midnight) is from the header of the position message //PPSTimeOffset accounts for time becoming known ~20msec AFTER the 1PPS imuRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + timeFromPPS.read_us()/1000.0; wait_us(10); spi.write((int) HIGH_REGISTER[0]); wait_us(10); // 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]); wait_us(10) ; 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]); wait_us(10); } imuRec.dataWord[i] = wd.dataWord; //data word is a signed long } IMURecordCounter++; //write the IMU data if (recordData && (fpNav != NULL)) { totalBytesWritten += fwrite(&imuRec, 1, sizeof(IMUREC), fpNav); } IMUClockCounter++; IMUDataReady = false; } /* if (messageDetected) //some GPS message header has been detected { toPC.printf(" msgTime = %4d \n", timeFromPPS.read_us()); messageDetected = false; } */ if (camera1EventDetected) //we have detected a camera trigger event { toPC.printf("WMsg TRIGGERTIME %5.3lf\n", camera1Time); camera1EventDetected = false; } if (detectedGPS1PPS) //true if we are exactly at a 1PPS event detection { //toPC.printf(" PPSCounter=%4d byteCounter=%10d Msgs Received=%3d IMUClock=%4d bytesWritten=%8d\n", // PPSCounter, savedByteCounter, savedPerSecMessageCounter, savedIMUClockCounter, totalBytesWritten); detectedGPS1PPS = false; } } fclose(fpNav); toPC.printf(" normal termination \n"); }