Leon Wehmeier / Mbed OS fiasco_max32630

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   SoftSerial MAX14690 Buffer

Fork of rtos_threading_with_callback by mbed_example

GPS/GPS.cpp@3:d7ec6dc025b0, 2018-03-30 (annotated)

Committer:
lwehmeier
Date:
Fri Mar 30 10:32:10 2018 +0000
Revision:
3:d7ec6dc025b0
implemented basic GPS read support, freed serial connection for uart jpg camera module

Who changed what in which revision?

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