taip.c

gps开发专用的源代码

/*
*
*		GPSSK
*		GPS Starter Kit
*		Setup, Diagnostics, And Data Monitor
*		For The
*		Trimble ASCII Interface Protocol (TAIP)
*
*		Trimble Navigation
*		645 N. Mary Ave
*		Sunnyvale, CA 94088
*		Copyright 1992
*
*
*		TAIP Message Encoding/Decoding Source Code
*		----------------------------------------------------------------
*		TAIP.C contains 'C' source code for encoding and decoding TAIP
*		messages.  There is no warranty of any kind on this software.
*		It is provided only as an example of TAIP message processing.
*		Use it at your own risk.
*
*		Author: Jim Reynolds
*
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "sio.h"						/* Serial I/O */

#ifndef FALSE
#define FALSE		0					/* Boolean flags for False */
#define TRUE		(!FALSE)			/*   and True */
#endif

#define VERSION		"1.00"

/*
*	Unit Conversions
*/
#define M2F			3.2808				/* Meters to Feet */
#define MILES2FT	5280.0				/* Miles to Feet */
#define F2M			(1.0/M2F)			/* Feet to Meters */
#define HRS2SEC		3600.0				/* Hours to Seconds */
#define MPH2MPS		((MILES2FT*F2M)/HRS2SEC)	/* Miles/hour to meters/sec */
#define MPS2MPH		(1.0/MPH2MPS) 	/* meters/sec to Miles/hour */
#define PI			3.1415926535898		/* PI */
#define D2R			(PI/180.0)			/* Degrees to radians */
#define R2D			(180.0/PI)			/* Radians to Degrees */

typedef unsigned char	/* define "byte" as an unsigned character */
	byte;

/*
*	POS.fix_type
*/
#define POS_FIX_A2		0		/* Autonomous 2D Fix */
#define POS_FIX_A3		1		/* Autonomous 3D Fix */
#define POS_FIX_D2		2		/* Differential 2D Fix */
#define POS_FIX_D3		3		/* Differential 3D Fix */
#define POS_FIX_UNK		4		/* Unknown source for Fix */
#define POS_FIX_DR		6		/* Dead-Reckoned Fix */
#define POS_FIX_DDR		8		/* Degraded Dead-Reckoned Fix */

/*
*	POS.fix_age
*/
#define POS_AGE_NEW		2		/* Position is "new" (less than 10 sec old) */
#define POS_AGE_OLD		1		/* Position is "old" (10 sec old or more) */
#define POS_AGE_UNK		0		/* Position is of unknown age */

#define TAIP_ID_LEN		4		/* Length of a TAIP ID */
#define POS_MSG_LEN		80		/* Maximum length of AM message text field */

#define BUFSIZ			512		/* Serial port manager buffer size (bytes) */

/*
*	TAIP message delimeters
*/
#define TAIP_START		'>'		/* All TAIP messages begin with a '>' */
#define TAIP_END		'<'		/* All TAIP messages end with a '<' */

/*
*	TAIP decoder's standard TAIP position structure
*	This structure is used by TAIP messages CP, PV, AM, and LN.
*	Some fields will not be filled.
*/
typedef struct {
	char
		id [3],				/* ID of TAIP message that filled TAIP_POS */
		alarm_code,			/* Alarm code from the AM message */
		msg [POS_MSG_LEN+1];	/* Message from the AM message */
	int
		fix_type,			/* Fix type, see #define POS_FIX_XXX above */
		fix_age,			/* Fix age, see #define POS_AGE_XXX above */
		nsvs,				/* Quantity of satellites used in position */
		svs [4],			/* List of satellites used in position */
		iodes [4];			/* satellite's IODEs */
	double
		time,				/* GPS time of position */
		lat,				/* latitude in radians */
		lon,				/* longtiude in radians */
		alt,				/* altitude in meters */
		cog,				/* course over ground in radians from north */
		sog,				/* speed over ground in meters/second */
		vs;					/* vertical speed in meters/second */
} TAIP_POS;

