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