taip.c

gps开发专用的源代码

*		pos - a pointer to a TAIP_POS structure to be filled
*		buf - a pointer to an PV message without the leading ">RPV"
*	Return (in pv)
*		parsed PV message
*/
static void parse_pv (pos, buf)
	TAIP_POS *pos;	/* pointer to a TAIP_POS structure to be filled */
	char *buf;		/* pointer to a PV message without the leading ">RPV" */
{
/*
	0123456789012345678901234567890123456789
	68069+3739434-1220383600100012<
*/
	/* id of TAIP message providing position */
	strcpy (pos->id, "PV");

	/* Time */
	pos->time = antof (&buf [0], 5);

	/* Latitude converted to radians */
	pos->lat  = D2R * antof (&buf [5], 8) * 1.0e-5;

	/* Longitude converted to radians */
	pos->lon  = D2R * antof (&buf [13], 9) * 1.0e-5;

	/* Speed over ground (SOG) converted to meters per second */
	pos->sog  = MPH2MPS * antof (&buf [22], 3);

	/* Course over ground (COG) converted to radians */
	pos->cog  = D2R * antof (&buf [25], 3);
	if (pos->cog < 0.0) {
		pos->cog += 2.0 * PI;
	}

	/* Fix Type */
	pos->fix_type = parse_fix_type (buf [28]);

	/* Fix Age */
	pos->fix_age = parse_fix_age (buf [29]);
}

/*
*	parse_am (TAIP_AM *am, char *buf)
*
*	Parse a TAIP "AM" message to a TAIP_AM structure
*
*	Sample AM Message: >RAM60662+3739435-12203830000000U1200<
*
*	Given
*		pos - a pointer to a TAIP_POS structure to be filled
*		buf - a pointer to an AM message without the leading ">RAM"
*	Return (in am)
*		parsed AM message
*/
static void parse_am (pos, buf)
	TAIP_POS *pos;	/* pointer to a TAIP_POS structure to be filled */
	char *buf;		/* pointer to a AM message without the leading ">RAM" */
{
/*
	0123456789012345678901234567890123
	60662+3739435-12203830000000U1200<
*/
	int
		msg_len;

	/* id of TAIP message providing position */
	strcpy (pos->id, "AM");

	/* Time */
	pos->time = antof (&buf [0], 5);

	/* Latitude converted to radians */
	pos->lat  = D2R * antof (&buf [5], 8) * 1.0e-5;

	/* Longitude converted to radians */
	pos->lon  = D2R * antof (&buf [13], 9) * 1.0e-5;

	/* Speed over ground (SOG) converted to meters per second */
	pos->sog  = MPH2MPS * antof (&buf [22], 3);

	/* Course over ground (COG) converted to radians */
	pos->cog  = D2R * antof (&buf [25], 3);
	if (pos->cog < 0.0) {
		pos->cog += 2.0 * PI;
	}

	/* Alarm Code */
	pos->alarm_code = buf [28];

	/* Fix Type */
	pos->fix_type = parse_fix_type (buf [29]);

	/* Fix Age */
	pos->fix_age = parse_fix_age (buf [30]);

	/* Text Message */
	msg_len = antoi (&buf [31], 2);
	if (msg_len > POS_MSG_LEN) msg_len = POS_MSG_LEN;
	if (msg_len == 0) {
		strcpy (pos->msg, "(None)");
	}
	else {
		strncpy (pos->msg, &buf [33], msg_len);
		pos->msg [msg_len] = 0;
	}
}

