Happy Turkey Day
GPSINT.cpp@0:f14093b3b3cf, 2019-11-28 (annotated)
- Committer:
- jbeason3
- Date:
- Thu Nov 28 01:38:50 2019 +0000
- Revision:
- 0:f14093b3b3cf
Happy Turkey Day
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jbeason3 | 0:f14093b3b3cf | 1 | /* GPSINT.cpp |
jbeason3 | 0:f14093b3b3cf | 2 | * Original program from |
jbeason3 | 0:f14093b3b3cf | 3 | * jbradshaw (20141101) |
jbeason3 | 0:f14093b3b3cf | 4 | * The program is modified for ublox NEO 6M. |
jbeason3 | 0:f14093b3b3cf | 5 | */ |
jbeason3 | 0:f14093b3b3cf | 6 | |
jbeason3 | 0:f14093b3b3cf | 7 | #include "GPSINT.h" |
jbeason3 | 0:f14093b3b3cf | 8 | |
jbeason3 | 0:f14093b3b3cf | 9 | GPSINT::GPSINT(PinName tx, PinName rx) : _gps(tx, rx) { |
jbeason3 | 0:f14093b3b3cf | 10 | _gps.baud(9600); |
jbeason3 | 0:f14093b3b3cf | 11 | GPSidx=0; // Index for GPS buffer |
jbeason3 | 0:f14093b3b3cf | 12 | GPSstate=0; // Used to wait for '$' in GPS interrupt |
jbeason3 | 0:f14093b3b3cf | 13 | _gps.attach(this,&GPSINT::GPSSerialRecvInterrupt, _gps.RxIrq); // Recv interrupt handler |
jbeason3 | 0:f14093b3b3cf | 14 | } |
jbeason3 | 0:f14093b3b3cf | 15 | |
jbeason3 | 0:f14093b3b3cf | 16 | int GPSINT::nmea_validate(char *nmeastr){ |
jbeason3 | 0:f14093b3b3cf | 17 | char check[3]; |
jbeason3 | 0:f14093b3b3cf | 18 | char checkcalcstr[3]; |
jbeason3 | 0:f14093b3b3cf | 19 | int i; |
jbeason3 | 0:f14093b3b3cf | 20 | int calculated_check; |
jbeason3 | 0:f14093b3b3cf | 21 | |
jbeason3 | 0:f14093b3b3cf | 22 | i=0; |
jbeason3 | 0:f14093b3b3cf | 23 | calculated_check=0; |
jbeason3 | 0:f14093b3b3cf | 24 | |
jbeason3 | 0:f14093b3b3cf | 25 | // check to ensure that the string starts with a $ |
jbeason3 | 0:f14093b3b3cf | 26 | if(nmeastr[i] == '$') |
jbeason3 | 0:f14093b3b3cf | 27 | i++; |
jbeason3 | 0:f14093b3b3cf | 28 | else |
jbeason3 | 0:f14093b3b3cf | 29 | return 0; |
jbeason3 | 0:f14093b3b3cf | 30 | |
jbeason3 | 0:f14093b3b3cf | 31 | //No NULL reached, 75 char largest possible NMEA message, no '*' reached |
jbeason3 | 0:f14093b3b3cf | 32 | while((nmeastr[i] != 0) && (nmeastr[i] != '*') && (i < 75)){ |
jbeason3 | 0:f14093b3b3cf | 33 | calculated_check ^= nmeastr[i];// calculate the checksum |
jbeason3 | 0:f14093b3b3cf | 34 | i++; |
jbeason3 | 0:f14093b3b3cf | 35 | } |
jbeason3 | 0:f14093b3b3cf | 36 | |
jbeason3 | 0:f14093b3b3cf | 37 | if(i >= 75){ |
jbeason3 | 0:f14093b3b3cf | 38 | return 0;// the string was too long so return an error |
jbeason3 | 0:f14093b3b3cf | 39 | } |
jbeason3 | 0:f14093b3b3cf | 40 | |
jbeason3 | 0:f14093b3b3cf | 41 | if (nmeastr[i] == '*'){ |
jbeason3 | 0:f14093b3b3cf | 42 | check[0] = nmeastr[i+1]; //put hex chars in check string |
jbeason3 | 0:f14093b3b3cf | 43 | check[1] = nmeastr[i+2]; |
jbeason3 | 0:f14093b3b3cf | 44 | check[2] = 0; |
jbeason3 | 0:f14093b3b3cf | 45 | } |
jbeason3 | 0:f14093b3b3cf | 46 | else |
jbeason3 | 0:f14093b3b3cf | 47 | return 0;// no checksum separator found therefor invalid |
jbeason3 | 0:f14093b3b3cf | 48 | |
jbeason3 | 0:f14093b3b3cf | 49 | sprintf(checkcalcstr,"%02X",calculated_check); |
jbeason3 | 0:f14093b3b3cf | 50 | return((checkcalcstr[0] == check[0]) |
jbeason3 | 0:f14093b3b3cf | 51 | && (checkcalcstr[1] == check[1])) ? 1 : 0 ; |
jbeason3 | 0:f14093b3b3cf | 52 | } |
jbeason3 | 0:f14093b3b3cf | 53 | |
jbeason3 | 0:f14093b3b3cf | 54 | void GPSINT::parseGPSString(char *GPSstrParse){ |
jbeason3 | 0:f14093b3b3cf | 55 | //check if $GPGGA string |
jbeason3 | 0:f14093b3b3cf | 56 | if(!strncmp(GPSstrParse, "$GPGGA", 6)){ |
jbeason3 | 0:f14093b3b3cf | 57 | if (sscanf(GPSstrParse, "$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) { |
jbeason3 | 0:f14093b3b3cf | 58 | // printf("%s", GPSstrParse); |
jbeason3 | 0:f14093b3b3cf | 59 | return; |
jbeason3 | 0:f14093b3b3cf | 60 | } |
jbeason3 | 0:f14093b3b3cf | 61 | else{ |
jbeason3 | 0:f14093b3b3cf | 62 | // printf("BAD parse %s", GPSstrParse); |
jbeason3 | 0:f14093b3b3cf | 63 | } |
jbeason3 | 0:f14093b3b3cf | 64 | } |
jbeason3 | 0:f14093b3b3cf | 65 | // Check if $GPRMC string |
jbeason3 | 0:f14093b3b3cf | 66 | else if (!strncmp(GPSstrParse, "$GPRMC", 6)){ |
jbeason3 | 0:f14093b3b3cf | 67 | if(sscanf(GPSstrParse, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,%d", &utc_time, &status,&nmea_latitude, &ns, &nmea_longitude, &ew, &speed_k, &course_d, &date) >= 1) { |
jbeason3 | 0:f14093b3b3cf | 68 | // printf("%s", GPSstrParse); |
jbeason3 | 0:f14093b3b3cf | 69 | return; |
jbeason3 | 0:f14093b3b3cf | 70 | } |
jbeason3 | 0:f14093b3b3cf | 71 | else{ |
jbeason3 | 0:f14093b3b3cf | 72 | // printf("BAD parse %s", GPSstrParse); |
jbeason3 | 0:f14093b3b3cf | 73 | } |
jbeason3 | 0:f14093b3b3cf | 74 | } |
jbeason3 | 0:f14093b3b3cf | 75 | // GLL - Geographic Position-Lat/Lon |
jbeason3 | 0:f14093b3b3cf | 76 | else if (!strncmp(GPSstrParse, "$GPGLL", 6)){ |
jbeason3 | 0:f14093b3b3cf | 77 | if(sscanf(GPSstrParse, "$GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) { |
jbeason3 | 0:f14093b3b3cf | 78 | // printf("%s", GPSstrParse); |
jbeason3 | 0:f14093b3b3cf | 79 | return; |
jbeason3 | 0:f14093b3b3cf | 80 | } |
jbeason3 | 0:f14093b3b3cf | 81 | else{ |
jbeason3 | 0:f14093b3b3cf | 82 | // printf("BAD parse %s", GPSstrParse); |
jbeason3 | 0:f14093b3b3cf | 83 | } |
jbeason3 | 0:f14093b3b3cf | 84 | } |
jbeason3 | 0:f14093b3b3cf | 85 | // VTG-Course Over Ground and Ground Speed |
jbeason3 | 0:f14093b3b3cf | 86 | else if (!strncmp(GPSstrParse, "$GPVTG", 6)){ |
jbeason3 | 0:f14093b3b3cf | 87 | if(sscanf(GPSstrParse, "$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) { |
jbeason3 | 0:f14093b3b3cf | 88 | // printf("%s", GPSstrParse); |
jbeason3 | 0:f14093b3b3cf | 89 | return; |
jbeason3 | 0:f14093b3b3cf | 90 | } |
jbeason3 | 0:f14093b3b3cf | 91 | else{ |
jbeason3 | 0:f14093b3b3cf | 92 | // printf("BAD parse %s", GPSstrParse); |
jbeason3 | 0:f14093b3b3cf | 93 | } |
jbeason3 | 0:f14093b3b3cf | 94 | } |
jbeason3 | 0:f14093b3b3cf | 95 | }//parseGPSstring() |
jbeason3 | 0:f14093b3b3cf | 96 | |
jbeason3 | 0:f14093b3b3cf | 97 | void GPSINT::GPSSerialRecvInterrupt(void) |
jbeason3 | 0:f14093b3b3cf | 98 | { |
jbeason3 | 0:f14093b3b3cf | 99 | char c; |
jbeason3 | 0:f14093b3b3cf | 100 | c =_gps.getc(); // On receive interrupt, get the character. |
jbeason3 | 0:f14093b3b3cf | 101 | // pc.printf("%c", c); |
jbeason3 | 0:f14093b3b3cf | 102 | |
jbeason3 | 0:f14093b3b3cf | 103 | switch(GPSstate){ |
jbeason3 | 0:f14093b3b3cf | 104 | case 0: |
jbeason3 | 0:f14093b3b3cf | 105 | if(c =='$'){ |
jbeason3 | 0:f14093b3b3cf | 106 | GPSidx=0; |
jbeason3 | 0:f14093b3b3cf | 107 | Temp_GPSbuf[GPSidx] = c; //load char in current idx of array |
jbeason3 | 0:f14093b3b3cf | 108 | GPSidx++; |
jbeason3 | 0:f14093b3b3cf | 109 | GPSstate = 1; |
jbeason3 | 0:f14093b3b3cf | 110 | } |
jbeason3 | 0:f14093b3b3cf | 111 | break; |
jbeason3 | 0:f14093b3b3cf | 112 | case 1: |
jbeason3 | 0:f14093b3b3cf | 113 | Temp_GPSbuf[GPSidx] = c; //load char in current idx of array |
jbeason3 | 0:f14093b3b3cf | 114 | GPSidx++; |
jbeason3 | 0:f14093b3b3cf | 115 | if(c == '\n'){ //if last char was a newline |
jbeason3 | 0:f14093b3b3cf | 116 | Temp_GPSbuf[GPSidx] = '\0'; //append a NULL |
jbeason3 | 0:f14093b3b3cf | 117 | strcpy(GPSbuf, Temp_GPSbuf); //copy temp buf into GPS buf |
jbeason3 | 0:f14093b3b3cf | 118 | GPSidx=0; //reset index |
jbeason3 | 0:f14093b3b3cf | 119 | GPSstate = 0; //reset GPS state |
jbeason3 | 0:f14093b3b3cf | 120 | if(nmea_validate(GPSbuf)){ |
jbeason3 | 0:f14093b3b3cf | 121 | parseGPSString(GPSbuf); |
jbeason3 | 0:f14093b3b3cf | 122 | } |
jbeason3 | 0:f14093b3b3cf | 123 | } |
jbeason3 | 0:f14093b3b3cf | 124 | break; |
jbeason3 | 0:f14093b3b3cf | 125 | |
jbeason3 | 0:f14093b3b3cf | 126 | default: |
jbeason3 | 0:f14093b3b3cf | 127 | break; |
jbeason3 | 0:f14093b3b3cf | 128 | |
jbeason3 | 0:f14093b3b3cf | 129 | }//switch state |
jbeason3 | 0:f14093b3b3cf | 130 | } |
jbeason3 | 0:f14093b3b3cf | 131 | |
jbeason3 | 0:f14093b3b3cf | 132 | float GPSINT::nmea_to_dec(float deg_coord, char nsew) { |
jbeason3 | 0:f14093b3b3cf | 133 | int degree = (int)(deg_coord/100); |
jbeason3 | 0:f14093b3b3cf | 134 | float minutes = deg_coord - degree*100; |
jbeason3 | 0:f14093b3b3cf | 135 | float dec_deg = minutes / 60; |
jbeason3 | 0:f14093b3b3cf | 136 | float decimal = degree + dec_deg; |
jbeason3 | 0:f14093b3b3cf | 137 | if (nsew == 'S' || nsew == 'W') { // return negative |
jbeason3 | 0:f14093b3b3cf | 138 | decimal *= -1; |
jbeason3 | 0:f14093b3b3cf | 139 | } |
jbeason3 | 0:f14093b3b3cf | 140 | return decimal; |
jbeason3 | 0:f14093b3b3cf | 141 | } |
jbeason3 | 0:f14093b3b3cf | 142 | |
jbeason3 | 0:f14093b3b3cf | 143 | // NAVIGATION FUNCTIONS //////////////////////////////////////////////////////////// |
jbeason3 | 0:f14093b3b3cf | 144 | float GPSINT::calc_course_to(float pointLat, float pontLong) { |
jbeason3 | 0:f14093b3b3cf | 145 | const double d2r = PI / 180.0; |
jbeason3 | 0:f14093b3b3cf | 146 | const double r2d = 180.0 / PI; |
jbeason3 | 0:f14093b3b3cf | 147 | double dlat = abs(pointLat - dec_latitude) * d2r; |
jbeason3 | 0:f14093b3b3cf | 148 | double dlong = abs(pontLong - dec_longitude) * d2r; |
jbeason3 | 0:f14093b3b3cf | 149 | double y = sin(dlong) * cos(pointLat * d2r); |
jbeason3 | 0:f14093b3b3cf | 150 | double x = cos(dec_latitude*d2r)*sin(pointLat*d2r) - sin(dec_latitude*d2r)*cos(pointLat*d2r)*cos(dlong); |
jbeason3 | 0:f14093b3b3cf | 151 | return atan2(y,x)*r2d; |
jbeason3 | 0:f14093b3b3cf | 152 | } |
jbeason3 | 0:f14093b3b3cf | 153 | |
jbeason3 | 0:f14093b3b3cf | 154 | /* |
jbeason3 | 0:f14093b3b3cf | 155 | var y = Math.sin(dLon) * Math.cos(lat2); |
jbeason3 | 0:f14093b3b3cf | 156 | var x = Math.cos(lat1)*Math.sin(lat2) - |
jbeason3 | 0:f14093b3b3cf | 157 | Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); |
jbeason3 | 0:f14093b3b3cf | 158 | var brng = Math.atan2(y, x).toDeg(); |
jbeason3 | 0:f14093b3b3cf | 159 | */ |
jbeason3 | 0:f14093b3b3cf | 160 | |
jbeason3 | 0:f14093b3b3cf | 161 | /* |
jbeason3 | 0:f14093b3b3cf | 162 | The Haversine formula according to Dr. Math. |
jbeason3 | 0:f14093b3b3cf | 163 | http://mathforum.org/library/drmath/view/51879.html |
jbeason3 | 0:f14093b3b3cf | 164 | |
jbeason3 | 0:f14093b3b3cf | 165 | dlon = lon2 - lon1 |
jbeason3 | 0:f14093b3b3cf | 166 | dlat = lat2 - lat1 |
jbeason3 | 0:f14093b3b3cf | 167 | a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2 |
jbeason3 | 0:f14093b3b3cf | 168 | c = 2 * atan2(sqrt(a), sqrt(1-a)) |
jbeason3 | 0:f14093b3b3cf | 169 | d = R * c |
jbeason3 | 0:f14093b3b3cf | 170 | |
jbeason3 | 0:f14093b3b3cf | 171 | Where |
jbeason3 | 0:f14093b3b3cf | 172 | * dlon is the change in longitude |
jbeason3 | 0:f14093b3b3cf | 173 | * dlat is the change in latitude |
jbeason3 | 0:f14093b3b3cf | 174 | * c is the great circle distance in Radians. |
jbeason3 | 0:f14093b3b3cf | 175 | * R is the radius of a spherical Earth. |
jbeason3 | 0:f14093b3b3cf | 176 | * The locations of the two points in |
jbeason3 | 0:f14093b3b3cf | 177 | spherical coordinates (longitude and |
jbeason3 | 0:f14093b3b3cf | 178 | latitude) are lon1,lat1 and lon2, lat2. |
jbeason3 | 0:f14093b3b3cf | 179 | */ |
jbeason3 | 0:f14093b3b3cf | 180 | double GPSINT::calc_dist_to_mi(float pointLat, float pontLong) { |
jbeason3 | 0:f14093b3b3cf | 181 | const double d2r = PI / 180.0; |
jbeason3 | 0:f14093b3b3cf | 182 | double dlat = pointLat - dec_latitude; |
jbeason3 | 0:f14093b3b3cf | 183 | double dlong = pontLong - dec_longitude; |
jbeason3 | 0:f14093b3b3cf | 184 | double a = pow(sin(dlat/2.0),2.0) + cos(dec_latitude*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0); |
jbeason3 | 0:f14093b3b3cf | 185 | double c = 2.0 * asin(sqrt(abs(a))); |
jbeason3 | 0:f14093b3b3cf | 186 | double d = 63.765 * c; |
jbeason3 | 0:f14093b3b3cf | 187 | |
jbeason3 | 0:f14093b3b3cf | 188 | return d; |
jbeason3 | 0:f14093b3b3cf | 189 | } |
jbeason3 | 0:f14093b3b3cf | 190 | |
jbeason3 | 0:f14093b3b3cf | 191 | double GPSINT::calc_dist_to_ft(float pointLat, float pontLong) { |
jbeason3 | 0:f14093b3b3cf | 192 | return calc_dist_to_mi(pointLat, pontLong)*5280.0; |
jbeason3 | 0:f14093b3b3cf | 193 | } |
jbeason3 | 0:f14093b3b3cf | 194 | |
jbeason3 | 0:f14093b3b3cf | 195 | double GPSINT::calc_dist_to_km(float pointLat, float pontLong) { |
jbeason3 | 0:f14093b3b3cf | 196 | return calc_dist_to_mi(pointLat, pontLong)*1.609344; |
jbeason3 | 0:f14093b3b3cf | 197 | } |
jbeason3 | 0:f14093b3b3cf | 198 | |
jbeason3 | 0:f14093b3b3cf | 199 | double GPSINT::calc_dist_to_m(float pointLat, float pontLong) { |
jbeason3 | 0:f14093b3b3cf | 200 | return calc_dist_to_mi(pointLat, pontLong)*1609.344; |
jbeason3 | 0:f14093b3b3cf | 201 | } |