GPS.cpp

00001 #include "GPS.h"
00002 #include "math.h"
00003 #include "inttypes.h"
00004 Serial debug(USBTX, USBRX);
00005 GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) {
00006     _gps.baud(9600);
00007     nmea_longitude = 0.0;
00008     nmea_latitude = 0.0;
00009     utc_time = 0;
00010     ns = ' ';
00011     ew = ' ';
00012     lock = 0;
00013     satelites = 0;
00014     msl_altitude = 0.0;
00015     msl_units = ' ';
00016     satellites[0] = 0;
00017     satellites[1] = 0;
00018     satellites[2] = 0;
00019     satellites[3] = 0;
00020     satellites[4] = 0;
00021     satellites[5] = 0;
00022     satellites[6] = 0;
00023     satellites[7] = 0;
00024     satellites[8] = 0;
00025     satellites[9] = 0;
00026     satellites[10] = 0;
00027     satellites[11] = 0;    
00028     pdop = 0.0;
00029     hdop = 0.0;
00030     vdop = 0.0;
00031     navigation_mode = 1;
00032     gprmc_status = 'V';
00033     tdformat[0] = 0;
00034     
00035 
00036     rmc_status = ' ';
00037     speed_k = 0.0;
00038     course_d = 0.0;
00039     date = 0;
00040 
00041     dec_longitude = 0.0;
00042     dec_latitude = 0.0;
00043 
00044     gll_status = ' ';
00045 
00046     course_t = 0.0; // ground speed true
00047     course_t_unit = ' ';
00048     course_m = 0.0; // magnetic
00049     course_m_unit = ' ';
00050     speed_k_unit = ' ';
00051     speed_km = 0.0; // speed km/hr
00052     speed_km_unit = ' ';
00053 
00054     altitude_ft = 0.0;
00055 }
00056 
00057 float GPS::nmea_to_dec(float deg_coord, char nsew) {
00058     int degree = (int)(deg_coord/100);
00059     float minutes = deg_coord - degree*100;
00060     float dec_deg = minutes / 60;
00061     float decimal = degree + dec_deg;
00062     if (nsew == 'S' || nsew == 'W') { // return negative
00063         decimal *= -1;
00064     }
00065     return decimal;
00066 }
00067 
00068 char * GPS::get_nmea_to_td() {
00069     int lat_degree = (int)(nmea_latitude/100);
00070     float lat_minutes = (rint((nmea_latitude - lat_degree*100)*1000)/1000.0); // round to 3 digits 
00071     uint8_t lat_sign = 0;
00072     if (ns == 'S' )lat_sign = 0x40;
00073     int lng_degree = (int)(nmea_longitude/100);
00074     float lng_minutes = rint((nmea_longitude - lng_degree*100)*1000)/1000.0; // round to 3 digits
00075     uint8_t lng_sign = 0;
00076     if (ew == 'W' )lng_sign = 0x80;
00077     uint32_t height = rint(msl_altitude/2.0);
00078     debug.printf("lng degree: %d, lng min %f,lat degree: %d, lat min %f" , lng_degree, lng_minutes,lat_degree, lat_minutes);
00079     char temp[32];
00080     sprintf(temp, "%d%05.0f%d%05.0f",lng_degree, lng_minutes*1000,lat_degree, lat_minutes*1000);
00081     unsigned long long ret;
00082     ret = strtoull(temp, NULL, 10);   
00083     //debug.printf("the string: %s\r\n",temp);
00084     //debug.printf("the long variable: %llu\r\n",ret);
00085     //debug.printf("the long variable in hex: %012llx",ret);
00086     sprintf(tdformat, "01010%012llx%03x%02x",ret,height&0xfff,(lng_sign+lat_sign)&0xff); // to do: add sattelites in view, altiude sign and horizontal dillution
00087     //debug.printf("Formatted string:%s \r\n", tdformat);
00088     return tdformat;
00089 }
00090 
00091 int GPS::sample() {
00092     int line_parsed = 0;
00093 
00094     if (_gps.readable()) {
00095         getline();
00096         debug.printf("%s\r\n",msg);
00097         // Check if it is a GPGGA msg (matches both locked and non-locked msg)
00098         // $xxGGA,time,lat,NS,long,EW,quality,numSV,HDOP,alt,M,sep,M,diffAge,diffStation*cs<CR><LF>
00099         if (sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", &utc_time, &nmea_latitude, &ns, &nmea_longitude, &ew, &lock, &satelites, &hdop, &msl_altitude, &msl_units) >= 1) {
00100             line_parsed = GGA;
00101         }
00102         // Check if it is a GPSA msg (navigational mode)
00103         // $xxGSA,opMode,navMode{,sv},PDOP,HDOP,VDOP*cs<CR><LF>
00104         else if (sscanf(msg, "GPGSA,%c,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f", &operating_mode, &navigation_mode, &satellites[0], &satellites[1], &satellites[2], &satellites[3], &satellites[4], &satellites[5], &satellites[6], &satellites[7], &satellites[8], &satellites[9], &satellites[10], &satellites[11],&pdop,&hdop,&vdop) >= 1) {
00105             line_parsed = GSA;
00106         }
00107         // Check if it is a GPRMC msg
00108         // $xxRMC,time,status,lat,NS,long,EW,spd,cog,date,mv,mvEW,posMode*cs<CR><LF>
00109         else if (sscanf(msg, "GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,%d", &utc_time, &gprmc_status, &nmea_latitude, &ns, &nmea_longitude,&ew,&speed_k,&course_d,&date) >= 1) {
00110             line_parsed = RMC;
00111         }
00112         // GLL - Geographic Position-Lat/Lon
00113         // $xxGLL,lat,NS,long,EW,time,status,posMode*cs<CR><LF>
00114         else if (sscanf(msg, "GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) {
00115             line_parsed = GLL;
00116         }
00117         // VTG-Course Over Ground and Ground Speed
00118         else if (sscanf(msg, "GPVTG,%f,%c,%f,%c,%f,%c,%f,%c", &course_t, &course_t_unit, &course_m, &course_m_unit, &speed_k, &speed_k_unit, &speed_km, &speed_km_unit) >= 1) {
00119             line_parsed = VTG;
00120         }
00121         
00122         /*if(satelites == 0) {
00123             lock = 0;
00124         }*/
00125         if(gprmc_status == 'A'|| satelites > 0 || navigation_mode > 1) lock = 1;
00126         else lock = 0;
00127     }
00128     if (!lock) {
00129         return NO_LOCK;
00130     } else if (line_parsed) {
00131         return line_parsed;
00132     } else {
00133         return NOT_PARSED;
00134     }
00135 }
00136 
00137 
00138 // INTERNAL FUNCTINS ////////////////////////////////////////////////////////////
00139 float GPS::trunc(float v) {
00140     if (v < 0.0) {
00141         v*= -1.0;
00142         v = floor(v);
00143         v*=-1.0;
00144     } else {
00145         v = floor(v);
00146     }
00147     return v;
00148 }
00149 
00150 void GPS::getline() {
00151     while (_gps.getc() != '$');   // wait for the start of a line
00152     for (int i=0; i<1022; i++) {
00153         msg[i] = _gps.getc();
00154         if (msg[i] == '\r') {
00155             msg[i] = 0;
00156             return;
00157         }
00158     }
00159     error("Overflow in getline");
00160 }
00161 
00162 // GET FUNCTIONS /////////////////////////////////////////////////////////////////
00163 float GPS::get_msl_altitude() {
00164     if (!lock)
00165         return 0.0;
00166     else
00167         return msl_altitude;
00168 }
00169 
00170 int GPS::get_satelites() {
00171     if (!lock)
00172         return 0;
00173     else
00174         return satelites;
00175 }
00176 
00177 float GPS::get_nmea_longitude() {
00178     if (!lock)
00179         return 0.0;
00180     else
00181         return nmea_longitude;
00182 }
00183 
00184 float GPS::get_dec_longitude() {
00185     dec_longitude = nmea_to_dec(nmea_longitude, ew);
00186     if (!lock)
00187         return 0.0;
00188     else
00189         return dec_longitude;
00190 }
00191 
00192 float GPS::get_nmea_latitude() {
00193     if (!lock)
00194         return 0.0;
00195     else
00196         return nmea_latitude;
00197 }
00198 
00199 float GPS::get_dec_latitude() {
00200     dec_latitude = nmea_to_dec(nmea_latitude, ns);
00201     if (!lock)
00202         return 0.0;
00203     else
00204         return dec_latitude;
00205 }
00206 
00207 float GPS::get_course_t() {
00208     if (!lock)
00209         return 0.0;
00210     else
00211         return course_t;
00212 }
00213 
00214 float GPS::get_course_m() {
00215     if (!lock)
00216         return 0.0;
00217     else
00218         return course_m;
00219 }
00220 
00221 float GPS::get_speed_k() {
00222     if (!lock)
00223         return 0.0;
00224     else
00225         return speed_k;
00226 }
00227 
00228 float GPS::get_speed_km() {
00229     if (!lock)
00230         return 0.0;
00231     else
00232         return speed_km;
00233 }
00234 
00235 float GPS::get_altitude_ft() {
00236     if (!lock)
00237         return 0.0;
00238     else
00239         return 3.280839895*msl_altitude;
00240 }
00241 
00242 // NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
00243 float GPS::calc_course_to(float pointLat, float pontLong) {
00244     const double d2r = PI / 180.0;
00245     const double r2d = 180.0 / PI;
00246     double dlat = abs(pointLat - get_dec_latitude()) * d2r;
00247     double dlong = abs(pontLong - get_dec_longitude()) * d2r;
00248     double y = sin(dlong) * cos(pointLat * d2r);
00249     double x = cos(get_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(get_dec_latitude()*d2r)*cos(pointLat*d2r)*cos(dlong);
00250     return atan2(y,x)*r2d;
00251 }    
00252 
00253 /*
00254 var y = Math.sin(dLon) * Math.cos(lat2);
00255 var x = Math.cos(lat1)*Math.sin(lat2) -
00256         Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
00257 var brng = Math.atan2(y, x).toDeg();
00258 */
00259 
00260 /*
00261             The Haversine formula according to Dr. Math.
00262             http://mathforum.org/library/drmath/view/51879.html
00263                 
00264             dlon = lon2 - lon1
00265             dlat = lat2 - lat1
00266             a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
00267             c = 2 * atan2(sqrt(a), sqrt(1-a)) 
00268             d = R * c
00269                 
00270             Where
00271                 * dlon is the change in longitude
00272                 * dlat is the change in latitude
00273                 * c is the great circle distance in Radians.
00274                 * R is the radius of a spherical Earth.
00275                 * The locations of the two points in 
00276                     spherical coordinates (longitude and 
00277                     latitude) are lon1,lat1 and lon2, lat2.
00278 */
00279 double GPS::calc_dist_to_mi(float pointLat, float pontLong) {
00280     const double d2r = PI / 180.0;
00281     double dlat = pointLat - get_dec_latitude();
00282     double dlong = pontLong - get_dec_longitude();
00283     double a = pow(sin(dlat/2.0),2.0) + cos(get_dec_latitude()*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
00284     double c = 2.0 * asin(sqrt(abs(a)));
00285     double d = 63.765 * c;
00286     
00287     return d;
00288 }
00289 
00290 double GPS::calc_dist_to_ft(float pointLat, float pontLong) {
00291     return calc_dist_to_mi(pointLat, pontLong)*5280.0;
00292 }
00293 
00294 double GPS::calc_dist_to_km(float pointLat, float pontLong) {
00295     return calc_dist_to_mi(pointLat, pontLong)*1.609344;
00296 }
00297 
00298 double GPS::calc_dist_to_m(float pointLat, float pontLong) {
00299     return calc_dist_to_mi(pointLat, pontLong)*1609.344;
00300 }
00301 
00302