/*	AL message structure */
typedef struct {
	int
		fix_type,			/* Fix type, see #define POS_FIX_XXX above */
		fix_age;			/* Fix age, see #define POS_AGE_XXX above */
	double
		time,				/* GPS time of position */
		alt,				/* altitude in meters */
		vspd;				/* vertical speed in meters/second */
} TAIP_AL;

/*
*	Decoder's standard TAIP port parameters structure
*	This structure is used by TAIP messages AP, PT
*/
typedef struct {
	int
		baud,				/* baud rate */
		data_bits,			/* quantity of data bits */
		stop_bits,			/* quantity of stop bits */
		parity,				/* parity */
		port;				/* port */
} TAIP_PORT;

/*	RM message structure */
typedef struct {
	int
		id_flag,			/* ID_FLAG, send ID with messages */
		cs_flag,			/* CS_FLAG, send checksums with messages */
		ec_flag,			/* EC_FLAG, echo Set commands */
		fr_flag;			/* FR_FLAG, enable Frequency reporting */
} TAIP_RM;

/*	IP message structure */
typedef struct {
	int
		lat_deg,			/* Initial latitude in degrees */
		lon_deg;			/* Initial longitude in degrees */
	long
		alt;				/* Initial altitude in meters */
} TAIP_IP;

/*	VR message structure */
#define VR_NAME_LEN		40		/* Maximum length of product name */
#define VR_VERS_LEN		4		/* Length of version */
#define VR_REL_LEN		8		/* Length of date */
typedef struct {
	char
		name [VR_NAME_LEN+1],		/* Product name */
		vers [VR_VERS_LEN+1],		/* Version */
		rel_date [VR_REL_LEN+1];	/* Release date */
} TAIP_VR;

/*	ID message structure */
typedef struct {
	char
		id [TAIP_ID_LEN+1];			/* GPS receiver's ID */
} TAIP_ID;

/*	ST message structure */
typedef struct {
	int
		codes [5];					/* 5 status codes */
} TAIP_ST;

/*	TM message structure */
typedef struct {
	int
		yr, mo, dy,					/* year, month, day */
		hr, mn, sc, ms,				/* hour, minute, second, millisecond */
		gpsutc,						/* GPS to UTC time offset in seconds */
		gpsutc_valid,				/* is gpsutc valid? */
		fix_type,					/* Fix type, see #define POS_FIX_XXX */
		n_sv;						/* Quantity of satellites */
} TAIP_TM;

/*	X1 message structure */
typedef struct {
	int
		msg,						/*	Sub-Message ID */
		odom_sta,					/*	Odometer Status */
		gyro_sta;					/*	Gyro Status */
	long
		odom_sf;					/*	Odometer Scale Factor */
	double
		gyro_rate_bias,				/*	Gyro rate bias */
		gyro_lsf,					/*	Gyro left scale factor */
		gyro_rsf;					/*	Gyro right scale factor */
} TAIP_X1;

/*
*	union of all supported TAIP messages used for parser to
*	displayer data transfer
*/
typedef union {
	TAIP_POS pos;
	TAIP_AL al;
	TAIP_PORT port;
	TAIP_RM rm;
	TAIP_IP ip;
	TAIP_VR vr;
	TAIP_ID id;
	TAIP_ST st;
	TAIP_TM tm;
	TAIP_X1 x1;
} TAIP_DATA;

char
	*fix_type_str[] = {"GPS-2D", "GPS-3D", "DGPS-2D",
		"DGPS-3D", "Unknown", "Unknown", "DR",
		"Unknown", "Deg-DR", "Unknown"},
	*fix_age_str [] = {"N/A", "Old", "New"},
	*parity_str [] = {"None", "Odd", "Even"};

#define NMSGS		(sizeof (msgs) / sizeof (MSG))

typedef struct {
	char
		*id;
	void
		(*parse_f)(),
		(*disp_f)();
} MSG;

/*****************************************************************************
*	Function Prototypes
*****************************************************************************/
double
	antof (byte *, int);
int
	antoi (char *, int),
	checksum_ok (unsigned char *),
	hatoi2 (char *),
	parse_fix_age (int),
	parse_fix_type (int),
	vid_parse (char *, char *);
