tsip_ifc.c

gps开发专用的源代码

	cmd->buf[3] = extra;
	cmd->len = 4;
	cmd->code = 0x70;
}

void cmd_0x71q (TSIPPKT *cmd)
/* request position-velocity filter parameters */
{
	cmd->len = 0;
	cmd->code = 0x71;
}

void cmd_0x71s (TSIPPKT *cmd,
	short pf,
   TSIP_POS_FILT_PARMS *params)
/* enable position-velocity filter with default parameters */
{
	cmd->buf[0] = pf;
	bPutFloat (&params->flt1, &cmd->buf[1]);
	cmd->buf[5] = params->chr1;
	cmd->buf[6] = params->chr2;
	cmd->buf[7] = params->chr3;
	cmd->buf[8] = params->chr4;
	bPutFloat (&params->flt2, &cmd->buf[9]);
	bPutFloat (&params->flt3, &cmd->buf[13]);
	bPutFloat (&params->flt4, &cmd->buf[17]);
	bPutFloat (&params->flt5, &cmd->buf[21]);
	cmd->buf[25] = params->chr5;
	cmd->len = 26;
	cmd->code = 0x71;
}

void cmd_0x73q (TSIPPKT *cmd)
/* request altitude filter parameters */
{
	cmd->len = 0;
	cmd->code = 0x73;
}

void cmd_0x73s (TSIPPKT *cmd,
	float time_constant)
/* set altitude filter parameters */
{
	bPutFloat (&time_constant, &cmd->buf[0]);
	cmd->len = 4;
	cmd->code = 0x73;
}

void cmd_0x73d (TSIPPKT *cmd)
/* disable altitude filter */
{
	float ftemp;

   ftemp = 1.0;
	bPutFloat (&ftemp, &cmd->buf[0]);
	cmd->len = 4;
	cmd->code = 0x73;
}

void cmd_0x73c (TSIPPKT *cmd)
/* set altitude filter parameters to default */
{
	float ftemp;

   ftemp = 0.0;
	bPutFloat (&ftemp, &cmd->buf[0]);
	cmd->len = 4;
	cmd->code = 0x73;
}

void cmd_0x77q (TSIPPKT *cmd)
/* request DC max age */
{
	cmd->len = 0;
	cmd->code = 0x77;
}

void cmd_0x77s (TSIPPKT *cmd,
	short maxage)
/* request DC max age */
{
	cmd->buf[0]= maxage >> 8;
	cmd->buf[1]= maxage & 0x00ff;
	cmd->len = 2;
	cmd->code = 0x77;
}

void cmd_0x7A_0s  (TSIPPKT *cmd,
	unsigned char nmea_interval,
	unsigned long nmea_mask)
/* set NMEA interval [and message mask] */
{

	cmd->buf[0] = 0;		/* subcode */
	cmd->buf[1] = nmea_interval;
	bPutULong (&nmea_mask, &cmd->buf[2]);
	cmd->len = 6;
	cmd->code = 0x7A;
}

void cmd_0x7A_0q  (TSIPPKT *cmd)
/* request NMEA interval and message mask */
{

	cmd->buf[0] = 0;		/* subcode */
	cmd->len = 1;
	cmd->code = 0x7A;
}


void cmd_0xBBs(TSIPPKT *cmd,
	TSIP_RCVR_CFG *TsipxBB)
	/* set Stinger receiver configuration */
{

	/* Manual  29473-00 Rev B. is in error.  Byte count is 40, not 41. */
	cmd->len		= 40;
	cmd->code	= 0xBB;
   memset (cmd->buf, 0, cmd->len);	/* set reserved bytes to 0 */
   if (TsipxBB->bSubcode == 0)
   {
		cmd->buf[0]	=	0;
		cmd->buf[1]	=	TsipxBB->operating_mode;
		cmd->buf[2]	=	TsipxBB->dgps_mode;
		cmd->buf[3]	=	TsipxBB->dyn_code;
/*		cmd->buf[4]	=	TsipxBB->trackmode; not changeable in v7.52 or 7.68 */
		bPutFloat (&TsipxBB->elev_mask, &cmd->buf[5]);
		bPutFloat (&TsipxBB->cno_mask, &cmd->buf[9]);
		bPutFloat (&TsipxBB->dop_mask, &cmd->buf[13]);
		bPutFloat (&TsipxBB->dop_switch, &cmd->buf[17]);
		cmd->buf[21]	=	TsipxBB->dgps_age_limit;
   }
   else if (TsipxBB->bSubcode == 3)
   {
		cmd->buf[0]	=	3;
		cmd->buf[1]	=	TsipxBB->operating_mode;
		cmd->buf[2]	=	TsipxBB->dgps_mode;
		cmd->buf[3]	=	TsipxBB->dyn_code;
		cmd->buf[6]	=	TsipxBB->trackmode;
		bPutFloat (&TsipxBB->elev_mask, &cmd->buf[15]);
		bPutFloat (&TsipxBB->cno_mask, &cmd->buf[19]);
		bPutFloat (&TsipxBB->dop_mask, &cmd->buf[23]);
		bPutFloat (&TsipxBB->dop_switch, &cmd->buf[27]);
		cmd->buf[35]	=	TsipxBB->dgps_age_limit;
   }
}

