87

Unix/Linux 网络时间协议版本3 Network Time Protocol Version 3 (NTP) distribution for Unix systems

{
	register struct mx4200unit *up;
	struct refclockproc *pp;

	pp = peer->procptr;
	up = (struct mx4200unit *)pp->unitptr;

	/*
	 * "007" Control Port Configuration
	 * Zero the output list (do it twice to flush possible junk)
	 */
	mx4200_send(peer, "%s,%03d,,%d,,,,,,", pmvxg,
	    PMVXG_S_PORTCONF,
			/* control port output block Label */
	    1);		/* clear current output control list (1=yes) */
			/* add/delete sentences from list */
			/* must be null */
			/* sentence output rate (sec) */
			/* precision for position output */
			/* nmea version for cga & gll output */
			/* pass-through control */
	mx4200_send(peer, "%s,%03d,,%d,,,,,,", pmvxg,
	    PMVXG_S_PORTCONF, 1);

	/*
	 * Request software configuration so we can syslog the firmware version
	 */
	mx4200_send(peer, "%s,%03d", "CDGPQ", PMVXG_D_SOFTCONF);

	/*
	 * "001" Initialization/Mode Control, Part A
	 * Where ARE we?
	 */
	mx4200_send(peer, "%s,%03d,,,,,,,,,,", pmvxg,
	    PMVXG_S_INITMODEA);
			/* day of month */
			/* month of year */
			/* year */
			/* gmt */
			/* latitude   DDMM.MMMM */
			/* north/south */
			/* longitude DDDMM.MMMM */
			/* east/west */
			/* height */
			/* Altitude Reference 1=MSL */

	/*
	 * "001" Initialization/Mode Control, Part B
	 * Start off in 2d/3d coast mode, holding altitude to last known
	 * value if only 3 satellites available.
	 */
	mx4200_send(peer, "%s,%03d,%d,,%.1f,%.1f,%d,%d,%d,%c,%d",
	    pmvxg, PMVXG_S_INITMODEB,
	    3,		/* 2d/3d coast */
			/* reserved */
	    0.1,	/* hor accel fact as per Steve (m/s**2) */
	    0.1,	/* ver accel fact as per Steve (m/s**2) */
	    10,		/* vdop */
	    10,		/* hdop limit as per Steve */
	    5,		/* elevation limit as per Steve (deg) */
	    'U',	/* time output mode (UTC) */
	    0);		/* local time offset from gmt (HHHMM) */

	/*
	 * "023" Time Recovery Configuration
	 * Get UTC time from a stationary receiver.
	 * (Set field 1 'D' == dynamic if we are on a moving platform).
	 * (Set field 1 'S' == static  if we are not moving).
	 * (Set field 1 'K' == known position if we can initialize lat/lon/alt).
	 */
	mx4200_send(peer, "%s,%03d,%c,%c,%c,%d,%d,%d,", pmvxg,
	    PMVXG_S_TRECOVCONF,
	    'S',	/* static: solve for pos, alt, time, while stationary */
	    'U',	/* synchronize to UTC */
	    'A',	/* always output a time pulse */
	    500,	/* max time error in ns */
	    0,		/* user bias in ns */
	    1);		/* output "830" sentences to control port */
			/* Multi-satellite mode */

	/*
	 * "007" Control Port Configuration
	 * Output "004" mode data
	 */
	mx4200_send(peer, "%s,%03d,%03d,%d,%d,,%d,,,", pmvxg,
	    PMVXG_S_PORTCONF,
	    PMVXG_D_MODEDATA, /* control port output block Label */
	    0,		/* clear current output control list (0=no) */
	    1,		/* add/delete sentences from list (1=add) */
			/* must be null */
	    INTERVAL*10);/* sentence output rate (sec) */
	    		/* precision for position output */
			/* nmea version for cga & gll output */
			/* pass-through control */

	/*
	 * "007" Control Port Configuration
	 * Output "022" DOPs
	 */
	mx4200_send(peer, "%s,%03d,%03d,%d,%d,,%d,,,", pmvxg,
	    PMVXG_S_PORTCONF,
	    PMVXG_D_DOPS, /* control port output block Label */
	    0,		/* clear current output control list (0=no) */
	    1,		/* add/delete sentences from list (1=add) */
			/* must be null */
	    INTERVAL);	/* sentence output rate (sec) */
	    		/* precision for position output */
			/* nmea version for cga & gll output */
			/* pass-through control */

	/*
	 * "007" Control Port Configuration
	 * Output "523" time recovery parameters currently in use
	 */
	mx4200_send(peer, "%s,%03d,%03d,%d,%d,,%d,,,", pmvxg,
	    PMVXG_S_PORTCONF,
	    PMVXG_D_TRECOVUSEAGE, /* control port output block Label */
	    0,		/* clear current output control list (0=no) */
	    1,		/* add/delete sentences from list (1=add) */
			/* must be null */
	    INTERVAL*10); /* sentence output rate (sec) */
	    		/* precision for position output */
			/* nmea version for cga & gll output */
			/* pass-through control */

	/*
	 * "007" Control Port Configuration
	 * Output "021" position, height, velocity reports
	 */
	mx4200_send(peer, "%s,%03d,%03d,%d,%d,,%d,,,", pmvxg,
	    PMVXG_S_PORTCONF,
	    PMVXG_D_PHV, /* control port output block Label */
	    0,		/* clear current output control list (0=no) */
	    1,		/* add/delete sentences from list (1=add) */
			/* must be null */
	    INTERVAL);	/* sentence output rate (sec) */
	    		/* precision for position output */
			/* nmea version for cga & gll output */
			/* pass-through control */


}

