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