long
	antol (char *, int);
unsigned int
	checksum (unsigned char *);
void
	disp_al (TAIP_AL *),
	disp_am (TAIP_POS *),
	disp_ap (TAIP_PORT *),
	disp_cp (TAIP_POS *),
	disp_id (TAIP_ID *),
	disp_ip (TAIP_IP *),
	disp_ln (TAIP_POS *),
	disp_pt (TAIP_PORT *),
	disp_pv (TAIP_POS *),
	disp_rm (TAIP_RM *),
	disp_st (TAIP_ST *),
	disp_tm (TAIP_TM *),
	disp_vr (TAIP_VR *),
	disp_x1 (TAIP_X1 *),
	fmt_taip (char *, char *),
	parse_al (TAIP_AL *, char *),
	parse_am (TAIP_POS *, char *),
	parse_ap (TAIP_PORT *, char *),
	parse_cp (TAIP_POS *, char *),
	parse_id (TAIP_ID *, char *),
	parse_ip (TAIP_IP *, char *),
	parse_ln (TAIP_POS *, char *),
	parse_pt (TAIP_PORT *, char *),
	parse_pv (TAIP_POS *, char *),
	parse_rm (TAIP_RM *, char *),
	parse_st (TAIP_ST *, char *),
	parse_tm (TAIP_TM *, char *),
	parse_vr (TAIP_VR *, char *),
	parse_x1 (TAIP_X1 *, char *),
	rad2dms (double, int *, int *, double *),
	ser_send (int, byte *, int),
	str_h2i (char *, int *),
	str_hms (char *, double),
	str_lat (char *, double),
	str_ll (char *, double, char []),
	str_lon (char *, double),
	taip_byte (int),
	taip_parse (char *);

/*****************************************************************************
*	List of supported TAIP messages and their associated
*	parsing and display routines
*****************************************************************************/
MSG
	msgs [] = {
	{"AL", parse_al, disp_al},
	{"AM", parse_am, disp_am},
	{"AP", parse_ap, disp_ap},
	{"CP", parse_cp, disp_cp},
	{"ID", parse_id, disp_id},
	{"IP", parse_ip, disp_ip},
	{"LN", parse_ln, disp_ln},
	{"PT", parse_pt, disp_pt},
	{"PV", parse_pv, disp_pv},
	{"RM", parse_rm, disp_rm},
	{"ST", parse_st, disp_st},
	{"TM", parse_tm, disp_tm},
	{"VR", parse_vr, disp_vr},
	{"X1", parse_x1, disp_x1}
};

/****************************************************************************/
void main ()
{
	char
		taip [80];
	static char
		rbuf [BUFSIZ], tbuf [BUFSIZ];
	int
		done, pid, chr;

	printf ("\n");
	printf ("       Sample TAIP Query/Response Program\n");
	printf ("       Copyright Trimble Navigation 1992\n");
	printf ("       Version %s\n\n", VERSION);

	/*
	*	Configure COM1
	*/
	pid = 0;
	SIO_Open (NULL);
	SIO_IntOpen  (0, 8+4, -1, NULL);
	SIO_PortOpen (pid, 0x3f8, 0, HS_DTR | HS_RTS | HS_CTS, NULL);
	SIO_PortMode (pid, 4800L, 8, NOPARITY, ONEBIT, ONEBIT, NULL);
	SIO_RxOpen   (pid, rbuf, (long)BUFSIZ, HS_NONE, NULL);
	SIO_TxOpen   (pid, tbuf, (long)BUFSIZ, HS_NONE, NULL);

	printf ("       Serial Parameters: COM1, 4800 Baud, 8 data bits, 1 stop bit, no parity\n\n");
	
	printf ("       Press <Enter> to Poll for a PV packet\n");
	printf ("       Press <Escape> to quit\n");
	
	for (done = FALSE; !done; ) {	/* While not done... */
		while (SIO_RxGet (pid, &chr) == 0) { /* While characters coming in */
			taip_byte (chr);	/* send them to the TAIP decoder */
		}
		if (kbhit ()) {		/* if keyboard is hit... */
			switch (getch ()) {		/* get key and switch() */
			case 27:	/* <Escape>, Quit */
				done = TRUE;
				break;
			case '\r':	/* <Enter>, Poll for a PV packet */
				fmt_taip (taip, "QPV");		/* format a TAIP packet */
				ser_send (0, taip, strlen (taip));	/* send it */
				printf ("\nSend: %s\n", taip);		/* print it */
				break;
			}
		}
	}
	SIO_Close (0);	/* Shutdown serial ports */
}

