File content as of revision 10:a6e1707fdec0:

/*
 * @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 calc_latitude = calc_dec_latitude();
    double calc_longitude = calc_dec_longitude();
    
    double dlat = abs(pointLat - calc_latitude) * d2r;
    double dlong = abs(pontLong - calc_longitude) * d2r;
    double y = sin(dlong) * cos(pointLat * d2r);
    double x = cos(calc_latitude*d2r)*sin(pointLat*d2r) - sin(calc_latitude*d2r)*cos(pointLat*d2r)*cos(dlong);
    return 360.0-(atan2(y,x)*r2d);
    
    // (atan2(y,x*r2d) + 360.0) % 360.0 ??? http://www.movable-type.co.uk/scripts/latlong.html
}

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;
}