File content as of revision 0:67f22e813b74:

#include "GPS.h"
#include "math.h"
#include "inttypes.h"
Serial debug(USBTX, USBRX);
GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) {
    _gps.baud(9600);
    nmea_longitude = 0.0;
    nmea_latitude = 0.0;
    utc_time = 0;
    ns = ' ';
    ew = ' ';
    lock = 0;
    satelites = 0;
    msl_altitude = 0.0;
    msl_units = ' ';
    satellites[0] = 0;
    satellites[1] = 0;
    satellites[2] = 0;
    satellites[3] = 0;
    satellites[4] = 0;
    satellites[5] = 0;
    satellites[6] = 0;
    satellites[7] = 0;
    satellites[8] = 0;
    satellites[9] = 0;
    satellites[10] = 0;
    satellites[11] = 0;    
    pdop = 0.0;
    hdop = 0.0;
    vdop = 0.0;
    navigation_mode = 1;
    gprmc_status = 'V';
    tdformat[0] = 0;
    

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

    dec_longitude = 0.0;
    dec_latitude = 0.0;

    gll_status = ' ';

    course_t = 0.0; // ground speed true
    course_t_unit = ' ';
    course_m = 0.0; // magnetic
    course_m_unit = ' ';
    speed_k_unit = ' ';
    speed_km = 0.0; // speed km/hr
    speed_km_unit = ' ';

    altitude_ft = 0.0;
}

float GPS::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;
}

char * GPS::get_nmea_to_td() {
    int lat_degree = (int)(nmea_latitude/100);
    float lat_minutes = (rint((nmea_latitude - lat_degree*100)*1000)/1000.0); // round to 3 digits 
    uint8_t lat_sign = 0;
    if (ns == 'S' )lat_sign = 0x40;
    int lng_degree = (int)(nmea_longitude/100);
    float lng_minutes = rint((nmea_longitude - lng_degree*100)*1000)/1000.0; // round to 3 digits
    uint8_t lng_sign = 0;
    if (ew == 'W' )lng_sign = 0x80;
    uint32_t height = rint(msl_altitude/2.0);
    debug.printf("lng degree: %d, lng min %f,lat degree: %d, lat min %f" , lng_degree, lng_minutes,lat_degree, lat_minutes);
    char temp[32];
    sprintf(temp, "%d%05.0f%d%05.0f",lng_degree, lng_minutes*1000,lat_degree, lat_minutes*1000);
    unsigned long long ret;
    ret = strtoull(temp, NULL, 10);   
    //debug.printf("the string: %s\r\n",temp);
    //debug.printf("the long variable: %llu\r\n",ret);
    //debug.printf("the long variable in hex: %012llx",ret);
    sprintf(tdformat, "01010%012llx%03x%02x",ret,height&0xfff,(lng_sign+lat_sign)&0xff); // to do: add sattelites in view, altiude sign and horizontal dillution
    //debug.printf("Formatted string:%s \r\n", tdformat);
    return tdformat;
}

int GPS::sample() {
    int line_parsed = 0;

    if (_gps.readable()) {
        getline();
        debug.printf("%s\r\n",msg);
        // Check if it is a GPGGA msg (matches both locked and non-locked msg)
        // $xxGGA,time,lat,NS,long,EW,quality,numSV,HDOP,alt,M,sep,M,diffAge,diffStation*cs<CR><LF>
        if (sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", &utc_time, &nmea_latitude, &ns, &nmea_longitude, &ew, &lock, &satelites, &hdop, &msl_altitude, &msl_units) >= 1) {
            line_parsed = GGA;
        }
        // Check if it is a GPSA msg (navigational mode)
        // $xxGSA,opMode,navMode{,sv},PDOP,HDOP,VDOP*cs<CR><LF>
        else if (sscanf(msg, "GPGSA,%c,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f", &operating_mode, &navigation_mode, &satellites[0], &satellites[1], &satellites[2], &satellites[3], &satellites[4], &satellites[5], &satellites[6], &satellites[7], &satellites[8], &satellites[9], &satellites[10], &satellites[11],&pdop,&hdop,&vdop) >= 1) {
            line_parsed = GSA;
        }
        // Check if it is a GPRMC msg
        // $xxRMC,time,status,lat,NS,long,EW,spd,cog,date,mv,mvEW,posMode*cs<CR><LF>
        else if (sscanf(msg, "GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,%d", &utc_time, &gprmc_status, &nmea_latitude, &ns, &nmea_longitude,&ew,&speed_k,&course_d,&date) >= 1) {
            line_parsed = RMC;
        }
        // GLL - Geographic Position-Lat/Lon
        // $xxGLL,lat,NS,long,EW,time,status,posMode*cs<CR><LF>
        else if (sscanf(msg, "GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) {
            line_parsed = GLL;
        }
        // VTG-Course Over Ground and Ground Speed
        else if (sscanf(msg, "GPVTG,%f,%c,%f,%c,%f,%c,%f,%c", &course_t, &course_t_unit, &course_m, &course_m_unit, &speed_k, &speed_k_unit, &speed_km, &speed_km_unit) >= 1) {
            line_parsed = VTG;
        }
        
        /*if(satelites == 0) {
            lock = 0;
        }*/
        if(gprmc_status == 'A'|| satelites > 0 || navigation_mode > 1) lock = 1;
        else lock = 0;
    }
    if (!lock) {
        return NO_LOCK;
    } else if (line_parsed) {
        return line_parsed;
    } else {
        return NOT_PARSED;
    }
}


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

void GPS::getline() {
    while (_gps.getc() != '$');   // wait for the start of a line
    for (int i=0; i<1022; i++) {
        msg[i] = _gps.getc();
        if (msg[i] == '\r') {
            msg[i] = 0;
            return;
        }
    }
    error("Overflow in getline");
}

// GET FUNCTIONS /////////////////////////////////////////////////////////////////
float GPS::get_msl_altitude() {
    if (!lock)
        return 0.0;
    else
        return msl_altitude;
}

int GPS::get_satelites() {
    if (!lock)
        return 0;
    else
        return satelites;
}

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

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

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

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

float GPS::get_course_t() {
    if (!lock)
        return 0.0;
    else
        return course_t;
}

float GPS::get_course_m() {
    if (!lock)
        return 0.0;
    else
        return course_m;
}

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

float GPS::get_speed_km() {
    if (!lock)
        return 0.0;
    else
        return speed_km;
}

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

// NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
float GPS::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 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::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::calc_dist_to_ft(float pointLat, float pontLong) {
    return calc_dist_to_mi(pointLat, pontLong)*5280.0;
}

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

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