File content as of revision 9:b45feb91ba38:

//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");
                    //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
                         );
        }
        

}