Devuan fork of gpsd
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

768 lines
20 KiB

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