Tyler Weaver / GPS

nmea gps library

Dependents:   mDot_LoRa_Connect_CSA_Light mDot_LoRa_Connect_CSA_Rain mDot_LoRa_Connect_CSA_RH_Temp mDot_LoRa_Connect_CSA_Thermistor

GPS.cpp@1:39d75e44b214, 2012-11-23 (annotated)

Committer:
tylerjw
Date:
Fri Nov 23 19:57:17 2012 +0000
Revision:
1:39d75e44b214
Parent:
0:3611af72bfd7 
gps library for micro track gps (nmea); location correct; distance and heading incorrect

Who changed what in which revision?

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