refclock_mx4200.c

网络时间协议NTP 源码 版本v4.2.0b 该源码用于linux平台下

	pp->second = second;

	/*
	 * Toss if sentence is marked invalid
	 */
	if (!valid || pp->leap == LEAP_NOTINSYNC) {
		mx4200_debug(peer, "mx4200_parse_t: time mark not valid\n");
		refclock_report(peer, CEVNT_BADTIME);
		return ("pulse invalid");
	}

	return (NULL);
}

/*
 * Calculate the checksum
 */
static u_char
mx4200_cksum(
	register char *cp,
	register int n
	)
{
	register u_char ck;

	for (ck = 0; n-- > 0; cp++)
		ck ^= *cp;
	return (ck);
}

/*
 * Tables to compute the day of year.  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};

/*
 * Calculate the the Julian Day
 */
static int
mx4200_jday(
	int year,
	int month,
	int day_of_month
	)
{
	register int day, i;
	int leap_year;

	/*
	 * Is this a leap year ?
	 */
	if (year % 4) {
		leap_year = 0; /* FALSE */
	} else {
		if (year % 100) {
			leap_year = 1; /* TRUE */
		} else {
			if (year % 400) {
				leap_year = 0; /* FALSE */
			} else {
				leap_year = 1; /* TRUE */
			}
		}
	}

	/*
	 * Calculate the Julian Date
	 */
	day = day_of_month;

	if (leap_year) {
		/* a leap year */
		if (day > day2tab[month - 1]) {
			return (0);
		}
		for (i = 0; i < month - 1; i++)
		    day += day2tab[i];
	} else {
		/* not a leap year */
		if (day > day1tab[month - 1]) {
			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 float UTC measurement time (seconds into week)
 *	2 float WGS-84 Lattitude (degrees, minutes)
 *	3  char N=North, S=South
 *	4 float WGS-84 Longitude (degrees, minutes)
 *	5  char E=East, W=West
 *	6 float Altitude (meters above mean sea level)
 *	7 float Geoidal height (meters)
 *	8 float East velocity (m/sec)
 *	9 float West Velocity (m/sec)
 *	10  int 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(
	struct peer *peer
	)
{
	struct refclockproc *pp;
	struct mx4200unit *up;
	int sentence_type, mode;
	double mtime, lat, lon, alt, geoid, vele, veln;
	char   north_south, east_west;

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

	/* Should never happen! */
	if (up->moving) return ("mobile platform - no pos!");

	sscanf ( pp->a_lastcode,
		"$PMVXG,%d,%lf,%lf,%c,%lf,%c,%lf,%lf,%lf,%lf,%d",
		&sentence_type, &mtime, &lat, &north_south, &lon, &east_west,
		&alt, &geoid, &vele, &veln, &mode);

	/* Sentence type */
	if (sentence_type != PMVXG_D_PHV)
		return ("wrong rec-type");

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

	/* Latitude (always +ve) and convert DDMM.MMMM to decimal */
	if (lat <  0.0) return ("negative latitude");
	if (lat > 9000.0) lat = 9000.0;
	lat *= 0.01;
	lat = ((int)lat) + (((lat - (int)lat)) * 1.6666666666666666);

	/* North/South */
	switch (north_south) {
		case 'N':
			break;
		case 'S':
			lat *= -1.0;
			break;
		default:
			return ("invalid north/south indicator");
	}

	/* Longitude (always +ve) and convert DDDMM.MMMM to decimal */
	if (lon <   0.0) return ("negative longitude");
	if (lon > 180.0) lon = 180.0;
	lon *= 0.01;
	lon = ((int)lon) + (((lon - (int)lon)) * 1.6666666666666666);

	/* East/West */
	switch (east_west) {
		case 'E':
			break;
		case 'W':
			lon *= -1.0;
			break;
		default:
			return ("invalid east/west indicator");
	}

	/*
	 * Normalize longitude to near 0 degrees.
	 * Assume all data are clustered around first reading.
	 */
	if (up->central_meridian == NOT_INITIALIZED) {
		up->central_meridian = lon;
		mx4200_debug(peer,
		    "mx4200_receive: central meridian =  %.9f \n",
		    up->central_meridian);
	}
	lon -= up->central_meridian;
	if (lon < -180.0) lon += 360.0;
	if (lon >  180.0) lon -= 360.0;

	/*
	 * Calculate running averages
	 */

	up->avg_lon = (up->N_fixes * up->avg_lon) + lon;
	up->avg_lat = (up->N_fixes * up->avg_lat) + lat;
	up->avg_alt = (up->N_fixes * up->avg_alt) + alt;

	up->N_fixes += 1.0;

	up->avg_lon /= up->N_fixes;
	up->avg_lat /= up->N_fixes;
	up->avg_alt /= up->N_fixes;

	mx4200_debug(peer,
	    "mx4200_receive: position rdg %.0f: %.9f %.9f %.4f (CM=%.9f)\n",
	    up->N_fixes, lat, lon, alt, up->central_meridian);

	return (NULL);
}

/*
 * Parse a mx4200 Status sentence
 * Parse a mx4200 Mode Data sentence
 * Parse a mx4200 Software Configuration sentence
 * Parse a mx4200 Time Recovery Parameters Currently in Use sentence
 * (used only for logging raw strings)
 *
 * A typical message looks like this.  Checksum has already been stripped.
 *
 * $PMVXG,000,XXX,XX,X,HHMM,X
 *
 *	Field	Field Contents
 *	-----	--------------
 *		Block Label: $PMVXG
 *		Sentence Type: 000=Status.
 *			Returns status of the receiver to the controller.
 *	1	Current Receiver Status:
 *		ACQ = Satellite re-acquisition
 *		ALT = Constellation selection
 *		COR = Providing corrections (for reference stations only)
 *		IAC = Initial acquisition
 *		IDL = Idle, no satellites
 *		NAV = Navigation
 *		STS = Search the Sky (no almanac available)
 *		TRK = Tracking
 *	2	Number of satellites that should be visible
 *	3	Number of satellites being tracked
 *	4	Time since last navigation status if not currently navigating
 *		(hours, minutes)
 *	5	Initialization status:
 *		0 = Waiting for initialization parameters
 *		1 = Initialization completed
 *
 * A typical message looks like this.  Checksum has already been stripped.
 *
 * $PMVXG,004,C,R,D,H.HH,V.VV,TT,HHHH,VVVV,T
 *
 *	Field	Field Contents
 *	-----	--------------
 *		Block Label: $PMVXG
 *		Sentence Type: 004=Software Configuration.
 *			Defines the navigation mode and criteria for
 *			acceptable navigation for the receiver.
 *	1	Constrain Altitude Mode:
 *		0 = Auto.  Constrain altitude (2-D solution) and use
 *		    manual altitude input when 3 sats avalable.  Do
 *		    not constrain altitude (3-D solution) when 4 sats
 *		    available.
 *		1 = Always constrain altitude (2-D solution).
 *		2 = Never constrain altitude (3-D solution).
 *		3 = Coast.  Constrain altitude (2-D solution) and use
 *		    last GPS altitude calculation when 3 sats avalable.
 *		    Do not constrain altitude (3-D solution) when 4 sats
 *		    available.
 *	2	Altitude Reference: (always 0 for MX4200)
 *		0 = Ellipsoid
 *		1 = Geoid (MSL)
 *	3	Differential Navigation Control:
 *		0 = Disabled
 *		1 = Enabled
 *	4	Horizontal Acceleration Constant (m/sec**2)
 *	5	Vertical Acceleration Constant (m/sec**2) (0 for MX4200)
 *	6	Tracking Elevation Limit (degrees)
 *	7	HDOP Limit
 *	8	VDOP Limit
 *	9	Time Output Mode:
 *		U = UTC
 *		L = Local time
 *	10	Local Time Offset (minutes) (absent on MX4200)
 *
 * 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
 *
 * A typical message looks like this.  Checksum has already been stripped.
 *
 * $PMVXG,523,M,S,M,EEEE,BBBBBB,C,R
 *
 *	Field	Field Contents
 *	-----	--------------
 *		Block Label: $PMVXG
 *		Sentence Type: 523=Time Recovery Parameters Currently in Use.
 *			This sentence contains the configuration of the
 *			time recovery feature of the receiver.
 *	1	Time Recovery Mode:
 *		D = Dynamic; solve for position and time while moving
 *		S = Static; solve for position and time while stationary
 *		K = Known position input, solve for time only
 *		N = No time recovery
 *	2	Time Synchronization:
 *		U = UTC time
 *		G = GPS time
 *	3	Time Mark Mode:
 *		A = Always output a time pulse
 *		V = Only output time pulse if time is valid (as determined
 *		    by Maximum Time Error)
 *	4	Maximum Time Error - the maximum error (in nanoseconds) for
 *		which a time mark will be considered valid.
 *	5	User Time Bias - external bias in nanoseconds
 *	6	Time Message Control:
 *		0 = Do not output the time recovery message
 *		1 = Output the time recovery message (record 830) to
 *		    Control port
 *		2 = Output the time recovery message (record 830) to
 *		    Equipment port
 *	7	Reserved
 *	8	Position Known PRN (absent on MX 4200)
 *
 */
static char *
mx4200_parse_s(
	struct peer *peer
	)
{
	struct refclockproc *pp;
	struct mx4200unit *up;
	int sentence_type;

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

        sscanf ( pp->a_lastcode, "$PMVXG,%d", &sentence_type);

	/* Sentence type */
	switch (sentence_type) {

		case PMVXG_D_STATUS:
			msyslog(LOG_DEBUG,
			  "mx4200: status: %s", pp->a_lastcode);
			break;
		case PMVXG_D_MODEDATA:
			msyslog(LOG_DEBUG,
			  "mx4200: mode data: %s", pp->a_lastcode);
			break;
		case PMVXG_D_SOFTCONF:
			msyslog(LOG_DEBUG,
			  "mx4200: firmware configuration: %s", pp->a_lastcode);
			break;
		case PMVXG_D_TRECOVUSEAGE:
			msyslog(LOG_DEBUG,
			  "mx4200: time recovery parms: %s", pp->a_lastcode);
			break;
		default:
			return ("wrong rec-type");
	}

	return (NULL);
}

/*
 * Process a PPS signal, placing a timestamp in pp->lastrec.
 */
static int
mx4200_pps(
	struct peer *peer
	)
{
	int temp_serial;
	struct refclockproc *pp;
	struct mx4200unit *up;

	struct timespec timeout;

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

	/*
	 * Grab the timestamp of the PPS signal.
	 */
	temp_serial = up->pps_i.assert_sequence;
	timeout.tv_sec  = 0;
	timeout.tv_nsec = 0;
	if (time_pps_fetch(up->pps_h, PPS_TSFMT_TSPEC, &(up->pps_i),
			&timeout) < 0) {
		mx4200_debug(peer,
		  "mx4200_pps: time_pps_fetch: serial=%ul, %s\n",
		     (unsigned long)up->pps_i.assert_sequence, strerror(errno));
		refclock_report(peer, CEVNT_FAULT);
		return(1);
	}
	if (temp_serial == up->pps_i.assert_sequence) {
		mx4200_debug(peer,
		   "mx4200_pps: assert_sequence serial not incrementing: %ul\n",
			(unsigned long)up->pps_i.assert_sequence);
		refclock_report(peer, CEVNT_FAULT);
		return(1);
	}
	/*
	 * Check pps serial number against last one
	 */
	if (up->lastserial + 1 != up->pps_i.assert_sequence &&
	    up->lastserial != 0) {
		if (up->pps_i.assert_sequence == up->lastserial) {
			mx4200_debug(peer, "mx4200_pps: no new pps event\n");
		} else {
			mx4200_debug(peer, "mx4200_pps: missed %ul pps events\n",
			    up->pps_i.assert_sequence - up->lastserial - 1UL);
		}
		refclock_report(peer, CEVNT_FAULT);
	}
	up->lastserial = up->pps_i.assert_sequence;

	/*
	 * Return the timestamp in pp->lastrec
	 */

	pp->lastrec.l_ui = up->pps_i.assert_timestamp.tv_sec +
			   (u_int32) JAN_1970;
	pp->lastrec.l_uf = ((double)(up->pps_i.assert_timestamp.tv_nsec) *
			   4.2949672960) + 0.5;

	return(0);
}

/*
 * mx4200_debug - print debug messages
 */
#if defined(__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 /* __STDC__ */
{
	va_list ap;
	struct refclockproc *pp;
	struct mx4200unit *up;

	if (debug) {

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

		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.
 */
#if defined(__STDC__)
static void
mx4200_send(struct peer *peer, char *fmt, ...)
#else
static void
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 defined(__STDC__)
	va_start(ap, fmt);
#else
	va_start(ap);
#endif /* __STDC__ */

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

	cp = buf;
	*cp++ = '$';
	n = VSNPRINTF((cp, sizeof(buf) - 1, fmt, ap));
	ck = mx4200_cksum(cp, n);
	cp += n;
	++n;
	n += SNPRINTF((cp, sizeof(buf) - n - 5, "*%02X\r\n", ck));

	m = write(pp->io.fd, buf, (unsigned)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
int refclock_mx4200_bs;
#endif /* REFCLOCK */