/* $Id$ */ /* libgpsd_core.c -- direct access to GPSes on serial or USB devices. */ #include #include #ifndef S_SPLINT_S #include #include #endif /* S_SPLINT_S */ #include #include #include #include #ifndef S_SPLINT_S #include #endif /* S_SPLINT_S */ #include #include #include #include "gpsd_config.h" #include "gpsd.h" #if defined(PPS_ENABLE) && defined(TIOCMIWAIT) #ifndef S_SPLINT_S #include /* pacifies OpenBSD's compiler */ #endif #endif int gpsd_switch_driver(struct gps_device_t *session, char* type_name) { const struct gps_type_t **dp; bool identified = (session->device_type != NULL); gpsd_report(LOG_PROG, "switch_driver(%s) called...\n", type_name); if (identified && strcmp(session->device_type->type_name, type_name) == 0) return 0; /*@ -compmempass @*/ for (dp = gpsd_drivers; *dp; dp++) if (strcmp((*dp)->type_name, type_name) == 0) { gpsd_report(LOG_PROG, "selecting %s driver...\n", (*dp)->type_name); gpsd_assert_sync(session); /*@i@*/session->device_type = *dp; session->gpsdata.dev.mincycle = session->device_type->min_cycle; /* reconfiguration might be required */ if (identified && session->device_type->event_hook != NULL) session->device_type->event_hook(session, event_driver_switch); return 1; } gpsd_report(LOG_ERROR, "invalid GPS type \"%s\".\n", type_name); return 0; /*@ +compmempass @*/ } void gpsd_init(struct gps_device_t *session, struct gps_context_t *context, char *device) /* initialize GPS polling */ { /*@ -mayaliasunique @*/ if (device != NULL) (void)strlcpy(session->gpsdata.dev.path, device, sizeof(session->gpsdata.dev.path)); /*@ -mustfreeonly @*/ session->device_type = NULL; /* start by hunting packets */ session->observed = 0; session->rtcmtime = 0; /*@ -temptrans @*/ session->context = context; /*@ +temptrans @*/ /*@ +mayaliasunique @*/ /*@ +mustfreeonly @*/ gps_clear_fix(&session->gpsdata.fix); session->gpsdata.set = 0; session->gpsdata.dop.hdop = NAN; session->gpsdata.dop.vdop = NAN; session->gpsdata.dop.pdop = NAN; session->gpsdata.dop.tdop = NAN; session->gpsdata.dop.gdop = NAN; session->gpsdata.epe = NAN; session->mag_var = NAN; session->gpsdata.dev.cycle = session->gpsdata.dev.mincycle = 1; /* tty-level initialization */ gpsd_tty_init(session); /* necessary in case we start reading in the middle of a GPGSV sequence */ gpsd_zero_satellites(&session->gpsdata); /* initialize things for the packet parser */ packet_reset(&session->packet); } void gpsd_deactivate(struct gps_device_t *session) /* temporarily release the GPS device */ { #ifdef NTPSHM_ENABLE (void)ntpshm_free(session->context, session->shmindex); session->shmindex = -1; # ifdef PPS_ENABLE (void)ntpshm_free(session->context, session->shmTimeP); session->shmTimeP = -1; # endif /* PPS_ENABLE */ #endif /* NTPSHM_ENABLE */ #ifdef ALLOW_RECONFIGURE if (!session->context->readonly && session->device_type != NULL && session->device_type->event_hook != NULL) { session->device_type->event_hook(session, event_deactivate); } if (session->device_type!=NULL) { if (session->back_to_nmea && session->device_type->mode_switcher!=NULL) session->device_type->mode_switcher(session, 0); } #endif /* ALLOW_RECONFIGURE */ gpsd_report(LOG_INF, "closing GPS=%s (%d)\n", session->gpsdata.dev.path, session->gpsdata.gps_fd); (void)gpsd_close(session); } #if defined(PPS_ENABLE) && defined(TIOCMIWAIT) static /*@null@*/void *gpsd_ppsmonitor(void *arg) { struct gps_device_t *session = (struct gps_device_t *)arg; int cycle,duration, state = 0, laststate = -1, unchanged = 0; struct timeval tv; struct timeval pulse[2] = {{0,0},{0,0}}; int pps_device = TIOCM_CAR; #if defined(PPS_ON_CTS) pps_device = TIOCM_CTS; #endif gpsd_report(LOG_PROG, "Create Thread gpsd_ppsmonitor\n"); /* wait for status change on the device's carrier-detect line */ while (ioctl(session->gpsdata.gps_fd, TIOCMIWAIT, pps_device) == 0) { (void)gettimeofday(&tv,NULL); /*@ +ignoresigns */ if (ioctl(session->gpsdata.gps_fd, TIOCMGET, &state) != 0) break; /*@ -ignoresigns */ state = (int)((state & pps_device) != 0); if (state == laststate) { if (++unchanged == 10) { gpsd_report(LOG_WARN, "TIOCMIWAIT returns unchanged state, ppsmonitor terminates\n"); break; } } else { gpsd_report(LOG_RAW, "pps-detect (%s) on %s changed to %d\n", ((pps_device==TIOCM_CAR) ? "DCD" : "CTS"), session->gpsdata.dev.path, state); laststate = state; unchanged = 0; } /*@ +boolint @*/ if ( session->context->fixcnt > 3 ) { /* Garmin doc says PPS is valid after four good fixes. */ /* * The PPS pulse is normally a short pulse with a frequency of * 1 Hz, and the UTC second is defined by the front edge. But we * don't know the polarity of the pulse (different receivers * emit different polarities). The duration variable is used to * determine which way the pulse is going. The code assumes * that the UTC second is changing when the signal has not * been changing for at least 800ms, i.e. it assumes the duty * cycle is at most 20%. * * Some GPS instead output a square wave that is 0.5 Hz and each * edge denotes the start of a second. * * A few stupid GPS, like the Furuno GPSClock, output a 1.0 Hz * square wave where the leading edge is the start of a second * * 5Hz GPS (Garmin 18-5Hz) pulses at 5Hz. Set the pulse length to * 40ms which gives a 160ms pulse before going high. * */ #define timediff(x, y) (int)((x.tv_sec-y.tv_sec)*1000000+x.tv_usec-y.tv_usec) cycle = timediff(tv, pulse[state]); duration = timediff(tv, pulse[state == 0]); #undef timediff #define min_interval(center) ((center) * 9 / 10) #define max_interval(center) ((center) * 11 / 10) #define in_interval(x, center) ((x) > min_interval(center) && (x) < max_interval(center)) if (in_interval(cycle, 200000)) { /* 5Hz cycle */ /* looks like 5hz PPS pulse */ if (duration > min_interval(160000)) (void)ntpshm_pps(session, &tv, 5); gpsd_report(LOG_RAW, "5Hz PPS pulse. cycle: %d, duration: %d\n", cycle, duration); } else if (in_interval(cycle, 1000000)) { /* looks like PPS pulse or square wave */ if (in_interval(duration, 500000) #if defined(NMEA_ENABLE) && defined(GPSCLOCK_ENABLE) && session->driver.nmea.ignore_trailing_edge #endif /* GPSCLOCK_ENABLE */ ) { /* looks like 1.0 Hz square wave, ignore trailing edge */ if (state == 1) { (void)ntpshm_pps(session, &tv, 1); } } else if (duration > min_interval(800000)) { /* looks like PPS pulse */ (void)ntpshm_pps(session, &tv, 1); } gpsd_report(LOG_RAW, "PPS pulse. cycle: %d, duration: %d\n", cycle, duration); } else if (in_interval(cycle, 2000000)) { /* looks like 0.5 Hz square wave */ (void)ntpshm_pps(session, &tv, 1); gpsd_report(LOG_RAW, "PPS square wave. cycle: %d, duration: %d\n", cycle, duration); } else { gpsd_report(LOG_INF, "PPS pulse rejected. cycle: %d, duration: %d\n", cycle, duration); } } else { gpsd_report(LOG_INF, "PPS pulse rejected. No fix.\n"); } /*@ -boolint @*/ #undef min_interval #undef max_interval #undef in_interval pulse[state] = tv; } return NULL; } #endif /* PPS_ENABLE */ /*@ -branchstate @*/ int gpsd_activate(struct gps_device_t *session) /* acquire a connection to the GPS device */ { if (gpsd_open(session) < 0) return -1; else { #ifdef NON_NMEA_ENABLE const struct gps_type_t **dp; /*@ -mustfreeonly @*/ for (dp = gpsd_drivers; *dp; dp++) { (void)tcflush(session->gpsdata.gps_fd, TCIOFLUSH); /* toss stale data */ if ((*dp)->probe_detect!=NULL && (*dp)->probe_detect(session)!=0) { gpsd_report(LOG_PROG, "probe found %s driver...\n", (*dp)->type_name); session->device_type = *dp; gpsd_assert_sync(session); goto foundit; } } /*@ +mustfreeonly @*/ gpsd_report(LOG_PROG, "no probe matched...\n"); foundit: #endif /* NON_NMEA_ENABLE */ session->gpsdata.online = timestamp(); #ifdef SIRF_ENABLE session->driver.sirf.satcounter = 0; #endif /* SIRF_ENABLE */ session->packet.char_counter = 0; session->packet.retry_counter = 0; gpsd_report(LOG_INF, "gpsd_activate(): opened GPS (fd %d)\n", session->gpsdata.gps_fd); // session->gpsdata.online = 0; session->gpsdata.fix.mode = MODE_NOT_SEEN; session->gpsdata.status = STATUS_NO_FIX; session->gpsdata.fix.track = NAN; session->gpsdata.separation = NAN; session->mag_var = NAN; session->releasetime = 0; /* clear the private data union */ memset(&session->driver, '\0', sizeof(session->driver)); /* * We might know the device's type, but we shoudn't assume it has * retained its settings. A revert hook might well have undone * them on the previous close. Fire a reactivate event so drivers * can do something about this if they choose. */ if (session->device_type != NULL && session->device_type->event_hook != NULL) session->device_type->event_hook(session, event_reactivate); } return session->gpsdata.gps_fd; } /*@ +branchstate @*/ void ntpd_link_activate(struct gps_device_t *session) { #if defined(PPS_ENABLE) && defined(TIOCMIWAIT) pthread_t pt; #endif /* defined(PPS_ENABLE) && defined(TIOCMIWAIT) */ #ifdef NTPSHM_ENABLE /* If we are talking to ntpd, allocate a shared-memory segment for "NMEA" time data */ if (session->context->enable_ntpshm) session->shmindex = ntpshm_alloc(session->context); #if defined(PPS_ENABLE) && defined(TIOCMIWAIT) /* If we also have the 1pps capability, allocate a shared-memory segment for * the 1pps time data and launch a thread to capture the 1pps transitions */ if (session->shmindex >= 0 && session->context->shmTimePPS) if ((session->shmTimeP = ntpshm_alloc(session->context)) >= 0) (void)pthread_create(&pt,NULL,gpsd_ppsmonitor,(void *)session); #endif /* defined(PPS_ENABLE) && defined(TIOCMIWAIT) */ #endif /* NTPSHM_ENABLE */ } char /*@observer@*/ *gpsd_id(/*@in@*/struct gps_device_t *session) /* full ID of the device for reports, including subtype */ { static char buf[128]; if ((session == NULL) || (session->device_type == NULL) || (session->device_type->type_name == NULL)) return "unknown,"; (void)strlcpy(buf, session->device_type->type_name, sizeof(buf)); if (session->subtype[0] != '\0') { (void)strlcat(buf, " ", sizeof(buf)); (void)strlcat(buf, session->subtype, sizeof(buf)); } return(buf); } void gpsd_error_model(struct gps_device_t *session, struct gps_fix_t *fix, struct gps_fix_t *oldfix) /* compute errors and derived quantities */ { /* * Now we compute derived quantities. This is where the tricky error- * modeling stuff goes. Presently we don't know how to derive * time error. * * Some drivers set the position-error fields. Only the Zodiacs * report speed error. Nobody reports track error or climb error. * * The UERE constants are our assumption about the base error of * GPS fixes in different directions. */ #define H_UERE_NO_DGPS 15.0 /* meters, 95% confidence */ #define H_UERE_WITH_DGPS 3.75 /* meters, 95% confidence */ #define V_UERE_NO_DGPS 23.0 /* meters, 95% confidence */ #define V_UERE_WITH_DGPS 5.75 /* meters, 95% confidence */ #define P_UERE_NO_DGPS 19.0 /* meters, 95% confidence */ #define P_UERE_WITH_DGPS 4.75 /* meters, 95% confidence */ double h_uere, v_uere, p_uere; if (NULL == session) return; h_uere = (session->gpsdata.status == STATUS_DGPS_FIX ? H_UERE_WITH_DGPS : H_UERE_NO_DGPS); v_uere = (session->gpsdata.status == STATUS_DGPS_FIX ? V_UERE_WITH_DGPS : V_UERE_NO_DGPS); p_uere = (session->gpsdata.status == STATUS_DGPS_FIX ? P_UERE_WITH_DGPS : P_UERE_NO_DGPS); /* * OK, this is not an error computation, but * we're at the right place in the architrcture for it. * Compute climb/sink in the simplest possible way. * FIXME: Someday we should compute speed here too. */ if (fix->mode>=MODE_3D && oldfix->mode>=MODE_3D && isnan(fix->climb)!=0) { if (fix->time == oldfix->time) fix->climb = 0; else if (isnan(fix->altitude)==0 && isnan(oldfix->altitude)==0) { fix->climb = (fix->altitude-oldfix->altitude)/(fix->time-oldfix->time); } } /* * Field reports match the theoretical prediction that * expected time error should be half the resolution of * the GPS clock, so we put the bound of the error * in as a constant pending getting it from each driver. */ if (isnan(fix->time)==0 && isnan(fix->ept)!=0) fix->ept = 0.005; /* Other error computations depend on having a valid fix */ if (fix->mode >= MODE_2D) { if (isnan(fix->epx)!=0 && finite(session->gpsdata.dop.hdop)!=0) fix->epx = session->gpsdata.dop.xdop * h_uere; if (isnan(fix->epy)!=0 && finite(session->gpsdata.dop.hdop)!=0) fix->epy = session->gpsdata.dop.ydop * h_uere; if ((fix->mode >= MODE_3D) && isnan(fix->epv)!=0 && finite(session->gpsdata.dop.vdop)!=0) fix->epv = session->gpsdata.dop.vdop * v_uere; if (isnan(session->gpsdata.epe)!=0 && finite(session->gpsdata.dop.pdop)!=0) session->gpsdata.epe = session->gpsdata.dop.pdop * p_uere; else session->gpsdata.epe = NAN; /* * If we have a current fix and an old fix, and the packet handler * didn't set the speed error and climb error members itself, * try to compute them now. */ if (isnan(fix->eps)!=0) { if (oldfix->mode > MODE_NO_FIX && fix->mode > MODE_NO_FIX && isnan(oldfix->epx)==0 && isnan(oldfix->epy)==0 && isnan(oldfix->time)==0 && isnan(oldfix->time)==0 && fix->time > oldfix->time) { double t = fix->time-oldfix->time; double e = EMIX(oldfix->epx,oldfix->epy) + EMIX(fix->epx,fix->epy); fix->eps = e/t; } else fix->eps = NAN; } if ((fix->mode >= MODE_3D) && isnan(fix->epc)!=0 && fix->time > oldfix->time) { if (oldfix->mode > MODE_3D && fix->mode > MODE_3D) { double t = fix->time-oldfix->time; double e = oldfix->epv + fix->epv; /* if vertical uncertainties are zero this will be too */ fix->epc = e/t; } /* * We compute a track error estimate solely from the * position of this fix and the last one. The maximum * track error, as seen from the position of last fix, is * the angle subtended by the two most extreme possible * error positions of the current fix; the expected track * error is half that. Let the position of the old fix be * A and of the new fix B. We model the view from A as * two right triangles ABC and ABD with BC and BD both * having the length of the new fix's estimated error. * adj = len(AB), opp = len(BC) = len(BD), hyp = len(AC) = * len(AD). This leads to spurious uncertainties * near 180 when we're moving slowly; to avoid reporting * garbage, throw back NaN if the distance from the previous * fix is less than the error estimate. */ fix->epd = NAN; if (oldfix->mode >= MODE_2D) { double adj = earth_distance( oldfix->latitude, oldfix->longitude, fix->latitude, fix->longitude); if (isnan(adj)==0 && adj > EMIX(fix->epx, fix->epy)) { double opp = EMIX(fix->epx, fix->epy); double hyp = sqrt(adj*adj + opp*opp); fix->epd = RAD_2_DEG * 2 * asin(opp / hyp); } } } } /* save old fix for later error computations */ /*@ -mayaliasunique @*/ if (fix->mode >= MODE_2D) (void)memcpy(oldfix, fix, sizeof(struct gps_fix_t)); /*@ +mayaliasunique @*/ } gps_mask_t gpsd_poll(struct gps_device_t *session) /* update the stuff in the scoreboard structure */ { ssize_t newlen; bool first_sync = false; gps_clear_fix(&session->gpsdata.fix); #ifdef TIMING_ENABLE if (session->packet.outbuflen == 0) session->d_xmit_time = timestamp(); #endif /* TIMING_ENABLE */ /* can we get a full packet from the device? */ if (session->device_type) { newlen = session->device_type->get_packet(session); gpsd_report(LOG_RAW, "%s is known to be %s\n", session->gpsdata.dev.path, session->device_type->type_name); } else { const struct gps_type_t **dp; newlen = generic_get(session); gpsd_report(LOG_RAW, "packet sniff on %s finds type %d\n", session->gpsdata.dev.path, session->packet.type); if (session->packet.type > COMMENT_PACKET) { /*@i2@*/session->observed |= PACKET_TYPEMASK(session->packet.type); first_sync = (session->device_type == NULL); for (dp = gpsd_drivers; *dp; dp++) if (session->packet.type == (*dp)->packet_type) { (void)gpsd_switch_driver(session, (*dp)->type_name); break; } } else if (!gpsd_next_hunt_setting(session)) return ERROR_SET; } /* update the scoreboard structure from the GPS */ gpsd_report(LOG_RAW+2, "%s sent %zd new characters\n", session->gpsdata.dev.path, newlen); if (newlen == -1) { /* read error */ gpsd_report(LOG_INF, "GPS on %s is offline (%lf sec since data)\n", session->gpsdata.dev.path, timestamp() - session->gpsdata.online); session->gpsdata.online = 0; return 0; } else if (newlen == 0) { /* no new data */ if (session->device_type != NULL && timestamp()>session->gpsdata.online+session->gpsdata.dev.cycle+1){ gpsd_report(LOG_INF, "GPS on %s is offline (%lf sec since data)\n", session->gpsdata.dev.path, timestamp() - session->gpsdata.online); session->gpsdata.online = 0; return 0; } else return ONLINE_SET; } else if (session->packet.outbuflen == 0) { /* got new data, but no packet */ gpsd_report(LOG_RAW+3, "New data on %s, not yet a packet\n", session->gpsdata.dev.path); return ONLINE_SET; } else { /* we have recognized a packet */ gps_mask_t received = PACKET_SET, dopmask = 0; session->gpsdata.online = timestamp(); gpsd_report(LOG_RAW+3, "Accepted packet on %s.\n", session->gpsdata.dev.path); #ifdef TIMING_ENABLE session->d_recv_time = timestamp(); #endif /* TIMING_ENABLE */ /* track the packet count since achieving sync on the device */ if (first_sync) session->packet.counter = 0; else session->packet.counter++; /* fire the configure hook */ if (session->device_type->event_hook != NULL) session->device_type->event_hook(session, event_configure); /* * If this is the first time we've achieved sync on this device, that's * a significant event that the caller needs to know about. Using * DEVICE_SET this way is a bit shaky but we're short of bits in * the flag mask (client library uses it differently). */ if (first_sync) received |= DEVICE_SET; /* Get data from current packet into the fix structure */ if (session->packet.type != COMMENT_PACKET) if (session->device_type != NULL && session->device_type->parse_packet!=NULL) received |= session->device_type->parse_packet(session); /* * Compute fix-quality data from the satellite positions. * These will not overwrite DOPs reported from the packet we just got. */ if (session->gpsdata.fix.mode > MODE_NO_FIX && (session->gpsdata.set & SATELLITE_SET) != 0 && session->gpsdata.satellites_visible > 0) { dopmask = fill_dop(&session->gpsdata, &session->gpsdata.dop); session->gpsdata.epe = NAN; } session->gpsdata.set = ONLINE_SET | dopmask | received; /* * Count good fixes. We used to check * session->gpsdata.status > STATUS_NO_FIX * here, but that wasn't quite right. That tells us whether * we think we have a valid fix for the current cycle, but remains * true while following non-fix packets are received. What we * really want to know is whether the last packet received was a * fix packet AND held a valid fix. We must ignore non-fix packets * AND packets which have fix data but are flagged as invalid. Some * devices output fix packets on a regular basis, even when unable * to derive a good fix. Such packets should set STATUS_NO_FIX. */ if ((session->gpsdata.set & LATLON_SET )!=0 && session->gpsdata.status > STATUS_NO_FIX) session->context->fixcnt++; #ifdef TIMING_ENABLE session->d_decode_time = timestamp(); #endif /* TIMING_ENABLE */ return session->gpsdata.set; } } void gpsd_wrap(struct gps_device_t *session) /* end-of-session wrapup */ { if (session->gpsdata.gps_fd != -1) gpsd_deactivate(session); } void gpsd_zero_satellites(/*@out@*/struct gps_data_t *out) { (void)memset(out->PRN, 0, sizeof(out->PRN)); (void)memset(out->elevation, 0, sizeof(out->elevation)); (void)memset(out->azimuth, 0, sizeof(out->azimuth)); (void)memset(out->ss, 0, sizeof(out->ss)); out->satellites_visible = 0; }