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
PCMessaging.h
- Committer:
- jekain314
- Date:
- 2013-05-04
- Revision:
- 5:2ce1be9d4bef
- Parent:
- 2:7039be3daf6e
- Child:
- 6:71da5b99de97
File content as of revision 5:2ce1be9d4bef:
//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 unsigned char FIRE_TRIGGER_MSG =8; 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 const unsigned short serBuffMax = 1024; char serBuf[serBuffMax]; 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; bool fireTrigger =false; const unsigned char numMessages = 9; //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"); sprintf(msgList[FIRE_TRIGGER_MSG], "WMsg TRIGGER"); //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 unsigned char inChar = 0; if(toPC.readable() ) 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 // serBuffMax = 1024 -- largest serBuffMax is 1023 -- but we add one below for the '\0' if (serBufChars >= (serBuffMax-2)) {toPC.printf("WMsg overun char buff \n"); serBufChars = 0; }; serBuf[serBufChars] = inChar; //set this char in a char array serBufChars++; //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); rxMsg = !rxMsg; 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 timeFromPosVelMessageReceipt.reset(); //start time and close SD card file if too long 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; case FIRE_TRIGGER_MSG: fireTrigger = 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 = GPSTimemsecs/1000 + 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 ); txMsg = !txMsg; } 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 { //if we are receiving POSVEL requests -- always open the file for storage and store the data //th file is closed(in main) if we dont receive POSVAL messages for 60 secs if (fpNav == NULL) { toPC.printf(" opening the SD card file \n"); fpNav = fopen("/sd/Data/NAV.bin", "wb"); wait_ms(10); recordData = true; } 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 { txMsg = !txMsg; 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", GPSTimemsecs, 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; //reset the flag so we dont continue to come through here char recChar = 'N'; //recordData set to true only if we receive a STARTDATA from the PC if (recordData) //here we have received a message to record the data { if ((fpNav == NULL)) //if file not opened -- open it { toPC.printf(" opening the SD card file \n"); fpNav = fopen("/sd/Data/NAV.bin", "wb"); wait_ms(10); } if (fpNav != NULL) //if the fie was already opened we will respond to the PC with a Y { recChar = 'Y'; } else //is the file was not opened we will write a message to the PC { toPC.printf(" Could not open the SD card \n\n"); } } //recordData set to false only if we receive a STOPDATA from the PC else //here we have received a message to stop the recording { if (fpNav != NULL) //if the file is open -- close it { toPC.printf(" closing the SD card file \n\n"); fflush(fpNav); fclose(fpNav); wait_ms(1000); //toPC.printf("\n after closing the SD card file \n\n"); fpNav = NULL; } //if stop recording received and the file is already closed -- just do nothing recordData = false; } 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 ); } }