/*
 * mx4200_ref - Reconfigure unit as a reference station at a known position.
 */
static void
mx4200_ref(peer)
	struct peer *peer;
{
	register struct mx4200unit *up;
	struct refclockproc *pp;
	double dtemp, lat, lon, alt;
	char lats[32], lons[32];
	char nsc, ewc;

	pp = peer->procptr;
	up = (struct mx4200unit *)pp->unitptr;

	/*
	 * "001" Initialization/Mode Control, Part B
	 * Put receiver in fully-constrained 2d nav mode
	 */
	mx4200_send(peer, "%s,%03d,%d,,%.1f,%.1f,%d,%d,%d,%c,%d",
	    pmvxg, PMVXG_S_INITMODEB,
	    2,		/* 2d nav */
			/* reserved */
	    0.1,	/* hor accel fact as per Steve (m/s**2) */
	    0.1,	/* ver accel fact as per Steve (m/s**2) */
	    10,		/* vdop */
	    10,		/* hdop limit as per Steve */
	    5,		/* elevation limit as per Steve (deg) */
	    'U',	/* time output mode (UTC) */
	    0);		/* local time offset from gmt (HHHMM) */

	/*
	 * "023" Time Recovery Configuration
	 * Get UTC time from a stationary receiver.  Solve for time only.
	 * This should improve the time resolution dramatically.
	 */
	mx4200_send(peer, "%s,%03d,%c,%c,%c,%d,%d,%d,", pmvxg,
	    PMVXG_S_TRECOVCONF,
	    'K',	/* known position: solve for time only */
	    'U',	/* synchronize to UTC */
	    'A',	/* always output a time pulse */
	    500,	/* max time error in ns */
	    0,		/* user bias in ns */
	    1);		/* output "830" sentences to control port */
			/* Multi-satellite mode */

	/*
	 * "000" Initialization/Mode Control - Part A
	 * Fix to our averaged position.
	 */
	if (up->avg_lat >= 0.0) {
		lat = up->avg_lat;
		nsc = 'N';
	} else {
		lat = up->avg_lat * (-1.0);
		nsc = 'S';
	}
	if (up->avg_lon >= 0.0) {
		lon = up->avg_lon;
		ewc = 'E';
	} else {
		lon = up->avg_lon * (-1.0);
		ewc = 'W';
	}
	alt = up->avg_alt;
	dtemp = (lat - (double)(int)lat) * 600.0 / 10.0;
	sprintf(lats,"%02d%02.4f", (int)lat, dtemp);
	dtemp = (lon - (double)(int)lon) * 600.0 / 10.0;
	sprintf(lons,"%02d%02.4f", (int)lon, dtemp);

	mx4200_send(peer, "%s,%03d,,,,,%s,%c,%s,%c,%.2f,%d", pmvxg,
	    PMVXG_S_INITMODEA,
			/* day of month */
			/* month of year */
			/* year */
			/* gmt */
	    lats,	/* latitude   DDMM.MMMM */
	    nsc,	/* north/south */
	    lons,	/* longitude DDDMM.MMMM */
	    ewc,	/* east/west */
	    alt,	/* height */
	    1);		/* Altitude Reference 1=MSL */