void cmd_0xBBq (TSIPPKT *cmd,
	unsigned char subcode)
{

	cmd->len = 1;
	cmd->code = 0xBB;
	cmd->buf[0] = subcode;
}

void cmd_0xBCq  (TSIPPKT *cmd, unsigned char port_num)
/* request receiver port configuration */
{

	cmd->buf[0] = port_num;
	cmd->len = 1;
	cmd->code = 0xBC;
}

void cmd_0xBCs  (TSIPPKT *cmd,
	unsigned char port_num,
	unsigned char in_baud,
	unsigned char out_baud,
	unsigned char data_bits,
	unsigned char parity,
	unsigned char stop_bits,
	unsigned char flow_control,
	unsigned char protocols_in,
	unsigned char protocols_out,
	unsigned char reserved)
/* set receiver port configuration */
{

	cmd->buf[0] = port_num;
	cmd->buf[1] = in_baud;
	cmd->buf[2] = out_baud;
	cmd->buf[3] = data_bits;
	cmd->buf[4] = parity;
	cmd->buf[5] = stop_bits;
	cmd->buf[6] = flow_control;
	cmd->buf[7] = protocols_in;
	cmd->buf[8] = protocols_out;
	cmd->buf[9] = reserved;
	cmd->len = 10;
	cmd->code = 0xBC;
}

/**** Superpackets ****/
void cmd_0x8E03q (TSIPPKT *cmd)
/* request Channel B configuration for dual-port operation */
{
	cmd->len = 1;
	cmd->code = 0x8E;
   cmd->buf[0] = 0x03;
}

void cmd_0x8E03s (TSIPPKT *cmd,
	unsigned char baud_out, unsigned char baud_inp,
   unsigned char char_code, unsigned char stopbitcode,
   unsigned char output_mode, unsigned char input_mode)
/* set Channel B configuration for dual-port operation */
{
	cmd->buf[0] = 0x03;
	cmd->buf[1] = baud_out;		/* XMT baud rate */
	cmd->buf[2] = baud_inp;		/* RCV baud rate */
	cmd->buf[3] = char_code;	   /* parity and #bits per byte */
	cmd->buf[4] = stopbitcode;	/* number of stop bits code */
	cmd->buf[5] = output_mode;	/* Ch. A transmission mode */
	cmd->buf[6] = input_mode;	/* Ch. A reception mode */
	cmd->len = 7;
	cmd->code = 0x8E;
}

void cmd_0x8E15q  (TSIPPKT *cmd)
/* datum query */
{

	cmd->len = 1;
	cmd->code = 0x8E;
	cmd->buf[0] = 0x15;
}

void cmd_0x8E15s  (TSIPPKT *cmd, unsigned char datum_index)
/* datum set - standard parameters */
{

	cmd->buf[0] = 0x15;
	cmd->buf[1] = 0;	/* datum index is a short; this is always 0 */
	cmd->buf[2] = datum_index;
	cmd->len = 3;
	cmd->code = 0x8E;
}

