PCMessaging.h

00001 //these are defines for the messages that are sent from the PC across the USB
00002 //these messages produce reactions on the mbed
00003 const unsigned char  STATUS_MSG         =0;
00004 const unsigned char  POSVEL_MSG         =1;
00005 const unsigned char  STARTDATA_MSG      =2;
00006 const unsigned char  STOPDATA_MSG       =3;
00007 const unsigned char  STARTSTREAM_MSG    =4;
00008 const unsigned char  STOPSTREAM_MSG     =5;
00009 const unsigned char  STARTLOGINFO_MSG   =6;
00010 const unsigned char  STOPLOGINFO_MSG    =7;
00011 
00012 const double  DEGREES_TO_RADIANS = acos(-1.0)/180.0;
00013 const double eccen          = 0.0818191908426;  //WGS84 earth eccentricity
00014 const double earthRadius    = 6378137;          //WGS84 earthRadius in meters
00015 
00016 char serBuf[128];
00017 int serBufChars=0;
00018 
00019 //flags to control the PC command actions
00020 bool sendPosVel     =false;
00021 bool sendStatus     =false;
00022 bool sendRecData    =false;
00023 bool streamPos      =false;
00024 bool sendStreamPos  =false;
00025 bool logMsgInfo     =false;
00026 bool sendLogMsgInfo =false;
00027 bool recordData     =false;
00028 
00029 
00030 const unsigned char numMessages = 8;    //number of potential messages
00031 char msgList[numMessages][32];          //text array storing the command messages from the PC
00032 char minMessageSize = 11;               //minimum size of a text message
00033 unsigned char CR = 0x0d;                //ASCII Carriage Return
00034 unsigned char LF = 0x0a;                //ASCII Line Feed
00035 
00036 void setUpMessages(void)
00037 { 
00038     //set up the ASCII text records that are candidates to be passed from the PC
00039     sprintf(msgList[STATUS_MSG],        "WMsg STATUS");
00040     sprintf(msgList[POSVEL_MSG],        "WMsg POSVEL");
00041     sprintf(msgList[STARTDATA_MSG],     "WMsg RECORDDATA Y");
00042     sprintf(msgList[STOPDATA_MSG],      "WMsg RECORDDATA N");
00043     sprintf(msgList[STARTSTREAM_MSG],   "WMsg POSSTREAM Y");
00044     sprintf(msgList[STOPSTREAM_MSG],    "WMsg POSSTREAM N");
00045     sprintf(msgList[STARTLOGINFO_MSG],  "WMsg LOGINFO Y");
00046     sprintf(msgList[STOPLOGINFO_MSG],   "WMsg LOGINFO N");
00047     //message length is from 10 to 16 chars
00048     
00049     toPC.printf(" finished setting up messages \n");
00050 }
00051 
00052 void readFromPC()
00053 {
00054      //The received commands only occur at the initialization stage -- time is not that critical there.
00055     //during the real-time action, we will never pass the followong if test ( no characters received)
00056     if (toPC.readable()) //read a PC serial byte and test it for a command
00057     {
00058         
00059         // Read in next character
00060         char inChar = toPC.getc();  //read char from the USB serial link to the PC
00061         //toPC.printf("%02x ",inChar);
00062         
00063         //incoming messages will end witb a CR / LF -- disregard these chars
00064         if (inChar == CR || inChar == LF)  return; //CR is a 0x0a
00065         
00066         serBuf[serBufChars++] = inChar; //set this char in a char array
00067         
00068         //no need to continue if numChars are less than the shortest candidate message
00069         //if (serBufChars < minMessageSize) return;
00070                 
00071         // Append end of string
00072         //We always assume we have a complete message string and test for this below
00073         serBuf[serBufChars] = '\0';
00074         
00075         bool validMessage = false;
00076         
00077         
00078         // Check for valid message -- there are numMessages possible messages
00079         for (int m = 0; m < numMessages && !validMessage; m++) //check for all messages ... 
00080         {
00081             //toPC.printf(" \n\n found chars to test %3d %3d %s \n\n\n ", serBufChars, strlen(msgList[m]), serBuf );
00082 
00083             //check the current partial message against ALL possible messages
00084             //messages must match in both strength length and text
00085             if (serBufChars == strlen(msgList[m]) && strncmp(serBuf, msgList[m], serBufChars) == 0 )
00086             {
00087             
00088                 //toPC.printf( "\n       found valid message %s  \n\n", serBuf);
00089 
00090                 validMessage = true;
00091                 serBufChars = 0; //reset the character count to reset for next message
00092     
00093                 //set programmatic action flags based on the message
00094                 switch(m)
00095                 {
00096                     case STATUS_MSG:
00097                         sendStatus = true;  //send a status message back to PC
00098                     break;
00099                     case POSVEL_MSG:
00100                         sendPosVel = true;  //send a posvel message back to PC
00101                     break;
00102                     case STARTDATA_MSG:  //start the data recording to the SD card
00103                         recordData = true;
00104                         sendRecData = true;
00105                         break;
00106                     case STOPDATA_MSG:   //stop the data recording to the SD card
00107                         recordData = false;
00108                         sendRecData = true;
00109                         break;
00110                     case STARTSTREAM_MSG:
00111                     case STOPSTREAM_MSG:
00112                         streamPos = (m == STARTSTREAM_MSG);
00113                         sendStreamPos = true;
00114                     break;
00115                     case STARTLOGINFO_MSG:
00116                     case STOPLOGINFO_MSG:
00117                         logMsgInfo = (m == STARTLOGINFO_MSG);
00118                         sendLogMsgInfo = true;
00119                     break;  
00120                 }  //end Switch statement
00121                 break;
00122             } //end test for a valid message
00123         }  //end message text loop
00124     }  //end pc.readable
00125 };
00126 
00127 void earthCoefficients(double latitudeRad, double longitudeRad, double height, double &latRateFac, double &lonRateFac)
00128 {
00129     //compute the lat and lon factors for use in the interpolation of the lat and lon between 1 sec epochs
00130     //latRateFac & lonRateFac multiplied by Vnorth or VEast to get latRate and lonRate
00131     //see this document (page 32)    www.fas.org/spp/military/program/nav/basicnav.pdf
00132 
00133     double eccenSinLat = eccen * sin(latitudeRad); 
00134     double temp1 = 1.0 - eccenSinLat*eccenSinLat;
00135     double temp2 = sqrt(temp1);
00136     double r_meridian = earthRadius * ( 1.0 - eccen*eccen)/ (temp1 * temp2);
00137     double r_normal   = earthRadius / temp2;
00138         
00139     //divide Vnorth by latRateFac to get the latitude rate in deg per sec
00140     latRateFac = (r_meridian + height)* DEGREES_TO_RADIANS;
00141     
00142     //divide VEast by lonRateFac to get the longitude rate in deg per sec
00143     lonRateFac =  (r_normal + height) * cos(latitudeRad)* DEGREES_TO_RADIANS;
00144 }
00145 
00146 void sendPosVelMessageToPC(OEM615BESTPOS posMsg, OEM615BESTVEL velMsg)
00147 {
00148             //north and east velocity from the horizontal speed and heading
00149             //velMsg may not be the "current" message --- but is the one also associated with a position message
00150             double nVel = velMsg.horizontalSpeed*cos(velMsg.heading*DEGREES_TO_RADIANS);
00151             double eVel = velMsg.horizontalSpeed*sin(velMsg.heading*DEGREES_TO_RADIANS);
00152             
00153             double latRateFac;
00154             double lonRateFac;
00155                         
00156             earthCoefficients(  posMsg.latitude*DEGREES_TO_RADIANS, 
00157                                 posMsg.longitude*DEGREES_TO_RADIANS, 
00158                                 posMsg.height, 
00159                                 latRateFac, lonRateFac);
00160             
00161             //commented calculations are for a spherical earth (Chris's original computation)
00162             // For the 1 second deltas with which we are dealing
00163             // This calculation should be close enough for now
00164             // Approximately 1 nautical mile / minute latitude, 60 minutes/degree, 1852 meters/nautical mile
00165             //double latMetersPerDeg = 60.0*1852.0;
00166             // longitude separation is approximately equal to latitude separation * cosine of latitude
00167             //double lonMetersPerDeg = latMetersPerDeg*cos(posMsg.latitude*DEGREES_TO_RADIANS);
00168 
00169             // Elapsed time since last known GPS position
00170             //PPSTimeOffset is a result of possibly missing a prior GPS position message
00171             // timeFromPPS.read() is always the time from the moset recent 1PPS
00172             double elTime = (double)PPSTimeOffset + timeFromPPS.read();
00173             
00174             // Position time -- GPSTime is the time of the last valid GPS position message
00175             double posTime = GPSTime + elTime;
00176             
00177             //toPC.printf(" elTime = %6.3f PPSimeOffset = %6.3f \n", elTime, PPSTimeOffset);
00178             //toPC.printf(" latRateFac = %10.3f  lonRateFac = %10.3f \n", latRateFac, lonRateFac);
00179             //toPC.printf(" latRateFac = %10.3f  lonRateFac = %10.3f \n", latMetersPerDeg, lonMetersPerDeg);
00180 
00181             // Estimated position based on previous position and velocity
00182             // posMsg is the last time when the BESTVEL and BESTPOS messages had identical times
00183             //double latPos = posMsg.latitude  + (nVel/latMetersPerDeg)*elTime;
00184             //double lonPos = posMsg.longitude + (eVel/lonMetersPerDeg)*elTime;
00185             
00186             double latPos = posMsg.latitude  + (nVel/latRateFac)*elTime;
00187             double lonPos = posMsg.longitude + (eVel/lonRateFac)*elTime;            
00188             double htPos  = posMsg.height    + velMsg.verticalSpeed/(60*1852)*elTime;
00189             
00190             char solReady = 'N';
00191             //solStatus 
00192             if (posMsg.solStatus == 0) //see description of solution status in OEMV615.h
00193             {
00194                 solReady = 'Y';
00195             }
00196             
00197             toPC.printf("WMsg POSVEL %5.3lf %1d %c %8.5lf %9.5lf %4.3lf %4.3lf %4.3lf %4.3lf\n", 
00198                          posTime, 
00199                          posMsg.numSolSV,
00200                          solReady,
00201                          latPos,
00202                          lonPos,
00203                          htPos,
00204                          nVel,
00205                          eVel,
00206                          velMsg.verticalSpeed
00207                          );
00208 }
00209 
00210 void processPCmessages(FILE* &fpNav, OEM615BESTPOS posMsg, OEM615BESTVEL velMsg)
00211 {
00212 
00213         //we should put the below stuff into the readPC() procedure.
00214         //only do these actions in response to a command so no need for the tests w/o an inoput byte from the PC
00215         //perform the activities as a response to the commands
00216         if (sendPosVel)  //true if we want to return a position solution
00217         {
00218             sendPosVel=false; //set to true if a POSVEL is requested from the PC
00219             sendPosVelMessageToPC(posMsg, velMsg);
00220         }
00221         
00222         //all this does is assess the GPS convergence -- really available in the above
00223         if (sendStatus)  //send the status message to the PC
00224         {
00225             sendStatus=false;
00226             char solReady = 'N';
00227             //solStatus 
00228             if (posMsg.solStatus == 0) //see description of solution status in OEMV615.h
00229             {
00230                 solReady = 'Y';
00231             }
00232             toPC.printf("WMsg STATUS %5.3lf %c\n", 
00233                          GPSTime, 
00234                          solReady
00235                          );
00236         }
00237         
00238         //should just record ALL the data -- can pick over it in the post-processing
00239         if (sendRecData)  //begin to (or stop) record the serial data
00240         {
00241             sendRecData=false;
00242             char recChar = 'N';
00243             if (recordData)
00244             {
00245                 if ((fpNav == NULL))
00246                 {
00247                     fpNav = fopen("/sd/Data/NAV.bin", "wb");
00248                     //toPC.printf("\n opened the SD card file recordData=%10d \n\n", fpNav);
00249                 }
00250                 if (fpNav != NULL)
00251                 {
00252                     recChar = 'Y';
00253                 }
00254                 else
00255                 {
00256                     toPC.printf(" Could not open the SD card \n\n");
00257                 }
00258             }
00259             else
00260             {
00261                 if (fpNav != NULL)
00262                 {
00263                     toPC.printf(" closing the SD card file \n\n");
00264                     fclose(fpNav);
00265                     wait(1.0);
00266                     //toPC.printf("\n after closing the SD card file \n\n");
00267                     recordData = false;
00268                     fpNav = NULL;
00269                 }
00270             }
00271             toPC.printf("WMsg RECORDDATA %c\n", 
00272                          recChar
00273                          );
00274         }
00275         
00276         if (sendStreamPos)  //stream the position data to the PC
00277         {
00278             sendStreamPos=false;
00279             char streamChar = 'N';
00280             if (streamPos)
00281             {
00282                 streamChar = 'Y';
00283             }
00284             toPC.printf("WMsg POSSTREAM %c\n", 
00285                          streamChar
00286                          );
00287         }
00288         
00289         //not sure this is ever used ..
00290         if (sendLogMsgInfo)  //send log info to the PC
00291         {
00292             sendLogMsgInfo=false;
00293             char logChar = 'N';
00294             if (logMsgInfo)
00295             {
00296                 logChar = 'Y';
00297             }
00298             toPC.printf("WMsg LOGINFO %c\n", 
00299                          logChar
00300                          );
00301         }
00302         
00303 
00304 }