	msyslog(LOG_DEBUG,
	    "mx4200_ref: reconfig to fixed location: %s %c, %s %c, %.2f m MSL",
	    lats, nsc, lons, ewc, alt );
}

/*
 * mx4200_poll - mx4200 watchdog routine
 */
static void
mx4200_poll(unit, peer)
	int unit;
	struct peer *peer;
{
	register struct mx4200unit *up;
	struct refclockproc *pp;

	pp = peer->procptr;
	up = (struct mx4200unit *)pp->unitptr;

	/*
	 * You don't need to poll this clock.  It puts out timecodes
	 * once per second.  If asked for a timestamp, take note.
	 * The next time a timecode comes in, it will be fed back.
	 */

	/*
	 * If we haven't had a response in a while, reset the receiver.
	 */
	if (up->pollcnt > 0) {
		up->pollcnt--;
	} else {
		refclock_report(peer, CEVNT_TIMEOUT);

		/*
		 * Request a "000" status message which should trigger a
		 * reconfig
		 */
		mx4200_send(peer, "%s,%03d",
		    "CDGPQ",		/* query from CDU to GPS */
		    PMVXG_D_STATUS);	/* label of desired sentence */
	}

	/*
	 * polled every 64 seconds. Ask mx4200_receive to hand in
	 * a timestamp.
	 */
	up->polled = 1;
	pp->polls++;
}

static char char2hex[] = "0123456789ABCDEF";

/*
 * mx4200_receive - receive gps data
 */
static void
mx4200_receive(rbufp)
	struct recvbuf *rbufp;
{
	register struct mx4200unit *up;
	struct refclockproc *pp;
	struct peer *peer;
	char *cp;
	int sentence_type;
	u_char ck;

	/*
	 * Initialize pointers and read the timecode and timestamp.
	 */
	peer = (struct peer *)rbufp->recv_srcclock;
	pp = peer->procptr;
	up = (struct mx4200unit *)pp->unitptr;

	/*
	 * Read clock output.  Automatically handles STREAMS, CLKLDISC.
	 */
	pp->lencode = refclock_gtlin(rbufp, pp->lastcode, BMAX, &pp->lastrec);

	/*
	 * There is a case where <cr><lf> generates 2 timestamps.
	 */
	if (pp->lencode == 0)
		return;

	up->pollcnt = 2;
	pp->lastcode[pp->lencode] = '\0';
	record_clock_stats(&peer->srcadr, pp->lastcode);
	mx4200_debug(peer, "mx4200_receive: %d %s\n",
		pp->lencode, pp->lastcode);

	/*
	 * The structure of the control port sentences is based on the
	 * NMEA-0183 Standard for interfacing Marine Electronics
	 * Navigation Devices (Version 1.5)
	 *
	 *	$PMVXG,XXX, ....................*CK<cr><lf>
	 *
	 *		$	Sentence Start Identifier (reserved char)
	 *			   (Start-of-Sentence Identifier)
	 *		P	Special ID (Proprietary)
	 *		MVX	Originator ID (Magnavox)
	 *		G	Interface ID (GPS)
	 *		,	Field Delimiters (reserved char)
	 *		XXX	Sentence Type
	 *		......	Data
	 *		*	Checksum Field Delimiter (reserved char)
	 *		CK	Checksum
	 *		<cr><lf> Carriage-Return/Line Feed (reserved chars)
	 *			   (End-of-Sentence Identifier)
	 *
	 * Reject if any important landmarks are missing.
	 */
	cp = pp->lastcode + pp->lencode - 3;
	if (cp < pp->lastcode || *pp->lastcode != '$' || cp[0] != '*' ) {
		mx4200_debug(peer, "mx4200_receive: bad format\n");
		refclock_report(peer, CEVNT_BADREPLY);
		return;
	}

	/*
	 * Check and discard the checksum
	 */
	ck = mx4200_cksum(&pp->lastcode[1], pp->lencode - 4);
	if (char2hex[ck >> 4] != cp[1] || char2hex[ck & 0xf] != cp[2]) {
		mx4200_debug(peer, "mx4200_receive: bad checksum\n");
		refclock_report(peer, CEVNT_BADREPLY);
		return;
	}
	*cp = '\0';

	/*
	 * Get the sentence type.
	 */
	sentence_type = 0;
	if ((cp = strchr(pp->lastcode, ',')) == NULL) {
		mx4200_debug(peer, "mx4200_receive: no sentence\n", cp);
		refclock_report(peer, CEVNT_BADREPLY);
		return;
	}
	cp++;
	sentence_type = strtol(cp, &cp, 10);

