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
+                         );
+        }
+        
+
+}