This library (beta release) parses the GPS data coming from the TD1204. Beware, not all functionality has been fully tested.
Diff: GPS.cpp
- Revision:
- 0:67f22e813b74
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPS.cpp Wed May 18 14:47:17 2016 +0000 @@ -0,0 +1,302 @@ +#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; +} + + \ No newline at end of file