gps_locate.cpp

00001 #include "gps_locate.h"
00002 #include "GPS.h"
00003 
00004 int gps_locate(struct gps_data_t* gps_data, int timeout)
00005 {
00006     printf("GPS Location begins\r\n");
00007     
00008     // Power on gps
00009     GPSI2C gps;
00010     
00011     // Timeout timer
00012     Timer timer;
00013     timer.start();
00014 
00015     bool coord_ok = false, altitude_ok = false, speed_ok = false;
00016 
00017     int ret = 0;
00018     char buf[512] = {0};
00019     while(!coord_ok || !altitude_ok || !speed_ok) {
00020         
00021         if(timer.read() > timeout) {
00022             printf("GPS Location TimeOut, abort...\r\n");
00023             timer.stop();
00024             return 0;
00025         }
00026         
00027         while ((ret = gps.getMessage(buf, sizeof(buf))) > 0) {
00028             int len = LENGTH(ret);
00029             if ((PROTOCOL(ret) == GPSParser::NMEA) && (len > 6)) {
00030                 if (!strncmp("$GPGLL", buf, 6)) {
00031                     double la = 0, lo = 0;
00032                     char ch;
00033                     if (gps.getNmeaAngle(1,buf,len,la) &&
00034                             gps.getNmeaAngle(3,buf,len,lo) &&
00035                             gps.getNmeaItem(6,buf,len,ch) && ch == 'A') {
00036                         gps_data->lo = lo;
00037                         gps_data->la = la;
00038                         coord_ok = true;
00039                     }
00040                 } else if (!strncmp("$GPGGA", buf, 6)) {
00041                     double a = 0;
00042                     if (gps.getNmeaItem(9,buf,len,a)) // altitude msl [m]
00043                         gps_data->altitude = a;
00044                     altitude_ok = true;
00045                 } else if (!strncmp("$GPVTG", buf, 6)) {
00046                     double s = 0;
00047                     if (gps.getNmeaItem(7,buf,len,s)) // speed [km/h]
00048                         gps_data->speed = s;
00049                     speed_ok = true;
00050                 }
00051 
00052             }
00053         }
00054     }
00055     
00056     timer.stop();
00057     return 1;
00058 }