87

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

			return (0);
		}
		for (i = 0; i < month - 1; i++)
			day += day1tab[i];
	}
	return (day);
}

/*
 * Parse a mx4200 position/height/velocity sentence.
 *
 * A typical message looks like this.  Checksum has already been stripped.
 *
 * $PMVXG,021,SSSSSS.SS,DDMM.MMMM,N,DDDMM.MMMM,E,HHHHH.H,GGGG.G,EEEE.E,WWWW.W,MM
 *
 *	Field	Field Contents
 *	-----	--------------
 *		Block Label: $PMVXG
 *		Sentence Type: 021=Position, Height Velocity Data
 *			This sentence gives the receiver position, height,
 *			navigation mode, and velocity north/east.
 *			*This sentence is intended for post-analysis
 *			applications.*
 *	1	UTC measurement time (seconds into week)
 *	2	WGS-84 Lattitude (degrees, minutes)
 *	3	N=North, S=South
 *	4	WGS-84 Longitude (degrees, minutes)
 *	5	E=East, W=West
 *	6	Altitude (meters above mean sea level)
 *	7	Geoidal height (meters)
 *	8	East velocity (m/sec)
 *	9	West Velocity (m/sec)
 *	10	Navigation Mode
 *		    Mode if navigating:
 *			1 = Position from remote device
 *			2 = 2-D position
 *			3 = 3-D position
 *			4 = 2-D differential position
 *			5 = 3-D differential position
 *			6 = Static
 *			8 = Position known -- reference station
 *			9 = Position known -- Navigator
 *		    Mode if not navigating:
 *			51 = Too few satellites
 *			52 = DOPs too large
 *			53 = Position STD too large
 *			54 = Velocity STD too large
 *			55 = Too many iterations for velocity
 *			56 = Too many iterations for position
 *			57 = 3 sat startup failed
 *			58 = Command abort
 */
static char *
mx4200_parse_p(peer)
	struct peer *peer;
{
	struct refclockproc *pp;
	struct mx4200unit *up;
	int sentence_type, mode;
	double dtemp, dtemp2, mtime, lat, lon, alt, geoid, vele, veln, weight;
	char *cp;

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

	cp = pp->lastcode;

	if ((cp = strchr(cp, ',')) == NULL)
		return ("no rec-type");
	cp++;

	/* Sentence type */
	sentence_type = strtol(cp, &cp, 10);
	if (sentence_type != PMVXG_D_PHV)
		return ("wrong rec-type");

	/* Measurement Time */
	if (*cp++ != ',')
		return ("no measurement time");
	mtime = strtod(cp,&cp);

	/* Latitude (always +ve) */
	if (*cp++ != ',')
		return ("no latitude");
	dtemp = atof(cp);
	dtemp2 = strtod(cp,&cp);
	if (dtemp < 0.0)
		return ("negative latitude");
	lat = (double) ( (int)dtemp / 100);
	lat += (dtemp - (lat*100.0)) * 10.0 / 600.0;

	/* North/South */
	if (*cp++ != ',')
		return ("no north/south indicator");
	if (*cp == 'N')
		lat = lat;
	else if (*cp == 'S')
		lat *= -1.0;
	else
		return ("invalid north/south indicator");
	cp++;

	/* Longitude (always +ve) */
	if (*cp++ != ',')
		return ("no longitude");
	dtemp = atof(cp);
	dtemp2 = strtod(cp,&cp);
	if (dtemp < 0.0)
		return ("negative latitude");
	lon = (double) ( (int)dtemp / 100);
	lon += (dtemp - (lon*100.0)) * 10.0 / 600.0;

	/* East/West */
	if (*cp++ != ',')
		return ("no east/west indicator");
	if (*cp == 'E')
		lon = lon;
	else if (*cp == 'W')
		lon *= -1.0;
	else
		return ("invalid east/west indicator");
	cp++;

	/* Altitude */
	if (*cp++ != ',')
		return ("no altitude");
	alt = atof(cp);
	dtemp2 = strtod(cp,&cp);

	/* geoid height */
	if (*cp++ != ',')
		return ("no geoid height");
	geoid = strtod(cp,&cp);

	/* East velocity */
	if (*cp++ != ',')
		return ("no east velocity");
	vele = strtod(cp,&cp);

	/* north velocity */
	if (*cp++ != ',')
		return ("no north velocity");
	veln = strtod(cp,&cp);

	/* nav mode */
	if (*cp++ != ',')
		return ("no nav mode");
	mode = strtol(cp, &cp, 10);


	/*
	 * return if not navigating
	 */
	if (mode > 10)
		return ("not navigating");

	if (mode != 3 && mode != 5)
		return ("not navigating in 3D");