#define BUF_LEN		132
/*
*	taip_byte (int chr)
*
*	All data from the GPS receiver is sent here
*/
static void taip_byte (chr)
	int chr;	/* character from serial port */
{
	static byte
		buf [BUF_LEN];	/* buffer to hold TAIP message being received */
	static int
		buf_len;		/* length of buffer "buf" */

	/*	Buffer overflow protection */
	if (buf_len == BUF_LEN) {
		buf_len = 0;
	}

	/*	Add character to message buffer */
	buf [buf_len++] = chr;

	/*	If the buffer's first character is not a '>', start over */
	if (buf [0] != TAIP_START) {
		buf_len = 0;
	}
	else if (chr == TAIP_END) {	/* If we receive a '<' character */
		buf [buf_len] = 0;	/* mark the end of the buffer with a 0 */
		printf ("Recv: %s\n", buf);	/* print received packet */
		/*
		*	if the checksum passes (or message does not contain one)...
		*/
		if (checksum_ok (buf)) {
			taip_parse (buf);	/* process the TAIP message */
		}
		buf_len = 0;	/* start the incoming message buffer over */
	}
}

/*
*	fmt_taip (char *taip, char *msg)
*
*		Wrap TAIP data into a TAIP packet with a checksum
*
*		>"data";*CS<
*
*	Given
*		msg - TAIP formatted message data area ("data" above)
*	Return
*		taip - packet ready for sending to the GPS reciever, complete with
*			   a checksum.
*/
static void fmt_taip (taip, msg)
	char *taip, *msg;
{
	char
		*ptr;

	ptr = taip;
	ptr += sprintf (ptr, ">%s", msg);
	ptr += sprintf (ptr, ";*");
	ptr += sprintf (ptr, "%02X", checksum (taip));
	sprintf (ptr, "<");
}

/*
*	Send a buffer of length "len" out of serial port "port"
*/
static void ser_send (port, buf, len)
	int port, len;
	byte *buf;
{
	int
		i;

	for (i = 0; i < len; i++) {
		SIO_TxPut (port, buf [i]);
	}
}

/*
*	int checksum_ok (char *text)
*
*	Verify TAIP packet's checksum
*
*	Given
*		text -	a complete TAIP packet
*	Returns (as the value of the function)
*		whether the checksum is correct for the packet (TRUE or FALSE)
*/
static int checksum_ok (text)
	unsigned char *text;	/* TAIP packet */
{
	int
		i,
		passed,	/* did the checksum pass? */
		len;	/* length of TAIP packet */
	unsigned int
		xcs;	/* expected checksum */

	len = strlen (text);	/* length of TAIP packet */
	passed = TRUE;	/* set passed TRUE until found otherwise */

	/*
	*	Search for the checksum identifier ('*') starting at
	*	the end of the packet
	*/
	for (i = len - 1; i > 0; i--) {
		if (text [i] == '*') {	/* Checksum identifier found */
			/* convert checksum characters into an integer */
			str_h2i (&text [i + 1], &xcs);
			/* if calculated checksum matches the expected checksum... */
			if (checksum (text) != xcs) {
				passed = FALSE;	/* checksum not correct */
			}
			break;
		}
	}
	return passed;	/* return whether checksum passed (TRUE or FALSE) */
}

/*
*	taip_parse (char *buf)
*
*	process a TAIP message
*
*/
static void taip_parse (buf)
	char *buf;	/* Pointer to 0 terminated TAIP message */
{
	int
		mid;		/* index into message list of this message */
	TAIP_DATA
		taip_data;	/* parser/displayer data transfer storage space */