source: trunk/third/xntp/xntpd/refclock_nmea.c @ 10832

/*
 * refclock_nmea.c - clock driver for an NMEA GPS CLOCK
 *              Michael Petry Jun 20, 1994
 *               based on refclock_heath.c
 */
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#if defined(REFCLOCK) && defined(NMEA)

#define DEBUG   1

#include <stdio.h>
#include <ctype.h>
#include <sys/time.h>

#include "ntpd.h"
#include "ntp_io.h"
#include "ntp_refclock.h"
#include "ntp_stdlib.h"

/*
 * This driver supports the NMEA GPS Receiver with
 *
 * Protype was refclock_trak.c, Thanks a lot.
 *
 * The receiver used spits out the NMEA sentences for boat navigation.
 * And you thought it was an information superhighway.  Try a raging river
 * filled with rapids and whirlpools that rip away your data and warp time.
 */

/*
 * Definitions
 */
#define DEVICE          "/dev/gps%d"    /* name of radio device */
#define SPEED232        B4800   /* uart speed (4800 bps) */
#define PRECISION       (-9)    /* precision assumed (about 2 ms) */
#define DCD_PRECISION   (-20)   /* precision assumed (about 1 us) */
#define REFID           "GPS\0" /* reference id */
#define DESCRIPTION     "NMEA GPS Clock" /* who we are */

#define NSAMPLES        3       /* stages of median filter */
#define LENNMEA         75      /* min timecode length */

/*
 * Imported from ntp_timer module
 */
extern u_long current_time;     /* current time (s) */

/*
 * Imported from ntp_loopfilter module
 */
extern int fdpps;               /* pps file descriptor */

/*
 * Imported from ntpd module
 */
extern int debug;               /* global debug flag */

/*
 * Tables to compute the ddd of year form icky dd/mm timecode. Viva la
 * leap.
 */
