Diff: nmea_parser.cpp

Revision:

9:9b2351e25a84

Parent:

8:59acef1c795b

Child:

10:a6e1707fdec0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nmea_parser.cpp	Mon Dec 17 22:11:24 2012 +0000
@@ -0,0 +1,304 @@
+/*
+ * @file nmea_parser.cpp
+ * @author Tyler Weaver
+ *
+ * @section LICENSE
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * NMEA GPS Serial Output parser.
+ * Routine taken from NMEA Software Standard (NMEA 0183)
+ * http://www.winsystems.com/software/nmea.pdf
+ *
+ * Only handles GGA and RMC Messages
+ */
+ 
+#include "nmea_parser.h"
+
+NmeaParser::NmeaParser()
+{
+    longitude_ = 0.0;
+    latitude_ = 0.0;
+    utc_time_ = 0;
+    lat_reference_ = '\0';
+    long_reference_ = '\0';
+    quality_ = 0;
+    satellite_count_ = 0;
+    hdop_ = 0.0;
+    msl_altitude_ = 0.0;
+    msl_altitude_unit_ = '\0';
+
+    rmc_status_ = '\0';
+    speed_ = 0.0;
+    track_ = 0.0;
+    date_ = 0;
+
+    dec_longitude_ = 0.0;
+    dec_latitude_ = 0.0;
+
+    current_ = NULL;
+}
+
+float NmeaParser::nmea_to_dec(float deg_coord, char nsew)
+{
+    int degree = static_cast<int>(deg_coord/100);
+    float minutes = deg_coord - degree*100;
+    float dec_deg = minutes / 60;
+    float decimal = degree + dec_deg;
+    if (nsew == 'S' || nsew == 'W') { // return negative
+        decimal *= -1;
+    }
+    return decimal;
+}
+
+// GET FUNCTIONS /////////////////////////////////////////////////////////////////
+
+float NmeaParser::msl_altitude()
+{
+    if (!quality_)
+        return 0.0;
+    else
+        return msl_altitude_;
+}
+
+int NmeaParser::satellite_count()
+{
+    if (!quality_)
+        return 0;
+    else
+        return satellite_count_;
+}
+
+float NmeaParser::longitude()
+{
+    if (!quality_)
+        return 0.0;
+    else
+        return longitude_;
+}
+
+float NmeaParser::calc_dec_longitude()
+{
+    dec_longitude_ = nmea_to_dec(longitude_, long_reference_);
+    if (!quality_)
+        return 0.0;
+    else
+        return dec_longitude_;
+}
+
+float NmeaParser::latitude()
+{
+    if (!quality_)
+        return 0.0;
+    else
+        return latitude_;
+}
+
+float NmeaParser::calc_dec_latitude()
+{
+    dec_latitude_ = nmea_to_dec(latitude_, lat_reference_);
+    if (!quality_)
+        return 0.0;
+    else
+        return dec_latitude_;
+}
+
+float NmeaParser::track()
+{
+    if (!quality_)
+        return 0.0;
+    else
+        return track_;
+}
+
+float NmeaParser::speed()
+{
+    if (!quality_)
+        return 0.0;
+    else
+        return speed_;
+}
+
+float NmeaParser::calc_altitude_ft()
+{
+    if (!quality_)
+        return 0.0;
+    else
+        return 3.280839895*msl_altitude_;
+}
+
+int NmeaParser::quality()
+{
+    return quality_;
+}
+
+int NmeaParser::date()
+{
+    return date_;
+}
+
+float NmeaParser::utc_time()
+{
+    return utc_time_;
+}
+
+// NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
+float NmeaParser::calc_initial_bearing(float pointLat, float pontLong)
+{
+    const double d2r = PI / 180.0;
+    const double r2d = 180.0 / PI;
+    double dlat = abs(pointLat - calc_dec_latitude()) * d2r;
+    double dlong = abs(pontLong - calc_dec_longitude()) * d2r;
+    double y = sin(dlong) * cos(pointLat * d2r);
+    double x = cos(calc_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(calc_dec_latitude()*d2r)*cos(pointLat*d2r)*cos(dlong);
+    return 360.0-(atan2(y,x)*r2d);
+}
+
+/*
+var y = Math.sin(dLon) * Math.cos(lat2);
+var x = Math.cos(lat1)*Math.sin(lat2) -
+        Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
+var brng = Math.atan2(y, x).toDeg();
+*/
+
+/*
+            The Haversine formula according to Dr. Math.
+            http://mathforum.org/library/drmath/view/51879.html
+
+            dlon = lon2 - lon1
+            dlat = lat2 - lat1
+            a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
+            c = 2 * atan2(sqrt(a), sqrt(1-a))
+            d = R * c
+
+            Where
+                * dlon is the change in longitude
+                * dlat is the change in latitude
+                * c is the great circle distance in Radians.
+                * R is the radius of a spherical Earth.
+                * The locations of the two points in
+                    spherical coordinates (longitude and
+                    latitude) are lon1,lat1 and lon2, lat2.
+*/
+
+double NmeaParser::calc_dist_to_mi(float pointLat, float pontLong)
+{
+    const double d2r = PI / 180.0;
+    double dlat = pointLat - calc_dec_latitude();
+    double dlong = pontLong - calc_dec_longitude();
+    double a = pow(sin(dlat/2.0),2.0) + cos(calc_dec_latitude()*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
+    double c = 2.0 * asin(sqrt(abs(a)));
+    double d = 63.765 * c;
+
+    return d;
+}
+
+double NmeaParser::calc_dist_to_km(float pointLat, float pontLong)
+{
+    return calc_dist_to_mi(pointLat, pontLong)*1.609344;
+}
+
+char *NmeaParser::my_token(char *source,char token)
+{
+    char *start;
+    /* The source string is real only for the first call. Subsequent calls
+                are made with the source string pointer as NULL
+                */
+    if(source != NULL) {
+        /* If the string is empty return NULL */
+        if(strlen(source) == 0)
+            return NULL;
+        strcpy(stat_string_,source);
+        /* Current is our 'current' position within the string */
+        current_ = stat_string_;
+    }
+    start = current_;
+
+    while (true) {
+        /* If we're at the end of the string, return NULL */
+        if((*current_ == '\0') && (current_ == start))
+            return NULL;
+        /* If we're at the end now, but weren't when we started, we need
+        to return the pointer for the last field before the end of string
+        */
+        if(*current_ == '\0')
+            return start;
+        /* If we've located our specified token (comma) in the string
+                                                load its location in the copy with an end of string marker
+                                                so that it can be handled correctly by the calling program.
+                                */
+        if(*current_ == token) {
+            *current_ = '\0';
+            current_++;
+            return start;
+        } else {
+            current_++;
+        }
+    }
+}
+
+int NmeaParser::parse(char *string)
+{
+    int field_count;
+    field_count = 0;
+    /* NMEA 0183 fields are delimited by commas. The my_token function returns
+    pointers to the fields.
+                */
+    /* Get the first field pointer */
+    field_[0] = my_token(string,',');
+    field_count++;
+
+    while (true) {
+        /* Contiue retrieving fields until there are no more (NULL) */
+        field_[field_count] = my_token(NULL,',');
+        if(field_[field_count] == NULL)
+            break;
+        field_count++;
+    }
+    /* If we got at least ONE field */
+    if(field_count) {
+        /* Check the first field for the valid NMEA 0183 headers */
+        if(strcmp(field_[0],"$GPGGA") == 0) {
+            /* Retrieve the values from the remaining fields */
+            utc_time_ = atof(field_[1]);
+            latitude_ = atof(field_[2]);
+            lat_reference_ = *(field_[3]);
+            longitude_ = atof(field_[4]);
+            long_reference_ = *(field_[5]);
+            quality_ = atoi(field_[6]);
+            satellite_count_ = atoi(field_[7]);
+            hdop_ = atof(field_[8]);
+            msl_altitude_ = atof(field_[9]);
+            msl_altitude_unit_ = *(field_[10]);
+            return GGA;
+        }
+        if(strcmp(field_[0],"$GPRMC") == 0) {
+            /* Retrieve the data from the remaining fields */
+            utc_time_ = atof(field_[1]);
+            latitude_ = atof(field_[3]);
+            lat_reference_ = *(field_[4]);
+            longitude_ = atof(field_[5]);
+            long_reference_ = *(field_[6]);
+            speed_ = atof(field_[7]);
+            track_ = atof(field_[8]);
+            date_ = atol(field_[9]);
+            return RMC;
+        }
+    }
+    return NOT_PARSED;
+}
\ No newline at end of file