void cmd_0x8E15s_2  (TSIPPKT *cmd, double datum_coeffs[5])
/* datum set - special parameters */
{

	cmd->buf[0] = 0x15;  /*Subcode 15 */
	bPutDouble (&datum_coeffs[0], &cmd->buf[1]);
	bPutDouble (&datum_coeffs[1], &cmd->buf[9]);
	bPutDouble (&datum_coeffs[2], &cmd->buf[17]);
	bPutDouble (&datum_coeffs[3], &cmd->buf[25]);
	bPutDouble (&datum_coeffs[4], &cmd->buf[33]);
	cmd->len = 41;
	cmd->code = 0x8E;
}

void cmd_0x8E19q (TSIPPKT *cmd)
/* UTM msg status */
{

	cmd->len = 1;
	cmd->code = 0x8E;
	cmd->buf[0] = 0x19;
}

void cmd_0x8E19e (TSIPPKT *cmd)
/* UTM msg enable/disable */
{

	cmd->len = 2;
	cmd->code = 0x8E;
	cmd->buf[0] = 0x19;
	cmd->buf[1] = 0x45;
}

void cmd_0x8E19d (TSIPPKT *cmd)
/* UTM msg enable/disable */
{

	cmd->len = 2;
	cmd->code = 0x8E;
	cmd->buf[0] = 0x19;
	cmd->buf[1] = 0x44;
}

void cmd_0x8E20q (TSIPPKT *cmd)
/* UTM SP msg query */
{

	cmd->len = 1;
	cmd->code = 0x8E;
	cmd->buf[0] = 0x20;
}

void cmd_0x8E20d (TSIPPKT *cmd)
/* 8F-20 msg auto-output disable */
{

	cmd->len = 2;
	cmd->code = 0x8E;
	cmd->buf[0] = 0x20;
	cmd->buf[1] = 0;
}

void cmd_0x8E20e (TSIPPKT *cmd)
/* 8F-20 msg auto-output enable */
{

	cmd->len = 2;
	cmd->code = 0x8E;
	cmd->buf[0] = 0x20;
	cmd->buf[1] = 1;
}


void cmd_0x8E26s (TSIPPKT *cmd)
/* 8F-26 save to SEEPROM */
{

	cmd->len = 1;
	cmd->code = 0x8E;
	cmd->buf[0] = 0x26;
}

void cmd_0x8E40q (TSIPPKT *cmd)
{
	cmd->len = 1;
   cmd->code = 0x8E;
   cmd->buf[ 0] = 0x40;
}

void cmd_0x8E40s (TSIPPKT *cmd,
	unsigned char Flags,
	unsigned char HeartBeatSentence,
   short TopOfHourOffset,
	short Frequency,
   char VehicleID[4])
{
	cmd->len = 11;
   cmd->code = 0x8E;
   cmd->buf[ 0] = 0x40;
   cmd->buf[ 1] = Flags;
   cmd->buf[ 2] = HeartBeatSentence;
   bPutShort (&TopOfHourOffset, &cmd->buf[3]);
   bPutShort (&Frequency, &cmd->buf[5]);
   cmd->buf[ 7] = VehicleID[0];
   cmd->buf[ 8] = VehicleID[1];
   cmd->buf[ 9] = VehicleID[2];
   cmd->buf[10] = VehicleID[3];
}

void cmd_0x8E  (TSIPPKT *cmd, unsigned char hexbyte[], unsigned char hexnum)
/* turn on superpacket, etc. */
{
	memcpy (cmd->buf, hexbyte, (short)hexnum);
	cmd->len = hexnum;
	cmd->code = 0x8E;
}

void cmd_0xxx  (TSIPPKT *cmd, unsigned char hexcode, unsigned char hexbyte[], unsigned char hexnum)
/* turn on superpacket, etc. */
{
	memcpy (cmd->buf, hexbyte, (short)hexnum);
	cmd->len = hexnum;
	cmd->code = hexcode;
}

#ifdef BYTESWAP
static short bGetShort (unsigned char *bp)
{
	short outval;
   unsigned char *optr;

   optr = (unsigned char*)&outval + 1;
   *optr-- = *bp++;
   *optr = *bp;
	return outval;
}

static long bGetLong (unsigned char *bp)
{
	long outval;
   unsigned char *optr;

   optr = (unsigned char*)&outval + 3;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr = *bp;
	return outval;
}

static unsigned long bGetULong (unsigned char *bp)
{
	unsigned long outval;
   unsigned char *optr;

   optr = (unsigned char*)&outval + 3;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr = *bp;
	return outval;
}

