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
Diff: PCMessaging.h
- Revision:
- 0:432b860b6ff7
- Child:
- 1:8e24e633f8d8
diff -r 000000000000 -r 432b860b6ff7 PCMessaging.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PCMessaging.h Mon Apr 22 21:26:04 2013 +0000 @@ -0,0 +1,305 @@ +//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 = 11; //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 + //toPC.printf("%02x ",inChar); + + //incoming messages will end witb a CR / LF -- disregard these chars + if (inChar == CR || inChar == LF) return; //CR is a 0x0a + + 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 %s \n\n", serBuf); + + 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; + + char solReady = 'N'; + //solStatus + if (posMsg.solStatus == 0) //see description of solution status in OEMV615.h + { + solReady = 'Y'; + } + + toPC.printf("WMsg POSVEL %5.3lf %1d %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 + ); +} + +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"); + setvbuf(fpNav, NULL, _IONBF, 512); + //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); + wait(1.0); + //toPC.printf("\n after closing the SD card file \n\n"); + 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 + ); + } + + +}