	/*
	 * Calculate running weighted averages
	 */

	weight = (USUAL_EDOP/up->edop);
	weight = weight * weight;
	up->avg_lon = up->filt_lon * up->avg_lon + weight * lon;
	up->filt_lon += weight;
	up->avg_lon = up->avg_lon / up->filt_lon;

	weight = (USUAL_NDOP/up->ndop);
	weight = weight * weight;
	up->avg_lat = up->filt_lat * up->avg_lat + weight * lat;
	up->filt_lat += weight;
	up->avg_lat = up->avg_lat / up->filt_lat;

	weight = (USUAL_VDOP/up->vdop);
	weight = weight * weight;
	up->avg_alt = up->filt_alt * up->avg_alt + weight * alt;
	up->filt_alt += weight;
	up->avg_alt = up->avg_alt / up->filt_alt;

	/*
	 * Are we moving?
	 */
	if ((vele*vele + veln*veln) > MAX_VEL_SQUARED)
		up->moving++;

	return (NULL);
}

/*
 * Parse a mx4200 DOP sentence.
 *
 * A typical message looks like this.  Checksum has already been stripped.
 *
 * $PMVXG,022,SSSSSS.SSEE.E,NN.N,VV.V,XX,XX,XX,XX,XX,XX
 *
 *	Field	Field Contents
 *	-----	--------------
 *		Block Label: $PMVXG
 *		Sentence Type: 022=DOPs.  The DOP values in this sentence
 *			correspond to the satellites listed.  The PRNs in
 *			the message are listed in receiver channel number order
 *	1	UTC measurement time (seconds into week)
 *	2	EDOP (east DOP)
 *	3	NDOP (north DOP)
 *	4	VDOP (vertical DOP)
 *	5	PRN on channel 1
 *	6	PRN on channel 2
 *	7	PRN on channel 3
 *	8	PRN on channel 4
 *	9	PRN on channel 5
 *	10	PRN on channel 6
 */
static char *
mx4200_parse_d(peer)
	struct peer *peer;
{
	struct refclockproc *pp;
	struct mx4200unit *up;
	int sentence_type;
	double mtime, dtemp2, edop, ndop, vdop;
	char *cp;

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

	cp = pp->lastcode;

	if ((cp = strchr(cp, ',')) == NULL)
		return ("no rec-type");
	cp++;

	/* Sentence type */
	sentence_type = strtol(cp, &cp, 10);
	if (sentence_type != PMVXG_D_DOPS)
		return ("wrong rec-type");

	/* Measurement Time */
	if (*cp++ != ',')
		return ("no measurement time");
	mtime = strtod(cp,&cp);

	/* EDOP */
	if (*cp++ != ',')
		return ("no edop");
	edop = atof(cp);
	dtemp2 = strtod(cp,&cp);

	/* NDOP */
	if (*cp++ != ',')
		return ("no ndop");
	ndop = atof(cp);
	dtemp2 = strtod(cp,&cp);

	/* VDOP */
	if (*cp++ != ',')
		return ("no vdop");
	vdop = atof(cp);
	dtemp2 = strtod(cp,&cp);

	/* Ignore the PRNs... */


	/* Update values */
	if (ndop <= 0.0 || ndop<= 0.0 || vdop <= 0.0)
		return ("nonpositive dop");
	up->edop = edop;
	up->ndop = ndop;
	up->vdop = vdop;

	return (NULL);
}

/*
 * Parse a mx4200 Software Configuration sentence
 *
 * A typical message looks like this.  Checksum has already been stripped.
 *
 * $PMVXG,030,NNNN,FFF
 *
 *	Field	Field Contents
 *	-----	--------------
 *		Block Label: $PMVXG
 *		Sentence Type: 030=Software Configuration.
 *			This sentence contains the navigation processor
 *			and baseband firmware version numbers.
 *	1	Nav Processor Version Number
 *	2	Baseband Firmware Version Number
 */
static char *
mx4200_parse_s(peer)
	struct peer *peer;
{
	struct refclockproc *pp;
	struct mx4200unit *up;
	int sentence_type;
	char *cp;

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

	cp = pp->lastcode;

	if ((cp = strchr(cp, ',')) == NULL)
		return ("no rec-type");
	cp++;

	/* Sentence type */
	sentence_type = strtol(cp, &cp, 10);
	if (sentence_type != PMVXG_D_SOFTCONF)
		return ("wrong rec-type");

	/* Log the software version */
	cp++;
	msyslog(LOG_DEBUG, "mx4200_parse_s: firmware configuration: %s", cp);

}

/*
 * Process a PPS signal, returning a timestamp.
 */
