File content as of revision 8:59acef1c795b:

#include "GPS_parser.h"

GPS_Parser::GPS_Parser()
{
    nmea_longitude = 0.0;
    nmea_latitude = 0.0;
    utc_time = 0;
    ns = ' ';
    ew = ' ';
    lock = 0;
    satellites = 0;
    hdop = 0.0;
    msl_altitude = 0.0;
    msl_units = ' ';

    rmc_status = ' ';
    speed_k = 0.0;
    course_d = 0.0;
    date = 0;

    dec_longitude = 0.0;
    dec_latitude = 0.0;

    altitude_ft = 0.0;

    current = NULL;
}

float GPS_Parser::nmea_to_dec(float deg_coord, char nsew)
{
    int degree = (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;
}


// INTERNAL FUNCTINS ////////////////////////////////////////////////////////////
float GPS_Parser::trunc(float v)
{
    if (v < 0.0) {
        v*= -1.0;
        v = floor(v);
        v*=-1.0;
    } else {
        v = floor(v);
    }
    return v;
}

// GET FUNCTIONS /////////////////////////////////////////////////////////////////

float GPS_Parser::get_msl_altitude()
{
    if (!lock)
        return 0.0;
    else
        return msl_altitude;
}

int GPS_Parser::get_satellites()
{
    if (!lock)
        return 0;
    else
        return satellites;
}

float GPS_Parser::get_nmea_longitude()
{
    if (!lock)
        return 0.0;
    else
        return nmea_longitude;
}

float GPS_Parser::get_dec_longitude()
{
    dec_longitude = nmea_to_dec(nmea_longitude, ew);
    if (!lock)
        return 0.0;
    else
        return dec_longitude;
}

float GPS_Parser::get_nmea_latitude()
{
    if (!lock)
        return 0.0;
    else
        return nmea_latitude;
}

float GPS_Parser::get_dec_latitude()
{
    dec_latitude = nmea_to_dec(nmea_latitude, ns);
    if (!lock)
        return 0.0;
    else
        return dec_latitude;
}

float GPS_Parser::get_course_d()
{
    if (!lock)
        return 0.0;
    else
        return course_d;
}

float GPS_Parser::get_speed_k()
{
    if (!lock)
        return 0.0;
    else
        return speed_k;
}

float GPS_Parser::get_altitude_ft()
{
    if (!lock)
        return 0.0;
    else
        return 3.280839895*msl_altitude;
}

// NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
float GPS_Parser::calc_course_to(float pointLat, float pontLong)
{
    const double d2r = PI / 180.0;
    const double r2d = 180.0 / PI;
    double dlat = abs(pointLat - get_dec_latitude()) * d2r;
    double dlong = abs(pontLong - get_dec_longitude()) * d2r;
    double y = sin(dlong) * cos(pointLat * d2r);
    double x = cos(get_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(get_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 GPS_Parser::calc_dist_to_mi(float pointLat, float pontLong)
{
    const double d2r = PI / 180.0;
    double dlat = pointLat - get_dec_latitude();
    double dlong = pontLong - get_dec_longitude();
    double a = pow(sin(dlat/2.0),2.0) + cos(get_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 GPS_Parser::calc_dist_to_ft(float pointLat, float pontLong)
{
    return calc_dist_to_mi(pointLat, pontLong)*5280.0;
}

double GPS_Parser::calc_dist_to_km(float pointLat, float pontLong)
{
    return calc_dist_to_mi(pointLat, pontLong)*1.609344;
}

double GPS_Parser::calc_dist_to_m(float pointLat, float pontLong)
{
    return calc_dist_to_mi(pointLat, pontLong)*1609.344;
}

char *GPS_Parser::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 GPS_Parser::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]);
            nmea_latitude = atof(field[2]);
            ns = *(field[3]);
            nmea_longitude = atof(field[4]);
            ew = *(field[5]);
            lock = atoi(field[6]);
            satellites = atoi(field[7]);
            hdop = atof(field[8]);
            msl_altitude = atof(field[9]);
            msl_units = *(field[10]);
            return GGA;
        }
        if(strcmp(field[0],"$GPRMC") == 0) {
            /* Retrieve the data from the remaining fields */
            utc_time = atof(field[1]);
            nmea_latitude = atof(field[3]);
            ns = *(field[4]);
            nmea_longitude = atof(field[5]);
            ew = *(field[6]);
            speed_k = atof(field[7]);
            course_d = atof(field[8]);
            date = atol(field[9]);
            return RMC;
        }
    }
    return NOT_PARSED;
}