JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of GPS_Incremental by
main.cpp
- Committer:
- jekain314
- Date:
- 2013-04-19
- Revision:
- 9:b45feb91ba38
- Parent:
- 8:13724ed3f825
File content as of revision 9:b45feb91ba38:
#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"); } //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 115200 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(115200); 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 MESSAGEHEADER msgHdr; 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; /* unsigned long CRC = 0; CRC32Value(CRC, 0xAA); CRC32Value(CRC, 0x44); CRC32Value(CRC, 0x12); CRC32Value(CRC, 0x1C); toPC.printf(" CRC after AA44121C header: %08x \n", CRC); wait(20); */ int CRCerrors = 0; recordData = true; sendRecData = true; while(PPSCounter < 300) /////////////////////////////////////////////////////////////////////////// // 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 this loop ///////////////////////////////////////////////////////////////////////////// if (completeMessageAvailable && !IMUDataReady) { msgHdr = *((MESSAGEHEADER*)&msgBuffer[messageLocation[savedMessageCounter-1]]); GPSTimemsecs = msgHdr.GPSTime_msecs; ///////////////////////////////////////////////////////////////////////////////////////// //IMPORTANT: we reset the PPSTimeOffset when we have a matching position and velocity PPSTimeOffset = 0; ///////////////////////////////////////////////////////////////////////////////////////// unsigned long msgCRC = *((unsigned long*)&msgBuffer[messageLocation[savedMessageCounter-1] + 28 + msgHdr.messageLength]); //toPC.printf("tmeFrom1PPS= %5d ID= %3d Ln = %3d computedCRC= %08x msgCRC= %08x msgCntr = %3d CRCerr=%4d\n", // timeFromPPS.read_us(), msgHdr.messageID, msgHdr.messageLength, computedCRC, msgCRC, savedMessageCounter, TotalBadCRCmatches); if ( msgCRC != computedCRC) { toPC.printf(" bad CRC match for messageID %3d total CRC errors = %4d \n", msgHdr.messageLength, TotalBadCRCmatches++); } if (msgHdr.messageID == 42) { curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[savedMessageCounter-1]]); posMsg = curPos; if (streamPos) { toPC.printf("WMsg BESTPOS %5d %1d %8.5lf %9.5lf %5.3lf %d %d %d\n", curPos.msgHeader.GPSTime_msecs, curPos.solStatus, curPos.latitude, curPos.longitude, curPos.height, curPos.numSV, curPos.numSolSV, curPos.numGGL1); } ///////////////////////////////////////////////////////////////////////////////////////// //IMPORTANT: we reset the PPSTimeOffset when we have a matching position and velocity PPSTimeOffset = 0; ///////////////////////////////////////////////////////////////////////////////////////// } else if (msgHdr.messageID == 99) { curVel = *((OEM615BESTVEL*)&msgBuffer[ messageLocation[savedMessageCounter-1] ]); velMsg = curVel; } if (recordData && (fpNav != NULL) && (byteCounter > 0)) { //wait_us(10); int totalMessageLength = 28 + msgHdr.messageLength + 4; //header length + message Length + CRC word size totalBytesWritten += fwrite(&msgBuffer[messageLocation[savedMessageCounter-1]], 1, totalMessageLength, fpNav); // this writes out a complete set of messages for this sec //wait_us(10); } completeMessageAvailable = false; } //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 IMUtimeFrom1PPS = timeFromPPS.read_us(); imuRec.GPSTime = GPSTimemsecs + PPSTimeOffset*1000 + IMUtimeFrom1PPS/1000.0; //wait_us(1); spi.write((int) HIGH_REGISTER[0]); //wait_us(1); // 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(1) ; 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 && (fpNav != NULL)) { totalBytesWritten += fwrite(&imuRec, 1, sizeof(IMUREC), fpNav); } IMUDataReady = 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("\n PPSCounter=%4d byteCounter=%10d Msgs Received=%3d IMUClock=%4d IMURec = %3d bytesWritten=%8d\n", PPSCounter, savedByteCounter, savedPerSecMessageCounter, savedIMUClockCounter, IMURecordCounter, totalBytesWritten); IMURecordCounter = 0; detectedGPS1PPS = false; } } fclose(fpNav); toPC.printf(" normal termination \n"); }