nmea_parser.cpp

00001 /*
00002  * @file nmea_parser.cpp
00003  * @author Tyler Weaver
00004  *
00005  * @section LICENSE
00006  *
00007  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00008  * and associated documentation files (the "Software"), to deal in the Software without restriction,
00009  * including without limitation the rights to use, copy, modify, merge, publish, distribute,
00010  * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
00011  * furnished to do so, subject to the following conditions:
00012  *
00013  * The above copyright notice and this permission notice shall be included in all copies or
00014  * substantial portions of the Software.
00015  *
00016  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00017  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00018  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00019  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00020  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00021  *
00022  * @section DESCRIPTION
00023  *
00024  * NMEA GPS Serial Output parser.
00025  * Routine taken from NMEA Software Standard (NMEA 0183)
00026  * http://www.winsystems.com/software/nmea.pdf
00027  *
00028  * Only handles GGA and RMC Messages
00029  */
00030  
00031 #include "nmea_parser.h"
00032 
00033 NmeaParser::NmeaParser()
00034 {
00035     longitude_ = 0.0;
00036     latitude_ = 0.0;
00037     utc_time_ = 0;
00038     lat_reference_ = '\0';
00039     long_reference_ = '\0';
00040     quality_ = 0;
00041     satellite_count_ = 0;
00042     hdop_ = 0.0;
00043     msl_altitude_ = 0.0;
00044     msl_altitude_unit_ = '\0';
00045 
00046     rmc_status_ = '\0';
00047     speed_ = 0.0;
00048     track_ = 0.0;
00049     date_ = 0;
00050 
00051     dec_longitude_ = 0.0;
00052     dec_latitude_ = 0.0;
00053 
00054     current_ = NULL;
00055 }
00056 
00057 float NmeaParser::nmea_to_dec(float deg_coord, char nsew)
00058 {
00059     int degree = static_cast<int>(deg_coord/100);
00060     float minutes = deg_coord - degree*100;
00061     float dec_deg = minutes / 60;
00062     float decimal = degree + dec_deg;
00063     if (nsew == 'S' || nsew == 'W') { // return negative
00064         decimal *= -1;
00065     }
00066     return decimal;
00067 }
00068 
00069 // GET FUNCTIONS /////////////////////////////////////////////////////////////////
00070 
00071 float NmeaParser::msl_altitude ()
00072 {
00073     if (!quality_)
00074         return 0.0;
00075     else
00076         return msl_altitude_;
00077 }
00078 
00079 int NmeaParser::satellite_count ()
00080 {
00081     if (!quality_)
00082         return 0;
00083     else
00084         return satellite_count_;
00085 }
00086 
00087 float NmeaParser::longitude ()
00088 {
00089     if (!quality_)
00090         return 0.0;
00091     else
00092         return longitude_;
00093 }
00094 
00095 float NmeaParser::calc_dec_longitude ()
00096 {
00097     dec_longitude_ = nmea_to_dec(longitude_, long_reference_);
00098     if (!quality_)
00099         return 0.0;
00100     else
00101         return dec_longitude_;
00102 }
00103 
00104 float NmeaParser::latitude ()
00105 {
00106     if (!quality_)
00107         return 0.0;
00108     else
00109         return latitude_;
00110 }
00111 
00112 float NmeaParser::calc_dec_latitude ()
00113 {
00114     dec_latitude_ = nmea_to_dec(latitude_, lat_reference_);
00115     if (!quality_)
00116         return 0.0;
00117     else
00118         return dec_latitude_;
00119 }
00120 
00121 float NmeaParser::track ()
00122 {
00123     if (!quality_)
00124         return 0.0;
00125     else
00126         return track_;
00127 }
00128 
00129 float NmeaParser::speed ()
00130 {
00131     if (!quality_)
00132         return 0.0;
00133     else
00134         return speed_;
00135 }
00136 
00137 float NmeaParser::calc_altitude_ft ()
00138 {
00139     if (!quality_)
00140         return 0.0;
00141     else
00142         return 3.280839895*msl_altitude_;
00143 }
00144 
00145 int NmeaParser::quality ()
00146 {
00147     return quality_;
00148 }
00149 
00150 int NmeaParser::date()
00151 {
00152     return date_;
00153 }
00154 
00155 float NmeaParser::utc_time ()
00156 {
00157     return utc_time_;
00158 }
00159 
00160 // NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
00161 float NmeaParser::calc_initial_bearing(float pointLat, float pontLong)
00162 {
00163     const double d2r = PI / 180.0;
00164     const double r2d = 180.0 / PI;
00165     double calc_latitude = calc_dec_latitude ();
00166     double calc_longitude = calc_dec_longitude ();
00167     
00168     double dlat = abs(pointLat - calc_latitude) * d2r;
00169     double dlong = abs(pontLong - calc_longitude) * d2r;
00170     double y = sin(dlong) * cos(pointLat * d2r);
00171     double x = cos(calc_latitude*d2r)*sin(pointLat*d2r) - sin(calc_latitude*d2r)*cos(pointLat*d2r)*cos(dlong);
00172     return 360.0-(atan2(y,x)*r2d);
00173     
00174     // (atan2(y,x*r2d) + 360.0) % 360.0 ??? http://www.movable-type.co.uk/scripts/latlong.html
00175 }
00176 
00177 double NmeaParser::calc_dist_to_mi(float pointLat, float pontLong)
00178 {
00179     const double d2r = PI / 180.0;
00180     double dlat = pointLat - calc_dec_latitude ();
00181     double dlong = pontLong - calc_dec_longitude ();
00182     double a = pow(sin(dlat/2.0),2.0) + cos(calc_dec_latitude ()*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
00183     double c = 2.0 * asin(sqrt(abs(a)));
00184     double d = 63.765 * c;
00185 
00186     return d;
00187 }
00188 
00189 double NmeaParser::calc_dist_to_km(float pointLat, float pontLong)
00190 {
00191     return calc_dist_to_mi(pointLat, pontLong)*1.609344;
00192 }
00193 
00194 char *NmeaParser::my_token(char *source,char token)
00195 {
00196     char *start;
00197     /* The source string is real only for the first call. Subsequent calls
00198                 are made with the source string pointer as NULL
00199                 */
00200     if(source != NULL) {
00201         /* If the string is empty return NULL */
00202         if(strlen(source) == 0)
00203             return NULL;
00204         strcpy(stat_string_,source);
00205         /* Current is our 'current' position within the string */
00206         current_ = stat_string_;
00207     }
00208     start = current_;
00209 
00210     while (true) {
00211         /* If we're at the end of the string, return NULL */
00212         if((*current_ == '\0') && (current_ == start))
00213             return NULL;
00214         /* If we're at the end now, but weren't when we started, we need
00215         to return the pointer for the last field before the end of string
00216         */
00217         if(*current_ == '\0')
00218             return start;
00219         /* If we've located our specified token (comma) in the string
00220                                                 load its location in the copy with an end of string marker
00221                                                 so that it can be handled correctly by the calling program.
00222                                 */
00223         if(*current_ == token) {
00224             *current_ = '\0';
00225             current_++;
00226             return start;
00227         } else {
00228             current_++;
00229         }
00230     }
00231 }
00232 
00233 int NmeaParser::parse(char *string)
00234 {
00235     int field_count;
00236     field_count = 0;
00237     /* NMEA 0183 fields are delimited by commas. The my_token function returns
00238     pointers to the fields.
00239                 */
00240     /* Get the first field pointer */
00241     field_[0] = my_token(string,',');
00242     field_count++;
00243 
00244     while (true) {
00245         /* Contiue retrieving fields until there are no more (NULL) */
00246         field_[field_count] = my_token(NULL,',');
00247         if(field_[field_count] == NULL)
00248             break;
00249         field_count++;
00250     }
00251     /* If we got at least ONE field */
00252     if(field_count) {
00253         /* Check the first field for the valid NMEA 0183 headers */
00254         if(strcmp(field_[0],"$GPGGA") == 0) {
00255             /* Retrieve the values from the remaining fields */
00256             utc_time_ = atof(field_[1]);
00257             latitude_ = atof(field_[2]);
00258             lat_reference_ = *(field_[3]);
00259             longitude_ = atof(field_[4]);
00260             long_reference_ = *(field_[5]);
00261             quality_ = atoi(field_[6]);
00262             satellite_count_ = atoi(field_[7]);
00263             hdop_ = atof(field_[8]);
00264             msl_altitude_ = atof(field_[9]);
00265             msl_altitude_unit_ = *(field_[10]);
00266             return GGA;
00267         }
00268         if(strcmp(field_[0],"$GPRMC") == 0) {
00269             /* Retrieve the data from the remaining fields */
00270             utc_time_ = atof(field_[1]);
00271             latitude_ = atof(field_[3]);
00272             lat_reference_ = *(field_[4]);
00273             longitude_ = atof(field_[5]);
00274             long_reference_ = *(field_[6]);
00275             speed_ = atof(field_[7]);
00276             track_ = atof(field_[8]);
00277             date_ = atol(field_[9]);
00278             return RMC;
00279         }
00280     }
00281     return NOT_PARSED;
00282 }