This library (beta release) parses the GPS data coming from the TD1204. Beware, not all functionality has been fully tested.
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?
User | Revision | Line number | New 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 |