File content as of revision 54:6ce53a145fa0:

// PONY_Loc.cpp
// Location code for PONY
// (C) 2015 Igor Institute

#include "PONY_Loc.h"

float kGPSloopPeriod = 0.25;      // Globally set our GPS read period



// Define our array of "talkers"
const char* kNMEA_talkers[] = { "", "GSA", "GSV", "RMC", "GLL", "GGA", "GNS", "VTG" };
const int kTalkerCount = 7;

//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------

int readyGPS(GPSbundle* GPSb, int loopmax) {
    
    for (int loopiter = 0; loopiter < loopmax; loopiter++) {
        if( (GPSb->bRet = (GPSb->bGPS).getMessage(GPSb->bBuf, GPSb->bBufLen)) > 0 ) {
            printf("GPS ready\r\n");
            return 1;
        }
        wait(1);
    }
    printf("GPS ready timeout\r\n");
    return 0;
}

//-----------------------------------------------------------------------------

int getNMEAtalker (const char* msg) {
    
    // TODO: Need to verify we're receiving NMEA sentences at some point in this chain
    #define _CHECK_TALKER(s) ((msg[3] == s[0]) && (msg[4] == s[1]) && (msg[5] == s[2]))
    
    for (int i = 1; i <= kTalkerCount; i++) {
        if( _CHECK_TALKER(kNMEA_talkers[i]) )
            return i;
    }
    return 0;
}



//-----------------------------------------------------------------------------

int fetchLocBundle(GPSbundle* GPSb, GPSloc* LOCb, int timeout) {
    
    int locFlag = 0;
    int altFlag = 0;
    int velFlag = 0;
    
    int loopCountMax = (float) timeout / kGPSloopPeriod;
    
    
    
    
    // Let's get some messages
    for (int loopCounter = 0; loopCounter < loopCountMax; loopCounter++) {
        
        GPSb->bRet = GPSb->bGPS.getMessage(GPSb->bBuf,GPSb->bBufLen);
        GPSb->bLen = LENGTH(GPSb->bRet);
        
        // TODO: Verify it's NMEA data
        
              
        int talker = getNMEAtalker(GPSb->bBuf);
        
        if(talker > 0)
            printf("%s talker\r\n", kNMEA_talkers[talker]);
        else
            printf("Non-LOC talker\r\n");
        
        
        
        // Talker switch statement
        switch(talker)
        {
            // GLL talker
            case 4:    
               
                if(fillLocBundle(GPSb,LOCb))
                    locFlag = 1;
                break;
                
            // GGA talker
            case 5:
                (GPSb->bGPS).getNmeaAngle(8,GPSb->bBuf,GPSb->bLen,LOCb->alt);
                altFlag = 1;
                break;
            
            // GNS talker
            case 6:
                (GPSb->bGPS).getNmeaAngle(9,GPSb->bBuf,GPSb->bLen,LOCb->alt); 
                break;
                
            // VTG talker
            case 7:   

                
                velFlag = 1;     
                break;
            
            default:
                printf("Unregistered talker\r\n");
            
        }
        
        if( locFlag && altFlag && velFlag )
            return 7;
        
        wait( kGPSloopPeriod );
    }
    return 0;   // False return for timeout        
    
    
}




int fillLocBundle(GPSbundle* GPSb, GPSloc* LocBundle) {
            
    if ( verifyGLL(GPSb, LocBundle) ) {
        printf("GLL verified!\r\n");
        (GPSb->bGPS).getNmeaAngle(1,GPSb->bBuf,GPSb->bLen,LocBundle->lat);
        (GPSb->bGPS).getNmeaAngle(3,GPSb->bBuf,GPSb->bLen,LocBundle->lon);
        (GPSb->bGPS).getNmeaItem(5,GPSb->bBuf,GPSb->bLen,LocBundle->tmstmp);
        return 1;
    }
    return 0;
}





