refclock_nmea.c revision 182007 
1/*
2 * refclock_nmea.c - clock driver for an NMEA GPS CLOCK
3 *		Michael Petry Jun 20, 1994
4 *		 based on refclock_heathn.c
5 */
6#ifdef HAVE_CONFIG_H
7#include <config.h>
8#endif
9
10#if defined(SYS_WINNT)
11#undef close
12#define close closesocket
13#endif
14
15#if defined(REFCLOCK) && defined(CLOCK_NMEA)
16
17#include <stdio.h>
18#include <ctype.h>
19
20#include "ntpd.h"
21#include "ntp_io.h"
22#include "ntp_unixtime.h"
23#include "ntp_refclock.h"
24#include "ntp_stdlib.h"
25
26#ifdef HAVE_PPSAPI
27# include "ppsapi_timepps.h"
28#endif /* HAVE_PPSAPI */
29
30/*
31 * This driver supports the NMEA GPS Receiver with
32 *
33 * Protype was refclock_trak.c, Thanks a lot.
34 *
35 * The receiver used spits out the NMEA sentences for boat navigation.
36 * And you thought it was an information superhighway.  Try a raging river
37 * filled with rapids and whirlpools that rip away your data and warp time.
38 *
39 * If HAVE_PPSAPI is defined code to use the PPSAPI will be compiled in.
40 * On startup if initialization of the PPSAPI fails, it will fall back
41 * to the "normal" timestamps.
42 *
43 * The PPSAPI part of the driver understands fudge flag2 and flag3. If
44 * flag2 is set, it will use the clear edge of the pulse. If flag3 is
45 * set, kernel hardpps is enabled.
46 *
47 * GPS sentences other than RMC (the default) may be enabled by setting
48 * the relevent bits of 'mode' in the server configuration line
49 * server 127.127.20.x mode X
50 *
51 * bit 0 - enables RMC (1)
52 * bit 1 - enables GGA (2)
53 * bit 2 - enables GLL (4)
54 * multiple sentences may be selected
55 */
56
57/*
58 * Definitions
59 */
60#ifdef SYS_WINNT
61# define DEVICE "COM%d:" 	/* COM 1 - 3 supported */
62#else
63# define DEVICE	"/dev/gps%d"	/* name of radio device */
64#endif
65#define	SPEED232	B4800	/* uart speed (4800 bps) */
66#define	PRECISION	(-9)	/* precision assumed (about 2 ms) */
67#define	PPS_PRECISION	(-20)	/* precision assumed (about 1 us) */
68#define	REFID		"GPS\0"	/* reference id */
69#define	DESCRIPTION	"NMEA GPS Clock" /* who we are */
70#define NANOSECOND	1000000000 /* one second (ns) */
71#define RANGEGATE	500000	/* range gate (ns) */
72
73#define LENNMEA		75	/* min timecode length */
74
75/*
76 * Tables to compute the ddd of year form icky dd/mm timecode. Viva la
77 * leap.
78 */
79static int day1tab[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
80static int day2tab[] = {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
81
82/*
83 * Unit control structure
84 */
85struct nmeaunit {
86	int	pollcnt;	/* poll message counter */
87	int	polled;		/* Hand in a sample? */
88	l_fp	tstamp;		/* timestamp of last poll */
89#ifdef HAVE_PPSAPI
90	struct timespec ts;	/* last timestamp */
91	pps_params_t pps_params; /* pps parameters */
92	pps_info_t pps_info;	/* last pps data */
93	pps_handle_t handle;	/* pps handlebars */
94#endif /* HAVE_PPSAPI */
95};
96
97/*
98 * Function prototypes
99 */
100static	int	nmea_start	P((int, struct peer *));
101static	void	nmea_shutdown	P((int, struct peer *));
102#ifdef HAVE_PPSAPI
103static	void	nmea_control	P((int, struct refclockstat *, struct
104				    refclockstat *, struct peer *));
105static	int	nmea_ppsapi	P((struct peer *, int, int));
106static	int	nmea_pps	P((struct nmeaunit *, l_fp *));
107#endif /* HAVE_PPSAPI */
108static	void	nmea_receive	P((struct recvbuf *));
109static	void	nmea_poll	P((int, struct peer *));
110static	void	gps_send	P((int, const char *, struct peer *));
111static	char	*field_parse	P((char *, int));
112
113/*
114 * Transfer vector
115 */
116struct	refclock refclock_nmea = {
117	nmea_start,		/* start up driver */
118	nmea_shutdown,	/* shut down driver */
119	nmea_poll,		/* transmit poll message */
120#ifdef HAVE_PPSAPI
121	nmea_control,		/* fudge control */
122#else
123	noentry,		/* fudge control */
124#endif /* HAVE_PPSAPI */
125	noentry,		/* initialize driver */
126	noentry,		/* buginfo */
127	NOFLAGS			/* not used */
128};
129
130/*
131 * nmea_start - open the GPS devices and initialize data for processing
132 */
133static int
134nmea_start(
135	int unit,
136	struct peer *peer
137	)
138{
139	register struct nmeaunit *up;
140	struct refclockproc *pp;
141	int fd;
142	char device[20];
143
144	/*
145	 * Open serial port. Use CLK line discipline, if available.
146	 */
147	(void)sprintf(device, DEVICE, unit);
148
149	fd = refclock_open(device, SPEED232, LDISC_CLK);
150	if (fd <= 0) {
151#ifdef HAVE_READLINK
152          /* nmead support added by Jon Miner (cp_n18@yahoo.com)
153           *
154           * See http://home.hiwaay.net/~taylorc/gps/nmea-server/
155           * for information about nmead
156           *
157           * To use this, you need to create a link from /dev/gpsX to
158           * the server:port where nmead is running.  Something like this:
159           *
160           * ln -s server:port /dev/gps1
161           */
162          char buffer[80];
163          char *nmea_host;
164          int   nmea_port;
165          int   len;
166          struct hostent *he;
167          struct protoent *p;
168          struct sockaddr_in so_addr;
169
170          if ((len = readlink(device,buffer,sizeof(buffer))) == -1)
171            return(0);
172          buffer[len] = 0;
173
174          if ((nmea_host = strtok(buffer,":")) == NULL)
175            return(0);
176
177          nmea_port = atoi(strtok(NULL,":"));
178
179          if ((he = gethostbyname(nmea_host)) == NULL)
180            return(0);
181          if ((p = getprotobyname("ip")) == NULL)
182            return(0);
183          so_addr.sin_family = AF_INET;
184          so_addr.sin_port = htons(nmea_port);
185          so_addr.sin_addr = *((struct in_addr *) he->h_addr);
186
187          if ((fd = socket(PF_INET,SOCK_STREAM,p->p_proto)) == -1)
188            return(0);
189          if (connect(fd,(struct sockaddr *)&so_addr,SOCKLEN(&so_addr)) == -1) {
190            close(fd);
191            return (0);
192          }
193#else
194            return (0);
195#endif
196        }
197
198	/*
199	 * Allocate and initialize unit structure
200	 */
201	up = (struct nmeaunit *)emalloc(sizeof(struct nmeaunit));
202	if (up == NULL) {
203		(void) close(fd);
204		return (0);
205	}
206	memset((char *)up, 0, sizeof(struct nmeaunit));
207	pp = peer->procptr;
208	pp->io.clock_recv = nmea_receive;
209	pp->io.srcclock = (caddr_t)peer;
210	pp->io.datalen = 0;
211	pp->io.fd = fd;
212	if (!io_addclock(&pp->io)) {
213		(void) close(fd);
214		free(up);
215		return (0);
216	}
217	pp->unitptr = (caddr_t)up;
218
219	/*
220	 * Initialize miscellaneous variables
221	 */
222	peer->precision = PRECISION;
223	pp->clockdesc = DESCRIPTION;
224	memcpy((char *)&pp->refid, REFID, 4);
225	up->pollcnt = 2;
226	gps_send(pp->io.fd,"$PMOTG,RMC,0000*1D\r\n", peer);
227
228#ifdef HAVE_PPSAPI
229	/*
230	 * Start the PPSAPI interface if it is there. Default to use
231	 * the assert edge and do not enable the kernel hardpps.
232	 */
233	if (time_pps_create(fd, &up->handle) < 0) {
234		up->handle = 0;
235		msyslog(LOG_ERR,
236		    "refclock_nmea: time_pps_create failed: %m");
237		return (1);
238	}
239	return(nmea_ppsapi(peer, 0, 0));
240#else
241	return (1);
242#endif /* HAVE_PPSAPI */
243}
244
245/*
246 * nmea_shutdown - shut down a GPS clock
247 */
248static void
249nmea_shutdown(
250	int unit,
251	struct peer *peer
252	)
253{
254	register struct nmeaunit *up;
255	struct refclockproc *pp;
256
257	pp = peer->procptr;
258	up = (struct nmeaunit *)pp->unitptr;
259#ifdef HAVE_PPSAPI
260	if (up->handle != 0)
261		time_pps_destroy(up->handle);
262#endif /* HAVE_PPSAPI */
263	io_closeclock(&pp->io);
264	free(up);
265}
266
267#ifdef HAVE_PPSAPI
268/*
269 * nmea_control - fudge control
270 */
271static void
272nmea_control(
273	int unit,		/* unit (not used */
274	struct refclockstat *in, /* input parameters (not uded) */
275	struct refclockstat *out, /* output parameters (not used) */
276	struct peer *peer	/* peer structure pointer */
277	)
278{
279	struct refclockproc *pp;
280
281	pp = peer->procptr;
282	nmea_ppsapi(peer, pp->sloppyclockflag & CLK_FLAG2,
283	    pp->sloppyclockflag & CLK_FLAG3);
284}
285
286
287/*
288 * Initialize PPSAPI
289 */
290int
291nmea_ppsapi(
292	struct peer *peer,	/* peer structure pointer */
293	int enb_clear,		/* clear enable */
294	int enb_hardpps		/* hardpps enable */
295	)
296{
297	struct refclockproc *pp;
298	struct nmeaunit *up;
299	int capability;
300
301	pp = peer->procptr;
302	up = (struct nmeaunit *)pp->unitptr;
303	if (time_pps_getcap(up->handle, &capability) < 0) {
304		msyslog(LOG_ERR,
305		    "refclock_nmea: time_pps_getcap failed: %m");
306		return (0);
307	}
308	memset(&up->pps_params, 0, sizeof(pps_params_t));
309	if (enb_clear)
310		up->pps_params.mode = capability & PPS_CAPTURECLEAR;
311	else
312		up->pps_params.mode = capability & PPS_CAPTUREASSERT;
313	if (!up->pps_params.mode) {
314		msyslog(LOG_ERR,
315		    "refclock_nmea: invalid capture edge %d",
316		    !enb_clear);
317		return (0);
318	}
319	up->pps_params.mode |= PPS_TSFMT_TSPEC;
320	if (time_pps_setparams(up->handle, &up->pps_params) < 0) {
321		msyslog(LOG_ERR,
322		    "refclock_nmea: time_pps_setparams failed: %m");
323		return (0);
324	}
325	if (enb_hardpps) {
326		if (time_pps_kcbind(up->handle, PPS_KC_HARDPPS,
327				    up->pps_params.mode & ~PPS_TSFMT_TSPEC,
328				    PPS_TSFMT_TSPEC) < 0) {
329			msyslog(LOG_ERR,
330			    "refclock_nmea: time_pps_kcbind failed: %m");
331			return (0);
332		}
333		pps_enable = 1;
334	}
335	peer->precision = PPS_PRECISION;
336
337#if DEBUG
338	if (debug) {
339		time_pps_getparams(up->handle, &up->pps_params);
340		printf(
341		    "refclock_ppsapi: capability 0x%x version %d mode 0x%x kern %d\n",
342		    capability, up->pps_params.api_version,
343		    up->pps_params.mode, enb_hardpps);
344	}
345#endif
346
347	return (1);
348}
349
350/*
351 * Get PPSAPI timestamps.
352 *
353 * Return 0 on failure and 1 on success.
354 */
355static int
356nmea_pps(
357	struct nmeaunit *up,
358	l_fp *tsptr
359	)
360{
361	pps_info_t pps_info;
362	struct timespec timeout, ts;
363	double dtemp;
364	l_fp tstmp;
365
366	/*
367	 * Convert the timespec nanoseconds field to ntp l_fp units.
368	 */
369	if (up->handle == 0)
370		return (0);
371	timeout.tv_sec = 0;
372	timeout.tv_nsec = 0;
373	memcpy(&pps_info, &up->pps_info, sizeof(pps_info_t));
374	if (time_pps_fetch(up->handle, PPS_TSFMT_TSPEC, &up->pps_info,
375	    &timeout) < 0)
376		return (0);
377	if (up->pps_params.mode & PPS_CAPTUREASSERT) {
378		if (pps_info.assert_sequence ==
379		    up->pps_info.assert_sequence)
380			return (0);
381		ts = up->pps_info.assert_timestamp;
382	} else if (up->pps_params.mode & PPS_CAPTURECLEAR) {
383		if (pps_info.clear_sequence ==
384		    up->pps_info.clear_sequence)
385			return (0);
386		ts = up->pps_info.clear_timestamp;
387	} else {
388		return (0);
389	}
390	if ((up->ts.tv_sec == ts.tv_sec) && (up->ts.tv_nsec == ts.tv_nsec))
391		return (0);
392	up->ts = ts;
393
394	tstmp.l_ui = ts.tv_sec + JAN_1970;
395	dtemp = ts.tv_nsec * FRAC / 1e9;
396	tstmp.l_uf = (u_int32)dtemp;
397	*tsptr = tstmp;
398	return (1);
399}
400#endif /* HAVE_PPSAPI */
401
402/*
403 * nmea_receive - receive data from the serial interface
404 */
405static void
406nmea_receive(
407	struct recvbuf *rbufp
408	)
409{
410	register struct nmeaunit *up;
411	struct refclockproc *pp;
412	struct peer *peer;
413	int month, day;
414	int i;
415	char *cp, *dp;
416	int cmdtype;
417	/* Use these variables to hold data until we decide its worth keeping */
418	char	rd_lastcode[BMAX];
419	l_fp	rd_tmp;
420	u_short	rd_lencode;
421
422	/*
423	 * Initialize pointers and read the timecode and timestamp
424	 */
425	peer = (struct peer *)rbufp->recv_srcclock;
426	pp = peer->procptr;
427	up = (struct nmeaunit *)pp->unitptr;
428	rd_lencode = (u_short)refclock_gtlin(rbufp, rd_lastcode, BMAX, &rd_tmp);
429
430	/*
431	 * There is a case that a <CR><LF> gives back a "blank" line
432	 */
433	if (rd_lencode == 0)
434	    return;
435
436#ifdef DEBUG
437	if (debug)
438	    printf("nmea: gpsread %d %s\n", rd_lencode,
439		   rd_lastcode);
440#endif
441
442	/*
443	 * We check the timecode format and decode its contents. The
444	 * we only care about a few of them.  The most important being
445	 * the $GPRMC format
446	 * $GPRMC,hhmmss,a,fddmm.xx,n,dddmmm.xx,w,zz.z,yyy.,ddmmyy,dd,v*CC
447	 * For Magellan (ColorTrak) GLL probably datum (order of sentences)
448	 * also mode (0,1,2,3) select sentence ANY/ALL, RMC, GGA, GLL
449	 * $GPGLL,3513.8385,S,14900.7851,E,232420.594,A*21
450  	 * $GPGGA,232420.59,3513.8385,S,14900.7851,E,1,05,3.4,00519,M,,,,*3F
451	 * $GPRMB,...
452	 * $GPRMC,232418.19,A,3513.8386,S,14900.7853,E,00.0,000.0,121199,12.,E*77
453	 * $GPAPB,...
454	 * $GPGSA,...
455	 * $GPGSV,...
456	 * $GPGSV,...
457	 */
458#define GPXXX	0
459#define GPRMC	1
460#define GPGGA	2
461#define GPGLL	4
462	cp = rd_lastcode;
463	cmdtype=0;
464	if(strncmp(cp,"$GPRMC",6)==0) {
465		cmdtype=GPRMC;
466	}
467	else if(strncmp(cp,"$GPGGA",6)==0) {
468		cmdtype=GPGGA;
469	}
470	else if(strncmp(cp,"$GPGLL",6)==0) {
471		cmdtype=GPGLL;
472	}
473	else if(strncmp(cp,"$GPXXX",6)==0) {
474		cmdtype=GPXXX;
475	}
476	else
477	    return;
478
479
480	/* See if I want to process this message type */
481	if ( ((peer->ttl == 0) && (cmdtype != GPRMC))
482           || ((peer->ttl != 0) && !(cmdtype & peer->ttl)) )
483		return;
484
485	pp->lencode = rd_lencode;
486	strcpy(pp->a_lastcode,rd_lastcode);
487	cp = pp->a_lastcode;
488
489	pp->lastrec = up->tstamp = rd_tmp;
490	up->pollcnt = 2;
491
492#ifdef DEBUG
493	if (debug)
494	    printf("nmea: timecode %d %s\n", pp->lencode,
495		   pp->a_lastcode);
496#endif
497
498
499	/* Grab field depending on clock string type */
500	switch( cmdtype ) {
501	    case GPRMC:
502		/*
503		 * Test for synchronization.  Check for quality byte.
504		 */
505		dp = field_parse(cp,2);
506		if( dp[0] != 'A')
507			pp->leap = LEAP_NOTINSYNC;
508		else
509			pp->leap = LEAP_NOWARNING;
510
511		/* Now point at the time field */
512		dp = field_parse(cp,1);
513		break;
514
515
516	    case GPGGA:
517		/*
518		 * Test for synchronization.  Check for quality byte.
519		 */
520		dp = field_parse(cp,6);
521		if( dp[0] == '0')
522			pp->leap = LEAP_NOTINSYNC;
523		else
524			pp->leap = LEAP_NOWARNING;
525
526		/* Now point at the time field */
527		dp = field_parse(cp,1);
528		break;
529
530
531	    case GPGLL:
532		/*
533		 * Test for synchronization.  Check for quality byte.
534		 */
535		dp = field_parse(cp,6);
536		if( dp[0] != 'A')
537			pp->leap = LEAP_NOTINSYNC;
538		else
539			pp->leap = LEAP_NOWARNING;
540
541		/* Now point at the time field */
542		dp = field_parse(cp,5);
543		break;
544
545
546	    case GPXXX:
547		return;
548	    default:
549		return;
550
551	}
552
553		/*
554		 *	Check time code format of NMEA
555		 */
556
557		if( !isdigit((int)dp[0]) ||
558		    !isdigit((int)dp[1]) ||
559		    !isdigit((int)dp[2]) ||
560		    !isdigit((int)dp[3]) ||
561		    !isdigit((int)dp[4]) ||
562		    !isdigit((int)dp[5])
563		    ) {
564			refclock_report(peer, CEVNT_BADREPLY);
565			return;
566		}
567
568
569	/*
570	 * Convert time and check values.
571	 */
572	pp->hour = ((dp[0] - '0') * 10) + dp[1] - '0';
573	pp->minute = ((dp[2] - '0') * 10) + dp[3] -  '0';
574	pp->second = ((dp[4] - '0') * 10) + dp[5] - '0';
575	/* Default to 0 milliseconds, if decimal convert milliseconds in
576	   one, two or three digits
577	*/
578	pp->nsec = 0;
579	if (dp[6] == '.') {
580		if (isdigit((int)dp[7])) {
581			pp->nsec = (dp[7] - '0') * 100000000;
582			if (isdigit((int)dp[8])) {
583				pp->nsec += (dp[8] - '0') * 10000000;
584				if (isdigit((int)dp[9])) {
585					pp->nsec += (dp[9] - '0') * 1000000;
586				}
587			}
588		}
589	}
590
591	if (pp->hour > 23 || pp->minute > 59 || pp->second > 59
592	  || pp->nsec > 1000000000) {
593		refclock_report(peer, CEVNT_BADTIME);
594		return;
595	}
596
597
598	/*
599	 * Convert date and check values.
600	 */
601	if (cmdtype==GPRMC) {
602	    dp = field_parse(cp,9);
603	    day = dp[0] - '0';
604	    day = (day * 10) + dp[1] - '0';
605	    month = dp[2] - '0';
606	    month = (month * 10) + dp[3] - '0';
607	    pp->year = dp[4] - '0';
608	    pp->year = (pp->year * 10) + dp[5] - '0';
609	}
610	else {
611	/* only time */
612	    time_t tt = time(NULL);
613	    struct tm * t = gmtime(&tt);
614	    day = t->tm_mday;
615	    month = t->tm_mon + 1;
616	    pp->year= t->tm_year;
617	}
618
619	if (month < 1 || month > 12 || day < 1) {
620		refclock_report(peer, CEVNT_BADTIME);
621		return;
622	}
623
624        /* Hmmmm this will be a nono for 2100,2200,2300 but I don't think I'll be here */
625        /* good thing that 2000 is a leap year */
626	/* pp->year will be 00-99 if read from GPS, 00->  (years since 1900) from tm_year */
627	if (pp->year % 4) {
628		if (day > day1tab[month - 1]) {
629			refclock_report(peer, CEVNT_BADTIME);
630			return;
631		}
632		for (i = 0; i < month - 1; i++)
633		    day += day1tab[i];
634	} else {
635		if (day > day2tab[month - 1]) {
636			refclock_report(peer, CEVNT_BADTIME);
637			return;
638		}
639		for (i = 0; i < month - 1; i++)
640		    day += day2tab[i];
641	}
642	pp->day = day;
643
644
645#ifdef HAVE_PPSAPI
646	/*
647	 * If the PPSAPI is working, rather use its timestamps.
648	 * assume that the PPS occurs on the second so blow any msec
649	 */
650	if (nmea_pps(up, &rd_tmp) == 1) {
651		pp->lastrec = up->tstamp = rd_tmp;
652		pp->nsec = 0;
653	}
654#endif /* HAVE_PPSAPI */
655
656	/*
657	 * Process the new sample in the median filter and determine the
658	 * reference clock offset and dispersion. We use lastrec as both
659	 * the reference time and receive time, in order to avoid being
660	 * cute, like setting the reference time later than the receive
661	 * time, which may cause a paranoid protocol module to chuck out
662	 * the data.
663	 */
664
665	if (!refclock_process(pp)) {
666		refclock_report(peer, CEVNT_BADTIME);
667		return;
668	}
669
670
671
672	/*
673	 * Only go on if we had been polled.
674	 */
675	if (!up->polled)
676	    return;
677	up->polled = 0;
678	pp->lastref = pp->lastrec;
679	refclock_receive(peer);
680
681        /* If we get here - what we got from the clock is OK, so say so */
682         refclock_report(peer, CEVNT_NOMINAL);
683
684	record_clock_stats(&peer->srcadr, pp->a_lastcode);
685
686}
687
688/*
689 * nmea_poll - called by the transmit procedure
690 *
691 * We go to great pains to avoid changing state here, since there may be
692 * more than one eavesdropper receiving the same timecode.
693 */
694static void
695nmea_poll(
696	int unit,
697	struct peer *peer
698	)
699{
700	register struct nmeaunit *up;
701	struct refclockproc *pp;
702
703	pp = peer->procptr;
704	up = (struct nmeaunit *)pp->unitptr;
705	if (up->pollcnt == 0)
706	    refclock_report(peer, CEVNT_TIMEOUT);
707	else
708	    up->pollcnt--;
709	pp->polls++;
710	up->polled = 1;
711
712	/*
713	 * usually nmea_receive can get a timestamp every second
714	 */
715
716	gps_send(pp->io.fd,"$PMOTG,RMC,0000*1D\r\n", peer);
717}
718
719/*
720 *
721 *	gps_send(fd,cmd, peer)  Sends a command to the GPS receiver.
722 *	 as	gps_send(fd,"rqts,u\r", peer);
723 *
724 *	We don't currently send any data, but would like to send
725 *	RTCM SC104 messages for differential positioning. It should
726 *	also give us better time. Without a PPS output, we're
727 *	Just fooling ourselves because of the serial code paths
728 *
729 */
730static void
731gps_send(
732	int fd,
733	const char *cmd,
734	struct peer *peer
735	)
736{
737
738	if (write(fd, cmd, strlen(cmd)) == -1) {
739		refclock_report(peer, CEVNT_FAULT);
740	}
741}
742
743static char *
744field_parse(
745	char *cp,
746	int fn
747	)
748{
749	char *tp;
750	int i = fn;
751
752	for (tp = cp; *tp != '\0'; tp++) {
753		if (*tp == ',')
754		    i--;
755		if (i == 0)
756		    break;
757	}
758	return (++tp);
759}
760#else
761int refclock_nmea_bs;
762#endif /* REFCLOCK */
763