static int
mx4200_pps(peer)
	struct peer *peer;
{
#ifdef PPS
	struct refclockproc *pp;
	struct mx4200unit *up;

	int temp_serial;

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


	/*
	 * Grab the timestamp of the PPS signal.
	 */
	temp_serial = up->ppsev.serial;
	if (ioctl(fdpps, CIOGETEV, (caddr_t)&up->ppsev) < 0) {
		/* XXX Actually, if this fails, we're pretty much screwed */
		mx4200_debug(peer, "mx4200_pps: CIOGETEV: ");
		if (errno < sys_nerr)
			mx4200_debug(peer, "%s", sys_errlist[errno]);
		mx4200_debug(peer, "\n");
		refclock_report(peer, CEVNT_FAULT);
		return(1);
	}
	if (temp_serial == up->ppsev.serial) {
		mx4200_debug(peer,
			"mx4200_pps: ppsev serial not incrementing: %d\n",
				up->ppsev.serial);
		refclock_report(peer, CEVNT_FAULT);
		return(1);
	}

	/*
	 * Check pps serial number against last one
	 */
	if (up->lastserial + 1 != up->ppsev.serial && up->lastserial != 0) {
		if (up->ppsev.serial == up->lastserial)
			mx4200_debug(peer, "mx4200_pps: no new pps event\n");
		else
			mx4200_debug(peer, "mx4200_pps: missed %d pps events\n",
				up->ppsev.serial - up->lastserial - 1);
		refclock_report(peer, CEVNT_FAULT);
	}
	up->lastserial = up->ppsev.serial;

	/*
	 * Return the timestamp in pp->lastrec
	 */
	up->ppsev.tv.tv_sec += (u_int32) JAN_1970;
	TVTOTS(&up->ppsev.tv,&pp->lastrec);

#endif /* PPS */

	return(0);
}

/*
 * mx4200_debug - print debug messages
 */
#if __STDC__
static void
mx4200_debug(struct peer *peer, char *fmt, ...)
#else
static void
mx4200_debug(peer, fmt, va_alist)
	struct peer *peer;
	char *fmt;
#endif
{
	va_list ap;
	struct refclockproc *pp;
	struct mx4200unit *up;

	if (debug) {

#if __STDC__
		va_start(ap, fmt);
#else
		va_start(ap);
#endif

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


		/*
		 * Print debug message to stdout
		 * In the future, we may want to get get more creative...
		 */
		vprintf(fmt, ap);

		va_end(ap);
	}
}

/*
 * Send a character string to the receiver.  Checksum is appended here.
 */
static void
#if __STDC__
mx4200_send(struct peer *peer, char *fmt, ...)
#else
mx4200_send(peer, fmt, va_alist)
	struct peer *peer;
	char *fmt;
	va_dcl
#endif /* __STDC__ */
{
	struct refclockproc *pp;
	struct mx4200unit *up;

	register char *cp;
	register int n, m;
	va_list ap;
	char buf[1024];
	u_char ck;

#if __STDC__
	va_start(ap, fmt);
#else
	va_start(ap);
#endif /* __STDC__ */

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

	cp = buf;
	*cp++ = '$';
#ifdef notdef
	/* BSD is rational */
	n = vsnprintf(cp, sizeof(buf) - 1, fmt, ap);
#else
	/* SunOS sucks */
	(void)vsprintf(cp, fmt, ap);
	n = strlen(cp);
#endif /* notdef */
	ck = mx4200_cksum(cp, n);
	cp += n;
	++n;
#ifdef notdef
	/* BSD is rational */
	n += snprintf(cp, sizeof(buf) - n - 5, "*%02X\r\n", ck);
#else
	/* SunOS sucks */
	sprintf(cp, "*%02X\r\n", ck);
	n += strlen(cp);
#endif /* notdef */

	m = write(pp->io.fd, buf, n);
	if (m < 0)
		msyslog(LOG_ERR, "mx4200_send: write: %m (%s)", buf);
	mx4200_debug(peer, "mx4200_send: %d %s\n", m, buf);
	va_end(ap);
}

#else /* not (REFCLOCK && MX4200 && PPS) */
int refclock_mx4200_bs;
#endif /* not (REFCLOCK && MX4200 && PPS) */

/*
 * History:
 *
 * refclock_mx4200.c
 *
 * Author:	Craig Leres?
 * Revised:	Marc Brett	1.10	1996-05-08
 * - rewrote to "new-style" xntpd driver standard.
 * - added position-averaging/fixed-location logic.
 * Revised:	Marc Brett	1.20	1997-02-11
 * - updated to xntp3-5.89.5 standards.
 *
 * Craig Leres	([e-mail])
 * Marc Brett	([e-mail])
 *
 */