File content as of revision 17:726bbc1b73ee:

#include "gps_locate.h"
#include "GPS.h"

int gps_locate(struct gps_data_t* gps_data, int timeout)
{
    printf("GPS Location begins\r\n");
    
    // Power on gps
    GPSI2C gps;
    
    // Timeout timer
    Timer timer;
    timer.start();

    bool coord_ok = false, altitude_ok = false, speed_ok = false;

    int ret = 0;
    char buf[512] = {0};
    while(!coord_ok || !altitude_ok || !speed_ok) {
        
        if(timer.read() > timeout) {
            printf("GPS Location TimeOut, abort...\r\n");
            timer.stop();
            return 0;
        }
        
        while ((ret = gps.getMessage(buf, sizeof(buf))) > 0) {
            int len = LENGTH(ret);
            if ((PROTOCOL(ret) == GPSParser::NMEA) && (len > 6)) {
                if (!strncmp("$GPGLL", buf, 6)) {
                    double la = 0, lo = 0;
                    char ch;
                    if (gps.getNmeaAngle(1,buf,len,la) &&
                            gps.getNmeaAngle(3,buf,len,lo) &&
                            gps.getNmeaItem(6,buf,len,ch) && ch == 'A') {
                        gps_data->lo = lo;
                        gps_data->la = la;
                        coord_ok = true;
                    }
                } else if (!strncmp("$GPGGA", buf, 6)) {
                    double a = 0;
                    if (gps.getNmeaItem(9,buf,len,a)) // altitude msl [m]
                        gps_data->altitude = a;
                    altitude_ok = true;
                } else if (!strncmp("$GPVTG", buf, 6)) {
                    double s = 0;
                    if (gps.getNmeaItem(7,buf,len,s)) // speed [km/h]
                        gps_data->speed = s;
                    speed_ok = true;
                }

            }
        }
    }
    
    timer.stop();
    return 1;
}