GPS_parser.cpp

00001 #include "GPS_parser.h"
00002 
00003 GPS_Parser::GPS_Parser()
00004 {
00005     nmea_longitude = 0.0;
00006     nmea_latitude = 0.0;
00007     utc_time = 0;
00008     ns = ' ';
00009     ew = ' ';
00010     lock = 0;
00011     satellites = 0;
00012     hdop = 0.0;
00013     msl_altitude = 0.0;
00014     msl_units = ' ';
00015 
00016     rmc_status = ' ';
00017     speed_k = 0.0;
00018     course_d = 0.0;
00019     date = 0;
00020 
00021     dec_longitude = 0.0;
00022     dec_latitude = 0.0;
00023 
00024     altitude_ft = 0.0;
00025 
00026     current = NULL;
00027 }
00028 
00029 float GPS_Parser::nmea_to_dec(float deg_coord, char nsew)
00030 {
00031     int degree = (int)(deg_coord/100);
00032     float minutes = deg_coord - degree*100;
00033     float dec_deg = minutes / 60;
00034     float decimal = degree + dec_deg;
00035     if (nsew == 'S' || nsew == 'W') { // return negative
00036         decimal *= -1;
00037     }
00038     return decimal;
00039 }
00040 
00041 
00042 // INTERNAL FUNCTINS ////////////////////////////////////////////////////////////
00043 float GPS_Parser::trunc(float v)
00044 {
00045     if (v < 0.0) {
00046         v*= -1.0;
00047         v = floor(v);
00048         v*=-1.0;
00049     } else {
00050         v = floor(v);
00051     }
00052     return v;
00053 }
00054 
00055 // GET FUNCTIONS /////////////////////////////////////////////////////////////////
00056 
00057 float GPS_Parser::get_msl_altitude()
00058 {
00059     if (!lock)
00060         return 0.0;
00061     else
00062         return msl_altitude;
00063 }
00064 
00065 int GPS_Parser::get_satellites()
00066 {
00067     if (!lock)
00068         return 0;
00069     else
00070         return satellites;
00071 }
00072 
00073 float GPS_Parser::get_nmea_longitude()
00074 {
00075     if (!lock)
00076         return 0.0;
00077     else
00078         return nmea_longitude;
00079 }
00080 
00081 float GPS_Parser::get_dec_longitude()
00082 {
00083     dec_longitude = nmea_to_dec(nmea_longitude, ew);
00084     if (!lock)
00085         return 0.0;
00086     else
00087         return dec_longitude;
00088 }
00089 
00090 float GPS_Parser::get_nmea_latitude()
00091 {
00092     if (!lock)
00093         return 0.0;
00094     else
00095         return nmea_latitude;
00096 }
00097 
00098 float GPS_Parser::get_dec_latitude()
00099 {
00100     dec_latitude = nmea_to_dec(nmea_latitude, ns);
00101     if (!lock)
00102         return 0.0;
00103     else
00104         return dec_latitude;
00105 }
00106 
00107 float GPS_Parser::get_course_d()
00108 {
00109     if (!lock)
00110         return 0.0;
00111     else
00112         return course_d;
00113 }
00114 
00115 float GPS_Parser::get_speed_k()
00116 {
00117     if (!lock)
00118         return 0.0;
00119     else
00120         return speed_k;
00121 }
00122 
00123 float GPS_Parser::get_altitude_ft()
00124 {
00125     if (!lock)
00126         return 0.0;
00127     else
00128         return 3.280839895*msl_altitude;
00129 }
00130 
00131 // NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
00132 float GPS_Parser::calc_course_to(float pointLat, float pontLong)
00133 {
00134     const double d2r = PI / 180.0;
00135     const double r2d = 180.0 / PI;
00136     double dlat = abs(pointLat - get_dec_latitude()) * d2r;
00137     double dlong = abs(pontLong - get_dec_longitude()) * d2r;
00138     double y = sin(dlong) * cos(pointLat * d2r);
00139     double x = cos(get_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(get_dec_latitude()*d2r)*cos(pointLat*d2r)*cos(dlong);
00140     return 360.0-(atan2(y,x)*r2d);
00141 }
00142 
00143 /*
00144 var y = Math.sin(dLon) * Math.cos(lat2);
00145 var x = Math.cos(lat1)*Math.sin(lat2) -
00146         Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
00147 var brng = Math.atan2(y, x).toDeg();
00148 */
00149 
00150 /*
00151             The Haversine formula according to Dr. Math.
00152             http://mathforum.org/library/drmath/view/51879.html
00153 
00154             dlon = lon2 - lon1
00155             dlat = lat2 - lat1
00156             a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
00157             c = 2 * atan2(sqrt(a), sqrt(1-a))
00158             d = R * c
00159 
00160             Where
00161                 * dlon is the change in longitude
00162                 * dlat is the change in latitude
00163                 * c is the great circle distance in Radians.
00164                 * R is the radius of a spherical Earth.
00165                 * The locations of the two points in
00166                     spherical coordinates (longitude and
00167                     latitude) are lon1,lat1 and lon2, lat2.
00168 */
00169 double GPS_Parser::calc_dist_to_mi(float pointLat, float pontLong)
00170 {
00171     const double d2r = PI / 180.0;
00172     double dlat = pointLat - get_dec_latitude();
00173     double dlong = pontLong - get_dec_longitude();
00174     double a = pow(sin(dlat/2.0),2.0) + cos(get_dec_latitude()*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
00175     double c = 2.0 * asin(sqrt(abs(a)));
00176     double d = 63.765 * c;
00177 
00178     return d;
00179 }
00180 
00181 double GPS_Parser::calc_dist_to_ft(float pointLat, float pontLong)
00182 {
00183     return calc_dist_to_mi(pointLat, pontLong)*5280.0;
00184 }
00185 
00186 double GPS_Parser::calc_dist_to_km(float pointLat, float pontLong)
00187 {
00188     return calc_dist_to_mi(pointLat, pontLong)*1.609344;
00189 }
00190 
00191 double GPS_Parser::calc_dist_to_m(float pointLat, float pontLong)
00192 {
00193     return calc_dist_to_mi(pointLat, pontLong)*1609.344;
00194 }
00195 
00196 char *GPS_Parser::my_token(char *source,char token)
00197 {
00198     char *start;
00199     /* The source string is real only for the first call. Subsequent calls
00200                 are made with the source string pointer as NULL
00201                 */
00202     if(source != NULL) {
00203         /* If the string is empty return NULL */
00204         if(strlen(source) == 0)
00205             return NULL;
00206         strcpy(stat_string,source);
00207         /* Current is our 'current' position within the string */
00208         current = stat_string;
00209     }
00210     start = current;
00211 
00212     while (true) {
00213         /* If we're at the end of the string, return NULL */
00214         if((*current == '\0') && (current == start))
00215             return NULL;
00216         /* If we're at the end now, but weren't when we started, we need
00217         to return the pointer for the last field before the end of string
00218         */
00219         if(*current == '\0')
00220             return start;
00221         /* If we've located our specified token (comma) in the string
00222                                                 load its location in the copy with an end of string marker
00223                                                 so that it can be handled correctly by the calling program.
00224                                 */
00225         if(*current == token) {
00226             *current = '\0';
00227             current++;
00228             return start;
00229         } else {
00230             current++;
00231         }
00232     }
00233 }
00234 
00235 int GPS_Parser::parse(char *string)
00236 {
00237     int field_count;
00238     field_count = 0;
00239     /* NMEA 0183 fields are delimited by commas. The my_token function returns
00240     pointers to the fields.
00241                 */
00242     /* Get the first field pointer */
00243     field[0] = my_token(string,',');
00244     field_count++;
00245 
00246     while (true) {
00247         /* Contiue retrieving fields until there are no more (NULL) */
00248         field[field_count] = my_token(NULL,',');
00249         if(field[field_count] == NULL)
00250             break;
00251         field_count++;
00252     }
00253     /* If we got at least ONE field */
00254     if(field_count) {
00255         /* Check the first field for the valid NMEA 0183 headers */
00256         if(strcmp(field[0],"$GPGGA") == 0) {
00257             /* Retrieve the values from the remaining fields */
00258             utc_time = atof(field[1]);
00259             nmea_latitude = atof(field[2]);
00260             ns = *(field[3]);
00261             nmea_longitude = atof(field[4]);
00262             ew = *(field[5]);
00263             lock = atoi(field[6]);
00264             satellites = atoi(field[7]);
00265             hdop = atof(field[8]);
00266             msl_altitude = atof(field[9]);
00267             msl_units = *(field[10]);
00268             return GGA;
00269         }
00270         if(strcmp(field[0],"$GPRMC") == 0) {
00271             /* Retrieve the data from the remaining fields */
00272             utc_time = atof(field[1]);
00273             nmea_latitude = atof(field[3]);
00274             ns = *(field[4]);
00275             nmea_longitude = atof(field[5]);
00276             ew = *(field[6]);
00277             speed_k = atof(field[7]);
00278             course_d = atof(field[8]);
00279             date = atol(field[9]);
00280             return RMC;
00281         }
00282     }
00283     return NOT_PARSED;
00284 }