	/*
	 * "000" Status message
	 */

	if (sentence_type == PMVXG_D_STATUS) {
		/*
		 * XXX
		 * Since we configure the receiver to not give us status
		 * messages and since the receiver outputs status messages by
		 * default after being reset to factory defaults when sent the
		 * "$PMVXG,018,C\r\n" message, any status message we get
		 * indicates the reciever needs to be initialized; thus, it is
		 * not necessary to decode the status message.
		 */
		mx4200_debug(peer, "mx4200_receive: reset receiver\n", cp);
		mx4200_config(peer);
		return;
	}

	/*
	 * "021" Position, Height, Velocity message,
	 *  if we are still averaging our position
	 */
	if (sentence_type == PMVXG_D_PHV && !up->known) {
		/*
		 * Parse the message, calculating our averaged position.
		 */
		if ((cp = mx4200_parse_p(peer)) != NULL) {
			mx4200_debug(peer, "mx4200_receive: pos: %s\n", cp);
			return;
		}
		mx4200_debug(peer,
			"mx4200_receive: position avg %.9f %.9f %.4f\n",
			up->avg_lat, up->avg_lon, up->avg_alt);
		mx4200_debug(peer,
			"mx4200_receive: position len %.4f %.4f %.4f\n",
			up->filt_lat, up->filt_lon, up->filt_alt);
		mx4200_debug(peer,
			"mx4200_receive: position dop %.2f %.2f %.2f\n",
			up->ndop, up->edop, up->vdop);
		/*
		 * Reinitialize as a reference station
		 * if position is well known.
		 */
		if (current_time > up->clamp_time) {
			up->known++;
			mx4200_debug(peer, "mx4200_receive: reconfiguring!\n");
			mx4200_ref(peer);
		}
		return;
	}

	/*
	 * "022" DOPs, if we are still averaging our position
	 */
	if (sentence_type == PMVXG_D_DOPS && !up->known) {
		if ((cp = mx4200_parse_d(peer)) != NULL) {
			mx4200_debug(peer, "mx4200_receive: dop: %s\n", cp);
			return;
		}
		return;
	}

	/*
	 * "030" Software Configuration
	 */
	if (sentence_type == PMVXG_D_SOFTCONF && !up->known) {
		if ((cp = mx4200_parse_s(peer)) != NULL) {
			mx4200_debug(peer, "mx4200_receive: sw conf: %s\n", cp);
			return;
		}
		return;
	}

	/*
	 * "830" Time Recovery Results message
	 */
	if (sentence_type == PMVXG_D_TRECOVOUT) {

		/*
		 * Capture the last PPS signal.
		 * Precision timestamp is returned in pp->lastrec
		 */
		if (mx4200_pps(peer) != NULL) {
			mx4200_debug(peer, "mx4200_receive: pps failure\n");
			refclock_report(peer, CEVNT_FAULT);
			return;
		}

		/*
		 * Parse the time recovery message, and keep the info
		 * to print the pretty billboards.
		 */
		if ((cp = mx4200_parse_t(peer)) != NULL) {
			mx4200_debug(peer, "mx4200_receive: time: %s\n", cp);
			refclock_report(peer, CEVNT_BADREPLY);
			return;
		}

		/*
		 * Add the new sample to a median filter.
		 */
		if ((cp =mx4200_offset(peer)) != NULL) {
			mx4200_debug(peer,"mx4200_receive: offset: %s\n", cp);
			refclock_report(peer, CEVNT_BADTIME);
			return;
		}

		/*
		 * The clock will blurt a timecode every second but we only
		 * want one when polled.  If we havn't been polled, bail out.
		 */
		if (!up->polled)
			return;

		/*
		 * It's a live one!  Remember this time.
		 */
		pp->lasttime   = current_time;

		/*
		 * Determine the reference clock offset and dispersion.
		 * NKEEP of NSAMPLE offsets are passed through a median filter.
		 * Save the (filtered) offset and dispersion in
		 * pp->offset and pp->dispersion.
		 */
		if ((cp =mx4200_process(peer)) != NULL) {
			mx4200_debug(peer,"mx4200_receive: process: %s\n", cp);
			refclock_report(peer, CEVNT_BADTIME);
			return;
		}

		/*
		 * Return offset and dispersion to control module.  We use
		 * lastrec as both the reference time and receive time in
		 * order to avoid being cute, like setting the reference time