/*
*	parse_ln (TAIP_LN *ln, char *buf)
*
*	Parse a TAIP "LN" message to a TAIP_LN structure
*
*	Sample LN Message: >RLN68069188+373943425-1220383623+000099610011-0023000204289323D2263C0350000370000012<
*
*	Given
*		pos - a pointer to a TAIP_POS structure to be filled
*		buf - a pointer to an LN message without the leading ">RLN"
*	Return (in ln)
*		parsed LN message
*/
static void parse_ln (pos, ptr)
	TAIP_POS *pos;	/* pointer to a TAIP_POS structure to be filled */
	char *ptr;		/* pointer to a LN message without the leading ">RLN" */
{
	int
		nsv;
	/*
	012345678901234567890123456789012345678901234567890123456789012345678901234567890
	71126844+373943576-1220384035+000021440000+000000000403D620A6237526EE000500000012
	*/

	/* id of TAIP message providing position */
	strcpy (pos->id, "LN");

	/* Time */
	pos->time = antof (ptr, 8) * 1.0e-3;
	ptr += 8;

	/* Latitude converted to radians */
	pos->lat  = D2R * antof (ptr, 10) * 1.0e-7;
	ptr += 10;

	/* Longitude converted to radians */
	pos->lon  = D2R * antof (ptr, 11) * 1.0e-7;
	ptr += 11;

	/* Altitude */
	pos->alt  = F2M * antof (ptr, 9) * 1.0e-2;
	ptr += 9;

	/* Speed over ground (SOG) converted to meters per second */
	pos->sog  = MPH2MPS * antof (ptr, 4) * 0.1;
	ptr += 4;

	/* Vertical speed */
	pos->vs = MPH2MPS * antof (ptr, 5) * 0.1;
	ptr += 5;

	/* Course over ground (COG) converted to radians */
	pos->cog  = D2R * antof (ptr, 4) * 0.1;
	if (pos->cog < 0.0) {
		pos->cog += 2.0 * PI;
	}
	ptr += 4;

	/*	Quantity of Satellites */
	pos->nsvs = antoi (ptr, 2);
	ptr += 2;

	/*	List of Satellite and their IODEs */
	for (nsv = 0; nsv < pos->nsvs; nsv++) {
		pos->svs [nsv] = antoi (ptr, 2);
		ptr += 2;
		pos->iodes [nsv] = hatoi2 (ptr);
		ptr += 2;
	}

	/*	Reserved area */
	ptr += 10;

	/* Fix Type */
	pos->fix_type = parse_fix_type (*ptr);
	ptr += 1;

	/* Fix Age */
	pos->fix_age = parse_fix_age (*ptr);
}

/*
*	parse_x1 (TAIP_X1 *x1, char *buf)
*
*	Parse a TAIP "X1" message to a TAIP_X1 structure
*
*	Sample X1 Message: >RX1,1,01,0020042,0a,00.0000,01.0293,01.0276
*
*	Given
*		x1 - a pointer to a TAIP_X1 structure to be filled
*		buf - a pointer to an X1 message without the leading ">RX1"
*	Return (in x1)
*		parsed X1 message
*/
static void parse_x1 (x1, buf)
	TAIP_X1 *x1;	/* pointer to a TAIP_X1 structure to be filled */
	char *buf;		/* pointer to a X1 message without the leading ">RX1" */
{
	/*	Sub-Message ID */
	x1->msg = buf [1] - '0';

	/*	Odometer Status */
	x1->odom_sta = hatoi2 (&buf [3]);

	/*	Odometer Scale Factor */
	x1->odom_sf = antol (&buf [6], 7);

	/*	Gyro Status */
	x1->gyro_sta = hatoi2 (&buf [14]);

	/*	Gyro rate bias */
	x1->gyro_rate_bias = antof (&buf [17], 7);

	/*	Gyro left scale factor */
	x1->gyro_lsf = antof (&buf [25], 7);

	/*	Gyro right scale factor */
	x1->gyro_rsf = antof (&buf [33], 7);
}

static int vid_parse (buf, id)
	char *buf, *id;
{
	char
		*ptr;
	int
		got_id;

	ptr = buf;
	got_id = FALSE;
	while ((ptr = strchr (ptr, ';')) != NULL) {
		if (strncmp (ptr, ";ID=", 4) == 0) {
			strncpy (id, ptr + 4, 4);
			id [4] = 0;
			got_id = TRUE;
			break;
		}
		++ptr;
	}
	return got_id;
}

/*
*	Given a quantity of seconds into the day, format a string of the
*	format  HH:MM:SS.SSS
*/
static void str_hms (str, t)
	char *str;
	double t;
{
	int
		h, m;
	double
		s;

	h = t / 3600.0;
	t -= h * 3600.0;
	m = t / 60.0;
	t -= m * 60.0;
	s = t;

	sprintf (str, "%02d:%02d:%06.3f", h, m, s);
}

/*
*	Given an angle in radians, convert to degrees, arc-minutes,
*	and arc-seconds.
*/
static void rad2dms (rad, d, m, s)
	double rad, *s;
	int *d, *m;
{
	if (rad < 0.0) rad = -rad;
	rad += 1.0e-10;
	rad *= R2D;
	*d = rad;
	rad = (rad - *d) * 60.0;
	*m = rad;
	rad = (rad - *m) * 60.0;
	*s = rad;
}

/*****************************************************************************
*	The following routines are used to create an ASCII string of the form
*		DDD^MM'SS.SSSS"H
*	where DDD     is degrees
*		  ^       is the degrees symbol (superscripted 'o')
*		  MM      is arc-minutes
*		  SS.SSSS is arc-seconds
*	and   H       is the hemisphere (N or S for lat, E or W for lon)
*
*		str_lat () - latitude
*		str_lon () - longitude
*
*****************************************************************************/

