JEK changes enabling proper recording of IMU/GPS datastrams - 02-APR-2013
Fork of GPS_Incremental by
PCMessaging.h
- Committer:
- jekain314
- Date:
- 2013-04-02
- Revision:
- 6:2a8486283198
- Child:
- 7:2e20e4cf53e6
File content as of revision 6:2a8486283198:
//these are defines for the messages that are sent from the PC across the USB //these messages produce reactions on the mbed const unsigned char STATUS_MSG =0; const unsigned char POSVEL_MSG =1; const unsigned char STARTDATA_MSG =2; const unsigned char STOPDATA_MSG =3; const unsigned char STARTSTREAM_MSG =4; const unsigned char STOPSTREAM_MSG =5; const unsigned char STARTLOGINFO_MSG =6; const unsigned char STOPLOGINFO_MSG =7; const double DEGREES_TO_RADIANS = acos(-1.0)/180.0; const double eccen = 0.0818191908426; //WGS84 earth eccentricity const double earthRadius = 6378137; //WGS84 earthRadius in meters char serBuf[128]; int serBufChars=0; //flags to control the PC command actions bool sendPosVel =false; bool sendStatus =false; bool sendRecData =false; bool streamPos =false; bool sendStreamPos =false; bool logMsgInfo =false; bool sendLogMsgInfo =false; bool recordData =false; const unsigned char numMessages = 8; //number of potential messages char msgList[numMessages][32]; //text array storing the command messages from the PC char minMessageSize = 10; //minimum size of a text message unsigned char CR = 0x0d; //ASCII Carriage Return unsigned char LF = 0x0a; //ASCII Line Feed void setUpMessages(void) { //set up the ASCII text records that are candidates to be passed from the PC 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"); sprintf(msgList[STARTLOGINFO_MSG], "WMsg LOGINFO Y"); sprintf(msgList[STOPLOGINFO_MSG], "WMsg LOGINFO N"); //message length is from 10 to 16 chars toPC.printf(" finished setting up messages \n"); } void readFromPC() { //The received commands only 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(); //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; serBuf[serBufChars++] = inChar; //set this char in a char array //no need to continue if numChars are less than the shortest candidate message if (serBufChars < minMessageSize) return; // Append end of string //We always assume we have a complete message string and test for this below serBuf[serBufChars] = '\0'; bool validMessage = false; // Check for valid message -- there are numMessages possible messages for (int m = 0; m < numMessages && !validMessage; m++) //check for all messages ... { //toPC.printf(" \n\n found chars to test %3d %3d %s \n\n\n ", serBufChars, strlen(msgList[m]), serBuf ); //check the current partial message against ALL possible messages //messages must match in both strength length and text if (serBufChars == strlen(msgList[m]) && strncmp(serBuf, msgList[m], serBufChars) == 0 ) { //toPC.printf( "\n found valid message \n\n"); validMessage = true; serBufChars = 0; //reset the character count to reset for next message //set programmatic action flags based on the message switch(m) { case STATUS_MSG: sendStatus = true; //send a status message back to PC break; case POSVEL_MSG: sendPosVel = true; //send a posvel message back to PC break; case STARTDATA_MSG: //start the data recording to the SD card recordData = true; sendRecData = true; break; case STOPDATA_MSG: //stop the data recording to the SD card recordData = false; sendRecData = true; break; case STARTSTREAM_MSG: case STOPSTREAM_MSG: streamPos = (m == STARTSTREAM_MSG); sendStreamPos = true; break; case STARTLOGINFO_MSG: case STOPLOGINFO_MSG: logMsgInfo = (m == STARTLOGINFO_MSG); sendLogMsgInfo = true; break; } //end Switch statement break; } //end test for a valid message } //end message text loop } //end pc.readable }; 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 //latRateFac & lonRateFac multiplied by Vnorth or VEast to get latRate and lonRate //see this document (page 32) www.fas.org/spp/military/program/nav/basicnav.pdf 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; //divide Vnorth by latRateFac to get the latitude rate in deg per sec latRateFac = (r_meridian + height)* DEGREES_TO_RADIANS; //divide VEast by lonRateFac to get the longitude rate in deg per sec lonRateFac = (r_normal + height) * cos(latitudeRad)* DEGREES_TO_RADIANS; } void sendPosVelMessageToPC(OEM615BESTPOS posMsg, OEM615BESTVEL velMsg) { //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); double latRateFac; double lonRateFac; earthCoefficients( posMsg.latitude*DEGREES_TO_RADIANS, posMsg.longitude*DEGREES_TO_RADIANS, posMsg.height, latRateFac, lonRateFac); //commented calculations are for a spherical earth (Chris's original computation) // 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 //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 -- GPSTime is the time of the last valid GPS position message double posTime = GPSTime + elTime; //toPC.printf(" elTime = %6.3f PPSimeOffset = %6.3f \n", elTime, PPSTimeOffset); //toPC.printf(" latRateFac = %10.3f lonRateFac = %10.3f \n", latRateFac, lonRateFac); //toPC.printf(" latRateFac = %10.3f lonRateFac = %10.3f \n", latMetersPerDeg, lonMetersPerDeg); // 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 latPos = posMsg.latitude + (nVel/latRateFac)*elTime; double lonPos = posMsg.longitude + (eVel/lonRateFac)*elTime; double htPos = posMsg.height + velMsg.verticalSpeed/(60*1852)*elTime; toPC.printf("WMsg POSVEL %5.3lf %1d %s %8.5lf %9.5lf %4.3lf %4.3lf %4.3lf %4.3lf\n", posTime, posMsg.numSolSV, posMsg.solStatus, latPos, lonPos, htPos, nVel, eVel, velMsg.verticalSpeed ); } void processPCmessages(FILE* &fpNav, OEM615BESTPOS posMsg, OEM615BESTVEL velMsg) { //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; //set to true if a POSVEL is requested from the PC sendPosVelMessageToPC(posMsg, velMsg); } //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'; //solStatus if (posMsg.solStatus == 0) //see description of solution status in OEMV615.h { solReady = 'Y'; } toPC.printf("WMsg STATUS %5.3lf %c\n", GPSTime, solReady ); } //should just record ALL the data -- can pick over it in the post-processing if (sendRecData) //begin to (or stop) record the serial data { sendRecData=false; char recChar = 'N'; if (recordData) { if ((fpNav == NULL)) { fpNav = fopen("/sd/Data/NAV.bin", "wb"); //toPC.printf("\n opened the SD card file recordData=%10d \n\n", fpNav); } if (fpNav != NULL) { recChar = 'Y'; } else { toPC.printf(" Could not open the SD card \n\n"); } } else { if (fpNav != NULL) { toPC.printf(" closing the SD card file \n\n"); fclose(fpNav); recordData = false; fpNav = NULL; } } toPC.printf("WMsg RECORDDATA %c\n", recChar ); } if (sendStreamPos) //stream the position data to the PC { sendStreamPos=false; char streamChar = 'N'; if (streamPos) { streamChar = 'Y'; } toPC.printf("WMsg POSSTREAM %c\n", streamChar ); } //not sure this is ever used .. if (sendLogMsgInfo) //send log info to the PC { sendLogMsgInfo=false; char logChar = 'N'; if (logMsgInfo) { logChar = 'Y'; } toPC.printf("WMsg LOGINFO %c\n", logChar ); } }