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.
 
 
 
 
 
 

590 lines
19 KiB

  1. /*
  2. * This file is Copyright (c) 2010-2018 by the GPSD project
  3. * SPDX-License-Identifier: BSD-2-clause
  4. */
  5. #include "gpsd_config.h" /* must be before all includes */
  6. #include <stdio.h>
  7. #include <stdlib.h>
  8. #include <string.h>
  9. #include <math.h>
  10. #include <time.h>
  11. #include "gpsd.h"
  12. #include "strfuncs.h"
  13. /*
  14. * Support for generic binary drivers. These functions dump NMEA for passing
  15. * to the client in raw mode. They assume that (a) the public gps.h structure
  16. * members are in a valid state, (b) that the private members hours, minutes,
  17. * and seconds have also been filled in, (c) that if the private member
  18. * mag_var is not NAN it is a magnetic variation in degrees that should be
  19. * passed on, and (d) if the private member separation does not have the
  20. * value NAN, it is a valid WGS84 geoidal separation in meters for the fix.
  21. */
  22. #define BUF_SZ 20
  23. static char *degtodm_str(double angle, const char *fmt, char *buf)
  24. /* decimal degrees to GPS-style, degrees first followed by minutes */
  25. {
  26. double fraction, integer;
  27. if (0 == isfinite(angle)) {
  28. buf[0] = '\0';
  29. } else {
  30. angle = fabs(angle);
  31. fraction = modf(angle, &integer);
  32. (void)snprintf(buf, BUF_SZ, fmt, floor(angle) * 100 + fraction * 60);
  33. }
  34. return buf;
  35. }
  36. /* format a float/lon/alt into a string, handle NAN, INFINITE */
  37. static char *f_str(double f, const char *fmt, char *buf)
  38. {
  39. if (0 == isfinite(f)) {
  40. buf[0] = '\0';
  41. } else {
  42. (void)snprintf(buf, BUF_SZ, fmt, f);
  43. }
  44. return buf;
  45. }
  46. /* make size of time string buffer large to shut up paranoid cc's */
  47. #define TIMESTR_SZ 48
  48. /* convert UTC to time str (hh:mm:ss.ss) and tm */
  49. static void utc_to_hhmmss(timespec_t time, char *buf, ssize_t buf_sz,
  50. struct tm *tm)
  51. {
  52. time_t integer;
  53. long fractional;
  54. if (0 >= time.tv_sec) {
  55. buf[0] = '\0';
  56. return;
  57. }
  58. integer = time.tv_sec;
  59. /* round to 100ths */
  60. fractional = (time.tv_nsec + 5000000L) / 10000000L;
  61. if (99 < fractional) {
  62. integer++;
  63. fractional = 0;
  64. }
  65. (void)gmtime_r(&integer, tm);
  66. (void)snprintf(buf, buf_sz, "%02d%02d%02d.%02ld",
  67. tm->tm_hour, tm->tm_min, tm->tm_sec, fractional);
  68. return;
  69. }
  70. static void dbl_to_str(const char *fmt, double val, char *bufp, size_t len,
  71. const char *suffix)
  72. {
  73. if (0 == isfinite(val)) {
  74. (void)strlcat(bufp, ",", len);
  75. } else {
  76. str_appendf(bufp, len, fmt, val);
  77. }
  78. if (NULL != suffix) {
  79. str_appendf(bufp, len, "%s", suffix);
  80. }
  81. }
  82. #define FIX_QUALITY_INVALID 0
  83. #define FIX_QUALITY_GPS 1
  84. #define FIX_QUALITY_DGPS 2
  85. #define FIX_QUALITY_PPS 3
  86. #define FIX_QUALITY_RTK 4
  87. #define FIX_QUALITY_RTK_FLT 5
  88. #define FIX_QUALITY_DR 6
  89. #define FIX_QUALITY_MANUAL 7
  90. #define FIX_QUALITY_SIMULATED 8
  91. /* Dump a $GPGGA.
  92. * looks like this is only called from net_ntrip.c and nmea_tpv_dump()
  93. */
  94. void gpsd_position_fix_dump(struct gps_device_t *session,
  95. char bufp[], size_t len)
  96. {
  97. struct tm tm;
  98. char time_str[TIMESTR_SZ];
  99. char lat_str[BUF_SZ];
  100. char lon_str[BUF_SZ];
  101. unsigned char fixquality;
  102. utc_to_hhmmss(session->gpsdata.fix.time, time_str, sizeof(time_str), &tm);
  103. if (session->gpsdata.fix.mode > MODE_NO_FIX) {
  104. switch(session->gpsdata.status) {
  105. case STATUS_NO_FIX:
  106. fixquality = FIX_QUALITY_INVALID;
  107. break;
  108. case STATUS_FIX:
  109. fixquality = FIX_QUALITY_GPS;
  110. break;
  111. case STATUS_DGPS_FIX:
  112. fixquality = FIX_QUALITY_DGPS;
  113. break;
  114. case STATUS_RTK_FIX:
  115. fixquality = FIX_QUALITY_RTK;
  116. break;
  117. case STATUS_RTK_FLT:
  118. fixquality = FIX_QUALITY_RTK_FLT;
  119. break;
  120. case STATUS_DR:
  121. // FALLTHROUGH
  122. case STATUS_GNSSDR:
  123. fixquality = FIX_QUALITY_DR;
  124. break;
  125. case STATUS_TIME:
  126. fixquality = FIX_QUALITY_MANUAL;
  127. break;
  128. case STATUS_SIM:
  129. fixquality = FIX_QUALITY_SIMULATED;
  130. break;
  131. default:
  132. fixquality = FIX_QUALITY_INVALID;
  133. break;
  134. }
  135. (void)snprintf(bufp, len,
  136. "$GPGGA,%s,%s,%c,%s,%c,%d,%02d,",
  137. time_str,
  138. degtodm_str(session->gpsdata.fix.latitude, "%09.4f",
  139. lat_str),
  140. ((session->gpsdata.fix.latitude > 0) ? 'N' : 'S'),
  141. degtodm_str(session->gpsdata.fix.longitude, "%010.4f",
  142. lon_str),
  143. ((session->gpsdata.fix.longitude > 0) ? 'E' : 'W'),
  144. fixquality,
  145. session->gpsdata.satellites_used);
  146. dbl_to_str("%.2f,", session->gpsdata.dop.hdop, bufp, len, NULL);
  147. dbl_to_str("%.2f,", session->gpsdata.fix.altMSL, bufp, len, "M,");
  148. dbl_to_str("%.3f,", session->gpsdata.fix.geoid_sep, bufp, len, "M,");
  149. /* empty place holders for Age of correction data, and
  150. * Differential base station ID */
  151. (void)strlcat(bufp, ",", len);
  152. nmea_add_checksum(bufp);
  153. }
  154. }
  155. static void gpsd_transit_fix_dump(struct gps_device_t *session,
  156. char bufp[], size_t len)
  157. {
  158. char time_str[TIMESTR_SZ];
  159. char time2_str[TIMESTR_SZ];
  160. char lat_str[BUF_SZ];
  161. char lon_str[BUF_SZ];
  162. char speed_str[BUF_SZ];
  163. char track_str[BUF_SZ];
  164. char var_str[BUF_SZ];
  165. char *var_dir = "";
  166. struct tm tm;
  167. utc_to_hhmmss(session->gpsdata.fix.time, time_str, sizeof(time_str), &tm);
  168. if ('\0' != time_str[0]) {
  169. tm.tm_mon++;
  170. tm.tm_year %= 100;
  171. (void)snprintf(time2_str, sizeof(time2_str),
  172. "%02d%02d%02d",
  173. tm.tm_mday, tm.tm_mon, tm.tm_year);
  174. } else {
  175. time2_str[0] = '\0';
  176. }
  177. if (0 != isfinite(session->gpsdata.fix.magnetic_var)) {
  178. f_str(session->gpsdata.fix.magnetic_var, "%.1f", var_str),
  179. var_dir = (session->gpsdata.fix.magnetic_var > 0) ? "E" : "W";
  180. } else {
  181. var_str[0] = '\0';
  182. var_dir = "";
  183. }
  184. (void)snprintf(bufp, len,
  185. "$GPRMC,%s,%c,%s,%c,%s,%c,%s,%s,%s,%s,%s",
  186. time_str,
  187. session->gpsdata.status ? 'A' : 'V',
  188. degtodm_str(session->gpsdata.fix.latitude, "%09.4f",
  189. lat_str),
  190. ((session->gpsdata.fix.latitude > 0) ? 'N' : 'S'),
  191. degtodm_str(session->gpsdata.fix.longitude, "%010.4f",
  192. lon_str),
  193. ((session->gpsdata.fix.longitude > 0) ? 'E' : 'W'),
  194. f_str(session->gpsdata.fix.speed * MPS_TO_KNOTS, "%.4f",
  195. speed_str),
  196. f_str(session->gpsdata.fix.track, "%.3f", track_str),
  197. time2_str,
  198. var_str, var_dir);
  199. nmea_add_checksum(bufp);
  200. }
  201. static void gpsd_binary_satellite_dump(struct gps_device_t *session,
  202. char bufp[], size_t len)
  203. {
  204. int i; /* index into skyview[] */
  205. int j; /* index into GPGSV */
  206. char *bufp2 = bufp;
  207. int satellites_visible = 0;
  208. bufp[0] = '\0';
  209. /* check skyview[] for valid sats first */
  210. for (i = 0; i < session->gpsdata.satellites_visible; i++) {
  211. if ( 1 > session->gpsdata.skyview[i].PRN) {
  212. /* bad prn, ignore */
  213. continue;
  214. }
  215. if (0 == isfinite(session->gpsdata.skyview[i].elevation) ||
  216. 90 < fabs(session->gpsdata.skyview[i].elevation)) {
  217. /* bad elevation, ignore */
  218. continue;
  219. }
  220. if (0 == isfinite(session->gpsdata.skyview[i].azimuth) ||
  221. 0 > session->gpsdata.skyview[i].azimuth ||
  222. 359 < session->gpsdata.skyview[i].azimuth) {
  223. /* bad azimuth, ignore */
  224. continue;
  225. }
  226. satellites_visible++;
  227. }
  228. for (i = 0, j= 0; i < session->gpsdata.satellites_visible; i++) {
  229. if ( 1 > session->gpsdata.skyview[i].PRN) {
  230. /* bad prn, skip */
  231. continue;
  232. }
  233. if (0 == isfinite(session->gpsdata.skyview[i].elevation) ||
  234. 90 < fabs(session->gpsdata.skyview[i].elevation)) {
  235. /* bad elevation, ignore */
  236. continue;
  237. }
  238. if (0 == isfinite(session->gpsdata.skyview[i].azimuth) ||
  239. 0 > session->gpsdata.skyview[i].azimuth ||
  240. 359 < session->gpsdata.skyview[i].azimuth) {
  241. /* bad azimuth, ignore */
  242. continue;
  243. }
  244. if (j % 4 == 0) {
  245. bufp2 = bufp + strlen(bufp);
  246. str_appendf(bufp, len,
  247. "$GPGSV,%d,%d,%02d",
  248. ((satellites_visible - 1) / 4) + 1, (j / 4) + 1,
  249. satellites_visible);
  250. }
  251. str_appendf(bufp, len, ",%02d,%02.0f,%03.0f,%02.0f",
  252. session->gpsdata.skyview[i].PRN,
  253. session->gpsdata.skyview[i].elevation,
  254. session->gpsdata.skyview[i].azimuth,
  255. session->gpsdata.skyview[i].ss);
  256. if (j % 4 == 3 || j == satellites_visible - 1) {
  257. nmea_add_checksum(bufp2);
  258. }
  259. j++;
  260. }
  261. #ifdef ZODIAC_ENABLE
  262. if (session->lexer.type == ZODIAC_PACKET
  263. && session->driver.zodiac.Zs[0] != 0) {
  264. bufp2 = bufp + strlen(bufp);
  265. str_appendf(bufp, len, "$PRWIZCH");
  266. for (i = 0; i < ZODIAC_CHANNELS; i++) {
  267. str_appendf(bufp, len,
  268. ",%02u,%X",
  269. session->driver.zodiac.Zs[i],
  270. session->driver.zodiac.Zv[i] & 0x0f);
  271. }
  272. nmea_add_checksum(bufp2);
  273. }
  274. #endif /* ZODIAC_ENABLE */
  275. }
  276. static void gpsd_binary_quality_dump(struct gps_device_t *session,
  277. char bufp[], size_t len)
  278. {
  279. char *bufp2;
  280. bufp[0] = '\0';
  281. if (session->device_type != NULL) {
  282. int i, j;
  283. int max_channels = session->device_type->channels;
  284. /* GPGSA commonly has exactly 12 channels, enforce that as a MAX */
  285. if ( 12 < max_channels ) {
  286. /* what to do with the excess channels? */
  287. max_channels = 12;
  288. }
  289. bufp2 = bufp + strlen(bufp);
  290. (void)snprintf(bufp, len,
  291. "$GPGSA,%c,%d,", 'A', session->gpsdata.fix.mode);
  292. j = 0;
  293. for (i = 0; i < max_channels; i++) {
  294. if (session->gpsdata.skyview[i].used == true){
  295. str_appendf(bufp, len, "%d,", session->gpsdata.skyview[i].PRN);
  296. j++;
  297. }
  298. }
  299. for (i = j; i < max_channels; i++) {
  300. /* fill out the empty slots */
  301. (void)strlcat(bufp, ",", len);
  302. }
  303. if (session->gpsdata.fix.mode == MODE_NO_FIX)
  304. (void)strlcat(bufp, ",,,", len);
  305. else {
  306. /* output the DOPs, NaN as blanks */
  307. if ( 0 != isfinite( session->gpsdata.dop.pdop ) ) {
  308. str_appendf(bufp, len, "%.1f,", session->gpsdata.dop.pdop);
  309. } else {
  310. (void)strlcat(bufp, ",", len);
  311. }
  312. if ( 0 != isfinite( session->gpsdata.dop.hdop ) ) {
  313. str_appendf(bufp, len, "%.1f,", session->gpsdata.dop.hdop);
  314. } else {
  315. (void)strlcat(bufp, ",", len);
  316. }
  317. if ( 0 != isfinite( session->gpsdata.dop.vdop ) ) {
  318. str_appendf(bufp, len, "%.1f*", session->gpsdata.dop.vdop);
  319. } else {
  320. (void)strlcat(bufp, "*", len);
  321. }
  322. }
  323. nmea_add_checksum(bufp2);
  324. }
  325. /* create $GPGBS if we have time, epx and epy. Optional epv.
  326. * Not really kosher, not have enough info to compute the RAIM
  327. */
  328. if (0 != isfinite(session->gpsdata.fix.epx) &&
  329. 0 != isfinite(session->gpsdata.fix.epy) &&
  330. 0 != session->gpsdata.fix.time.tv_sec) {
  331. struct tm tm;
  332. char time_str[TIMESTR_SZ];
  333. char epv_str[BUF_SZ];
  334. (void)utc_to_hhmmss(session->gpsdata.fix.time,
  335. time_str, sizeof(time_str), &tm);
  336. bufp2 = bufp + strlen(bufp);
  337. str_appendf(bufp, len,
  338. "$GPGBS,%s,%.3f,%.3f,%s,,,,",
  339. time_str,
  340. session->gpsdata.fix.epx,
  341. session->gpsdata.fix.epy,
  342. f_str(session->gpsdata.fix.epv, "%.3f", epv_str));
  343. nmea_add_checksum(bufp2);
  344. }
  345. }
  346. /* Dump $GPZDA if we have time and a fix */
  347. static void gpsd_binary_time_dump(struct gps_device_t *session,
  348. char bufp[], size_t len)
  349. {
  350. if (MODE_NO_FIX < session->newdata.mode &&
  351. 0 <= session->gpsdata.fix.time.tv_sec) {
  352. struct tm tm;
  353. char time_str[TIMESTR_SZ];
  354. utc_to_hhmmss(session->gpsdata.fix.time, time_str, sizeof(time_str),
  355. &tm);
  356. /* There used to be confusion, but we now know NMEA times are UTC,
  357. * when available. */
  358. (void)snprintf(bufp, len,
  359. "$GPZDA,%s,%02d,%02d,%04d,00,00",
  360. time_str,
  361. tm.tm_mday,
  362. tm.tm_mon + 1,
  363. tm.tm_year + 1900);
  364. nmea_add_checksum(bufp);
  365. }
  366. }
  367. static void gpsd_binary_almanac_dump(struct gps_device_t *session,
  368. char bufp[], size_t len)
  369. {
  370. if ( session->gpsdata.subframe.is_almanac ) {
  371. (void)snprintf(bufp, len,
  372. "$GPALM,1,1,%02d,%04d,%02x,%04x,%02x,%04x,%04x,%05x,"
  373. "%06x,%06x,%06x,%03x,%03x",
  374. (int)session->gpsdata.subframe.sub5.almanac.sv,
  375. (int)session->context->gps_week % 1024,
  376. (unsigned int)session->gpsdata.subframe.sub5.almanac.svh,
  377. (unsigned int)session->gpsdata.subframe.sub5.almanac.e,
  378. (unsigned int)session->gpsdata.subframe.sub5.almanac.toa,
  379. (unsigned int)session->gpsdata.subframe.sub5.almanac.deltai,
  380. (unsigned int)session->gpsdata.subframe.sub5.almanac.Omegad,
  381. (unsigned int)session->gpsdata.subframe.sub5.almanac.sqrtA,
  382. (unsigned int)session->gpsdata.subframe.sub5.almanac.omega,
  383. (unsigned int)session->gpsdata.subframe.sub5.almanac.Omega0,
  384. (unsigned int)session->gpsdata.subframe.sub5.almanac.M0,
  385. (unsigned int)session->gpsdata.subframe.sub5.almanac.af0,
  386. (unsigned int)session->gpsdata.subframe.sub5.almanac.af1);
  387. nmea_add_checksum(bufp);
  388. }
  389. }
  390. #ifdef AIVDM_ENABLE
  391. #define GETLEFT(a) (((a%6) == 0) ? 0 : (6 - (a%6)))
  392. static void gpsd_binary_ais_dump(struct gps_device_t *session,
  393. char bufp[], size_t len)
  394. {
  395. char type[8] = "!AIVDM";
  396. unsigned char data[256];
  397. unsigned int msg1, msg2;
  398. char numc[4];
  399. char channel;
  400. unsigned int left;
  401. unsigned int datalen;
  402. unsigned int offset;
  403. channel = 'A';
  404. if (session->driver.aivdm.ais_channel == 'B') {
  405. channel = 'B';
  406. }
  407. memset(data, 0, sizeof(data));
  408. datalen = ais_binary_encode(&session->gpsdata.ais, &data[0], 0);
  409. if (datalen > 6*60) {
  410. static int number1 = 0;
  411. msg1 = datalen / (6*60);
  412. if ((datalen % (6*60)) != 0) {
  413. msg1 += 1;
  414. }
  415. numc[0] = '0' + (char)(number1 & 0x0f);
  416. numc[1] = '\0';
  417. number1 += 1;
  418. if (number1 > 9) {
  419. number1 = 0;
  420. }
  421. offset = 0;
  422. for (msg2=1;msg2<=msg1;msg2++) {
  423. unsigned char old;
  424. old = '\0';
  425. if (strlen((char *)&data[(msg2-1)*60]) > 60) {
  426. old = data[(msg2-0)*60];
  427. data[(msg2-0)*60] = '\0';
  428. }
  429. if (datalen >= (6*60)) {
  430. left = 0;
  431. datalen -= 6*60;
  432. } else {
  433. left = GETLEFT(datalen);
  434. }
  435. (void)snprintf(&bufp[offset], len-offset,
  436. "%s,%u,%u,%s,%c,%s,%u",
  437. type,
  438. msg1,
  439. msg2,
  440. numc,
  441. channel,
  442. (char *)&data[(msg2-1)*60],
  443. left);
  444. nmea_add_checksum(&bufp[offset]);
  445. if (old != (unsigned char)'\0') {
  446. data[(msg2-0)*60] = old;
  447. }
  448. offset = (unsigned int) strlen(bufp);
  449. }
  450. } else if (datalen > 0) {
  451. msg1 = 1;
  452. msg2 = 1;
  453. numc[0] = '\0';
  454. left = GETLEFT(datalen);
  455. (void)snprintf(bufp, len,
  456. "%s,%u,%u,%s,%c,%s,%u",
  457. type,
  458. msg1,
  459. msg2,
  460. numc,
  461. channel,
  462. (char *)data,
  463. left);
  464. nmea_add_checksum(bufp);
  465. }
  466. if (session->gpsdata.ais.type == 24) {
  467. msg1 = 1;
  468. msg2 = 1;
  469. numc[0] = '\0';
  470. memset(data, 0, sizeof(data));
  471. datalen = ais_binary_encode(&session->gpsdata.ais, &data[0], 1);
  472. if (datalen > 0) {
  473. left = GETLEFT(datalen);
  474. offset = (unsigned int)strlen(bufp);
  475. (void)snprintf(&bufp[offset], len-offset,
  476. "%s,%u,%u,%s,%c,%s,%u",
  477. type,
  478. msg1,
  479. msg2,
  480. numc,
  481. channel,
  482. (char *)data,
  483. left);
  484. nmea_add_checksum(bufp+offset);
  485. }
  486. }
  487. }
  488. #endif /* AIVDM_ENABLE */
  489. /* *INDENT-OFF* */
  490. void nmea_tpv_dump(struct gps_device_t *session,
  491. char bufp[], size_t len)
  492. {
  493. bufp[0] = '\0';
  494. if ((session->gpsdata.set & TIME_SET) != 0)
  495. gpsd_binary_time_dump(session, bufp + strlen(bufp),
  496. len - strlen(bufp));
  497. if ((session->gpsdata.set & (LATLON_SET | MODE_SET)) != 0) {
  498. gpsd_position_fix_dump(session, bufp + strlen(bufp),
  499. len - strlen(bufp));
  500. gpsd_transit_fix_dump(session, bufp + strlen(bufp),
  501. len - strlen(bufp));
  502. }
  503. if ((session->gpsdata.set
  504. & (MODE_SET | DOP_SET | USED_IS | HERR_SET)) != 0)
  505. gpsd_binary_quality_dump(session, bufp + strlen(bufp),
  506. len - strlen(bufp));
  507. }
  508. /* *INDENT-ON* */
  509. void nmea_sky_dump(struct gps_device_t *session,
  510. char bufp[], size_t len)
  511. {
  512. bufp[0] = '\0';
  513. if ((session->gpsdata.set & SATELLITE_SET) != 0)
  514. gpsd_binary_satellite_dump(session, bufp + strlen(bufp),
  515. len - strlen(bufp));
  516. }
  517. void nmea_subframe_dump(struct gps_device_t *session,
  518. char bufp[], size_t len)
  519. {
  520. bufp[0] = '\0';
  521. if ((session->gpsdata.set & SUBFRAME_SET) != 0)
  522. gpsd_binary_almanac_dump(session, bufp + strlen(bufp),
  523. len - strlen(bufp));
  524. }
  525. #ifdef AIVDM_ENABLE
  526. void nmea_ais_dump(struct gps_device_t *session,
  527. char bufp[], size_t len)
  528. {
  529. bufp[0] = '\0';
  530. if ((session->gpsdata.set & AIS_SET) != 0)
  531. gpsd_binary_ais_dump(session, bufp + strlen(bufp),
  532. len - strlen(bufp));
  533. }
  534. #endif /* AIVDM_ENABLE */
  535. /* pseudonmea.c ends here */