static void str_lat (str, lat)
	char *str;
	double lat;
{
	str_ll (str, lat, "NS");
}
static void str_lon (str, lon)
	char *str;
	double lon;
{
	str_ll (str, lon, "EW");
}
static void str_ll (str, ll, h)
	char *str, h[];
	double ll;
{
	int
		d, m;
	double
		s;

	rad2dms (ll, &d, &m, &s);
	sprintf (str, "%3d\xf8%02d'%07.4f\"%c", d, m, s, ll >= 0.0 ? h[0] : h[1]);
}

/*****************************************************************************
*	The following routines are used to convert values contained in ASCII
*	strings to their numerical representations.
*
*		antoi () -	 Convert a specified quantity of characters
*					 from a string to an integer
*
*		antol () -	 Convert a specified quantity of characters
*					 from a string to a long integer
*
*		hatoi2 () -  Convert 2 characters from a hexidecimal string
*					 to an integer
*
*		antof () -	 Convert a specified quantity of characters from a string
*					 to a double precision floating point value (double)
*
*		str_h2i () - Convert a hexidecimal string to an integer
*
*****************************************************************************/

/*
*	antoi (char *ptr, int n);
*
*	Similar to ANSI's atoi() but processes only a fixed quantity of characters
*
*	Given
*		ptr - string pointer
*		n   - quantity of characters to process from ptr
*	Return (as the value of the function)
*		an integer value of the ASCII representation from ptr
*/
static int antoi (ptr, n)
	char *ptr;	/* string pointer */
	int n;	/* quantity of characters to process from ptr */
{
	char
		buf [80];			/* local string storage */

	memcpy (buf, ptr, n);	/* copy string to local temporary storage */
	buf [n] = 0;			/* truncate at n */
	return atoi (buf);		/* parse and return integer */
}

/*
*	antol (char *ptr, int n);
*
*	Similar to ANSI's atol() but processes only a fixed quantity of characters
*
*	Given
*		ptr - string pointer
*		n   - quantity of characters to process from ptr
*	Return (as the value of the function)
*		a long integer value of the ASCII representation from ptr
*/
static long antol (ptr, n)
	char *ptr;	/* string pointer */
	int n;	/* quantity of characters to process from ptr */
{
	char
		buf [80];			/* local string storage */

	memcpy (buf, ptr, n);	/* copy string to local temporary storage */
	buf [n] = 0;			/* truncate at n */
	return atol (buf);		/* parse and return long integer */
}

/*
*	hatoi2 (char *ptr);
*
*	Process 2 characters from string to an integer value.
*	String contains a hexidecimal representation of an integer.
*
*	Given
*		ptr - string pointer
*	Return (as the value of the function)
*		an integer value of the ASCII hexidecimal representation from ptr
*/
static int hatoi2 (ptr)
	char *ptr;	/* string pointer */
{
	char
		chr;
	int
		val, i;

	val = 0;	/* clear value accumulator */

	/* loop through 2 characters or until 0 character is reached */
	for (i = 0; i < 2 && *ptr != 0; ptr++, i++) {

		/* shift accululator up 1 place */
		val *= 16;

		/* promote character to upper case to simplify checking below */
		/*
		*	(this step is unnecessary for TAIP messages since they
		*	are all upper case anyway.)
		*/
		chr = toupper (*ptr);

		/* if character is 1-9, add value to accumulator */
		if (chr >= '0' && chr <= '9') {
			val += (chr - '0');
		}

		/* if character is A-F, add value to accumulator */
		else if (chr >= 'A' && chr <= 'F') {
			val += (chr + 10 - 'A');
		}
	}
	return val;	/* return integer value */
}

/*
*	antof (char *ptr, int n);
*
*	Similar to ANSI's atof() but processes only a fixed quantity of characters
*
*	Given
*		ptr - string pointer
*		n   - quantity of characters to process from ptr
*	Return (as the value of the function)
*		a double precision value of the ASCII representation from ptr
*/
static double antof (ptr, n)
	byte *ptr;
	int n;
{
	char
		buf [80];			/* local string storage */

	memcpy (buf, ptr, n);	/* copy string to local temporary storage */
	buf [n] = 0;			/* truncate at n */
	return atof (buf);		/* parse and return double precision value */
}

/*
*
*	void str_h2i (char *p, int *r) 
*
*	Convert hexadecimal string to integer
*
*	Given
*		char *p;	input hexidecimal string
*	Returns
*		int *r;		output integer
*
*	Performs a scan of the input string to convert a hexadecimal value
*	into an integer.  The scan terminates when a non-hex character is hit.
*	Valid hex characters are 0 to 9, A to F, and a to f.
*
*/
static void str_h2i (p, r)
	char *p;
	int *r;
{
	int
		i, j, c;

	i = j = 0;
	for (c = *p; isxdigit (c); c = p [++j]) {
		i <<= 4;
		if (isdigit (c)) i |= c - '0';
		if (isupper (c)) i |= c - 'A' + 10;
		if (islower (c)) i |= c - 'a' + 10;
	}
	*r = i;
}