static int day1tab[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
static int day2tab[] = {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};

/*
 * Unit control structure
 */
struct nmeaunit {
        int     pollcnt;        /* poll message counter */
        int     polled;         /* Hand in a sample? */
        l_fp    tstamp;         /* timestamp of last poll */
};

/*
 * Function prototypes
 */
static  int     nmea_start      P((int, struct peer *));
static  void    nmea_shutdown   P((int, struct peer *));
static  void    nmea_receive    P((struct recvbuf *));
static  void    nmea_poll       P((int, struct peer *));
static  void    gps_send        P((int, char *, struct peer *));
static  char    *field_parse    P((char *, int));

/*
 * Transfer vector
 */
struct  refclock refclock_nmea = {
        nmea_start,             /* start up driver */
        nmea_shutdown,  /* shut down driver */
        nmea_poll,              /* transmit poll message */
        noentry,                /* handle control */
        noentry,                /* initialize driver */
        noentry,                /* buginfo */
        NOFLAGS                 /* not used */
};

/*
 * nmea_start - open the GPS devices and initialize data for processing
 */
static int
nmea_start(unit, peer)
        int unit;
        struct peer *peer;
{
        register struct nmeaunit *up;
        struct refclockproc *pp;
        int fd;
        char device[20];

        /*
         * Open serial port. Use CLK line discipline, if available.
         */
        (void)sprintf(device, DEVICE, unit);
#ifdef TTYCLK
        if (!(fd = refclock_open(device, SPEED232, LDISC_CLK)))
#else
        if (!(fd = refclock_open(device, SPEED232, 0)))
#endif /* TTYCLK */
                return (0);

        /*
         * Allocate and initialize unit structure
         */
        if (!(up = (struct nmeaunit *)
            emalloc(sizeof(struct nmeaunit)))) {
                (void) close(fd);
                return (0);
        }
        memset((char *)up, 0, sizeof(struct nmeaunit));
        pp = peer->procptr;
        pp->io.clock_recv = nmea_receive;
        pp->io.srcclock = (caddr_t)peer;
        pp->io.datalen = 0;
        pp->io.fd = fd;
        if (!io_addclock(&pp->io)) {
                (void) close(fd);
                free(up);
                return (0);
        }
        pp->unitptr = (caddr_t)up;

        /*
         * Initialize miscellaneous variables
         */
        peer->precision = DCD_PRECISION;
        pp->clockdesc = DESCRIPTION;
        memcpy((char *)&pp->refid, REFID, 4);
        up->pollcnt = 2;
        gps_send(pp->io.fd,"$PMOTG,RMC,0000*1D\r\n", peer);

        return (1);
}

/*
 * nmea_shutdown - shut down a GPS clock
 */
static void
nmea_shutdown(unit, peer)
        int unit;
        struct peer *peer;
{
        register struct nmeaunit *up;
        struct refclockproc *pp;

        pp = peer->procptr;
        up = (struct nmeaunit *)pp->unitptr;
        io_closeclock(&pp->io);
        free(up);
}

/*
 * nmea_receive - receive data from the serial interface
 */
static void
nmea_receive(rbufp)
        struct recvbuf *rbufp;
{
        register struct nmeaunit *up;
        struct refclockproc *pp;
        struct peer *peer;
        l_fp trtmp;
        int month, day;
        int i;
        char *cp, *dp;
        int cmdtype;

        /*
         * Initialize pointers and read the timecode and timestamp
         */
        peer = (struct peer *)rbufp->recv_srcclock;
        pp = peer->procptr;
        up = (struct nmeaunit *)pp->unitptr;
        pp->lencode = refclock_gtlin(rbufp, pp->a_lastcode, BMAX, &trtmp);

        /*
         * There is a case that a <CR><LF> gives back a "blank" line
         */
        if (pp->lencode == 0)
                return;

        /*
         * We get a buffer and timestamp for each <cr>.
         */
        pp->lastrec = up->tstamp = trtmp;
        up->pollcnt = 2;
#ifdef DEBUG
        if (debug)
                printf("nmea: timecode %d %s\n", pp->lencode,
                    pp->a_lastcode);
#endif

        /*
         * We check the timecode format and decode its contents. The
         * we only care about a few of them.  The most important being
         * the $GPRMC format
         * $GPRMC,hhmmss,a,fddmm.xx,n,dddmmm.xx,w,zz.z,yyy.,ddmmyy,dd,v*CC
         */
#define GPRMC   0
#define GPXXX   1
        cp = pp->a_lastcode;
        pp->leap = 0;
        cmdtype=0;
        if(strncmp(cp,"$GPRMC",6)==0) {
                cmdtype=GPRMC;
                }
        else if(strncmp(cp,"$GPXXX",6)==0) {
                cmdtype=GPXXX;
                }
        else
                return;

        switch( cmdtype ) {
        case GPRMC:
                /*
                 *      Check time code format of NMEA
                 */

                dp = field_parse(cp,1);
                if( !isdigit(dp[0]) ||
                   !isdigit(dp[1]) ||
                   !isdigit(dp[2]) ||
                   !isdigit(dp[3]) ||
                   !isdigit(dp[4]) ||
                   !isdigit(dp[5])      
                  ) {
                    refclock_report(peer, CEVNT_BADREPLY);
                    return;
                }
                dp = field_parse(cp,2);
                if( dp[0] != 'A')  {
                    refclock_report(peer, CEVNT_BADREPLY);
                    return;
                    }
                break;
        case GPXXX:
                return;
        default:
                return;

        }

        /*
         * Only go on if we had been polled.
         */
        if(!up->polled)
                return;
        up->polled = 0;

        record_clock_stats(&peer->srcadr, pp->a_lastcode);

        dp = field_parse(cp,9);
        /*
         * Convert date and check values.
         */
        day = dp[0] - '0';
        day = (day * 10) + dp[1] - '0';
        month = dp[2] - '0';
        month = (month * 10) + dp[3] - '0';
        pp->year = dp[4] - '0';
        pp->year = (pp->year * 10) + dp[5] - '0';

        if (month < 1 || month > 12 || day < 1) {
                refclock_report(peer, CEVNT_BADTIME);
                return;
        }

        if (pp->year % 4) {
                if (day > day1tab[month - 1]) {
                        refclock_report(peer, CEVNT_BADTIME);
                        return;
                }
                for (i = 0; i < month - 1; i++)
                        day += day1tab[i];
        } else {
                if (day > day2tab[month - 1]) {
                        refclock_report(peer, CEVNT_BADTIME);
                        return;
                }
                for (i = 0; i < month - 1; i++)
                        day += day2tab[i];
        }
        pp->day = day;

        dp = field_parse(cp,1);
        /*
         * Convert time and check values.
         */
        pp->hour = ((dp[0] - '0') * 10) + dp[1] - '0';
        pp->minute = ((dp[2] - '0') * 10) + dp[3] -  '0';
        pp->second = ((dp[4] - '0') * 10) + dp[5] - '0';
        pp->msec = 0; 

        if (pp->hour > 23 || pp->minute > 59 || pp->second > 59) {
                refclock_report(peer, CEVNT_BADTIME);
                return;
        }

        /*
         * Test for synchronization  Check for quality byte. (soon)
         */
        pp->leap = 0;
        pp->lasttime = current_time;

        /*
         * Process the new sample in the median filter and determine the
         * reference clock offset and dispersion. We use lastrec as both
         * the reference time and receive time, in order to avoid being
         * cute, like setting the reference time later than the receive
         * time, which may cause a paranoid protocol module to chuck out
         * the data.
         */
        if (!refclock_process(pp, NSAMPLES, NSAMPLES)) {
                refclock_report(peer, CEVNT_BADTIME);
                return;
        }

        refclock_receive(peer, &pp->offset, 0, pp->dispersion,
            &pp->lastrec, &pp->lastrec, pp->leap);
}

/*
 * nmea_poll - called by the transmit procedure
 *
 * We go to great pains to avoid changing state here, since there may be
 * more than one eavesdropper receiving the same timecode.
 */
static void
nmea_poll(unit, peer)
        int unit;
        struct peer *peer;
{
        register struct nmeaunit *up;
        struct refclockproc *pp;

        pp = peer->procptr;
        up = (struct nmeaunit *)pp->unitptr;
        if (up->pollcnt == 0)
                refclock_report(peer, CEVNT_TIMEOUT);
        else
                up->pollcnt--;
        pp->polls++;
        up->polled = 1;

        /*
         * usually nmea_receive can get a timestamp every second
         */

        gps_send(pp->io.fd,"$PMOTG,RMC,0000*1D\r\n", peer);
}

/*
 *
 *      gps_send(fd,cmd, peer)  Sends a command to the GPS receiver.
 *       as     gps_send(fd,"rqts,u\r", peer);
 *
 *      We don't currently send any data, but would like to send
 *      RTCM SC104 messages for differential positioning. It should
 *      also give us better time. Without a PPS output, we're
 *      Just fooling ourselves because of the serial code paths
 *
 */
static void
gps_send(fd, cmd, peer)
        int fd;
        char *cmd;
        struct peer *peer;
{

        if (write(fd, cmd, strlen(cmd)) == -1) {
                refclock_report(peer, CEVNT_FAULT);
        }
}

static char *
field_parse(cp, fn)
        char *cp;
        int fn;
{
        char *tp;
        int i = fn;

        for (tp = cp; *tp != '\0'; tp++) {
                if (*tp == ',')
                        i--;
                if (i == 0)
                        break;
        }
        return (++tp);
}
#endif