static float bGetSingle (unsigned char *bp)
{
	float outval;
   unsigned char *optr;

   optr = (unsigned char*)&outval + 3;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr = *bp;
	return outval;
}

static double bGetDouble (unsigned char *bp)
{
	double outval;
   unsigned char *optr;

   optr = (unsigned char*)&outval + 7;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr-- = *bp++;
   *optr = *bp;
	return outval;
}

#else	/* not BYTESWAP */

#define bGetShort(bp) 	(*(short*)(bp))
#define bGetLong(bp) 	(*(long*)(bp))
#define bGetULong(bp) 	(*(unsigned long*)(bp))
#define bGetSingle(bp) 	(*(float*)(bp))
#define bGetDouble(bp)	(*(double*)(bp))

#endif


/**/
void tsip_input_proc (TSIPPKT *rpt, short inbyte)
/* reads bytes until serial buffer is empty or a complete report
 * has been received; end of report is signified by DLE ETX.
 */
{                 
	unsigned char newbyte;

	if (inbyte & 0xFF00) return;

	newbyte = (unsigned char)(inbyte & 0x00FF);
	switch (rpt->status)
	{
	case TSIP_PARSED_DLE_1:
		switch (newbyte)
		{
		case 0:
		case ETX:
      	/* illegal TSIP IDs */
         rpt->len = 0;
			rpt->status = TSIP_PARSED_EMPTY;
			break;
		case DLE:
      	/* try normal message start again */
			rpt->len = 0;
			rpt->status = TSIP_PARSED_DLE_1;
			break;
		default:
      	/* legal TSIP ID; start message */
			rpt->code = newbyte;
         rpt->len = 0;
			rpt->status = TSIP_PARSED_DATA;
			break;
		}
		break;
	case TSIP_PARSED_DATA:
		switch (newbyte) {
		case DLE:
      	/* expect DLE or ETX next */
			rpt->status = TSIP_PARSED_DLE_2;
			break;
		default:
      	/* normal data byte  */
			rpt->buf[rpt->len] = newbyte;
			rpt->len++;
         /* no change in rpt->status */
			break;
		}
		break;
	case TSIP_PARSED_DLE_2:
		switch (newbyte) {
		case DLE:
      	/* normal data byte */
			rpt->buf[rpt->len] = newbyte;
			rpt->len++;
			rpt->status = TSIP_PARSED_DATA;
			break;
		case ETX:
			/* end of message; return TRUE here. */
			rpt->status = TSIP_PARSED_FULL;
			break;
		default:
			/* error: treat as TSIP_PARSED_DLE_1; start new report packet */
			rpt->code = newbyte;
         rpt->len = 0;
			rpt->status = TSIP_PARSED_DATA;
		}
		break;
	case TSIP_PARSED_FULL:
	case TSIP_PARSED_EMPTY:
	default:
		switch (newbyte) {
		case DLE:
      	/* normal message start */
			rpt->len = 0;
			rpt->status = TSIP_PARSED_DLE_1;
			break;
		default:
			/* error: ignore newbyte */
			rpt->len = 0;
			rpt->status = TSIP_PARSED_EMPTY;
		}
		break;
	}
	if (rpt->len > MAX_RPTBUF) {
		/* error: start new report packet */
		rpt->status = TSIP_PARSED_EMPTY;
		rpt->len = 0;
	}
}

/**/
short rpt_0x3D (TSIPPKT *rpt,
	unsigned char *tx_baud_index,
	unsigned char *rx_baud_index,
	unsigned char *char_format_index,
	unsigned char *stop_bits,
	unsigned char *tx_mode_index,
	unsigned char *rx_mode_index)
/* Channel A configuration for dual port operation */
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 6) return TRUE;
	*tx_baud_index = buf[0];
	*rx_baud_index = buf[1];
	*char_format_index = buf[2];
	*stop_bits = (buf[3] == 0x07) ? 1 : 2;
	*tx_mode_index = buf[4];
	*rx_mode_index = buf[5];
	return FALSE;
}

/**/
short rpt_0x40 (TSIPPKT *rpt,
	unsigned char *sv_prn,
	short *week_num,
	float *t_zc,
	float *eccentricity,