1 /* gpsutils.c -- code shared between low-level and high-level interfaces
2 *
3 * This file is Copyright (c) 2010-2018 by the GPSD project
4 * SPDX-License-Identifier: BSD-2-clause
5 */
6
7 /* The strptime prototype is not provided unless explicitly requested.
8 * We also need to set the value high enough to signal inclusion of
9 * newer features (like clock_gettime). See the POSIX spec for more info:
10 * http://pubs.opengroup.org/onlinepubs/9699919799/functions/V2_chap02.html#tag_15_02_01_02 */
11
12 #include "gpsd_config.h" /* must be before all includes */
13
14 #include <ctype.h>
15 #include <errno.h>
16 #include <math.h>
17 #include <stdio.h>
18 #include <stdlib.h>
19 #include <string.h>
20 #include <sys/select.h> /* for to have a pselect(2) prototype a la POSIX */
21 #include <sys/time.h>
22 #include <sys/time.h> /* for to have a pselect(2) prototype a la SuS */
23 #include <time.h>
24
25 #include "gps.h"
26 #include "libgps.h"
27 #include "os_compat.h"
28 #include "timespec.h"
29
30 #ifdef USE_QT
31 #include <QDateTime>
32 #include <QStringList>
33 #endif
34
35 /*
36 * Berkeley implementation of strtod(), inlined to avoid locale problems
37 * with the decimal point and stripped down to an atof()-equivalent.
38 */
39
40 /* Takes a decimal ASCII floating-point number, optionally
41 * preceded by white space. Must have form "-I.FE-X",
42 * where I is the integer part of the mantissa, F is
43 * the fractional part of the mantissa, and X is the
44 * exponent. Either of the signs may be "+", "-", or
45 * omitted. Either I or F may be omitted, or both.
46 * The decimal point isn't necessary unless F is
47 * present. The "E" may actually be an "e". E and X
48 * may both be omitted (but not just one).
49 *
50 * returns NaN if *string is zero length
51 */
safe_atof(const char * string)52 double safe_atof(const char *string)
53 {
54 static int maxExponent = 511; /* Largest possible base 10 exponent. Any
55 * exponent larger than this will already
56 * produce underflow or overflow, so there's
57 * no need to worry about additional digits.
58 */
59 static double powersOf10[] = { /* Table giving binary powers of 10. Entry */
60 10., /* is 10^2^i. Used to convert decimal */
61 100., /* exponents into floating-point numbers. */
62 1.0e4,
63 1.0e8,
64 1.0e16,
65 1.0e32,
66 1.0e64,
67 1.0e128,
68 1.0e256
69 };
70
71 bool sign, expSign = false;
72 double fraction, dblExp, *d;
73 const char *p;
74 int c;
75 int exp = 0; /* Exponent read from "EX" field. */
76 int fracExp = 0; /* Exponent that derives from the fractional
77 * part. Under normal circumstatnces, it is
78 * the negative of the number of digits in F.
79 * However, if I is very long, the last digits
80 * of I get dropped (otherwise a long I with a
81 * large negative exponent could cause an
82 * unnecessary overflow on I alone). In this
83 * case, fracExp is incremented one for each
84 * dropped digit. */
85 int mantSize; /* Number of digits in mantissa. */
86 int decPt; /* Number of mantissa digits BEFORE decimal
87 * point. */
88 const char *pExp; /* Temporarily holds location of exponent
89 * in string. */
90
91 /*
92 * Strip off leading blanks and check for a sign.
93 */
94
95 p = string;
96 while (isspace((unsigned char) *p)) {
97 p += 1;
98 }
99 if (*p == '\0') {
100 return NAN;
101 } else if (*p == '-') {
102 sign = true;
103 p += 1;
104 } else {
105 if (*p == '+') {
106 p += 1;
107 }
108 sign = false;
109 }
110
111 /*
112 * Count the number of digits in the mantissa (including the decimal
113 * point), and also locate the decimal point.
114 */
115
116 decPt = -1;
117 for (mantSize = 0; ; mantSize += 1)
118 {
119 c = *p;
120 if (!isdigit(c)) {
121 if ((c != '.') || (decPt >= 0)) {
122 break;
123 }
124 decPt = mantSize;
125 }
126 p += 1;
127 }
128
129 /*
130 * Now suck up the digits in the mantissa. Use two integers to
131 * collect 9 digits each (this is faster than using floating-point).
132 * If the mantissa has more than 18 digits, ignore the extras, since
133 * they can't affect the value anyway.
134 */
135
136 pExp = p;
137 p -= mantSize;
138 if (decPt < 0) {
139 decPt = mantSize;
140 } else {
141 mantSize -= 1; /* One of the digits was the point. */
142 }
143 if (mantSize > 18) {
144 fracExp = decPt - 18;
145 mantSize = 18;
146 } else {
147 fracExp = decPt - mantSize;
148 }
149 if (mantSize == 0) {
150 fraction = 0.0;
151 //p = string;
152 goto done;
153 } else {
154 int frac1, frac2;
155 frac1 = 0;
156 for ( ; mantSize > 9; mantSize -= 1)
157 {
158 c = *p;
159 p += 1;
160 if (c == '.') {
161 c = *p;
162 p += 1;
163 }
164 frac1 = 10*frac1 + (c - '0');
165 }
166 frac2 = 0;
167 for (; mantSize > 0; mantSize -= 1)
168 {
169 c = *p;
170 p += 1;
171 if (c == '.') {
172 c = *p;
173 p += 1;
174 }
175 frac2 = 10*frac2 + (c - '0');
176 }
177 fraction = (1.0e9 * frac1) + frac2;
178 }
179
180 /*
181 * Skim off the exponent.
182 */
183
184 p = pExp;
185 if ((*p == 'E') || (*p == 'e')) {
186 p += 1;
187 if (*p == '-') {
188 expSign = true;
189 p += 1;
190 } else {
191 if (*p == '+') {
192 p += 1;
193 }
194 expSign = false;
195 }
196 while (isdigit((unsigned char) *p)) {
197 exp = exp * 10 + (*p - '0');
198 p += 1;
199 }
200 }
201 if (expSign) {
202 exp = fracExp - exp;
203 } else {
204 exp = fracExp + exp;
205 }
206
207 /*
208 * Generate a floating-point number that represents the exponent.
209 * Do this by processing the exponent one bit at a time to combine
210 * many powers of 2 of 10. Then combine the exponent with the
211 * fraction.
212 */
213
214 if (exp < 0) {
215 expSign = true;
216 exp = -exp;
217 } else {
218 expSign = false;
219 }
220 if (exp > maxExponent) {
221 exp = maxExponent;
222 errno = ERANGE;
223 }
224 dblExp = 1.0;
225 for (d = powersOf10; exp != 0; exp >>= 1, d += 1) {
226 if (exp & 01) {
227 dblExp *= *d;
228 }
229 }
230 if (expSign) {
231 fraction /= dblExp;
232 } else {
233 fraction *= dblExp;
234 }
235
236 done:
237 if (sign) {
238 return -fraction;
239 }
240 return fraction;
241 }
242
243 #define MONTHSPERYEAR 12 /* months per calendar year */
244
gps_clear_fix(struct gps_fix_t * fixp)245 void gps_clear_fix(struct gps_fix_t *fixp)
246 /* stuff a fix structure with recognizable out-of-band values */
247 {
248 memset(fixp, 0, sizeof(struct gps_fix_t));
249 fixp->altitude = NAN; // DEPRECATED, undefined
250 fixp->altHAE = NAN;
251 fixp->altMSL = NAN;
252 fixp->climb = NAN;
253 fixp->depth = NAN;
254 fixp->epc = NAN;
255 fixp->epd = NAN;
256 fixp->eph = NAN;
257 fixp->eps = NAN;
258 fixp->ept = NAN;
259 fixp->epv = NAN;
260 fixp->epx = NAN;
261 fixp->epy = NAN;
262 fixp->latitude = NAN;
263 fixp->longitude = NAN;
264 fixp->magnetic_track = NAN;
265 fixp->magnetic_var = NAN;
266 fixp->mode = MODE_NOT_SEEN;
267 fixp->sep = NAN;
268 fixp->speed = NAN;
269 fixp->track = NAN;
270 /* clear ECEF too */
271 fixp->ecef.x = NAN;
272 fixp->ecef.y = NAN;
273 fixp->ecef.z = NAN;
274 fixp->ecef.vx = NAN;
275 fixp->ecef.vy = NAN;
276 fixp->ecef.vz = NAN;
277 fixp->ecef.pAcc = NAN;
278 fixp->ecef.vAcc = NAN;
279 fixp->NED.relPosN = NAN;
280 fixp->NED.relPosE = NAN;
281 fixp->NED.relPosD = NAN;
282 fixp->NED.velN = NAN;
283 fixp->NED.velE = NAN;
284 fixp->NED.velD = NAN;
285 fixp->geoid_sep = NAN;
286 fixp->dgps_age = NAN;
287 fixp->dgps_station = -1;
288 }
289
gps_clear_att(struct attitude_t * attp)290 void gps_clear_att(struct attitude_t *attp)
291 /* stuff an attitude structure with recognizable out-of-band values */
292 {
293 memset(attp, 0, sizeof(struct attitude_t));
294 attp->pitch = NAN;
295 attp->roll = NAN;
296 attp->yaw = NAN;
297 attp->dip = NAN;
298 attp->mag_len = NAN;
299 attp->mag_x = NAN;
300 attp->mag_y = NAN;
301 attp->mag_z = NAN;
302 attp->acc_len = NAN;
303 attp->acc_x = NAN;
304 attp->acc_y = NAN;
305 attp->acc_z = NAN;
306 attp->gyro_x = NAN;
307 attp->gyro_y = NAN;
308 attp->temp = NAN;
309 attp->depth = NAN;
310 }
311
gps_clear_dop(struct dop_t * dop)312 void gps_clear_dop( struct dop_t *dop)
313 {
314 dop->xdop = dop->ydop = dop->vdop = dop->tdop = dop->hdop = dop->pdop =
315 dop->gdop = NAN;
316 }
317
gps_merge_fix(struct gps_fix_t * to,gps_mask_t transfer,struct gps_fix_t * from)318 void gps_merge_fix(struct gps_fix_t *to,
319 gps_mask_t transfer,
320 struct gps_fix_t *from)
321 /* merge new data into an old fix */
322 {
323 if ((NULL == to) || (NULL == from))
324 return;
325 if ((transfer & TIME_SET) != 0)
326 to->time = from->time;
327 if ((transfer & LATLON_SET) != 0) {
328 to->latitude = from->latitude;
329 to->longitude = from->longitude;
330 }
331 if ((transfer & MODE_SET) != 0)
332 to->mode = from->mode;
333 if ((transfer & ALTITUDE_SET) != 0) {
334 if (0 != isfinite(from->altHAE)) {
335 to->altHAE = from->altHAE;
336 }
337 if (0 != isfinite(from->altMSL)) {
338 to->altMSL = from->altMSL;
339 }
340 if (0 != isfinite(from->depth)) {
341 to->depth = from->depth;
342 }
343 }
344 if ((transfer & TRACK_SET) != 0)
345 to->track = from->track;
346 if ((transfer & MAGNETIC_TRACK_SET) != 0) {
347 if (0 != isfinite(from->magnetic_track)) {
348 to->magnetic_track = from->magnetic_track;
349 }
350 if (0 != isfinite(from->magnetic_var)) {
351 to->magnetic_var = from->magnetic_var;
352 }
353 }
354 if ((transfer & SPEED_SET) != 0)
355 to->speed = from->speed;
356 if ((transfer & CLIMB_SET) != 0)
357 to->climb = from->climb;
358 if ((transfer & TIMERR_SET) != 0)
359 to->ept = from->ept;
360 if (0 != isfinite(from->epx) &&
361 0 != isfinite(from->epy)) {
362 to->epx = from->epx;
363 to->epy = from->epy;
364 }
365 if (0 != isfinite(from->epd)) {
366 to->epd = from->epd;
367 }
368 if (0 != isfinite(from->eph)) {
369 to->eph = from->eph;
370 }
371 if (0 != isfinite(from->eps)) {
372 to->eps = from->eps;
373 }
374 /* spherical error probability, not geoid separation */
375 if (0 != isfinite(from->sep)) {
376 to->sep = from->sep;
377 }
378 /* geoid separation, not spherical error probability */
379 if (0 != isfinite(from->geoid_sep)) {
380 to->geoid_sep = from->geoid_sep;
381 }
382 if (0 != isfinite(from->epv)) {
383 to->epv = from->epv;
384 }
385 if ((transfer & SPEEDERR_SET) != 0)
386 to->eps = from->eps;
387 if ((transfer & ECEF_SET) != 0) {
388 to->ecef.x = from->ecef.x;
389 to->ecef.y = from->ecef.y;
390 to->ecef.z = from->ecef.z;
391 to->ecef.pAcc = from->ecef.pAcc;
392 }
393 if ((transfer & VECEF_SET) != 0) {
394 to->ecef.vx = from->ecef.vx;
395 to->ecef.vy = from->ecef.vy;
396 to->ecef.vz = from->ecef.vz;
397 to->ecef.vAcc = from->ecef.vAcc;
398 }
399 if ((transfer & NED_SET) != 0) {
400 to->NED.relPosN = from->NED.relPosN;
401 to->NED.relPosE = from->NED.relPosE;
402 to->NED.relPosD = from->NED.relPosD;
403 }
404 if ((transfer & VNED_SET) != 0) {
405 to->NED.velN = from->NED.velN;
406 to->NED.velE = from->NED.velE;
407 to->NED.velD = from->NED.velD;
408 }
409 if ('\0' != from->datum[0]) {
410 strlcpy(to->datum, from->datum, sizeof(to->datum));
411 }
412 if (0 != isfinite(from->dgps_age) &&
413 0 <= from->dgps_station) {
414 /* both, or neither */
415 to->dgps_age = from->dgps_age;
416 to->dgps_station = from->dgps_station;
417 }
418 }
419
420 /* mkgmtime(tm)
421 * convert struct tm, as UTC, to seconds since Unix epoch
422 * This differs from mktime() from libc.
423 * mktime() takes struct tm as localtime.
424 *
425 * The inverse of gmtime(time_t)
426 */
mkgmtime(struct tm * t)427 time_t mkgmtime(struct tm * t)
428 {
429 int year;
430 time_t result;
431 static const int cumdays[MONTHSPERYEAR] =
432 { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334 };
433
434 year = 1900 + t->tm_year + t->tm_mon / MONTHSPERYEAR;
435 result = (year - 1970) * 365 + cumdays[t->tm_mon % MONTHSPERYEAR];
436 result += (year - 1968) / 4;
437 result -= (year - 1900) / 100;
438 result += (year - 1600) / 400;
439 if ((year % 4) == 0 && ((year % 100) != 0 || (year % 400) == 0) &&
440 (t->tm_mon % MONTHSPERYEAR) < 2)
441 result--;
442 result += t->tm_mday - 1;
443 result *= 24;
444 result += t->tm_hour;
445 result *= 60;
446 result += t->tm_min;
447 result *= 60;
448 result += t->tm_sec;
449 /* this is UTC, no DST
450 * if (t->tm_isdst == 1)
451 * result -= 3600;
452 */
453 return (result);
454 }
455
iso8601_to_timespec(char * isotime)456 timespec_t iso8601_to_timespec(char *isotime)
457 /* ISO8601 UTC to Unix timespec, no leapsecond correction. */
458 {
459 timespec_t ret;
460
461 #ifndef __clang_analyzer__
462 #ifndef USE_QT
463 char *dp = NULL;
464 double usec = 0;
465 struct tm tm;
466 memset(&tm,0,sizeof(tm));
467
468 #ifdef HAVE_STRPTIME
469 dp = strptime(isotime, "%Y-%m-%dT%H:%M:%S", &tm);
470 #else
471 /* Fallback for systems without strptime (i.e. Windows)
472 This is a simplistic conversion for iso8601 strings only,
473 rather than embedding a full copy of strptime() that handles all formats */
474 double sec;
475 unsigned int tmp; // Thus avoiding needing to test for (broken) negative date/time numbers in token reading - only need to check the upper range
476 bool failed = false;
477 char *isotime_tokenizer = strdup(isotime);
478 if (isotime_tokenizer) {
479 char *tmpbuf;
480 char *pch = strtok_r(isotime_tokenizer, "-T:", &tmpbuf);
481 int token_number = 0;
482 while (pch != NULL) {
483 token_number++;
484 // Give up if encountered way too many tokens.
485 if (token_number > 10) {
486 failed = true;
487 break;
488 }
489 switch (token_number) {
490 case 1: // Year token
491 tmp = atoi(pch);
492 if (tmp < 9999)
493 tm.tm_year = tmp - 1900; // Adjust to tm year
494 else
495 failed = true;
496 break;
497 case 2: // Month token
498 tmp = atoi(pch);
499 if (tmp < 13)
500 tm.tm_mon = tmp - 1; // Month indexing starts from zero
501 else
502 failed = true;
503 break;
504 case 3: // Day token
505 tmp = atoi(pch);
506 if (tmp < 32)
507 tm.tm_mday = tmp;
508 else
509 failed = true;
510 break;
511 case 4: // Hour token
512 tmp = atoi(pch);
513 if (tmp < 24)
514 tm.tm_hour = tmp;
515 else
516 failed = true;
517 break;
518 case 5: // Minute token
519 tmp = atoi(pch);
520 if (tmp < 60)
521 tm.tm_min = tmp;
522 else
523 failed = true;
524 break;
525 case 6: // Seconds token
526 sec = safe_atof(pch);
527 // NB To handle timestamps with leap seconds
528 if (0 == isfinite(sec) &&
529 sec >= 0.0 && sec < 61.5 ) {
530 tm.tm_sec = (unsigned int)sec; // Truncate to get integer value
531 usec = sec - (unsigned int)sec; // Get the fractional part (if any)
532 }
533 else
534 failed = true;
535 break;
536 default: break;
537 }
538 pch = strtok_r(NULL, "-T:", &tmpbuf);
539 }
540 free(isotime_tokenizer);
541 // Split may result in more than 6 tokens if the TZ has any t's in it
542 // So check that we've seen enough tokens rather than an exact number
543 if (token_number < 6)
544 failed = true;
545 }
546 if (failed)
547 memset(&tm,0,sizeof(tm));
548 else {
549 // When successful this normalizes tm so that tm_yday is set
550 // and thus tm is valid for use with other functions
551 if (mktime(&tm) == (time_t)-1)
552 // Failed mktime - so reset the timestamp
553 memset(&tm,0,sizeof(tm));
554 }
555 #endif
556 if (dp != NULL && *dp == '.')
557 usec = strtod(dp, NULL);
558 /*
559 * It would be nice if we could say mktime(&tm) - timezone + usec instead,
560 * but timezone is not available at all on some BSDs. Besides, when working
561 * with historical dates the value of timezone after an ordinary tzset(3)
562 * can be wrong; you have to do a redirect through the IANA historical
563 * timezone database to get it right.
564 */
565 ret.tv_sec = mkgmtime(&tm);
566 ret.tv_nsec = usec * 1e9;;
567 #else
568 double usec = 0;
569
570 QString t(isotime);
571 QDateTime d = QDateTime::fromString(isotime, Qt::ISODate);
572 QStringList sl = t.split(".");
573 if (sl.size() > 1)
574 usec = sl[1].toInt() / pow(10., (double)sl[1].size());
575 ret.tv_sec = d.toTime_t();
576 ret.tv_nsec = usec * 1e9;;
577 #endif
578 #endif /* __clang_analyzer__ */
579 return ret;
580 }
581
582 /* Unix timespec UTC time to ISO8601, no timezone adjustment */
583 /* example: 2007-12-11T23:38:51.033Z */
timespec_to_iso8601(timespec_t fixtime,char isotime[],size_t len)584 char *timespec_to_iso8601(timespec_t fixtime, char isotime[], size_t len)
585 {
586 struct tm when;
587 char timestr[30];
588 long fracsec;
589
590 if (0 > fixtime.tv_sec) {
591 // Allow 0 for testing of 1970-01-01T00:00:00.000Z
592 return strncpy(isotime, "NaN", len);
593 }
594 if (999499999 < fixtime.tv_nsec) {
595 /* round up */
596 fixtime.tv_sec++;
597 fixtime.tv_nsec = 0;
598 }
599 #ifdef HAVE_GMTIME_R
600 (void)gmtime_r(&fixtime.tv_sec, &when);
601 #else
602 /* Fallback to try with gmtime_s - primarily for Windows */
603 (void)gmtime_s(&when, &fixtime.tv_sec);
604 #endif
605
606 /*
607 * Do not mess casually with the number of decimal digits in the
608 * format! Most GPSes report over serial links at 0.01s or 0.001s
609 * precision. Round to 0.001s
610 */
611 fracsec = (fixtime.tv_nsec + 500000) / 1000000;
612
613 (void)strftime(timestr, sizeof(timestr), "%Y-%m-%dT%H:%M:%S", &when);
614 (void)snprintf(isotime, len, "%s.%03ldZ",timestr, fracsec);
615
616 return isotime;
617 }
618
619 /* return time now as ISO8601, no timezone adjustment */
620 /* example: 2007-12-11T23:38:51.033Z */
now_to_iso8601(char * tbuf,size_t tbuf_sz)621 char *now_to_iso8601(char *tbuf, size_t tbuf_sz)
622 {
623 timespec_t ts_now;
624
625 (void)clock_gettime(CLOCK_REALTIME, &ts_now);
626 return timespec_to_iso8601(ts_now, tbuf, tbuf_sz);
627 }
628
629 #define Deg2Rad(n) ((n) * DEG_2_RAD)
630
631 /* Distance in meters between two points specified in degrees, optionally
632 with initial and final bearings. */
earth_distance_and_bearings(double lat1,double lon1,double lat2,double lon2,double * ib,double * fb)633 double earth_distance_and_bearings(double lat1, double lon1, double lat2, double lon2, double *ib, double *fb)
634 {
635 /*
636 * this is a translation of the javascript implementation of the
637 * Vincenty distance formula by Chris Veness. See
638 * http://www.movable-type.co.uk/scripts/latlong-vincenty.html
639 */
640 double a, b, f; // WGS-84 ellipsoid params
641 double L, L_P, U1, U2, s_U1, c_U1, s_U2, c_U2;
642 double uSq, A, B, d_S, lambda;
643 // cppcheck-suppress variableScope
644 double s_L, c_L, s_A, C;
645 double c_S, S, s_S, c_SqA, c_2SM;
646 int i = 100;
647
648 a = WGS84A;
649 b = WGS84B;
650 f = 1 / WGS84F;
651 L = Deg2Rad(lon2 - lon1);
652 U1 = atan((1 - f) * tan(Deg2Rad(lat1)));
653 U2 = atan((1 - f) * tan(Deg2Rad(lat2)));
654 s_U1 = sin(U1);
655 c_U1 = cos(U1);
656 s_U2 = sin(U2);
657 c_U2 = cos(U2);
658 lambda = L;
659
660 do {
661 s_L = sin(lambda);
662 c_L = cos(lambda);
663 s_S = sqrt((c_U2 * s_L) * (c_U2 * s_L) +
664 (c_U1 * s_U2 - s_U1 * c_U2 * c_L) *
665 (c_U1 * s_U2 - s_U1 * c_U2 * c_L));
666
667 if (s_S == 0)
668 return 0;
669
670 c_S = s_U1 * s_U2 + c_U1 * c_U2 * c_L;
671 S = atan2(s_S, c_S);
672 s_A = c_U1 * c_U2 * s_L / s_S;
673 c_SqA = 1 - s_A * s_A;
674 c_2SM = c_S - 2 * s_U1 * s_U2 / c_SqA;
675
676 if (0 == isfinite(c_2SM))
677 c_2SM = 0;
678
679 C = f / 16 * c_SqA * (4 + f * (4 - 3 * c_SqA));
680 L_P = lambda;
681 lambda = L + (1 - C) * f * s_A *
682 (S + C * s_S * (c_2SM + C * c_S * (2 * c_2SM * c_2SM - 1)));
683 } while ((fabs(lambda - L_P) > 1.0e-12) && (--i > 0));
684
685 if (i == 0)
686 return NAN; // formula failed to converge
687
688 uSq = c_SqA * ((a * a) - (b * b)) / (b * b);
689 A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
690 B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
691 d_S = B * s_S * (c_2SM + B / 4 *
692 (c_S * (-1 + 2 * c_2SM * c_2SM) - B / 6 * c_2SM *
693 (-3 + 4 * s_S * s_S) * (-3 + 4 * c_2SM * c_2SM)));
694
695 if (ib != NULL)
696 *ib = atan2(c_U2 * sin(lambda), c_U1 * s_U2 - s_U1 * c_U2 * cos(lambda));
697 if (fb != NULL)
698 *fb = atan2(c_U1 * sin(lambda), c_U1 * s_U2 * cos(lambda) - s_U1 * c_U2);
699
700 return (WGS84B * A * (S - d_S));
701 }
702
703 /* Distance in meters between two points specified in degrees. */
earth_distance(double lat1,double lon1,double lat2,double lon2)704 double earth_distance(double lat1, double lon1, double lat2, double lon2)
705 {
706 return earth_distance_and_bearings(lat1, lon1, lat2, lon2, NULL, NULL);
707 }
708
nanowait(int fd,int nanoseconds)709 bool nanowait(int fd, int nanoseconds)
710 {
711 fd_set fdset;
712 struct timespec to;
713
714 FD_ZERO(&fdset);
715 FD_SET(fd, &fdset);
716 to.tv_sec = nanoseconds / NS_IN_SEC;
717 to.tv_nsec = nanoseconds % NS_IN_SEC;
718 return pselect(fd + 1, &fdset, NULL, NULL, &to, NULL) == 1;
719 }
720
721 /* Accept a datum code, return matching string
722 *
723 * There are a ton of these, only a few are here
724 *
725 */
datum_code_string(int code,char * buffer,size_t len)726 void datum_code_string(int code, char *buffer, size_t len)
727 {
728 const char *datum_str;
729
730 switch (code) {
731 case 0:
732 datum_str = "WGS84";
733 break;
734 case 21:
735 datum_str = "WGS84";
736 break;
737 case 178:
738 datum_str = "Tokyo Mean";
739 break;
740 case 179:
741 datum_str = "Tokyo-Japan";
742 break;
743 case 180:
744 datum_str = "Tokyo-Korea";
745 break;
746 case 181:
747 datum_str = "Tokyo-Okinawa";
748 break;
749 case 182:
750 datum_str = "PZ90.11";
751 break;
752 case 999:
753 datum_str = "User Defined";
754 break;
755 default:
756 datum_str = NULL;
757 break;
758 }
759
760 if (NULL == datum_str) {
761 /* Fake it */
762 snprintf(buffer, len, "%d", code);
763 } else {
764 strlcpy(buffer, datum_str, len);
765 }
766 }
767 /* end */
768