File content as of revision 12:c2f60b3ceb74:

//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  POSVEL_MSG         =0;
const unsigned char  FIRE_TRIGGER_MSG   =1;
const unsigned char  STATUS_MSG         =2;
const unsigned char  STARTDATA_MSG      =3;
const unsigned char  STOPDATA_MSG       =4;
const unsigned char  STARTSTREAM_MSG    =5;
const unsigned char  STOPSTREAM_MSG     =6;
const unsigned char  STARTLOGINFO_MSG   =7;
const unsigned char  STOPLOGINFO_MSG    =8;
const unsigned char  GETFILE_MSG        =9;

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 = 18;
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;
bool get_file_msg   =false;


const unsigned char numMessages = 10;    //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

char preamble[5] = "WMsg";
char testPreamble[5];

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");
    sprintf(msgList[GETFILE_MSG],       "WMsg GETFILE");
    //message length is from 10 to 16 chars
    
 //   toPC.printf(" finished setting up messages \n");
}

void readFromPC()
{
    //  should this be a while rather than if ??? -- may have multiple bytes in buffer
    if (toPC.readable()) //read a PC serial byte and test it for a command
    {
        // Read in next character  -- why not read all available??
        unsigned char inChar = 0;
        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
        
        //all received messages assumed to have a WMsg preamble
        //if we have read 4 chars, test these for "WMsg", if they are not WMsg, reset the buffer counter
        //if we receive an occasional bad byte that messes up a message, this will resynch fast to a next message
        //this will let us miss a message --- but hopefully only one
        
        // serBuffMax = 18 -- largest serBuffMax is 16 -- but we add one below for the '\0'
        // if the following occurs we have had a trash byte following a WMsg header
        if (serBufChars >= (serBuffMax-2)) {toPC.printf("WMsg restart message search\n"); serBufChars = 0; }
        
        serBuf[serBufChars] = inChar; //set this char in a char array for testing complete message
  
        testPreamble[serBufChars] = inChar;  //char array for testing the preamble
        serBufChars++;
        
        if (serBufChars == 4)  //four initial chars detected (0, 1, 2, 3)
        {
            testPreamble[4] = '\0';  //form null-terminated string for compare
            if (strncmp(testPreamble, preamble, 5) != 0)   //compare the chars to the WMsg preamble
            {
                toPC.printf("WMsg preamble mismatch \n");
                serBufChars = 0;  //if they dont match -- restart the search for a preamble
                return;
            }
        }
        
        //if we get here, we have found a preamble and we are looking for a complete message
        //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
        //this assumes that the message buffer contains an exact message
        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;
                        toPC.printf("MBED received trigger command \n");
                        break;
                    
                    case GETFILE_MSG:
                        get_file_msg = 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("WMsg 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
                         );
        }
        

}