JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of GPS_Incremental by
main.cpp
- Committer:
- dannyman939
- Date:
- 2013-03-19
- Revision:
- 0:c746ee34feae
- Child:
- 1:cbb9104d826f
File content as of revision 0:c746ee34feae:
#include "mbed.h" //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 //#include "rtos.h" #include "OEM615.h" #include "ADIS16488.h" #include <string> #define STATUS_MSG 0 #define POSVEL_MSG 1 #define STARTDATA_MSG 2 #define STOPDATA_MSG 3 #define STARTSTREAM_MSG 4 #define STOPSTREAM_MSG 5 #define DEGREES_TO_RADIANS (3.14519/180.0) //general items 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); InterruptIn Camera2Int(p29); //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; int IMUClockCounter = 0; bool camera1EventDetected = false; bool camera2EventDetected = false; double camera1Time; double camera2Time; char serBuf[64]; int serBufChars=0; bool sendPosVel=false; bool sendStatus=false; bool sendRecData=false; bool streamPos=false; bool sendStreamPos=false; //ISR for detection of the GPS 1PPS void detect1PPSISR(void) { timeFromPPS.reset(); savedIMUClockCounter = IMUClockCounter; IMUClockCounter = 0; GPS_COM1.rxBufferFlush(); //byteCounter=0; detectedGPS1PPS = true; lookingForMessages = true; PPSCounter++; PPSTimeOffset++; ppsled = !ppsled; }; //ISR for detection of the hotshoe trigger 1 void Camera1ISR(void) { camera1Time = GPSTime + (double)PPSTimeOffset + timeFromPPS.read(); //detectedTrigger1 = true; trig1led = !trig1led; camera1EventDetected = true; }; //ISR for detection of the hotshoe trigger 2 void Camera2ISR(void) { camera2Time = GPSTime + (double)PPSTimeOffset + timeFromPPS.read(); //detectedTrigger1 = true; camera2EventDetected = true; }; void readFromPC() { 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 serBuf[serBufChars] = '\0'; // Need to parse message to determine behavior // Need to clean this up char msgList[6][32]; sprintf(msgList[STATUS_MSG], "WMsg STATUS"); sprintf(msgList[POSVEL_MSG], "WMsg POSVEL"); sprintf(msgList[STARTDATA_MSG], "WMsg RECORDDATA Y"); sprintf(msgList[STOPDATA_MSG], "WMsg RECORDDATA N"); sprintf(msgList[STARTSTREAM_MSG], "WMsg POSSTREAM Y"); sprintf(msgList[STOPSTREAM_MSG], "WMsg POSSTREAM N"); // assume an invalid message which needs to be reset bool validMessage = false; bool resetMessage = true; // Check for valid message for (int m = 0; m < 6 && !validMessage; m++) { if (strncmp(serBuf, msgList[m], serBufChars) == 0) { validMessage = true; // buffer length is same as message length if (serBufChars == strlen(msgList[m])) { switch(m) { case STATUS_MSG: sendStatus = true; break; case POSVEL_MSG: sendPosVel = true; break; case STARTDATA_MSG: case STOPDATA_MSG: recordData = (m == STARTDATA_MSG); sendRecData = true; break; case STARTSTREAM_MSG: case STOPSTREAM_MSG: streamPos = (m == STARTSTREAM_MSG); sendStreamPos = true; break; } } // message is still in progress, do not reset else { resetMessage = false; } } } // if message should be reset if (resetMessage) { // reset serial buffer character count serBufChars = 0; // if invalid message and most recent character is 'W' (first message character), // possible message collision if ((!validMessage) && (inChar == 'W')) { // start a new message serBuf[serBufChars++] = inChar; serBuf[serBufChars] == '\0'; } // Append end of string character serBuf[serBufChars] = '\0'; } } }; void sendASCII(char* ASCI_message, int numChars) { //char ASCI_message[] = "unlogall COM1"; int as = numChars - 1; unsigned char CR = 0x0d; //ASCII Carriage Return unsigned char LF = 0x0a; //ASCII Line Feed //printf("%s", ch); //printf("\n"); for (int i=0; i<as; i++) GPS_COM1.putc(ASCI_message[i]); GPS_COM1.putc(CR); //carriage return at end GPS_COM1.putc(LF); //line feed at end } //FILE* fp = NULL; 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.printf("\n\n released GPS from RESET and set to high baud rate \n\n"); //just wait to lauinch the GPS receiver for (int i=0; i<5; i++) { toPC.printf(" to start: %3d \n", 4-i); wait(1); } mkdir("/sd/Data", 0777); /* fp = fopen("/sd/Data/IMUGPS.bin", "wb"); if (fp == NULL) { toPC.printf(" cannot open the IMUGPS data file \n"); } else toPC.printf(" opened the IMUGPS data file \n"); */ //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 CDU can be used 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"; //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 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(921600); wait_ms(500); //without this wait -- the baud rate is not detected when using MODSERIAL }; void setupTriggers() { Camera1Int.mode(PullUp); Camera2Int.mode(PullUp); //establish Trigger ISR Camera1Int.rise(&Camera1ISR); Camera2Int.rise(&Camera2ISR); } int test = 0; unsigned short messageCounter = 0; unsigned short savedMessageCounter = 0; unsigned char msgBuffer[1024]; unsigned short messageLocation[4] = {0}; //see the mbed COOKBOOK for MODSERIAL //MODSERIAL is an easy to use library that extends Serial to add fully buffered input and output. void readSerialByte(MODSERIAL_IRQ_INFO *q) { MODSERIAL *serial = q->serial; unsigned char synch0 = serial->getc(); msgBuffer[byteCounter] = synch0; //we need to trap the GPS message header byte-string 0xAA44121C //generate a 4-byte sliding-window sequence from the input bytes //shift last 4-byte value left 8 bits & push recently read byte (synch0) into low-order byte test = (test<<8) | synch0; // if (test == 0xAA44121C) //test for the Receiver message header signature { messageLocation[perSecMessageCounter] = byteCounter; //store the location of this message (with 4 synch words) perSecMessageCounter++; messageDetected = true; } byteCounter++; //total per-sec byte counter (reset to zero in main when 1PPS detected) }; int main() { //FILE *fpIMU = NULL; //FILE *fpGPS = NULL; FILE *fpNav = NULL; OEM615BESTPOS posMsg; OEM615BESTPOS curPos; OEM615BESTVEL velMsg; OEM615BESTVEL curVel; //set up th GPS and mbed COM ports setupCOM(); //set up the ADIS16488 setupADIS(); //setup Hotshoe setupTriggers(); //attempt to use the mbed RTOS library //Thread thread(writeThread); toPC.printf("completed setting up COM ports \n"); //set up the interrupt to catch the serial byte availability GPS_COM1.attach(&readSerialByte, MODSERIAL::RxIrq); timeFromPPS.start(); //while(PPSCounter < 1000) while(1) { //read the USB serial data from the PC to check for commands readFromPC(); if (sendPosVel) { sendPosVel=false; char solReady = 'N'; if (posMsg.solStatus == 0) { solReady = 'Y'; } 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 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 double elTime = (double)PPSTimeOffset + timeFromPPS.read(); // Position time double posTime = GPSTime + elTime; // Estimated position based on previous position and velocity double latPos = posMsg.latitude + (nVel/latMetersPerDeg)*elTime; double lonPos = posMsg.longitude + (eVel/lonMetersPerDeg)*elTime; double htPos = posMsg.height + velMsg.verticalSpeed/(60*1852)*elTime; toPC.printf("WMsg POSVEL %5.3lf %d %c %8.5lf %9.5lf %4.3lf %4.3lf %4.3lf %4.3lf\n", posTime, posMsg.numSolSV, solReady, latPos, lonPos, htPos, nVel, eVel, velMsg.verticalSpeed ); } if (sendStatus) { sendStatus=false; char solReady = 'N'; if (posMsg.solStatus == 0) { solReady = 'Y'; } toPC.printf("WMsg STATUS %5.3lf %c\n", GPSTime, solReady ); } if (sendRecData) { sendRecData=false; char recChar = 'N'; if (recordData) { if ((fpNav == NULL)) { fpNav = fopen("/sd/Data/NAV.bin", "wb"); } if (fpNav != NULL) { recChar = 'Y'; } //recChar = 'Y'; } else { if (fpNav != NULL) { fclose(fpNav); fpNav = NULL; } /* if (fpIMU != NULL) { fclose(fpIMU); fpIMU = NULL; } if (fpGPS != NULL) { fclose(fpGPS); fpGPS = NULL; } */ } toPC.printf("WMsg RECORDDATA %c\n", recChar ); } recordDataled = recordData; if (sendStreamPos) { sendStreamPos=false; char streamChar = 'N'; if (streamPos) { streamChar = 'Y'; } toPC.printf("WMsg POSSTREAM %c\n", streamChar ); } if (IMUDataReady) { //write the IMU data //if (recordData && (fpIMU != NULL)) if (recordData && (fpNav != NULL)) { fwrite(imuRec.dataWord, sizeof(IMUREC), 1, fpNav); } IMUDataReady = false; } if (lookingForMessages && (timeFromPPS.read_us() > 15000)) //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++) { msgHeader[i] = (MESSAGEHEADER*)&msgBuffer[messageLocation[i]-3]; //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]-3], sizeof(OEM615BESTPOS)); curPos = *((OEM615BESTPOS*)&msgBuffer[messageLocation[i]-3]); 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); } } else if (msgHeader[i]->messageID == 99) { // Wait until position message has also been received before using as // base position //memcpy(&curVel, &msgBuffer[messageLocation[i]-3], sizeof(OEM615BESTVEL)); curVel = *((OEM615BESTVEL*)&msgBuffer[messageLocation[i]-3]); } 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; } } } //toPC.printf(" %3d %8d \n", msgHeader[0]->timeStatus, GPSTimemsecs); //if (recordData && (fpGPS != NULL)) if (recordData && (fpNav != NULL)) { fwrite(msgBuffer, byteCounter, 1, fpNav); } lookingForMessages = false; } if (messageDetected) { //toPC.printf(" msgTime = %4d \n", timeFromPPS.read_us()); messageDetected = false; } if (camera1EventDetected) { toPC.printf("WMsg TRIGGERTIME Camera1 %5.3lf\n", camera1Time); camera1EventDetected = false; } if (camera2EventDetected) { toPC.printf("WMsg TRIGGERTIME Camera2 %5.3lf\n", camera2Time); camera2EventDetected = false; } if (detectedGPS1PPS) { //toPC.printf(" PPSCounter = %4d byteCounter = %10d num Messages Received = %3d IMUClock = %4d\n", // PPSCounter, byteCounter, perSecMessageCounter, savedIMUClockCounter); byteCounter = 0; perSecMessageCounter=0; detectedGPS1PPS = false; } } toPC.printf(" normal termination \n"); }