1 /* libgpsd_core.c -- manage access to sensors
2 *
3 * Access to the driver layer goes through the entry points in this file.
4 * The idea is to present a session as an abstraction from which you get
5 * fixes (and possibly other data updates) by calling gpsd_multipoll(). The
6 * rest is setup and teardown. (For backward compatibility the older gpsd_poll()
7 * entry point has been retained.)
8 *
9 * This file is Copyright (c) 2010-2018 by the GPSD project
10 * SPDX-License-Identifier: BSD-2-clause
11 */
12
13 #include "gpsd_config.h" /* must be before all includes */
14
15 #include <assert.h>
16 #include <ctype.h>
17 #include <errno.h>
18 #include <fcntl.h>
19 #include <libgen.h>
20 #include <math.h>
21 #include <stdarg.h>
22 #include <stdbool.h>
23 #include <stdio.h>
24 #include <stdlib.h>
25 #include <string.h>
26 #include <syslog.h>
27 #include <sys/select.h>
28 #include <sys/socket.h>
29 #include <sys/stat.h>
30 #include <sys/types.h>
31 #include <sys/wait.h>
32 #include <time.h>
33 #include <unistd.h>
34
35 #include "gpsd.h"
36 #include "matrix.h"
37 #include "strfuncs.h"
38 #include "timespec.h"
39 #if defined(NMEA2000_ENABLE)
40 #include "driver_nmea2000.h"
41 #endif /* defined(NMEA2000_ENABLE) */
42
gpsd_write(struct gps_device_t * session,const char * buf,const size_t len)43 ssize_t gpsd_write(struct gps_device_t *session,
44 const char *buf,
45 const size_t len)
46 /* pass low-level data to devices straight through */
47 {
48 return session->context->serial_write(session, buf, len);
49 }
50
basic_report(const char * buf)51 static void basic_report(const char *buf)
52 {
53 (void)fputs(buf, stderr);
54 }
55
errout_reset(struct gpsd_errout_t * errout)56 void errout_reset(struct gpsd_errout_t *errout)
57 {
58 errout->debug = LOG_SHOUT;
59 errout->report = basic_report;
60 }
61
62 static pthread_mutex_t report_mutex;
63
gpsd_acquire_reporting_lock(void)64 void gpsd_acquire_reporting_lock(void)
65 {
66 int err;
67 err = pthread_mutex_lock(&report_mutex);
68 if ( 0 != err ) {
69 /* POSIX says pthread_mutex_lock() should only fail if the
70 thread holding the lock has died. Best for gppsd to just die
71 because things are FUBAR. */
72
73 (void) fprintf(stderr,"pthread_mutex_lock() failed: %s\n",
74 strerror(errno));
75 exit(EXIT_FAILURE);
76 }
77 }
78
gpsd_release_reporting_lock(void)79 void gpsd_release_reporting_lock(void)
80 {
81 int err;
82 err = pthread_mutex_unlock(&report_mutex);
83 if ( 0 != err ) {
84 /* POSIX says pthread_mutex_unlock() should only fail when
85 trying to unlock a lock that does not exist, or is not owned by
86 this thread. This should never happen, so best for gpsd to die
87 because things are FUBAR. */
88
89 (void) fprintf(stderr,"pthread_mutex_unlock() failed: %s\n",
90 strerror(errno));
91 exit(EXIT_FAILURE);
92 }
93 }
94
95 #ifndef SQUELCH_ENABLE
visibilize(char * outbuf,size_t outlen,const char * inbuf,size_t inlen)96 static void visibilize(char *outbuf, size_t outlen,
97 const char *inbuf, size_t inlen)
98 {
99 const char *sp;
100
101 outbuf[0] = '\0';
102 for (sp = inbuf; sp < inbuf + inlen && strlen(outbuf)+6 < outlen; sp++)
103 if (isprint((unsigned char) *sp) || (sp[0] == '\n' && sp[1] == '\0')
104 || (sp[0] == '\r' && sp[2] == '\0'))
105 (void)snprintf(outbuf + strlen(outbuf), 2, "%c", *sp);
106 else
107 (void)snprintf(outbuf + strlen(outbuf), 6, "\\x%02x",
108 0x00ff & (unsigned)*sp);
109 }
110 #endif /* !SQUELCH_ENABLE */
111
112
gpsd_vlog(const int errlevel,const struct gpsd_errout_t * errout,char * outbuf,size_t outlen,const char * fmt,va_list ap)113 static void gpsd_vlog(const int errlevel,
114 const struct gpsd_errout_t *errout,
115 char *outbuf, size_t outlen,
116 const char *fmt, va_list ap)
117 /* assemble msg in vprintf(3) style, use errout hook or syslog for delivery */
118 {
119 #ifdef SQUELCH_ENABLE
120 (void)errout;
121 (void)errlevel;
122 (void)fmt;
123 #else
124 char buf[BUFSIZ];
125 char *err_str;
126
127 // errout should never be NULL, but some code analyzers complain anyway
128 if (NULL == errout ||
129 errout->debug < errlevel) {
130 return;
131 }
132
133 gpsd_acquire_reporting_lock();
134 switch ( errlevel ) {
135 case LOG_ERROR:
136 err_str = "ERROR: ";
137 break;
138 case LOG_SHOUT:
139 err_str = "SHOUT: ";
140 break;
141 case LOG_WARN:
142 err_str = "WARN: ";
143 break;
144 case LOG_CLIENT:
145 err_str = "CLIENT: ";
146 break;
147 case LOG_INF:
148 err_str = "INFO: ";
149 break;
150 case LOG_DATA:
151 err_str = "DATA: ";
152 break;
153 case LOG_PROG:
154 err_str = "PROG: ";
155 break;
156 case LOG_IO:
157 err_str = "IO: ";
158 break;
159 case LOG_SPIN:
160 err_str = "SPIN: ";
161 break;
162 case LOG_RAW:
163 err_str = "RAW: ";
164 break;
165 default:
166 err_str = "UNK: ";
167 }
168
169 assert(errout->label != NULL);
170 (void)strlcpy(buf, errout->label, sizeof(buf));
171 (void)strlcat(buf, ":", sizeof(buf));
172 (void)strlcat(buf, err_str, sizeof(buf));
173 str_vappendf(buf, sizeof(buf), fmt, ap);
174
175 visibilize(outbuf, outlen, buf, strlen(buf));
176
177 if (getpid() == getsid(getpid()))
178 syslog((errlevel <= LOG_SHOUT) ? LOG_ERR : LOG_NOTICE, "%s", outbuf);
179 else if (errout->report != NULL)
180 errout->report(outbuf);
181 else
182 (void)fputs(outbuf, stderr);
183 gpsd_release_reporting_lock();
184 #endif /* !SQUELCH_ENABLE */
185 }
186
187 /* assemble msg in printf(3) style, use errout hook or syslog for delivery */
gpsd_log(const int errlevel,const struct gpsd_errout_t * errout,const char * fmt,...)188 void gpsd_log(const int errlevel, const struct gpsd_errout_t *errout,
189 const char *fmt, ...)
190 {
191 char buf[BUFSIZ];
192 va_list ap;
193
194 buf[0] = '\0';
195 va_start(ap, fmt);
196 gpsd_vlog(errlevel, errout, buf, sizeof(buf), fmt, ap);
197 va_end(ap);
198 }
199
gpsd_prettydump(struct gps_device_t * session)200 const char *gpsd_prettydump(struct gps_device_t *session)
201 /* dump the current packet in a form optimised for eyeballs */
202 {
203 return gpsd_packetdump(session->msgbuf, sizeof(session->msgbuf),
204 (char *)session->lexer.outbuffer,
205 session->lexer.outbuflen);
206 }
207
208
209
210 /* Define the possible hook strings here so we can get the length */
211 #define HOOK_ACTIVATE "ACTIVATE"
212 #define HOOK_DEACTIVATE "DEACTIVATE"
213
214 #define HOOK_CMD_MAX (sizeof(DEVICEHOOKPATH) + GPS_PATH_MAX \
215 + sizeof(HOOK_DEACTIVATE))
216
gpsd_run_device_hook(struct gpsd_errout_t * errout,char * device_name,char * hook)217 static void gpsd_run_device_hook(struct gpsd_errout_t *errout,
218 char *device_name, char *hook)
219 {
220 struct stat statbuf;
221
222 if (stat(DEVICEHOOKPATH, &statbuf) == -1)
223 GPSD_LOG(LOG_PROG, errout,
224 "no %s present, skipped running %s hook\n",
225 DEVICEHOOKPATH, hook);
226 else {
227 int status;
228 char buf[HOOK_CMD_MAX];
229 (void)snprintf(buf, sizeof(buf), "%s %s %s",
230 DEVICEHOOKPATH, device_name, hook);
231 GPSD_LOG(LOG_INF, errout, "running %s\n", buf);
232 status = system(buf);
233 if (status == -1)
234 GPSD_LOG(LOG_ERROR, errout, "error running %s\n", buf);
235 else
236 GPSD_LOG(LOG_INF, errout,
237 "%s returned %d\n", DEVICEHOOKPATH,
238 WEXITSTATUS(status));
239 }
240 }
241
gpsd_switch_driver(struct gps_device_t * session,char * type_name)242 int gpsd_switch_driver(struct gps_device_t *session, char *type_name)
243 {
244 const struct gps_type_t **dp;
245 bool first_sync = (session->device_type != NULL);
246 unsigned int i;
247
248 if (first_sync && strcmp(session->device_type->type_name, type_name) == 0)
249 return 0;
250
251 GPSD_LOG(LOG_PROG, &session->context->errout,
252 "switch_driver(%s) called...\n", type_name);
253 for (dp = gpsd_drivers, i = 0; *dp; dp++, i++)
254 if (strcmp((*dp)->type_name, type_name) == 0) {
255 GPSD_LOG(LOG_PROG, &session->context->errout,
256 "selecting %s driver...\n",
257 (*dp)->type_name);
258 gpsd_assert_sync(session);
259 session->device_type = *dp;
260 session->driver_index = i;
261 #ifdef RECONFIGURE_ENABLE
262 session->gpsdata.dev.mincycle = session->device_type->min_cycle;
263 #endif /* RECONFIGURE_ENABLE */
264 /* reconfiguration might be required */
265 if (first_sync && session->device_type->event_hook != NULL)
266 session->device_type->event_hook(session,
267 event_driver_switch);
268 #ifdef RECONFIGURE_ENABLE
269 if (STICKY(*dp))
270 session->last_controller = *dp;
271 #endif /* RECONFIGURE_ENABLE */
272 return 1;
273 }
274 GPSD_LOG(LOG_ERROR, &session->context->errout,
275 "invalid GPS type \"%s\".\n", type_name);
276 return 0;
277 }
278
gps_context_init(struct gps_context_t * context,const char * label)279 void gps_context_init(struct gps_context_t *context,
280 const char *label)
281 {
282 (void)memset(context, '\0', sizeof(struct gps_context_t));
283 //context.readonly = false;
284 context->leap_notify = LEAP_NOWARNING;
285 context->serial_write = gpsd_serial_write;
286
287 errout_reset(&context->errout);
288 context->errout.label = (char *)label;
289
290 (void)pthread_mutex_init(&report_mutex, NULL);
291 }
292
gpsd_init(struct gps_device_t * session,struct gps_context_t * context,const char * device)293 void gpsd_init(struct gps_device_t *session, struct gps_context_t *context,
294 const char *device)
295 /* initialize GPS polling */
296 {
297 if (device != NULL)
298 (void)strlcpy(session->gpsdata.dev.path, device,
299 sizeof(session->gpsdata.dev.path));
300 session->device_type = NULL; /* start by hunting packets */
301 #ifdef RECONFIGURE_ENABLE
302 session->last_controller = NULL;
303 #endif /* RECONFIGURE_ENABLE */
304 session->observed = 0;
305 session->sourcetype = source_unknown; /* gpsd_open() sets this */
306 session->servicetype = service_unknown; /* gpsd_open() sets this */
307 session->context = context;
308 memset(session->subtype, 0, sizeof(session->subtype));
309 memset(session->subtype1, 0, sizeof(session->subtype1));
310 #ifdef NMEA0183_ENABLE
311 memset(&(session->nmea), 0, sizeof(session->nmea));
312 #endif /* NMEA0183_ENABLE */
313 gps_clear_fix(&session->gpsdata.fix);
314 gps_clear_fix(&session->newdata);
315 gps_clear_fix(&session->lastfix);
316 gps_clear_fix(&session->oldfix);
317 session->gpsdata.set = 0;
318 gps_clear_att(&session->gpsdata.attitude);
319 gps_clear_dop(&session->gpsdata.dop);
320 session->gpsdata.dev.mincycle.tv_sec = 1;
321 session->gpsdata.dev.mincycle.tv_nsec = 0;
322 session->gpsdata.dev.cycle.tv_sec = 1;
323 session->gpsdata.dev.cycle.tv_nsec = 0;
324 session->sor.tv_sec = 0;
325 session->sor.tv_nsec = 0;
326 session->chars = 0;
327 /* tty-level initialization */
328 gpsd_tty_init(session);
329 /* necessary in case we start reading in the middle of a GPGSV sequence */
330 gpsd_zero_satellites(&session->gpsdata);
331
332 /* initialize things for the packet parser */
333 packet_reset(&session->lexer);
334 }
335
336 /* temporarily release the GPS device */
gpsd_deactivate(struct gps_device_t * session)337 void gpsd_deactivate(struct gps_device_t *session)
338 {
339 #ifdef RECONFIGURE_ENABLE
340 if (!session->context->readonly
341 && session->device_type != NULL
342 && session->device_type->event_hook != NULL) {
343 session->device_type->event_hook(session, event_deactivate);
344 }
345 #endif /* RECONFIGURE_ENABLE */
346 GPSD_LOG(LOG_INF, &session->context->errout,
347 "closing GPS=%s (%d)\n",
348 session->gpsdata.dev.path, session->gpsdata.gps_fd);
349 #if defined(NMEA2000_ENABLE)
350 if (session->sourcetype == source_can)
351 (void)nmea2000_close(session);
352 else
353 #endif /* of defined(NMEA2000_ENABLE) */
354 (void)gpsd_close(session);
355 if (session->mode == O_OPTIMIZE)
356 gpsd_run_device_hook(&session->context->errout,
357 session->gpsdata.dev.path,
358 HOOK_DEACTIVATE);
359 /* tell any PPS-watcher thread to die */
360 session->pps_thread.report_hook = NULL;
361 /* mark it inactivated */
362 session->gpsdata.online.tv_sec = 0;
363 session->gpsdata.online.tv_nsec = 0;
364 }
365
ppsthread_log(volatile struct pps_thread_t * pps_thread,int loglevel,const char * fmt,...)366 static void ppsthread_log(volatile struct pps_thread_t *pps_thread,
367 int loglevel, const char *fmt, ...)
368 /* shim function to decouple PPS monitor code from the session structure */
369 {
370 struct gps_device_t *device = (struct gps_device_t *)pps_thread->context;
371 char buf[BUFSIZ];
372 va_list ap;
373
374 switch (loglevel) {
375 case THREAD_ERROR:
376 loglevel = LOG_ERROR;
377 break;
378 case THREAD_WARN:
379 loglevel = LOG_WARN;
380 break;
381 case THREAD_INF:
382 loglevel = LOG_INF;
383 break;
384 case THREAD_PROG:
385 loglevel = LOG_PROG;
386 break;
387 case THREAD_RAW:
388 loglevel = LOG_RAW;
389 break;
390 }
391
392 buf[0] = '\0';
393 va_start(ap, fmt);
394 gpsd_vlog(loglevel, &device->context->errout, buf, sizeof(buf), fmt, ap);
395 va_end(ap);
396 }
397
398
gpsd_clear(struct gps_device_t * session)399 void gpsd_clear(struct gps_device_t *session)
400 /* device has been opened - clear its storage for use */
401 {
402 (void)clock_gettime(CLOCK_REALTIME, &session->gpsdata.online);
403 lexer_init(&session->lexer);
404 session->lexer.errout = session->context->errout;
405 // session->gpsdata.online = 0;
406 gps_clear_att(&session->gpsdata.attitude);
407 gps_clear_dop(&session->gpsdata.dop);
408 gps_clear_fix(&session->gpsdata.fix);
409 session->gpsdata.status = STATUS_NO_FIX;
410 session->releasetime = (time_t)0;
411 session->badcount = 0;
412
413 /* clear the private data union */
414 memset( (void *)&session->driver, '\0', sizeof(session->driver));
415 /* set up the context structure for the PPS thread monitor */
416 memset((void *)&session->pps_thread, 0, sizeof(session->pps_thread));
417 session->pps_thread.devicefd = session->gpsdata.gps_fd;
418 session->pps_thread.devicename = session->gpsdata.dev.path;
419 session->pps_thread.log_hook = ppsthread_log;
420 session->pps_thread.context = (void *)session;
421
422 session->opentime = time(NULL);
423 }
424
parse_uri_dest(struct gps_device_t * session,char * s,char ** host,char ** service)425 static int parse_uri_dest(struct gps_device_t *session, char *s,
426 char **host, char **service)
427 /* split s into host and service parts
428 * if service is not specified, *service is assigned to NULL
429 * return: -1 on error, 0 otherwise
430 */
431 {
432 if (s[0] != '[') {
433 *host = s;
434 s = strchr(s, ':');
435 } else { /* IPv6 literal */
436 char *cb = strchr(s, ']');
437 if (!cb || (cb[1] && cb[1] != ':')) {
438 GPSD_LOG(LOG_ERROR, &session->context->errout,
439 "Malformed URI specified.\n");
440 return -1;
441 }
442 *cb = '\0';
443 *host = s + 1;
444 s = cb + 1;
445 }
446 if (s && s[0] && s[1]) {
447 *s = '\0';
448 *service = s + 1;
449 } else
450 *service = NULL;
451 return 0;
452 }
453
gpsd_open(struct gps_device_t * session)454 int gpsd_open(struct gps_device_t *session)
455 /* open a device for access to its data *
456 * return: the opened file descriptor
457 * PLACEHOLDING_FD - for /dev/ppsX
458 * UNALLOCATED_FD - for open failure
459 * -1 - for open failure
460 */
461 {
462 #ifdef NETFEED_ENABLE
463 /* special case: source may be a URI to a remote GNSS or DGPS service */
464 if (netgnss_uri_check(session->gpsdata.dev.path)) {
465 session->gpsdata.gps_fd = netgnss_uri_open(session,
466 session->gpsdata.dev.path);
467 session->sourcetype = source_tcp;
468 GPSD_LOG(LOG_SPIN, &session->context->errout,
469 "netgnss_uri_open(%s) returns socket on fd %d\n",
470 session->gpsdata.dev.path, session->gpsdata.gps_fd);
471 return session->gpsdata.gps_fd;
472 /* otherwise, could be an TCP data feed */
473 } else if (str_starts_with(session->gpsdata.dev.path, "tcp://")) {
474 char server[GPS_PATH_MAX], *host, *port;
475 socket_t dsock;
476 (void)strlcpy(server, session->gpsdata.dev.path + 6, sizeof(server));
477 INVALIDATE_SOCKET(session->gpsdata.gps_fd);
478 if (parse_uri_dest(session, server, &host, &port) == -1 || !port) {
479 GPSD_LOG(LOG_ERROR, &session->context->errout,
480 "Missing service in TCP feed spec.\n");
481 return -1;
482 }
483 GPSD_LOG(LOG_INF, &session->context->errout,
484 "opening TCP feed at %s, port %s.\n", host,
485 port);
486 if ((dsock = netlib_connectsock(AF_UNSPEC, host, port, "tcp")) < 0) {
487 GPSD_LOG(LOG_ERROR, &session->context->errout,
488 "TCP device open error %s.\n",
489 netlib_errstr(dsock));
490 return -1;
491 } else
492 GPSD_LOG(LOG_SPIN, &session->context->errout,
493 "TCP device opened on fd %d\n", dsock);
494 session->gpsdata.gps_fd = dsock;
495 session->sourcetype = source_tcp;
496 return session->gpsdata.gps_fd;
497 /* or could be UDP */
498 } else if (str_starts_with(session->gpsdata.dev.path, "udp://")) {
499 char server[GPS_PATH_MAX], *host, *port;
500 socket_t dsock;
501 (void)strlcpy(server, session->gpsdata.dev.path + 6, sizeof(server));
502 INVALIDATE_SOCKET(session->gpsdata.gps_fd);
503 if (parse_uri_dest(session, server, &host, &port) == -1 || !port) {
504 GPSD_LOG(LOG_ERROR, &session->context->errout,
505 "Missing service in UDP feed spec.\n");
506 return -1;
507 }
508 GPSD_LOG(LOG_INF, &session->context->errout,
509 "opening UDP feed at %s, port %s.\n", host,
510 port);
511 if ((dsock = netlib_connectsock(AF_UNSPEC, host, port, "udp")) < 0) {
512 GPSD_LOG(LOG_ERROR, &session->context->errout,
513 "UDP device open error %s.\n",
514 netlib_errstr(dsock));
515 return -1;
516 } else
517 GPSD_LOG(LOG_SPIN, &session->context->errout,
518 "UDP device opened on fd %d\n", dsock);
519 session->gpsdata.gps_fd = dsock;
520 session->sourcetype = source_udp;
521 return session->gpsdata.gps_fd;
522 }
523 #endif /* NETFEED_ENABLE */
524 #ifdef PASSTHROUGH_ENABLE
525 if (str_starts_with(session->gpsdata.dev.path, "gpsd://")) {
526 char server[GPS_PATH_MAX], *host, *port;
527 socket_t dsock;
528 (void)strlcpy(server, session->gpsdata.dev.path + 7, sizeof(server));
529 INVALIDATE_SOCKET(session->gpsdata.gps_fd);
530 if (parse_uri_dest(session, server, &host, &port) == -1)
531 return -1;
532 if (!port)
533 port = DEFAULT_GPSD_PORT;
534 GPSD_LOG(LOG_INF, &session->context->errout,
535 "opening remote gpsd feed at %s, port %s.\n",
536 host, port);
537 if ((dsock = netlib_connectsock(AF_UNSPEC, host, port, "tcp")) < 0) {
538 GPSD_LOG(LOG_ERROR, &session->context->errout,
539 "remote gpsd device open error %s.\n",
540 netlib_errstr(dsock));
541 return -1;
542 } else
543 GPSD_LOG(LOG_SPIN, &session->context->errout,
544 "remote gpsd feed opened on fd %d\n", dsock);
545 /* watch to remote is issued when WATCH is */
546 session->gpsdata.gps_fd = dsock;
547 session->sourcetype = source_gpsd;
548 return session->gpsdata.gps_fd;
549 }
550 #endif /* PASSTHROUGH_ENABLE */
551 #if defined(NMEA2000_ENABLE)
552 if (str_starts_with(session->gpsdata.dev.path, "nmea2000://")) {
553 return nmea2000_open(session);
554 }
555 #endif /* defined(NMEA2000_ENABLE) */
556 /* fall through to plain serial open */
557 /* could be a naked /dev/ppsX */
558 return gpsd_serial_open(session);
559 }
560
gpsd_activate(struct gps_device_t * session,const int mode)561 int gpsd_activate(struct gps_device_t *session, const int mode)
562 /* acquire a connection to the GPS device */
563 {
564 if (mode == O_OPTIMIZE)
565 gpsd_run_device_hook(&session->context->errout,
566 session->gpsdata.dev.path, HOOK_ACTIVATE);
567 session->gpsdata.gps_fd = gpsd_open(session);
568 if (mode != O_CONTINUE)
569 session->mode = mode;
570
571 // cppcheck-suppress pointerLessThanZero
572 if (session->gpsdata.gps_fd < 0) {
573 /* return could be -1, PLACEHOLDING_FD, of UNALLOCATED_FD */
574 if ( PLACEHOLDING_FD == session->gpsdata.gps_fd ) {
575 /* it is /dev/ppsX, need to set devicename, etc. */
576 gpsd_clear(session);
577 }
578 return session->gpsdata.gps_fd;
579 }
580
581 #ifdef NON_NMEA0183_ENABLE
582 /* if it's a sensor, it must be probed */
583 if ((session->servicetype == service_sensor) &&
584 (session->sourcetype != source_can)) {
585 const struct gps_type_t **dp;
586
587 for (dp = gpsd_drivers; *dp; dp++) {
588 if ((*dp)->probe_detect != NULL) {
589 GPSD_LOG(LOG_PROG, &session->context->errout,
590 "Probing \"%s\" driver...\n",
591 (*dp)->type_name);
592 /* toss stale data */
593 (void)tcflush(session->gpsdata.gps_fd, TCIOFLUSH);
594 if ((*dp)->probe_detect(session) != 0) {
595 GPSD_LOG(LOG_PROG, &session->context->errout,
596 "Probe found \"%s\" driver...\n",
597 (*dp)->type_name);
598 session->device_type = *dp;
599 gpsd_assert_sync(session);
600 goto foundit;
601 } else
602 GPSD_LOG(LOG_PROG, &session->context->errout,
603 "Probe not found \"%s\" driver...\n",
604 (*dp)->type_name);
605 }
606 }
607 GPSD_LOG(LOG_PROG, &session->context->errout,
608 "no probe matched...\n");
609 }
610 foundit:
611 #endif /* NON_NMEA0183_ENABLE */
612
613 gpsd_clear(session);
614 GPSD_LOG(LOG_INF, &session->context->errout,
615 "gpsd_activate(%d): activated GPS (fd %d)\n",
616 session->mode, session->gpsdata.gps_fd);
617 /*
618 * We might know the device's type, but we shouldn't assume it has
619 * retained its settings. A revert hook might well have undone
620 * them on the previous close. Fire a reactivate event so drivers
621 * can do something about this if they choose.
622 */
623 if (session->device_type != NULL
624 && session->device_type->event_hook != NULL)
625 session->device_type->event_hook(session, event_reactivate);
626 return session->gpsdata.gps_fd;
627 }
628
629
630 /*****************************************************************************
631
632 Carl Carter of SiRF supplied this algorithm for computing DOPs from
633 a list of visible satellites (some typos corrected)...
634
635 For satellite n, let az(n) = azimuth angle from North and el(n) be elevation.
636 Let:
637
638 a(k, 1) = sin az(k) * cos el(k)
639 a(k, 2) = cos az(k) * cos el(k)
640 a(k, 3) = sin el(k)
641
642 Then form the line-of-sight matrix A for satellites used in the solution:
643
644 | a(1,1) a(1,2) a(1,3) 1 |
645 | a(2,1) a(2,2) a(2,3) 1 |
646 | : : : : |
647 | a(n,1) a(n,2) a(n,3) 1 |
648
649 And its transpose A~:
650
651 |a(1, 1) a(2, 1) . . . a(n, 1) |
652 |a(1, 2) a(2, 2) . . . a(n, 2) |
653 |a(1, 3) a(2, 3) . . . a(n, 3) |
654 | 1 1 . . . 1 |
655
656 Compute the covariance matrix (A~*A)^-1, which is guaranteed symmetric:
657
658 | s(x)^2 s(x)*s(y) s(x)*s(z) s(x)*s(t) |
659 | s(y)*s(x) s(y)^2 s(y)*s(z) s(y)*s(t) |
660 | s(z)*s(x) s(z)*s(y) s(z)^2 s(z)*s(t) |
661 | s(t)*s(x) s(t)*s(y) s(t)*s(z) s(t)^2 |
662
663 Then:
664
665 GDOP = sqrt(s(x)^2 + s(y)^2 + s(z)^2 + s(t)^2)
666 TDOP = sqrt(s(t)^2)
667 PDOP = sqrt(s(x)^2 + s(y)^2 + s(z)^2)
668 HDOP = sqrt(s(x)^2 + s(y)^2)
669 VDOP = sqrt(s(z)^2)
670
671 Here's how we implement it...
672
673 First, each compute element P(i,j) of the 4x4 product A~*A.
674 If S(k=1,k=n): f(...) is the sum of f(...) as k varies from 1 to n, then
675 applying the definition of matrix product tells us:
676
677 P(i,j) = S(k=1,k=n): B(i, k) * A(k, j)
678
679 But because B is the transpose of A, this reduces to
680
681 P(i,j) = S(k=1,k=n): A(k, i) * A(k, j)
682
683 This is not, however, the entire algorithm that SiRF uses. Carl writes:
684
685 > As you note, with rounding accounted for, most values agree exactly, and
686 > those that don't agree our number is higher. That is because we
687 > deweight some satellites and account for that in the DOP calculation.
688 > If a satellite is not used in a solution at the same weight as others,
689 > it should not contribute to DOP calculation at the same weight. So our
690 > internal algorithm does a compensation for that which you would have no
691 > way to duplicate on the outside since we don't output the weighting
692 > factors. In fact those are not even available to API users.
693
694 Queried about the deweighting, Carl says:
695
696 > In the SiRF tracking engine, each satellite track is assigned a quality
697 > value based on the tracker's estimate of that signal. It includes C/No
698 > estimate, ability to hold onto the phase, stability of the I vs. Q phase
699 > angle, etc. The navigation algorithm then ranks all the tracks into
700 > quality order and selects which ones to use in the solution and what
701 > weight to give those used in the solution. The process is actually a
702 > bit of a "trial and error" method -- we initially use all available
703 > tracks in the solution, then we sequentially remove the lowest quality
704 > ones until the solution stabilizes. The weighting is inherent in the
705 > Kalman filter algorithm. Once the solution is stable, the DOP is
706 > computed from those SVs used, and there is an algorithm that looks at
707 > the quality ratings and determines if we need to deweight any.
708 > Likewise, if we use altitude hold mode for a 3-SV solution, we deweight
709 > the phantom satellite at the center of the Earth.
710
711 So we cannot exactly duplicate what SiRF does internally. We'll leave
712 HDOP alone and use our computed values for VDOP and PDOP. Note, this
713 may have to change in the future if this code is used by a non-SiRF
714 driver.
715
716 ******************************************************************************/
717
718
fill_dop(const struct gpsd_errout_t * errout,const struct gps_data_t * gpsdata,struct dop_t * dop)719 static gps_mask_t fill_dop(const struct gpsd_errout_t *errout,
720 const struct gps_data_t * gpsdata,
721 struct dop_t * dop)
722 {
723 double prod[4][4];
724 double inv[4][4];
725 double satpos[MAXCHANNELS][4];
726 double xdop, ydop, hdop, vdop, pdop, tdop, gdop;
727 int i, j, k, n;
728
729 memset(satpos, 0, sizeof(satpos));
730
731 for (n = k = 0; k < gpsdata->satellites_visible; k++) {
732 if (!gpsdata->skyview[k].used) {
733 /* skip unused sats */
734 continue;
735 }
736 if (1 > gpsdata->skyview[k].PRN) {
737 /* skip bad PRN */
738 continue;
739 }
740 if (0 == isfinite(gpsdata->skyview[k].azimuth) ||
741 0 > gpsdata->skyview[k].azimuth ||
742 359 < gpsdata->skyview[k].azimuth) {
743 /* skip bad azimuth */
744 continue;
745 }
746 if (0 == isfinite(gpsdata->skyview[k].elevation) ||
747 90 < fabs(gpsdata->skyview[k].elevation)) {
748 /* skip bad elevation */
749 continue;
750 }
751 const struct satellite_t *sp = &gpsdata->skyview[k];
752 satpos[n][0] = sin(sp->azimuth * DEG_2_RAD)
753 * cos(sp->elevation * DEG_2_RAD);
754 satpos[n][1] = cos(sp->azimuth * DEG_2_RAD)
755 * cos(sp->elevation * DEG_2_RAD);
756 satpos[n][2] = sin(sp->elevation * DEG_2_RAD);
757 satpos[n][3] = 1;
758 GPSD_LOG(LOG_INF, errout, "PRN=%3d az=%.1f ael%.1f (%f, %f, %f)\n",
759 gpsdata->skyview[k].PRN,
760 gpsdata->skyview[k].azimuth,
761 gpsdata->skyview[k].elevation,
762 satpos[n][0], satpos[n][1], satpos[n][2]);
763 n++;
764 }
765 /* can't use gpsdata->satellites_used as that is a counter for xxGSA,
766 * and gets cleared at odd times */
767 GPSD_LOG(LOG_INF, errout, "Sats used (%d):\n", n);
768
769 /* If we don't have 4 satellites then we don't have enough information to calculate DOPS */
770 if (n < 4) {
771 #ifdef __UNUSED__
772 GPSD_LOG(LOG_RAW, errout,
773 "Not enough satellites available %d < 4:\n",
774 n);
775 #endif /* __UNUSED__ */
776 return 0; /* Is this correct return code here? or should it be ERROR_SET */
777 }
778
779 memset(prod, 0, sizeof(prod));
780 memset(inv, 0, sizeof(inv));
781
782 #ifdef __UNUSED__
783 GPSD_LOG(LOG_INF, errout, "Line-of-sight matrix:\n");
784 for (k = 0; k < n; k++) {
785 GPSD_LOG(LOG_INF, errout, "%f %f %f %f\n",
786 satpos[k][0], satpos[k][1], satpos[k][2], satpos[k][3]);
787 }
788 #endif /* __UNUSED__ */
789
790 for (i = 0; i < 4; ++i) { //< rows
791 for (j = 0; j < 4; ++j) { //< cols
792 prod[i][j] = 0.0;
793 for (k = 0; k < n; ++k) {
794 prod[i][j] += satpos[k][i] * satpos[k][j];
795 }
796 }
797 }
798
799 #ifdef __UNUSED__
800 GPSD_LOG(LOG_INF, errout, "product:\n");
801 for (k = 0; k < 4; k++) {
802 GPSD_LOG(LOG_INF, errout, "%f %f %f %f\n",
803 prod[k][0], prod[k][1], prod[k][2], prod[k][3]);
804 }
805 #endif /* __UNUSED__ */
806
807 if (matrix_invert(prod, inv)) {
808 #ifdef __UNUSED__
809 /*
810 * Note: this will print garbage unless all the subdeterminants
811 * are computed in the invert() function.
812 */
813 GPSD_LOG(LOG_RAW, errout, "inverse:\n");
814 for (k = 0; k < 4; k++) {
815 GPSD_LOG(LOG_RAW, errout,
816 "%f %f %f %f\n",
817 inv[k][0], inv[k][1], inv[k][2], inv[k][3]);
818 }
819 #endif /* __UNUSED__ */
820 } else {
821 #ifndef USE_QT
822 GPSD_LOG(LOG_DATA, errout,
823 "LOS matrix is singular, can't calculate DOPs - source '%s'\n",
824 gpsdata->dev.path);
825 #endif
826 return 0;
827 }
828
829 xdop = sqrt(inv[0][0]);
830 ydop = sqrt(inv[1][1]);
831 hdop = sqrt(inv[0][0] + inv[1][1]);
832 vdop = sqrt(inv[2][2]);
833 pdop = sqrt(inv[0][0] + inv[1][1] + inv[2][2]);
834 tdop = sqrt(inv[3][3]);
835 gdop = sqrt(inv[0][0] + inv[1][1] + inv[2][2] + inv[3][3]);
836
837 #ifndef USE_QT
838 GPSD_LOG(LOG_DATA, errout,
839 "DOPS computed/reported: X=%f/%f, Y=%f/%f, H=%f/%f, V=%f/%f, "
840 "P=%f/%f, T=%f/%f, G=%f/%f\n",
841 xdop, dop->xdop, ydop, dop->ydop, hdop, dop->hdop, vdop,
842 dop->vdop, pdop, dop->pdop, tdop, dop->tdop, gdop, dop->gdop);
843 #endif
844
845 /* Check to see which DOPs we already have. Save values if no value
846 * from the GPS. Do not overwrite values which came from the GPS */
847 if (isfinite(dop->xdop) == 0) {
848 dop->xdop = xdop;
849 }
850 if (isfinite(dop->ydop) == 0) {
851 dop->ydop = ydop;
852 }
853 if (isfinite(dop->hdop) == 0) {
854 dop->hdop = hdop;
855 }
856 if (isfinite(dop->vdop) == 0) {
857 dop->vdop = vdop;
858 }
859 if (isfinite(dop->pdop) == 0) {
860 dop->pdop = pdop;
861 }
862 if (isfinite(dop->tdop) == 0) {
863 dop->tdop = tdop;
864 }
865 if (isfinite(dop->gdop) == 0) {
866 dop->gdop = gdop;
867 }
868
869 return DOP_SET;
870 }
871
872 /* compute errors and derived quantities
873 * also a handy place to do final sanity checking */
gpsd_error_model(struct gps_device_t * session)874 static void gpsd_error_model(struct gps_device_t *session)
875 {
876 struct gps_fix_t *fix; /* current fix */
877 struct gps_fix_t *lastfix; /* last fix, maybe same time stamp */
878 struct gps_fix_t *oldfix; /* old fix, previsou time stamp */
879 double deltatime = -1.0; /* time span to compute rates */
880
881 /*
882 * Now we compute derived quantities. This is where the tricky error-
883 * modeling stuff goes. Presently we don't know how to derive
884 * time error.
885 *
886 * Some drivers set the position-error fields. Only the Zodiacs
887 * report speed error. No NMEA 183 reports climb error. GPXTE
888 * and PSRFEPE can report track error, but are rare.
889 *
890 * The UERE constants are our assumption about the base error of
891 * GPS fixes in different directions.
892 */
893 #define H_UERE_NO_DGPS 15.0 /* meters, 95% confidence */
894 #define H_UERE_WITH_DGPS 3.75 /* meters, 95% confidence */
895 #define V_UERE_NO_DGPS 23.0 /* meters, 95% confidence */
896 #define V_UERE_WITH_DGPS 5.75 /* meters, 95% confidence */
897 #define P_UERE_NO_DGPS 19.0 /* meters, 95% confidence */
898 #define P_UERE_WITH_DGPS 4.75 /* meters, 95% confidence */
899 double h_uere, v_uere, p_uere;
900
901 if (NULL == session)
902 return;
903
904 fix = &session->gpsdata.fix;
905 lastfix = &session->lastfix;
906 oldfix = &session->oldfix;
907
908 if (0 < fix->time.tv_sec) {
909 /* we have a time for this merge data */
910
911 deltatime = TS_SUB_D(&fix->time, &lastfix->time);
912
913 if (0.0099 < fabs(deltatime)) {
914 /* Time just moved, probably forward at least 10 ms.
915 * Lastfix is now the previous (old) fix. */
916 *oldfix = *lastfix;
917 } else {
918 // compute delta from old fix
919 deltatime = TS_SUB_D(&fix->time, &oldfix->time);
920 }
921 }
922 /* Sanity check for negative delta? */
923
924 h_uere =
925 (session->gpsdata.status ==
926 STATUS_DGPS_FIX ? H_UERE_WITH_DGPS : H_UERE_NO_DGPS);
927 v_uere =
928 (session->gpsdata.status ==
929 STATUS_DGPS_FIX ? V_UERE_WITH_DGPS : V_UERE_NO_DGPS);
930 p_uere =
931 (session->gpsdata.status ==
932 STATUS_DGPS_FIX ? P_UERE_WITH_DGPS : P_UERE_NO_DGPS);
933
934 if (0 == isfinite(fix->latitude) ||
935 0 == isfinite(fix->longitude) || /* both lat/lon, or none */
936 90.0 < fabs(fix->latitude) || /* lat out of range */
937 180.0 < fabs(fix->longitude)) { /* lon out of range */
938 fix->latitude = fix->longitude = NAN;
939 }
940 /* validate ECEF */
941 if (0 == isfinite(fix->ecef.x) ||
942 0 == isfinite(fix->ecef.y) ||
943 0 == isfinite(fix->ecef.z) ||
944 10.0 >= (fabs(fix->ecef.x) +
945 fabs(fix->ecef.y) +
946 fabs(fix->ecef.z))) { /* all zeros */
947 fix->ecef.x = fix->ecef.y = fix->ecef.z = NAN;
948 }
949
950 /* if we have not lat/lon, but do have ECEF, calculate lat/lon */
951 if ((0 == isfinite(fix->longitude) ||
952 0 == isfinite(fix->latitude)) &&
953 0 != isfinite(fix->ecef.x)) {
954 session->gpsdata.set |= ecef_to_wgs84fix(fix,
955 fix->ecef.x, fix->ecef.y,
956 fix->ecef.z, fix->ecef.vx,
957 fix->ecef.vy, fix->ecef.vz);
958 }
959
960 /* If you are in a rocket, and your GPS is ITAR unlocked, then
961 * triple check these sanity checks.
962 *
963 * u-blox 8: Max altitude: 50,000m
964 * Max horizontal speed: 250 m/s
965 * Max climb: 100 m/s
966 *
967 * u-blox ZED-F9P: Max Velocity: 500 m/s
968 */
969
970 /* sanity check the speed, 10,000 m/s should be a nice max
971 * Low Earth Orbit (LEO) is about 7,800 m/s */
972 if (9999.9 < fabs(fix->speed))
973 fix->speed = NAN;
974
975 if (9999.9 < fabs(fix->NED.velN))
976 fix->NED.velN = NAN;
977 if (9999.9 < fabs(fix->NED.velE))
978 fix->NED.velE = NAN;
979 if (9999.9 < fabs(fix->NED.velD))
980 fix->NED.velD = NAN;
981
982 /* sanity check the climb, 10,000 m/s should be a nice max */
983 if (9999.9 < fabs(fix->climb))
984 fix->climb = NAN;
985 if (0 != isfinite(fix->NED.velD) &&
986 0 == isfinite(fix->climb)) {
987 /* have good velD, use it for climb */
988 fix->climb = -fix->NED.velD;
989 }
990
991 /* compute speed and track from velN and velE if needed and possible */
992 if (0 != isfinite(fix->NED.velN) &&
993 0 != isfinite(fix->NED.velE)) {
994 if (0 == isfinite(fix->speed)) {
995 fix->speed = hypot(fix->NED.velN, fix->NED.velE);
996 }
997 if (0 == isfinite(fix->track)) {
998 fix->track = atan2(fix->NED.velE, fix->NED.velN) * RAD_2_DEG;
999 // normalized later
1000 }
1001 }
1002
1003 /*
1004 * OK, this is not an error computation, but we're at the right
1005 * place in the architecture for it. Compute geoid separation
1006 * and altHAE and altMSL in the simplest possible way.
1007 */
1008
1009 /* geoid (ellipsoid) separation and variation */
1010 if (0 != isfinite(fix->latitude) &&
1011 0 != isfinite(fix->longitude)) {
1012 if (0 == isfinite(fix->geoid_sep)) {
1013 fix->geoid_sep = wgs84_separation(fix->latitude,
1014 fix->longitude);
1015 }
1016 if (0 == isfinite(fix->magnetic_var) ||
1017 0.09 >= fabs(fix->magnetic_var)) {
1018 /* some GPS set 0.0,E, or 0,W instead of blank */
1019 fix->magnetic_var = mag_var(fix->latitude,
1020 fix->longitude);
1021 }
1022 }
1023
1024 if (0 != isfinite(fix->magnetic_var)) {
1025 if (0 == isfinite(fix->magnetic_track) &&
1026 0 != isfinite(fix->track)) {
1027
1028 // calculate mag track, normalized later
1029 fix->magnetic_track = fix->track + fix->magnetic_var;
1030 } else if (0 != isfinite(fix->magnetic_track) &&
1031 0 == isfinite(fix->track)) {
1032
1033 // calculate true track, normalized later
1034 fix->track = fix->magnetic_track - fix->magnetic_var;
1035 }
1036 }
1037 if (0 != isfinite(fix->track)) {
1038 // normalize true track
1039 DEG_NORM(fix->track);
1040 }
1041
1042 if (0 != isfinite(fix->magnetic_track)) {
1043 // normalize mag track
1044 DEG_NORM(fix->magnetic_track);
1045 }
1046
1047 if (0 != isfinite(fix->geoid_sep)) {
1048 if (0 != isfinite(fix->altHAE) &&
1049 0 == isfinite(fix->altMSL)) {
1050 /* compute missing altMSL */
1051 fix->altMSL = fix->altHAE - fix->geoid_sep;
1052 } else if (0 == isfinite(fix->altHAE) &&
1053 0 != isfinite(fix->altMSL)) {
1054 /* compute missing altHAE */
1055 fix->altHAE = fix->altMSL + fix->geoid_sep;
1056 }
1057 }
1058
1059 /*
1060 * OK, this is not an error computation, but we're at the right
1061 * place in the architecture for it. Compute speed over ground
1062 * and climb/sink in the simplest possible way.
1063 */
1064
1065 #ifdef __UNUSED__
1066 // debug code
1067 {
1068 char tbuf[JSON_DATE_MAX+1];
1069 GPSD_LOG(LOG_SHOUT, &session->context->errout,
1070 "time %s deltatime %f\n",
1071 timespec_to_iso8601(fix->time, tbuf, sizeof(tbuf)),
1072 deltatime);
1073 }
1074 #endif // __UNUSED__
1075
1076 if (0 < deltatime) {
1077 /* have a valid time duration */
1078 /* FIXME! ignore if large. maybe > 1 hour? */
1079
1080 if (MODE_2D <= fix->mode &&
1081 MODE_2D <= oldfix->mode) {
1082
1083 if (0 == isfinite(fix->speed)) {
1084 fix->speed = earth_distance(fix->latitude, fix->longitude,
1085 oldfix->latitude, oldfix->longitude)
1086 / deltatime;
1087 /* sanity check */
1088 if (9999.9 < fabs(fix->speed))
1089 fix->speed = NAN;
1090 }
1091
1092 if (MODE_3D <= fix->mode &&
1093 MODE_3D <= oldfix->mode &&
1094 0 == isfinite(fix->climb) &&
1095 0 != isfinite(fix->altHAE) &&
1096 0 != isfinite(oldfix->altHAE)) {
1097 fix->climb = (fix->altHAE - oldfix->altHAE) / deltatime;
1098
1099 /* sanity check the climb */
1100 if (9999.9 < fabs(fix->climb))
1101 fix->climb = NAN;
1102 }
1103 }
1104 }
1105
1106 /*
1107 * Field reports match the theoretical prediction that
1108 * expected time error should be half the resolution of
1109 * the GPS clock, so we put the bound of the error
1110 * in as a constant pending getting it from each driver.
1111 *
1112 * In an ideal world, we'd increase this if no leap-second has
1113 * been seen and it's less than 750s (one almanac load cycle) from
1114 * device powerup. Alas, we have no way to know when device
1115 * powerup occurred - depending on the receiver design it could be
1116 * when the hardware was first powered up or when it was first
1117 * opened. Also, some devices (notably plain NMEA0183 receivers)
1118 * never ship an indication of when they have valid leap second.
1119 */
1120 if (0 < fix->time.tv_sec &&
1121 0 == isfinite(fix->ept)) {
1122 /* can we compute ept from tdop? */
1123 fix->ept = 0.005;
1124 }
1125
1126 /* Other error computations depend on having a valid fix */
1127 if (MODE_2D <= fix->mode) {
1128 if (0 == isfinite(fix->epx) &&
1129 0 != isfinite(session->gpsdata.dop.hdop)) {
1130 fix->epx = session->gpsdata.dop.xdop * h_uere;
1131 }
1132
1133 if (0 == isfinite(fix->epy) &&
1134 0 != isfinite(session->gpsdata.dop.hdop)) {
1135 fix->epy = session->gpsdata.dop.ydop * h_uere;
1136 }
1137
1138 if (MODE_3D <= fix->mode &&
1139 0 == isfinite(fix->epv) &&
1140 0 != isfinite(session->gpsdata.dop.vdop)) {
1141 fix->epv = session->gpsdata.dop.vdop * v_uere;
1142 }
1143
1144 /* 2D error */
1145 if (0 == isfinite(fix->eph) &&
1146 0 != isfinite(session->gpsdata.dop.hdop)) {
1147 fix->eph = session->gpsdata.dop.hdop * p_uere;
1148 }
1149
1150 /* 3D error */
1151 if (0 == isfinite(fix->sep) &&
1152 0 != isfinite(session->gpsdata.dop.pdop)) {
1153 fix->sep = session->gpsdata.dop.pdop * p_uere;
1154 }
1155
1156 /*
1157 * If we have a current fix and an old fix, and the packet handler
1158 * didn't set the speed error, climb error or track error members
1159 * itself, try to compute them now.
1160 */
1161
1162 #define EMAX(x, y) (((x) > (y)) ? (x) : (y))
1163
1164 if (0 < deltatime &&
1165 MODE_2D <= oldfix->mode) {
1166
1167 if (0 == isfinite(fix->eps) &&
1168 0 != isfinite(oldfix->epx) &&
1169 0 != isfinite(oldfix->epy)) {
1170 fix->eps = (EMAX(oldfix->epx, oldfix->epy) +
1171 EMAX(fix->epx, fix->epy)) / deltatime;
1172 }
1173
1174 if (0 == isfinite(fix->epd)) {
1175 /*
1176 * We compute a track error estimate solely from the
1177 * position of this fix and the last one. The maximum
1178 * track error, as seen from the position of last fix, is
1179 * the angle subtended by the two most extreme possible
1180 * error positions of the current fix; the expected track
1181 * error is half that. Let the position of the old fix be
1182 * A and of the new fix B. We model the view from A as
1183 * two right triangles ABC and ABD with BC and BD both
1184 * having the length of the new fix's estimated error.
1185 * adj = len(AB), opp = len(BC) = len(BD), hyp = len(AC) =
1186 * len(AD). This leads to spurious uncertainties
1187 * near 180 when we're moving slowly; to avoid reporting
1188 * garbage, throw back NaN if the distance from the previous
1189 * fix is less than the error estimate.
1190 */
1191 double adj = earth_distance(oldfix->latitude, oldfix->longitude,
1192 fix->latitude, fix->longitude);
1193 double opp = EMAX(fix->epx, fix->epy);
1194 if (isfinite(adj) != 0 && adj > opp) {
1195 double hyp = sqrt(adj * adj + opp * opp);
1196 fix->epd = RAD_2_DEG * 2 * asin(opp / hyp);
1197 }
1198 }
1199
1200 if (0 == isfinite(fix->epc) &&
1201 0 != isfinite(fix->epv) &&
1202 0 != isfinite(oldfix->epv)) {
1203 /* Is this really valid? */
1204 /* if vertical uncertainties are zero this will be too */
1205 fix->epc = (oldfix->epv + fix->epv) / deltatime;
1206 }
1207 }
1208 }
1209
1210 #ifdef __UNUSED__
1211 {
1212 // Debug code.
1213 char tbuf[JSON_DATE_MAX+1];
1214 GPSD_LOG(&session->context->errout, 0,
1215 "DEBUG: %s deltatime %.3f, speed %0.3f climb %.3f "
1216 "epc %.3f fixHAE %.3f oldHAE %.3f\n",
1217 timespec_to_iso8601(fix->time, tbuf, sizeof(tbuf)),
1218 deltatime, fix->speed, fix->climb, fix->epc,
1219 fix->altHAE, oldfix->altHAE);
1220 }
1221 #endif // __UNUSED__
1222
1223 if (0 < fix->time.tv_sec) {
1224 /* save lastfix, not yet oldfix, for later error computations */
1225 *lastfix = *fix;
1226 }
1227 }
1228
gpsd_await_data(fd_set * rfds,fd_set * efds,const int maxfd,fd_set * all_fds,struct gpsd_errout_t * errout)1229 int gpsd_await_data(fd_set *rfds,
1230 fd_set *efds,
1231 const int maxfd,
1232 fd_set *all_fds,
1233 struct gpsd_errout_t *errout)
1234 /* await data from any socket in the all_fds set */
1235 {
1236 int status;
1237
1238 FD_ZERO(efds);
1239 *rfds = *all_fds;
1240 GPSD_LOG(LOG_RAW + 1, errout, "select waits\n");
1241 /*
1242 * Poll for user commands or GPS data. The timeout doesn't
1243 * actually matter here since select returns whenever one of
1244 * the file descriptors in the set goes ready. The point
1245 * of tracking maxfd is to keep the set of descriptors that
1246 * pselect(2) has to poll here as small as possible (for
1247 * low-clock-rate SBCs and the like).
1248 *
1249 * pselect(2) is preferable to vanilla select, to eliminate
1250 * the once-per-second wakeup when no sensors are attached.
1251 * This cuts power consumption.
1252 */
1253 errno = 0;
1254
1255 status = pselect(maxfd + 1, rfds, NULL, NULL, NULL, NULL);
1256 if (status == -1) {
1257 if (errno == EINTR)
1258 return AWAIT_NOT_READY;
1259 else if (errno == EBADF) {
1260 int fd;
1261 for (fd = 0; fd < (int)FD_SETSIZE; fd++)
1262 /*
1263 * All we care about here is a cheap, fast, uninterruptible
1264 * way to check if a file descriptor is valid.
1265 */
1266 if (FD_ISSET(fd, all_fds) && fcntl(fd, F_GETFL, 0) == -1) {
1267 FD_CLR(fd, all_fds);
1268 FD_SET(fd, efds);
1269 }
1270 return AWAIT_NOT_READY;
1271 } else {
1272 GPSD_LOG(LOG_ERROR, errout, "select: %s\n", strerror(errno));
1273 return AWAIT_FAILED;
1274 }
1275 }
1276
1277 if (errout->debug >= LOG_SPIN) {
1278 int i;
1279 char dbuf[BUFSIZ];
1280 timespec_t ts_now;
1281 char ts_str[TIMESPEC_LEN];
1282
1283 dbuf[0] = '\0';
1284 for (i = 0; i < (int)FD_SETSIZE; i++)
1285 if (FD_ISSET(i, all_fds))
1286 str_appendf(dbuf, sizeof(dbuf), "%d ", i);
1287 str_rstrip_char(dbuf, ' ');
1288 (void)strlcat(dbuf, "} -> {", sizeof(dbuf));
1289 for (i = 0; i < (int)FD_SETSIZE; i++)
1290 if (FD_ISSET(i, rfds))
1291 str_appendf(dbuf, sizeof(dbuf), " %d ", i);
1292
1293 (void)clock_gettime(CLOCK_REALTIME, &ts_now);
1294 GPSD_LOG(LOG_SPIN, errout,
1295 "pselect() {%s} at %s (errno %d)\n",
1296 dbuf,
1297 timespec_str(&ts_now, ts_str, sizeof(ts_str)),
1298 errno);
1299 }
1300
1301 return AWAIT_GOT_INPUT;
1302 }
1303
hunt_failure(struct gps_device_t * session)1304 static bool hunt_failure(struct gps_device_t *session)
1305 /* after a bad packet, what should cue us to go to next autobaud setting? */
1306 {
1307 /*
1308 * We have tried three different tests here.
1309 *
1310 * The first was session->badcount++>1. This worked very well on
1311 * ttys for years and years, but caused failure to sync on TCP/IP
1312 * sources, which have I/O boundaries in mid-packet more often
1313 * than RS232 ones. There's a test for this at
1314 * test/daemon/tcp-torture.log.
1315 *
1316 * The second was session->badcount++>1 && session->lexer.state==0.
1317 * Fail hunt only if we get a second consecutive bad packet
1318 * and the lexer is in ground state. We don't want to fail on
1319 * a first bad packet because the source might have a burst of
1320 * leading garbage after open. We don't want to fail if the
1321 * lexer is not in ground state, because that means the read
1322 * might have picked up a valid partial packet - better to go
1323 * back around the loop and pick up more data.
1324 *
1325 * The "&& session->lexer.state==0" guard causes an intermittent
1326 * hang while autobauding on SiRF IIIs (but not on SiRF-IIs, oddly
1327 * enough). Removing this conjunct resurrected the failure
1328 * of test/daemon/tcp-torture.log.
1329 *
1330 * Our third attempt, isatty(session->gpsdata.gps_fd) != 0
1331 * && session->badcount++>1, reverts to the old test that worked
1332 * well on ttys for ttys and prevents non-tty devices from *ever*
1333 * having hunt failures. This has the cost that non-tty devices
1334 * will never get kicked off for presenting bad packets.
1335 *
1336 * This test may need further revision.
1337 */
1338 return isatty(session->gpsdata.gps_fd) != 0 && session->badcount++>1;
1339 }
1340
gpsd_poll(struct gps_device_t * session)1341 gps_mask_t gpsd_poll(struct gps_device_t *session)
1342 /* update the stuff in the scoreboard structure */
1343 {
1344 ssize_t newlen;
1345 bool driver_change = false;
1346 timespec_t ts_now;
1347 timespec_t delta;
1348 char ts_buf[TIMESPEC_LEN];
1349
1350 gps_clear_fix(&session->newdata);
1351
1352 /*
1353 * Input just became available from a sensor, but no read from the
1354 * device has yet been done.
1355 *
1356 * What we actually do here is trickier. For latency-timing
1357 * purposes, we want to know the time at the start of the current
1358 * recording cycle. We rely on the fact that even at 4800bps
1359 * there's a quiet time perceptible to the human eye in gpsmon
1360 * between when the last character of the last packet in a
1361 * 1-second cycle ships and when the next reporting cycle
1362 * ships. Because the cycle time is fixed, higher baud rates will
1363 * make this gap larger.
1364 *
1365 * Thus, we look for an inter-character delay much larger than an
1366 * average 4800bps sentence time. How should this delay be set? Well,
1367 * counting framing bits and erring on the side of caution, it's
1368 * about 480 characters per second or 2083 microeconds per character;
1369 * that's almost exactly 0.125 seconds per average 60-char sentence.
1370 * Doubling this to avoid false positives, we look for an inter-character
1371 * delay of greater than 0.250s.
1372 *
1373 * The above assumes a cycle time of 1 second. To get the minimum size of
1374 * the quiet period, we multiply by the device cycle time.
1375 *
1376 * We can sanity-check these calculation by watching logs. If we have set
1377 * MINIMUM_QUIET_TIME correctly, the "transmission pause" message below
1378 * will consistently be emitted just before the sentence that shows up
1379 * as start-of-cycle in gpsmon, and never emitted at any other point
1380 * in the cycle.
1381 *
1382 * In practice, it seems that edge detection succeeds at 9600bps but
1383 * fails at 4800bps. This is not surprising, as previous profiling has
1384 * indicated that at 4800bps some devices overrun a 1-second cycle time
1385 * with the data they transmit.
1386 */
1387 #define MINIMUM_QUIET_TIME 0.25
1388 if (session->lexer.outbuflen == 0) {
1389 /* beginning of a new packet */
1390 (void)clock_gettime(CLOCK_REALTIME, &ts_now);
1391 if (NULL != session->device_type &&
1392 (0 < session->lexer.start_time.tv_sec ||
1393 0 < session->lexer.start_time.tv_nsec)) {
1394 #ifdef RECONFIGURE_ENABLE
1395 const double min_cycle = TSTONS(&session->device_type->min_cycle);
1396 #else
1397 // Assume that all GNSS receivers are 1Hz
1398 const double min_cycle = 1;
1399 #endif /* RECONFIGURE_ENABLE */
1400 double quiet_time = (MINIMUM_QUIET_TIME * min_cycle);
1401 double gap;
1402
1403 gap = TS_SUB_D(&ts_now, &session->lexer.start_time);
1404
1405 if (gap > min_cycle)
1406 GPSD_LOG(LOG_WARN, &session->context->errout,
1407 "cycle-start detector failed.\n");
1408 else if (gap > quiet_time) {
1409 GPSD_LOG(LOG_PROG, &session->context->errout,
1410 "transmission pause of %f\n", gap);
1411 session->sor = ts_now;
1412 session->lexer.start_char = session->lexer.char_counter;
1413 }
1414 }
1415 session->lexer.start_time = ts_now;
1416 }
1417
1418 if (session->lexer.type >= COMMENT_PACKET) {
1419 session->observed |= PACKET_TYPEMASK(session->lexer.type);
1420 }
1421
1422 /* can we get a full packet from the device? */
1423 if (session->device_type != NULL) {
1424 newlen = session->device_type->get_packet(session);
1425 /* coverity[deref_ptr] */
1426 GPSD_LOG(LOG_RAW, &session->context->errout,
1427 "%s is known to be %s\n",
1428 session->gpsdata.dev.path,
1429 session->device_type->type_name);
1430 } else {
1431 newlen = generic_get(session);
1432 }
1433
1434 /* update the scoreboard structure from the GPS */
1435 GPSD_LOG(LOG_RAW + 1, &session->context->errout,
1436 "%s sent %zd new characters\n",
1437 session->gpsdata.dev.path, newlen);
1438
1439 (void)clock_gettime(CLOCK_REALTIME, &ts_now);
1440 TS_SUB(&delta, &ts_now, &session->gpsdata.online);
1441 if (newlen < 0) { /* read error */
1442 GPSD_LOG(LOG_INF, &session->context->errout,
1443 "GPS on %s returned error %zd (%s sec since data)\n",
1444 session->gpsdata.dev.path, newlen,
1445 timespec_str(&delta, ts_buf, sizeof(ts_buf)));
1446 session->gpsdata.online.tv_sec = 0;
1447 session->gpsdata.online.tv_nsec = 0;
1448 return ERROR_SET;
1449 } else if (newlen == 0) { /* zero length read, possible EOF */
1450 /*
1451 * Multiplier is 2 to avoid edge effects due to sampling at the exact
1452 * wrong time...
1453 */
1454 if (0 < session->gpsdata.online.tv_sec &&
1455 // FIXME: do this with integer math...
1456 TSTONS(&delta) >= (TSTONS(&session->gpsdata.dev.cycle) * 2)) {
1457 GPSD_LOG(LOG_INF, &session->context->errout,
1458 "GPS on %s is offline (%s sec since data)\n",
1459 session->gpsdata.dev.path,
1460 timespec_str(&delta, ts_buf, sizeof(ts_buf)));
1461 session->gpsdata.online.tv_sec = 0;
1462 session->gpsdata.online.tv_nsec = 0;
1463 }
1464 return NODATA_IS;
1465 } else /* (newlen > 0) */ {
1466 GPSD_LOG(LOG_RAW, &session->context->errout,
1467 "packet sniff on %s finds type %d\n",
1468 session->gpsdata.dev.path, session->lexer.type);
1469 if (session->lexer.type == COMMENT_PACKET) {
1470 if (strcmp((const char *)session->lexer.outbuffer, "# EOF\n") == 0) {
1471 GPSD_LOG(LOG_PROG, &session->context->errout,
1472 "synthetic EOF\n");
1473 return EOF_IS;
1474 }
1475 else
1476 GPSD_LOG(LOG_PROG, &session->context->errout,
1477 "comment, sync lock deferred\n");
1478 /* FALL THROUGH */
1479 } else if (session->lexer.type > COMMENT_PACKET) {
1480 if (session->device_type == NULL)
1481 driver_change = true;
1482 else {
1483 int newtype = session->lexer.type;
1484 /*
1485 * Are we seeing a new packet type? Then we probably
1486 * want to change drivers.
1487 */
1488 bool new_packet_type =
1489 (newtype != session->device_type->packet_type);
1490 /*
1491 * Possibly the old driver has a mode-switcher method, in
1492 * which case we know it can handle NMEA itself and may
1493 * want to do special things (like tracking whether a
1494 * previous mode switch to binary succeeded in suppressing
1495 * NMEA).
1496 */
1497 #ifdef RECONFIGURE_ENABLE
1498 bool dependent_nmea = (newtype == NMEA_PACKET &&
1499 session->device_type->mode_switcher != NULL);
1500 #else
1501 bool dependent_nmea = false;
1502 #endif /* RECONFIGURE_ENABLE */
1503
1504 /*
1505 * Compute whether to switch drivers.
1506 * If the previous driver type was sticky and this one
1507 * isn't, we'll revert after processing the packet.
1508 */
1509 driver_change = new_packet_type && !dependent_nmea;
1510 }
1511 if (driver_change) {
1512 const struct gps_type_t **dp;
1513
1514 for (dp = gpsd_drivers; *dp; dp++)
1515 if (session->lexer.type == (*dp)->packet_type) {
1516 GPSD_LOG(LOG_PROG, &session->context->errout,
1517 "switching to match packet type %d: %s\n",
1518 session->lexer.type, gpsd_prettydump(session));
1519 (void)gpsd_switch_driver(session, (*dp)->type_name);
1520 break;
1521 }
1522 }
1523 session->badcount = 0;
1524 session->gpsdata.dev.driver_mode =
1525 (session->lexer.type > NMEA_PACKET) ? MODE_BINARY : MODE_NMEA;
1526 /* FALL THROUGH */
1527 } else if (hunt_failure(session) && !gpsd_next_hunt_setting(session)) {
1528 (void)clock_gettime(CLOCK_REALTIME, &ts_now);
1529 TS_SUB(&delta, &ts_now, &session->gpsdata.online);
1530 GPSD_LOG(LOG_INF, &session->context->errout,
1531 "hunt on %s failed (%s sec since data)\n",
1532 session->gpsdata.dev.path,
1533 timespec_str(&delta, ts_buf, sizeof(ts_buf)));
1534 return ERROR_SET;
1535 }
1536 }
1537
1538 if (session->lexer.outbuflen == 0) { /* got new data, but no packet */
1539 GPSD_LOG(LOG_RAW + 1, &session->context->errout,
1540 "New data on %s, not yet a packet\n",
1541 session->gpsdata.dev.path);
1542 return ONLINE_SET;
1543 } else { /* we have recognized a packet */
1544 gps_mask_t received = PACKET_SET;
1545 (void)clock_gettime(CLOCK_REALTIME, &session->gpsdata.online);
1546
1547 GPSD_LOG(LOG_RAW + 1, &session->context->errout,
1548 "Accepted packet on %s.\n",
1549 session->gpsdata.dev.path);
1550
1551 /* track the packet count since achieving sync on the device */
1552 if (driver_change &&
1553 (session->drivers_identified & (1 << session->driver_index)) == 0) {
1554 speed_t speed = gpsd_get_speed(session);
1555
1556 /* coverity[var_deref_op] */
1557 GPSD_LOG(LOG_INF, &session->context->errout,
1558 "%s identified as type %s, %ld sec @ %ubps\n",
1559 session->gpsdata.dev.path,
1560 session->device_type->type_name,
1561 (long)(time(NULL) - session->opentime),
1562 (unsigned int)speed);
1563
1564 /* fire the init_query method */
1565 if (session->device_type != NULL
1566 && session->device_type->init_query != NULL) {
1567 /*
1568 * We can force readonly off knowing this method does
1569 * not alter device state.
1570 */
1571 bool saved = session->context->readonly;
1572 session->context->readonly = false;
1573 session->device_type->init_query(session);
1574 session->context->readonly = saved;
1575 }
1576
1577 /* fire the identified hook */
1578 if (session->device_type != NULL
1579 && session->device_type->event_hook != NULL)
1580 session->device_type->event_hook(session, event_identified);
1581 session->lexer.counter = 0;
1582
1583 /* let clients know about this. */
1584 received |= DRIVER_IS;
1585
1586 /* mark the fact that this driver has been seen */
1587 session->drivers_identified |= (1 << session->driver_index);
1588 } else
1589 session->lexer.counter++;
1590
1591 /* fire the configure hook, on every packet. Seems excessive... */
1592 if (session->device_type != NULL
1593 && session->device_type->event_hook != NULL)
1594 session->device_type->event_hook(session, event_configure);
1595
1596 /*
1597 * The guard looks superfluous, but it keeps the rather expensive
1598 * gpsd_packetdump() function from being called even when the debug
1599 * level does not actually require it.
1600 */
1601 if (session->context->errout.debug >= LOG_RAW)
1602 GPSD_LOG(LOG_RAW, &session->context->errout,
1603 "raw packet of type %d, %zd:%s\n",
1604 session->lexer.type,
1605 session->lexer.outbuflen,
1606 gpsd_prettydump(session));
1607
1608
1609 /* Get data from current packet into the fix structure */
1610 if (session->lexer.type != COMMENT_PACKET)
1611 if (session->device_type != NULL
1612 && session->device_type->parse_packet != NULL)
1613 received |= session->device_type->parse_packet(session);
1614
1615 #ifdef RECONFIGURE_ENABLE
1616 /*
1617 * We may want to revert to the last driver that was marked
1618 * sticky. What this accomplishes is that if we've just
1619 * processed something like AIVDM, but a driver with control
1620 * methods or an event hook had been active before that, we
1621 * keep the information about those capabilities.
1622 */
1623 if (!STICKY(session->device_type)
1624 && session->last_controller != NULL
1625 && STICKY(session->last_controller))
1626 {
1627 session->device_type = session->last_controller;
1628 GPSD_LOG(LOG_PROG, &session->context->errout,
1629 "reverted to %s driver...\n",
1630 session->device_type->type_name);
1631 }
1632 #endif /* RECONFIGURE_ENABLE */
1633
1634 /* are we going to generate a report? if so, count characters */
1635 if ((received & REPORT_IS) != 0) {
1636 session->chars = session->lexer.char_counter - session->lexer.start_char;
1637 }
1638
1639 session->gpsdata.set = ONLINE_SET | received;
1640
1641 /*
1642 * Compute fix-quality data from the satellite positions.
1643 * These will not overwrite any DOPs reported from the packet
1644 * we just got.
1645 */
1646 if ((received & SATELLITE_SET) != 0
1647 && session->gpsdata.satellites_visible > 0) {
1648 session->gpsdata.set |= fill_dop(&session->context->errout,
1649 &session->gpsdata,
1650 &session->gpsdata.dop);
1651 }
1652
1653 /* copy/merge device data into staging buffers */
1654 if ((session->gpsdata.set & CLEAR_IS) != 0) {
1655 /* CLEAR_IS should only be set on first sentence of cycle */
1656 gps_clear_fix(&session->gpsdata.fix);
1657 gps_clear_att(&session->gpsdata.attitude);
1658 }
1659
1660 /* GPSD_LOG(LOG_PROG, &session->context->errout,
1661 "transfer mask: %s\n",
1662 gps_maskdump(session->gpsdata.set)); */
1663 gps_merge_fix(&session->gpsdata.fix,
1664 session->gpsdata.set, &session->newdata);
1665
1666 gpsd_error_model(session);
1667
1668 /*
1669 * Count good fixes. We used to check
1670 * session->gpsdata.status > STATUS_NO_FIX
1671 * here, but that wasn't quite right. That tells us whether
1672 * we think we have a valid fix for the current cycle, but remains
1673 * true while following non-fix packets are received. What we
1674 * really want to know is whether the last packet received was a
1675 * fix packet AND held a valid fix. We must ignore non-fix packets
1676 * AND packets which have fix data but are flagged as invalid. Some
1677 * devices output fix packets on a regular basis, even when unable
1678 * to derive a good fix. Such packets should set STATUS_NO_FIX.
1679 */
1680 if (0 != (session->gpsdata.set & (LATLON_SET|ECEF_SET))) {
1681 if ( session->gpsdata.status > STATUS_NO_FIX) {
1682 session->context->fixcnt++;
1683 session->fixcnt++;
1684 } else {
1685 session->context->fixcnt = 0;
1686 session->fixcnt = 0;
1687 }
1688 }
1689
1690 /*
1691 * Sanity check. This catches a surprising number of port and
1692 * driver errors, including 32-vs.-64-bit problems.
1693 */
1694 if ((session->gpsdata.set & TIME_SET) != 0) {
1695 if (session->newdata.time.tv_sec >
1696 (time(NULL) + (60 * 60 * 24 * 365))) {
1697 GPSD_LOG(LOG_WARN, &session->context->errout,
1698 "date (%lld) more than a year in the future!\n",
1699 (long long)session->newdata.time.tv_sec);
1700 } else if (session->newdata.time.tv_sec < 0) {
1701 GPSD_LOG(LOG_ERROR, &session->context->errout,
1702 "date (%lld) is negative!\n",
1703 (long long)session->newdata.time.tv_sec);
1704 }
1705 }
1706
1707 return session->gpsdata.set;
1708 }
1709 /* Should never get here */
1710 GPSD_LOG(LOG_EMERG, &session->context->errout,
1711 "fell out of gps_poll()!\n");
1712 return 0;
1713 }
1714
gpsd_multipoll(const bool data_ready,struct gps_device_t * device,void (* handler)(struct gps_device_t *,gps_mask_t),float reawake_time)1715 int gpsd_multipoll(const bool data_ready,
1716 struct gps_device_t *device,
1717 void (*handler)(struct gps_device_t *, gps_mask_t),
1718 float reawake_time)
1719 /* consume and handle packets from a specified device */
1720 {
1721 if (data_ready)
1722 {
1723 int fragments;
1724
1725 GPSD_LOG(LOG_RAW + 1, &device->context->errout,
1726 "polling %d\n", device->gpsdata.gps_fd);
1727
1728 #ifdef NETFEED_ENABLE
1729 /*
1730 * Strange special case - the opening transaction on an NTRIP connection
1731 * may not yet be completed. Try to ratchet things forward.
1732 */
1733 if (device->servicetype == service_ntrip
1734 && device->ntrip.conn_state != ntrip_conn_established) {
1735
1736 (void)ntrip_open(device, "");
1737 if (device->ntrip.conn_state == ntrip_conn_err) {
1738 GPSD_LOG(LOG_WARN, &device->context->errout,
1739 "connection to ntrip server failed\n");
1740 device->ntrip.conn_state = ntrip_conn_init;
1741 return DEVICE_ERROR;
1742 } else {
1743 return DEVICE_READY;
1744 }
1745 }
1746 #endif /* NETFEED_ENABLE */
1747
1748 for (fragments = 0; ; fragments++) {
1749 gps_mask_t changed = gpsd_poll(device);
1750
1751 if (changed == EOF_IS) {
1752 GPSD_LOG(LOG_WARN, &device->context->errout,
1753 "device signed off %s\n",
1754 device->gpsdata.dev.path);
1755 return DEVICE_EOF;
1756 } else if (changed == ERROR_SET) {
1757 GPSD_LOG(LOG_WARN, &device->context->errout,
1758 "device read of %s returned error or "
1759 "packet sniffer failed sync (flags %s)\n",
1760 device->gpsdata.dev.path,
1761 gps_maskdump(changed));
1762 return DEVICE_ERROR;
1763 } else if (changed == NODATA_IS) {
1764 /*
1765 * No data on the first fragment read means the device
1766 * fd may have been in an end-of-file condition on select.
1767 */
1768 if (fragments == 0) {
1769 GPSD_LOG(LOG_DATA, &device->context->errout,
1770 "%s returned zero bytes\n",
1771 device->gpsdata.dev.path);
1772 if (device->zerokill) {
1773 /* failed timeout-and-reawake, kill it */
1774 gpsd_deactivate(device);
1775 if (device->ntrip.works) {
1776 device->ntrip.works = false; // reset so we try this once only
1777 if (gpsd_activate(device, O_CONTINUE) < 0) {
1778 GPSD_LOG(LOG_WARN, &device->context->errout,
1779 "reconnect to ntrip server failed\n");
1780 return DEVICE_ERROR;
1781 } else {
1782 GPSD_LOG(LOG_INF, &device->context->errout,
1783 "reconnecting to ntrip server\n");
1784 return DEVICE_READY;
1785 }
1786 }
1787 } else if (reawake_time == 0) {
1788 return DEVICE_ERROR;
1789 } else {
1790 /*
1791 * Disable listening to this fd for long enough
1792 * that the buffer can fill up again.
1793 */
1794 GPSD_LOG(LOG_DATA, &device->context->errout,
1795 "%s will be repolled in %f seconds\n",
1796 device->gpsdata.dev.path, reawake_time);
1797 device->reawake = time(NULL) + reawake_time;
1798 return DEVICE_UNREADY;
1799 }
1800 }
1801 /*
1802 * No data on later fragment reads just means the
1803 * input buffer is empty. In this case break out
1804 * of the fragment-processing loop but consider
1805 * the device still good.
1806 */
1807 break;
1808 }
1809
1810 /* we got actual data, head off the reawake special case */
1811 device->zerokill = false;
1812 device->reawake = (time_t)0;
1813
1814 /* must have a full packet to continue */
1815 if ((changed & PACKET_SET) == 0)
1816 break;
1817
1818 /* conditional prevents mask dumper from eating CPU */
1819 if (device->context->errout.debug >= LOG_DATA) {
1820 if (device->lexer.type == BAD_PACKET)
1821 GPSD_LOG(LOG_DATA, &device->context->errout,
1822 "packet with bad checksum from %s\n",
1823 device->gpsdata.dev.path);
1824 else
1825 GPSD_LOG(LOG_DATA, &device->context->errout,
1826 "packet type %d from %s with %s\n",
1827 device->lexer.type,
1828 device->gpsdata.dev.path,
1829 gps_maskdump(device->gpsdata.set));
1830 }
1831
1832
1833 /* handle data contained in this packet */
1834 if (device->lexer.type != BAD_PACKET)
1835 handler(device, changed);
1836
1837 #ifdef __future__
1838 // this breaks: test/daemon/passthrough.log ??
1839 /*
1840 * Bernd Ocklin suggests:
1841 * Exit when a full packet was received and parsed.
1842 * This allows other devices to be serviced even if
1843 * this device delivers a full packet at every single
1844 * read.
1845 * Otherwise we can sit here for a long time without
1846 * any for-loop exit condition being met.
1847 * It might also reduce the latency from a received packet to
1848 * it being output by gpsd.
1849 */
1850 if ((changed & PACKET_SET) != 0)
1851 break;
1852 #endif /* __future__ */
1853 }
1854 }
1855 else if (device->reawake>0 && time(NULL) >device->reawake) {
1856 /* device may have had a zero-length read */
1857 GPSD_LOG(LOG_DATA, &device->context->errout,
1858 "%s reawakened after zero-length read\n",
1859 device->gpsdata.dev.path);
1860 device->reawake = (time_t)0;
1861 device->zerokill = true;
1862 return DEVICE_READY;
1863 }
1864
1865 /* no change in device descriptor state */
1866 return DEVICE_UNCHANGED;
1867 }
1868
gpsd_wrap(struct gps_device_t * session)1869 void gpsd_wrap(struct gps_device_t *session)
1870 /* end-of-session wrapup */
1871 {
1872 if (!BAD_SOCKET(session->gpsdata.gps_fd))
1873 gpsd_deactivate(session);
1874 }
1875
gpsd_zero_satellites(struct gps_data_t * out)1876 void gpsd_zero_satellites( struct gps_data_t *out)
1877 {
1878 int sat;
1879
1880 (void)memset(out->skyview, '\0', sizeof(out->skyview));
1881 out->satellites_visible = 0;
1882 /* zero is good inbound data for ss, elevation, and azimuth. */
1883 /* we need to set them to invalid values */
1884 for ( sat = 0; sat < MAXCHANNELS; sat++ ) {
1885 out->skyview[sat].azimuth = NAN;
1886 out->skyview[sat].elevation = NAN;
1887 out->skyview[sat].ss = NAN;
1888 out->skyview[sat].freqid = -1;
1889 }
1890 #if 0
1891 /*
1892 * We used to clear DOPs here, but this causes misbehavior on some
1893 * combined GPS/GLONASS/QZSS receivers like the Telit SL869; the
1894 * symptom is that the "satellites_used" field in a struct gps_data_t
1895 * filled in by gps_read() is always zero.
1896 */
1897 gps_clear_dop(&out->dop);
1898 #endif
1899 }
1900
1901 /* Latch the fact that we've saved a fix.
1902 * And add in the device fudge */
ntp_latch(struct gps_device_t * device,struct timedelta_t * td)1903 void ntp_latch(struct gps_device_t *device, struct timedelta_t *td)
1904 {
1905
1906 /* this should be an invariant of the way this function is called */
1907 if (0 >= device->newdata.time.tv_sec) {
1908 return;
1909 }
1910
1911 (void)clock_gettime(CLOCK_REALTIME, &td->clock);
1912 /* structure copy of time from GPS */
1913 td->real = device->newdata.time;
1914
1915 /* is there an offset method? */
1916 if (NULL != device->device_type &&
1917 NULL != device->device_type->time_offset) {
1918 double integral;
1919 double offset = device->device_type->time_offset(device);
1920
1921 /* add in offset which is double */
1922 td->real.tv_nsec += (long)(modf(offset, &integral) * 1e9);
1923 td->real.tv_sec += (time_t)integral;
1924 TS_NORM(&td->real);
1925 }
1926
1927 /* thread-safe update */
1928 pps_thread_fixin(&device->pps_thread, td);
1929 }
1930
1931 /* end */
1932