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