FreeBSD/contrib/ntp/ntpd/refclock_mx4200.c
/*
 * 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
 */
Parse a mx4200 position/height/velocity sentence.
A typical message looks like this.  Checksum has already been stripped.



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.
Parse a mx4200 position / height / velocity sentence . A typical message looks like this . Checksum has already been stripped . 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 .
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 = pp->unitptr;
	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);
	if (sentence_type != PMVXG_D_PHV)
		return ("wrong rec-type");
	if (mode > 10)
		return ("not navigating");
	if (mode != 3 && mode != 5)
		return ("not navigating in 3D");
	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);
	switch (north_south) {
		case 'N':
			break;
		case 'S':
			lat *= -1.0;
			break;
		default:
			return ("invalid north/south indicator");
	}
	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);
	switch (east_west) {
		case 'E':
			break;
		case 'W':
			lon *= -1.0;
			break;
		default:
			return ("invalid east/west indicator");
	}
	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;
	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);
}
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 = pp -> unitptr ; if ( up -> moving ) return ( " " ) ; sscanf ( pp -> a_lastcode , " " , & sentence_type , & mtime , & lat , & north_south , & lon , & east_west , & alt , & geoid , & vele , & veln , & mode ) ; if ( sentence_type != PMVXG_D_PHV ) return ( " " ) ; if ( mode > 10 ) return ( " " ) ; if ( mode != 3 && mode != 5 ) return ( " " ) ; if ( lat < 0.0 ) return ( " " ) ; if ( lat > 9000.0 ) lat = 9000.0 ; lat *= 0.01 ; lat = ( ( int ) lat ) + ( ( ( lat - ( int ) lat ) ) * 1.6666666666666666 ) ; switch ( north_south ) { case ' ' : break ; case ' ' : lat *= -1.0 ; break ; default : return ( " " ) ; } if ( lon < 0.0 ) return ( " " ) ; if ( lon > 180.0 ) lon = 180.0 ; lon *= 0.01 ; lon = ( ( int ) lon ) + ( ( ( lon - ( int ) lon ) ) * 1.6666666666666666 ) ; switch ( east_west ) { case ' ' : break ; case ' ' : lon *= -1.0 ; break ; default : return ( " " ) ; } if ( up -> central_meridian == NOT_INITIALIZED ) { up -> central_meridian = lon ; mx4200_debug ( peer , " \n " , up -> central_meridian ) ; } lon -= up -> central_meridian ; if ( lon < -180.0 ) lon += 360.0 ; if ( lon > 180.0 ) lon -= 360.0 ; 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 , " \n " , up -> N_fixes , lat , lon , alt , up -> central_meridian ) ; return ( NULL ) ; }
Parse a mx4200 position/height/velocity sentence.
Parse a mx4200 position / height / velocity sentence .
/* Should never happen! */ /* Sentence type */ /*
	 * return if not navigating
	 */ /* Latitude (always +ve) and convert DDMM.MMMM to decimal */ /* North/South */ /* Longitude (always +ve) and convert DDDMM.MMMM to decimal */ /* East/West */ /*
	 * Normalize longitude to near 0 degrees.
	 * Assume all data are clustered around first reading.
	 */ /*
	 * Calculate running averages
	 */