int fetchLaLo(GPSbundle* GPSb, GPSloc* LocBundle, int timeout) {
    
    int loopCountMax = (float) timeout / kGPSloopPeriod;
    
    
    for (int loopCounter = 0; loopCounter < loopCountMax; loopCounter++) {
        
        GPSb->bRet = GPSb->bGPS.getMessage(GPSb->bBuf,GPSb->bBufLen);
        GPSb->bLen = LENGTH(GPSb->bRet);
        
        
        // TODO: Verify it's NMEA data
        int talker = getNMEAtalker(GPSb->bBuf);
        printf("\r\nTalker=%i",talker);
        
        
        if( talker == 4 ) {     // Make sure that this matches our array indexing
            printf("GLL data\r\b");
   
            if ( verifyGLL(GPSb, LocBundle) ) {
                printf("GLL verified!\r\n");
                (GPSb->bGPS).getNmeaAngle(1,GPSb->bBuf,GPSb->bLen,LocBundle->lat);
                (GPSb->bGPS).getNmeaAngle(3,GPSb->bBuf,GPSb->bLen,LocBundle->lon);
                (GPSb->bGPS).getNmeaItem(5,GPSb->bBuf,GPSb->bLen,LocBundle->tmstmp);
                return 1;
            }
        }
        
            
        wait( kGPSloopPeriod );
    }
    return 0;   // False return for timeout    
    
}








int fetchAlt(GPSbundle* GPSb, GPSloc* LocBundle, int timeout) {
    
    int loopCountMax = (float) timeout / kGPSloopPeriod;
    
    
    for (int loopCounter = 0; loopCounter < loopCountMax; loopCounter++) {
        
        GPSb->bRet = GPSb->bGPS.getMessage(GPSb->bBuf,GPSb->bBufLen);
        GPSb->bLen = LENGTH(GPSb->bRet);
        
        
        // TODO: Verify it's NMEA data
        int talker = getNMEAtalker(GPSb->bBuf);
        printf("\r\nTalker=%i",talker);
        
        
        if( talker == 4 ) {     // Make sure that this matches our array indexing
            printf("GLL data\r\b");
   
            if ( verifyGLL(GPSb, LocBundle) ) {
                printf("GLL verified!\r\n");
                (GPSb->bGPS).getNmeaAngle(1,GPSb->bBuf,GPSb->bLen,LocBundle->lat);
                (GPSb->bGPS).getNmeaAngle(3,GPSb->bBuf,GPSb->bLen,LocBundle->lon);
                (GPSb->bGPS).getNmeaItem(5,GPSb->bBuf,GPSb->bLen,LocBundle->tmstmp);
                return 1;
            }
        }
        
            
        wait( kGPSloopPeriod );
    }
    return 0;   // False return for timeout    
    
}



//-----------------------------------------------------------------------------

int verifyGLL(GPSbundle* GPSb, GPSloc* LocBundle) {
    
    char ch = '-';

    (GPSb->bGPS).getNmeaItem(6,GPSb->bBuf,GPSb->bLen,ch);
    
    printf("\r\nCheck character: %c\r\n",ch);
    printf("\r\n%s\r\n", GPSb->bBuf);
    
    if(ch == 'A')
        return 1;
    else {
        
        return 0;
    }
}





void parseM2XLocation(const char* name,
                    double latitude,
                    double longitude,
                    double elevation,
                    const char* timestamp,
                    int index,
                    void* context) {
                        
  printf("Found previous M2X location, index: %d\r\n", index);
  printf("Name: %s  Latitude: %lf  Longitude: %lf\r\n", name, latitude, longitude);
  printf("Elevation: %lf  Timestamp: %s\r\n", elevation, timestamp);
  
}







void getFormattedTime(time_t* pSysTime, tm* formatTime) {
    
    formatTime = localtime(pSysTime);
    
    formatTime->tm_year +=1900; //Add 1900 in because mbed reports year as "since 1900"
    
    /*
    printf("\r\n\nSys Time as structure values:\r\n");
    printf("Year=%i\r\n", formatTime->tm_year);
    printf("Month=%i\r\n", formatTime->tm_mon);
    printf("Day=%i\r\n", formatTime->tm_mday);
    printf("Hour=%i\r\n", formatTime->tm_hour);
    printf("Minute=%i\r\n", formatTime->tm_min);
    printf("Second=%i\r\n", formatTime->tm_sec);
    */

    return;
};