gps.cpp

00001 #include "mbed.h"
00002 #include "gps.h"
00003 
00004 GPS::GPS(void){
00005     
00006     latitude = 0.0f;
00007     longitude = 0.0f;
00008     speed = 0.0f;
00009     mph = 0.0f;
00010     angle = 0.0f;    
00011     hour = 0;
00012     minute = 0;
00013     seconds = 0;
00014     year = 0;
00015     month = 0;
00016     day = 0;
00017     milliseconds = 0;
00018     fix = false;
00019     lineidx = 0;
00020     newSentence = false;
00021     firstfix = false;
00022     for (int i = 0; i<3; i++){
00023         positions[i].x = 0;
00024         positions[i].y = 0;
00025         }
00026     }
00027     
00028 void GPS::parse(char *nmea) {
00029     uint16_t sum;
00030 // do checksum check
00031 // first look if we even have one
00032   if (nmea[strlen(nmea)-4] == '*') {
00033     sum = parseHex(nmea[strlen(nmea)-3]) * 16;
00034     sum += parseHex(nmea[strlen(nmea)-2]);
00035 // check checksum 
00036     for (uint8_t i=1; i < (strlen(nmea)-4); i++) {
00037       sum ^= nmea[i];
00038     }
00039     if (sum != 0) {
00040 // bad checksum :(
00041     newSentence = false;
00042     return;
00043     }
00044   }
00045 // bad string
00046   float minutes;
00047   char degreebuff[10];
00048 // look for RMC sentence only
00049   if (strstr(nmea, "$GPRMC")) {
00050      newSentence = true;
00051      char *p = nmea;
00052 // get time
00053     p = strchr(p, ',')+1;
00054     timef = atof(p)*(float)10;
00055     uint32_t time = timef;
00056     hour = time / 100000;
00057     minute = (time % 100000) / 1000;
00058     seconds = (time % 1000)/10;
00059     milliseconds = time % 10;
00060 //Check for valid fix
00061     p = strchr(p, ',')+1;
00062     if (p[0] == 'A') 
00063         fix = true;
00064     else{
00065         fix = false;
00066         return;
00067         }
00068     firstfix = true;
00069 // parse out latitude
00070     p = strchr(p, ',')+1;
00071     if (',' != *p){
00072       strncpy(degreebuff, p, 2);
00073       p += 2;
00074       degreebuff[2] = '\0';
00075       latitude = atof(degreebuff);
00076       strncpy(degreebuff, p, 7); // float minutes
00077       degreebuff[7] = '\0';
00078       minutes = atof(degreebuff)/60;
00079       latitude += minutes;
00080         }
00081     p = strchr(p, ',')+1;
00082     if (',' != *p)
00083     {
00084       if (p[0] == 'S') longitude *= -1.0;
00085     }
00086 // parse out longitude
00087     p = strchr(p, ',')+1;
00088     if (',' != *p)
00089 //Not empty value due to no fix
00090     {
00091       strncpy(degreebuff, p, 2);
00092       p += 2;
00093       degreebuff[2] = '\0';
00094       longitude = atof(degreebuff);
00095       strncpy(degreebuff, p, 7); // float minutes
00096       degreebuff[7] = '\0';
00097       minutes = atof(degreebuff)/60;
00098       longitude += minutes;
00099     }
00100     p = strchr(p, ',')+1;
00101     if (',' != *p)
00102     {
00103       if (p[0] == 'W') longitude *= -1.0;
00104     }
00105     // speed
00106     p = strchr(p, ',')+1;
00107     if (',' != *p)
00108     {
00109       speed = atof(p)*knots_to_ms;
00110       mph = speed / 0.44704f;
00111     }
00112     
00113     // angle
00114     p = strchr(p, ',')+1;
00115     if (',' != *p)
00116     {
00117       angle = atof(p);
00118     }
00119     
00120     p = strchr(p, ',')+1;
00121     if (',' != *p)
00122     {
00123       uint32_t fulldate = atof(p);
00124       day = fulldate / 10000;
00125       month = (fulldate % 10000) / 100;
00126       year = (fulldate % 100);
00127     }
00128     positions[0] = positions[1];
00129     positions[1] = positions[2];
00130     positions[2] = Cartesian(latitude,longitude);
00131     if (mph > 10) radius = CalcRadius(positions[0], positions[1], positions[2]);
00132     else radius = 300.0f;
00133     if (radius > 300.0f) radius = 300.0f;
00134     return;
00135   }
00136   newSentence = false;
00137 }