Quicksand micro-electronics / QW_NMEA

This library (beta release) parses the GPS data coming from the TD1204. Beware, not all functionality has been fully tested.

Dependents:   QW-TEMP_GPS-NMEA

GPS.cpp@0:67f22e813b74, 2016-05-18 (annotated)

Committer:
quicksand
Date:
Wed May 18 14:47:17 2016 +0000
Revision:
0:67f22e813b74
Version for NMEA message parsing on the QW boards

Who changed what in which revision?

UserRevisionLine numberNew contents of line
quicksand 0:67f22e813b74 1 #include "GPS.h"
quicksand 0:67f22e813b74 2 #include "math.h"
quicksand 0:67f22e813b74 3 #include "inttypes.h"
quicksand 0:67f22e813b74 4 Serial debug(USBTX, USBRX);
quicksand 0:67f22e813b74 5 GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) {
quicksand 0:67f22e813b74 6 _gps.baud(9600);
quicksand 0:67f22e813b74 7 nmea_longitude = 0.0;
quicksand 0:67f22e813b74 8 nmea_latitude = 0.0;
quicksand 0:67f22e813b74 9 utc_time = 0;
quicksand 0:67f22e813b74 10 ns = ' ';
quicksand 0:67f22e813b74 11 ew = ' ';
quicksand 0:67f22e813b74 12 lock = 0;
quicksand 0:67f22e813b74 13 satelites = 0;
quicksand 0:67f22e813b74 14 msl_altitude = 0.0;
quicksand 0:67f22e813b74 15 msl_units = ' ';
quicksand 0:67f22e813b74 16 satellites[0] = 0;
quicksand 0:67f22e813b74 17 satellites[1] = 0;
quicksand 0:67f22e813b74 18 satellites[2] = 0;
quicksand 0:67f22e813b74 19 satellites[3] = 0;
quicksand 0:67f22e813b74 20 satellites[4] = 0;
quicksand 0:67f22e813b74 21 satellites[5] = 0;
quicksand 0:67f22e813b74 22 satellites[6] = 0;
quicksand 0:67f22e813b74 23 satellites[7] = 0;
quicksand 0:67f22e813b74 24 satellites[8] = 0;
quicksand 0:67f22e813b74 25 satellites[9] = 0;
quicksand 0:67f22e813b74 26 satellites[10] = 0;
quicksand 0:67f22e813b74 27 satellites[11] = 0;
quicksand 0:67f22e813b74 28 pdop = 0.0;
quicksand 0:67f22e813b74 29 hdop = 0.0;
quicksand 0:67f22e813b74 30 vdop = 0.0;
quicksand 0:67f22e813b74 31 navigation_mode = 1;
quicksand 0:67f22e813b74 32 gprmc_status = 'V';
quicksand 0:67f22e813b74 33 tdformat[0] = 0;
quicksand 0:67f22e813b74 34
quicksand 0:67f22e813b74 35
quicksand 0:67f22e813b74 36 rmc_status = ' ';
quicksand 0:67f22e813b74 37 speed_k = 0.0;
quicksand 0:67f22e813b74 38 course_d = 0.0;
quicksand 0:67f22e813b74 39 date = 0;
quicksand 0:67f22e813b74 40
quicksand 0:67f22e813b74 41 dec_longitude = 0.0;
quicksand 0:67f22e813b74 42 dec_latitude = 0.0;
quicksand 0:67f22e813b74 43
quicksand 0:67f22e813b74 44 gll_status = ' ';
quicksand 0:67f22e813b74 45
quicksand 0:67f22e813b74 46 course_t = 0.0; // ground speed true
quicksand 0:67f22e813b74 47 course_t_unit = ' ';
quicksand 0:67f22e813b74 48 course_m = 0.0; // magnetic
quicksand 0:67f22e813b74 49 course_m_unit = ' ';
quicksand 0:67f22e813b74 50 speed_k_unit = ' ';
quicksand 0:67f22e813b74 51 speed_km = 0.0; // speed km/hr
quicksand 0:67f22e813b74 52 speed_km_unit = ' ';
quicksand 0:67f22e813b74 53
quicksand 0:67f22e813b74 54 altitude_ft = 0.0;
quicksand 0:67f22e813b74 55 }
quicksand 0:67f22e813b74 56
quicksand 0:67f22e813b74 57 float GPS::nmea_to_dec(float deg_coord, char nsew) {
quicksand 0:67f22e813b74 58 int degree = (int)(deg_coord/100);
quicksand 0:67f22e813b74 59 float minutes = deg_coord - degree*100;
quicksand 0:67f22e813b74 60 float dec_deg = minutes / 60;
quicksand 0:67f22e813b74 61 float decimal = degree + dec_deg;
quicksand 0:67f22e813b74 62 if (nsew == 'S' || nsew == 'W') { // return negative
quicksand 0:67f22e813b74 63 decimal *= -1;
quicksand 0:67f22e813b74 64 }
quicksand 0:67f22e813b74 65 return decimal;
quicksand 0:67f22e813b74 66 }
quicksand 0:67f22e813b74 67
quicksand 0:67f22e813b74 68 char * GPS::get_nmea_to_td() {
quicksand 0:67f22e813b74 69 int lat_degree = (int)(nmea_latitude/100);
quicksand 0:67f22e813b74 70 float lat_minutes = (rint((nmea_latitude - lat_degree*100)*1000)/1000.0); // round to 3 digits
quicksand 0:67f22e813b74 71 uint8_t lat_sign = 0;
quicksand 0:67f22e813b74 72 if (ns == 'S' )lat_sign = 0x40;
quicksand 0:67f22e813b74 73 int lng_degree = (int)(nmea_longitude/100);
quicksand 0:67f22e813b74 74 float lng_minutes = rint((nmea_longitude - lng_degree*100)*1000)/1000.0; // round to 3 digits
quicksand 0:67f22e813b74 75 uint8_t lng_sign = 0;
quicksand 0:67f22e813b74 76 if (ew == 'W' )lng_sign = 0x80;
quicksand 0:67f22e813b74 77 uint32_t height = rint(msl_altitude/2.0);
quicksand 0:67f22e813b74 78 debug.printf("lng degree: %d, lng min %f,lat degree: %d, lat min %f" , lng_degree, lng_minutes,lat_degree, lat_minutes);
quicksand 0:67f22e813b74 79 char temp[32];
quicksand 0:67f22e813b74 80 sprintf(temp, "%d%05.0f%d%05.0f",lng_degree, lng_minutes*1000,lat_degree, lat_minutes*1000);
quicksand 0:67f22e813b74 81 unsigned long long ret;
quicksand 0:67f22e813b74 82 ret = strtoull(temp, NULL, 10);
quicksand 0:67f22e813b74 83 //debug.printf("the string: %s\r\n",temp);
quicksand 0:67f22e813b74 84 //debug.printf("the long variable: %llu\r\n",ret);
quicksand 0:67f22e813b74 85 //debug.printf("the long variable in hex: %012llx",ret);
quicksand 0:67f22e813b74 86 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
quicksand 0:67f22e813b74 87 //debug.printf("Formatted string:%s \r\n", tdformat);
quicksand 0:67f22e813b74 88 return tdformat;
quicksand 0:67f22e813b74 89 }
quicksand 0:67f22e813b74 90
quicksand 0:67f22e813b74 91 int GPS::sample() {
quicksand 0:67f22e813b74 92 int line_parsed = 0;
quicksand 0:67f22e813b74 93
quicksand 0:67f22e813b74 94 if (_gps.readable()) {
quicksand 0:67f22e813b74 95 getline();
quicksand 0:67f22e813b74 96 debug.printf("%s\r\n",msg);
quicksand 0:67f22e813b74 97 // Check if it is a GPGGA msg (matches both locked and non-locked msg)
quicksand 0:67f22e813b74 98 // $xxGGA,time,lat,NS,long,EW,quality,numSV,HDOP,alt,M,sep,M,diffAge,diffStation*cs<CR><LF>
quicksand 0:67f22e813b74 99 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) {
quicksand 0:67f22e813b74 100 line_parsed = GGA;
quicksand 0:67f22e813b74 101 }
quicksand 0:67f22e813b74 102 // Check if it is a GPSA msg (navigational mode)
quicksand 0:67f22e813b74 103 // $xxGSA,opMode,navMode{,sv},PDOP,HDOP,VDOP*cs<CR><LF>
quicksand 0:67f22e813b74 104 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) {
quicksand 0:67f22e813b74 105 line_parsed = GSA;
quicksand 0:67f22e813b74 106 }
quicksand 0:67f22e813b74 107 // Check if it is a GPRMC msg
quicksand 0:67f22e813b74 108 // $xxRMC,time,status,lat,NS,long,EW,spd,cog,date,mv,mvEW,posMode*cs<CR><LF>
quicksand 0:67f22e813b74 109 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) {
quicksand 0:67f22e813b74 110 line_parsed = RMC;
quicksand 0:67f22e813b74 111 }
quicksand 0:67f22e813b74 112 // GLL - Geographic Position-Lat/Lon
quicksand 0:67f22e813b74 113 // $xxGLL,lat,NS,long,EW,time,status,posMode*cs<CR><LF>
quicksand 0:67f22e813b74 114 else if (sscanf(msg, "GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) {
quicksand 0:67f22e813b74 115 line_parsed = GLL;
quicksand 0:67f22e813b74 116 }
quicksand 0:67f22e813b74 117 // VTG-Course Over Ground and Ground Speed
quicksand 0:67f22e813b74 118 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) {
quicksand 0:67f22e813b74 119 line_parsed = VTG;
quicksand 0:67f22e813b74 120 }
quicksand 0:67f22e813b74 121
quicksand 0:67f22e813b74 122 /*if(satelites == 0) {
quicksand 0:67f22e813b74 123 lock = 0;
quicksand 0:67f22e813b74 124 }*/
quicksand 0:67f22e813b74 125 if(gprmc_status == 'A'|| satelites > 0 || navigation_mode > 1) lock = 1;
quicksand 0:67f22e813b74 126 else lock = 0;
quicksand 0:67f22e813b74 127 }
quicksand 0:67f22e813b74 128 if (!lock) {
quicksand 0:67f22e813b74 129 return NO_LOCK;
quicksand 0:67f22e813b74 130 } else if (line_parsed) {
quicksand 0:67f22e813b74 131 return line_parsed;
quicksand 0:67f22e813b74 132 } else {
quicksand 0:67f22e813b74 133 return NOT_PARSED;
quicksand 0:67f22e813b74 134 }
quicksand 0:67f22e813b74 135 }
quicksand 0:67f22e813b74 136
quicksand 0:67f22e813b74 137
quicksand 0:67f22e813b74 138 // INTERNAL FUNCTINS ////////////////////////////////////////////////////////////
quicksand 0:67f22e813b74 139 float GPS::trunc(float v) {
quicksand 0:67f22e813b74 140 if (v < 0.0) {
quicksand 0:67f22e813b74 141 v*= -1.0;
quicksand 0:67f22e813b74 142 v = floor(v);
quicksand 0:67f22e813b74 143 v*=-1.0;
quicksand 0:67f22e813b74 144 } else {
quicksand 0:67f22e813b74 145 v = floor(v);
quicksand 0:67f22e813b74 146 }
quicksand 0:67f22e813b74 147 return v;
quicksand 0:67f22e813b74 148 }
quicksand 0:67f22e813b74 149
quicksand 0:67f22e813b74 150 void GPS::getline() {
quicksand 0:67f22e813b74 151 while (_gps.getc() != '$'); // wait for the start of a line
quicksand 0:67f22e813b74 152 for (int i=0; i<1022; i++) {
quicksand 0:67f22e813b74 153 msg[i] = _gps.getc();
quicksand 0:67f22e813b74 154 if (msg[i] == '\r') {
quicksand 0:67f22e813b74 155 msg[i] = 0;
quicksand 0:67f22e813b74 156 return;
quicksand 0:67f22e813b74 157 }
quicksand 0:67f22e813b74 158 }
quicksand 0:67f22e813b74 159 error("Overflow in getline");
quicksand 0:67f22e813b74 160 }
quicksand 0:67f22e813b74 161
quicksand 0:67f22e813b74 162 // GET FUNCTIONS /////////////////////////////////////////////////////////////////
quicksand 0:67f22e813b74 163 float GPS::get_msl_altitude() {
quicksand 0:67f22e813b74 164 if (!lock)
quicksand 0:67f22e813b74 165 return 0.0;
quicksand 0:67f22e813b74 166 else
quicksand 0:67f22e813b74 167 return msl_altitude;
quicksand 0:67f22e813b74 168 }
quicksand 0:67f22e813b74 169
quicksand 0:67f22e813b74 170 int GPS::get_satelites() {
quicksand 0:67f22e813b74 171 if (!lock)
quicksand 0:67f22e813b74 172 return 0;
quicksand 0:67f22e813b74 173 else
quicksand 0:67f22e813b74 174 return satelites;
quicksand 0:67f22e813b74 175 }
quicksand 0:67f22e813b74 176
quicksand 0:67f22e813b74 177 float GPS::get_nmea_longitude() {
quicksand 0:67f22e813b74 178 if (!lock)
quicksand 0:67f22e813b74 179 return 0.0;
quicksand 0:67f22e813b74 180 else
quicksand 0:67f22e813b74 181 return nmea_longitude;
quicksand 0:67f22e813b74 182 }
quicksand 0:67f22e813b74 183
quicksand 0:67f22e813b74 184 float GPS::get_dec_longitude() {
quicksand 0:67f22e813b74 185 dec_longitude = nmea_to_dec(nmea_longitude, ew);
quicksand 0:67f22e813b74 186 if (!lock)
quicksand 0:67f22e813b74 187 return 0.0;
quicksand 0:67f22e813b74 188 else
quicksand 0:67f22e813b74 189 return dec_longitude;
quicksand 0:67f22e813b74 190 }
quicksand 0:67f22e813b74 191
quicksand 0:67f22e813b74 192 float GPS::get_nmea_latitude() {
quicksand 0:67f22e813b74 193 if (!lock)
quicksand 0:67f22e813b74 194 return 0.0;
quicksand 0:67f22e813b74 195 else
quicksand 0:67f22e813b74 196 return nmea_latitude;
quicksand 0:67f22e813b74 197 }
quicksand 0:67f22e813b74 198
quicksand 0:67f22e813b74 199 float GPS::get_dec_latitude() {
quicksand 0:67f22e813b74 200 dec_latitude = nmea_to_dec(nmea_latitude, ns);
quicksand 0:67f22e813b74 201 if (!lock)
quicksand 0:67f22e813b74 202 return 0.0;
quicksand 0:67f22e813b74 203 else
quicksand 0:67f22e813b74 204 return dec_latitude;
quicksand 0:67f22e813b74 205 }
quicksand 0:67f22e813b74 206
quicksand 0:67f22e813b74 207 float GPS::get_course_t() {
quicksand 0:67f22e813b74 208 if (!lock)
quicksand 0:67f22e813b74 209 return 0.0;
quicksand 0:67f22e813b74 210 else
quicksand 0:67f22e813b74 211 return course_t;
quicksand 0:67f22e813b74 212 }
quicksand 0:67f22e813b74 213
quicksand 0:67f22e813b74 214 float GPS::get_course_m() {
quicksand 0:67f22e813b74 215 if (!lock)
quicksand 0:67f22e813b74 216 return 0.0;
quicksand 0:67f22e813b74 217 else
quicksand 0:67f22e813b74 218 return course_m;
quicksand 0:67f22e813b74 219 }
quicksand 0:67f22e813b74 220
quicksand 0:67f22e813b74 221 float GPS::get_speed_k() {
quicksand 0:67f22e813b74 222 if (!lock)
quicksand 0:67f22e813b74 223 return 0.0;
quicksand 0:67f22e813b74 224 else
quicksand 0:67f22e813b74 225 return speed_k;
quicksand 0:67f22e813b74 226 }
quicksand 0:67f22e813b74 227
quicksand 0:67f22e813b74 228 float GPS::get_speed_km() {
quicksand 0:67f22e813b74 229 if (!lock)
quicksand 0:67f22e813b74 230 return 0.0;
quicksand 0:67f22e813b74 231 else
quicksand 0:67f22e813b74 232 return speed_km;
quicksand 0:67f22e813b74 233 }
quicksand 0:67f22e813b74 234
quicksand 0:67f22e813b74 235 float GPS::get_altitude_ft() {
quicksand 0:67f22e813b74 236 if (!lock)
quicksand 0:67f22e813b74 237 return 0.0;
quicksand 0:67f22e813b74 238 else
quicksand 0:67f22e813b74 239 return 3.280839895*msl_altitude;
quicksand 0:67f22e813b74 240 }
quicksand 0:67f22e813b74 241
quicksand 0:67f22e813b74 242 // NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
quicksand 0:67f22e813b74 243 float GPS::calc_course_to(float pointLat, float pontLong) {
quicksand 0:67f22e813b74 244 const double d2r = PI / 180.0;
quicksand 0:67f22e813b74 245 const double r2d = 180.0 / PI;
quicksand 0:67f22e813b74 246 double dlat = abs(pointLat - get_dec_latitude()) * d2r;
quicksand 0:67f22e813b74 247 double dlong = abs(pontLong - get_dec_longitude()) * d2r;
quicksand 0:67f22e813b74 248 double y = sin(dlong) * cos(pointLat * d2r);
quicksand 0:67f22e813b74 249 double x = cos(get_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(get_dec_latitude()*d2r)*cos(pointLat*d2r)*cos(dlong);
quicksand 0:67f22e813b74 250 return atan2(y,x)*r2d;
quicksand 0:67f22e813b74 251 }
quicksand 0:67f22e813b74 252
quicksand 0:67f22e813b74 253 /*
quicksand 0:67f22e813b74 254 var y = Math.sin(dLon) * Math.cos(lat2);
quicksand 0:67f22e813b74 255 var x = Math.cos(lat1)*Math.sin(lat2) -
quicksand 0:67f22e813b74 256 Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
quicksand 0:67f22e813b74 257 var brng = Math.atan2(y, x).toDeg();
quicksand 0:67f22e813b74 258 */
quicksand 0:67f22e813b74 259
quicksand 0:67f22e813b74 260 /*
quicksand 0:67f22e813b74 261 The Haversine formula according to Dr. Math.
quicksand 0:67f22e813b74 262 http://mathforum.org/library/drmath/view/51879.html
quicksand 0:67f22e813b74 263
quicksand 0:67f22e813b74 264 dlon = lon2 - lon1
quicksand 0:67f22e813b74 265 dlat = lat2 - lat1
quicksand 0:67f22e813b74 266 a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
quicksand 0:67f22e813b74 267 c = 2 * atan2(sqrt(a), sqrt(1-a))
quicksand 0:67f22e813b74 268 d = R * c
quicksand 0:67f22e813b74 269
quicksand 0:67f22e813b74 270 Where
quicksand 0:67f22e813b74 271 * dlon is the change in longitude
quicksand 0:67f22e813b74 272 * dlat is the change in latitude
quicksand 0:67f22e813b74 273 * c is the great circle distance in Radians.
quicksand 0:67f22e813b74 274 * R is the radius of a spherical Earth.
quicksand 0:67f22e813b74 275 * The locations of the two points in
quicksand 0:67f22e813b74 276 spherical coordinates (longitude and
quicksand 0:67f22e813b74 277 latitude) are lon1,lat1 and lon2, lat2.
quicksand 0:67f22e813b74 278 */
quicksand 0:67f22e813b74 279 double GPS::calc_dist_to_mi(float pointLat, float pontLong) {
quicksand 0:67f22e813b74 280 const double d2r = PI / 180.0;
quicksand 0:67f22e813b74 281 double dlat = pointLat - get_dec_latitude();
quicksand 0:67f22e813b74 282 double dlong = pontLong - get_dec_longitude();
quicksand 0:67f22e813b74 283 double a = pow(sin(dlat/2.0),2.0) + cos(get_dec_latitude()*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
quicksand 0:67f22e813b74 284 double c = 2.0 * asin(sqrt(abs(a)));
quicksand 0:67f22e813b74 285 double d = 63.765 * c;
quicksand 0:67f22e813b74 286
quicksand 0:67f22e813b74 287 return d;
quicksand 0:67f22e813b74 288 }
quicksand 0:67f22e813b74 289
quicksand 0:67f22e813b74 290 double GPS::calc_dist_to_ft(float pointLat, float pontLong) {
quicksand 0:67f22e813b74 291 return calc_dist_to_mi(pointLat, pontLong)*5280.0;
quicksand 0:67f22e813b74 292 }
quicksand 0:67f22e813b74 293
quicksand 0:67f22e813b74 294 double GPS::calc_dist_to_km(float pointLat, float pontLong) {
quicksand 0:67f22e813b74 295 return calc_dist_to_mi(pointLat, pontLong)*1.609344;
quicksand 0:67f22e813b74 296 }
quicksand 0:67f22e813b74 297
quicksand 0:67f22e813b74 298 double GPS::calc_dist_to_m(float pointLat, float pontLong) {
quicksand 0:67f22e813b74 299 return calc_dist_to_mi(pointLat, pontLong)*1609.344;
quicksand 0:67f22e813b74 300 }
quicksand 0:67f22e813b74 301
quicksand 0:67f22e813b74 302