tsip_ifc.c

gps开发专用的源代码

	unsigned char *chan,
	unsigned char *acq_flag,
	unsigned char *eph_flag,
	float *signal_level,
	float *time_of_last_msmt,
	float *elev,
	float *azim,
	unsigned char *old_msmt_flag,
	unsigned char *integer_msec_flag,
	unsigned char *bad_data_flag,
	unsigned char *data_collect_flag)
/* satellite tracking status */
{
	unsigned char *buf;
	buf = rpt->buf;
	
	if (rpt->len != 24) return TRUE;
	*sv_prn = buf[0];
	*slot = (buf[1] & 0x07) + 1;
	*chan = buf[1] >> 3;
	if (*chan == 0x10) *chan = 2;
	else (*chan)++;
	*acq_flag = buf[2];
	*eph_flag = buf[3];
	*signal_level = bGetSingle (&buf[4]);
	*time_of_last_msmt = bGetSingle (&buf[8]);
	*elev = bGetSingle (&buf[12]);
	*azim = bGetSingle (&buf[16]);
	*old_msmt_flag = buf[20];
	*integer_msec_flag = buf[21];
	*bad_data_flag = buf[22];
	*data_collect_flag = buf[23];
	return FALSE;
}

/**/
short rpt_0x6D (TSIPPKT *rpt,
	unsigned char *manual_mode,
	unsigned char *nsvs,
	unsigned char *ndim,
	unsigned char sv_prn[],
	float *pdop,
	float *hdop,
	float *vdop,
	float *tdop)
/* over-determined satellite selection for position fixes, PDOP, fix mode */
{
	short islot;
	unsigned char *buf;
	buf = rpt->buf;

	*nsvs = (buf[0] & 0xF0) >> 4;
	if ((*nsvs)>8) return TRUE;
	if (rpt->len != 17 + (*nsvs) ) return TRUE;

	*manual_mode = buf[0] & 0x08;
	*ndim  = (buf[0] & 0x07);
	*pdop = bGetSingle (&buf[1]);
	*hdop = bGetSingle (&buf[5]);
	*vdop = bGetSingle (&buf[9]);
	*tdop = bGetSingle (&buf[13]);
	for (islot = 0; islot < (*nsvs); islot++)
		sv_prn[islot] = buf[islot + 17];
	return FALSE;
}

short rpt_0x6E (TSIPPKT *rpt,
	unsigned char *subcode,
	unsigned char *enable,
	unsigned char *interval)
{
	if (rpt->len != 3) return TRUE;
	*subcode = rpt->buf[0];
	*enable = rpt->buf[1];
	*interval = rpt->buf[2];
	return FALSE;
}


/* note: 6F-03 has same structure */
short rpt_0x6F01 (TSIPPKT *rpt,
	struct cph1_msg_struct *cph)
{
	unsigned char *buf;
	int i, isv;
	unsigned short checksum;
	struct cph1_datablock_struct *cdb;
	int db_size, header_size;

	db_size=27;
	header_size=21;
	buf = rpt->buf;

	if (rpt->len < 2) return TRUE;
	cph->length = bGetShort (&buf[2]);

	if (rpt->len != cph->length + 1) return TRUE;

	cph->subcode = buf[0];
	cph->preamble = buf[1];
	cph->receive_time = bGetDouble (&buf[4]);
	cph->clock_offset = bGetDouble (&buf[12]);
	cph->numsvs = buf[20];
	buf += header_size;
	for (isv = 0; isv < cph->numsvs; isv++) {
		cdb = &cph->db[isv];
		cdb->prn = buf[0];
		cdb->flagbyte = buf[1];
		cdb->reserved = buf[2];
		cdb->elev = buf[3];
		cdb->azim = bGetShort (&buf[4]);
		cdb->signal_strength = buf[6];
		cdb->pseudorange = bGetDouble (&buf[7]);
		cdb->carrier_phase = bGetDouble (&buf[15]);
		cdb->Doppler = bGetSingle (&buf[23]);
		buf += db_size;
	}

	for (i = 1, checksum = 0; i < rpt->len - 3; i++) checksum += buf[i];
	cph->checksum = bGetShort(&buf[rpt->len - 3]);
	cph->postamble = buf[rpt->len - 1];
	return FALSE;
}

/**/
short rpt_0x70 (TSIPPKT *rpt, unsigned char *dyn_switch, unsigned char *static_switch,
	unsigned char *alt_switch, unsigned char *extra)

{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 4) return TRUE;
	*dyn_switch = buf[0];
	*static_switch = buf[1];
	*alt_switch = buf[2];
	*extra = buf[3];
	return FALSE;
}

short rpt_0x72 (TSIPPKT *rpt,
	short *pf,
	TSIP_POS_FILT_PARMS *params)
/* position-velocity filter parameters */
{
/*
 * The receiver reports the filter parameters as ints,
 * but in reality they are chars.  To preserve parallel structure
 * with the commands, they are jammed into chars here.
 * If there is an overflow, the routine responds with FALSE.
 */

	unsigned char *buf;
	short temp;
	buf = rpt->buf;

	if (rpt->len != 32) return TRUE;

	*pf = bGetShort (&buf[0]);
	params->flt1 = bGetSingle (&buf[2]);
	temp = bGetShort (&buf[6]);
	params->chr1 = (unsigned char)temp;
	temp = bGetShort (&buf[8]);
	params->chr2 = (unsigned char)temp;
	temp = bGetShort (&buf[10]);
	params->chr3 = (unsigned char)temp;
	temp = bGetShort (&buf[12]);
	params->chr4 = (unsigned char)temp;
	params->flt2 = bGetSingle(&buf[14]);
	params->flt3 = bGetSingle(&buf[18]);
	params->flt4 = bGetSingle(&buf[22]);
	params->flt5 = bGetSingle(&buf[26]);
	temp = bGetShort (&buf[30]);
	params->chr5 = (unsigned char)temp;
	return FALSE;
}

short rpt_0x74 (TSIPPKT *rpt,
	float *alt_filter_parm)
/* altitude filter parameters */
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 4) return TRUE;
	*alt_filter_parm = bGetSingle(buf);
	return FALSE;
}

short rpt_0x78 (TSIPPKT *rpt,
	short *maxage)
/* DC max age parameters */
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 2) return TRUE;
	*maxage = bGetShort (&buf[0]);
   return FALSE;
}

short rpt_0x7B (TSIPPKT *rpt,
	unsigned char *subcode,
	unsigned char *nmea_interval,
	unsigned long *nmea_mask)
/* NMEA Control parameters */
{
	if(rpt->len != 6) return TRUE;
	*subcode = rpt->buf[0];
	if (*subcode!= 0) return TRUE;
	*nmea_interval = rpt->buf[1];
	*nmea_mask = bGetLong(&rpt->buf[2]);
	return FALSE;
}

/**/
short rpt_0x82 (TSIPPKT *rpt,
	unsigned char *diff_mode)
/* differential fix mode */
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 1) return TRUE;
	*diff_mode = buf[0];
	return FALSE;
}

short rpt_0x83 (TSIPPKT *rpt,
	double ECEF_pos[3],
	double *clock_bias,
	float *time_of_fix)
/* position, ECEF double precision */
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 36) return TRUE;
	ECEF_pos[0] = bGetDouble (buf);
	ECEF_pos[1] = bGetDouble (&buf[8]);
	ECEF_pos[2] = bGetDouble (&buf[16]);
	*clock_bias  = bGetDouble (&buf[24]);
	*time_of_fix = bGetSingle (&buf[32]);
	return FALSE;
}

short rpt_0x84 (TSIPPKT *rpt,
	double *lat,
	double *lon,
	double *alt,
	double *clock_bias,
	float *time_of_fix)
/* position, lat-lon-alt double precision */	
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 36) return TRUE;
	*lat = bGetDouble (buf);
	*lon = bGetDouble (&buf[8]);
	*alt = bGetDouble (&buf[16]);
	*clock_bias = bGetDouble (&buf[24]);
	*time_of_fix = bGetSingle (&buf[32]);
	return FALSE;
}

short rpt_0x85 (TSIPPKT *rpt,
	unsigned char *sv_prn,
	unsigned char *summary_status,
	unsigned char *station_health,
	unsigned char *SV_UDRE_parm,
	unsigned char *iode1,
	unsigned char *iode2,
	float *z_cnt_tow,
	float *range_corr,
	float *range_rate_corr,
	float *delta_range_corr)
/* satellite differential correction info */
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 22) return TRUE;
	*sv_prn = buf[0];
	*summary_status = buf[1];
	*station_health = buf[2];
	*SV_UDRE_parm = buf[3];
	*iode1 = buf[4];
	*iode2 = buf[5];
	*z_cnt_tow = bGetSingle (&buf[6]);
	*range_corr = bGetSingle (&buf[10]);
	*range_rate_corr = bGetSingle (&buf[14]);
	*delta_range_corr = bGetSingle (&buf[18]);
	return FALSE;
}


short rpt_0xBB(TSIPPKT *rpt,
	TSIP_RCVR_CFG *TsipxBB)
{

	unsigned char *buf;
	buf = rpt->buf;

	/* Manual  29473-00 Rev B. is in error.  Byte count is 40, not 41. */
	if (rpt->len != 40) return TRUE;

	TsipxBB->bSubcode		=	buf[0];
   if (TsipxBB->bSubcode == 0)
   {
		TsipxBB->operating_mode	=	buf[1]	;
		TsipxBB->dgps_mode		=	buf[2]	;
		TsipxBB->dyn_code			=	buf[3]	;
		TsipxBB->trackmode		=	buf[4]	;
		TsipxBB->elev_mask		=  bGetSingle (&buf[5]);
		TsipxBB->cno_mask			=	bGetSingle (&buf[9]);
		TsipxBB->dop_mask 		=  bGetSingle (&buf[13]);
		TsipxBB->dop_switch 	=  bGetSingle (&buf[17]);
		TsipxBB->dgps_age_limit	=	buf[21] ;
   }
   else if (TsipxBB->bSubcode == 3)
   {
		TsipxBB->operating_mode	=	buf[1]	;
		TsipxBB->dgps_mode		=	buf[2]	;
		TsipxBB->dyn_code			=	buf[3]	;
		TsipxBB->trackmode		=	buf[6]	;
		TsipxBB->elev_mask		=  	bGetSingle (&buf[15]);
		TsipxBB->cno_mask		=	bGetSingle (&buf[19]);
		TsipxBB->dop_mask 		=  	bGetSingle (&buf[23]);
		TsipxBB->dop_switch 	=  	bGetSingle (&buf[27]);
		TsipxBB->dgps_age_limit	=	buf[35] ;
   }
   else return TRUE;
	return FALSE;
}

short rpt_0xBC (TSIPPKT *rpt, 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)
/* Receiver serial port configuration */
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 10) return TRUE;
	*port_num = buf[0];
	*in_baud = buf[1];
	*out_baud = buf[2];
	*data_bits = buf[3];
	*parity = buf[4];
	*stop_bits = buf[5];
	*flow_control = buf[6];
	*protocols_in = buf[7];
	*protocols_out = buf[8];
	*reserved = buf[9];

	return FALSE;
}

/**** Superpackets ****/

short rpt_0x8F03 (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 B configuration for dual port operation */
{
	unsigned char *buf;
	buf = rpt->buf;
	
	if (rpt->len != 7) return TRUE;
	*tx_baud_index = buf[1];
	*rx_baud_index = buf[2];
	*char_format_index = buf[3];
	*stop_bits = (buf[4] == 0x07) ? 1 : 2;
   	*tx_mode_index = buf[5];
	*rx_mode_index = buf[6];
	return FALSE;
}

short rpt_0x8F15 (TSIPPKT *rpt,
	short *datum_idx,
	double datum_coeffs[5])
/*  datum index and coefficients  */
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 43) return TRUE;
	*datum_idx = bGetShort(&buf[1]);
	datum_coeffs[0] = bGetDouble (&buf[3]);
	datum_coeffs[1] = bGetDouble (&buf[11]);
	datum_coeffs[2] = bGetDouble (&buf[19]);
	datum_coeffs[3] = bGetDouble (&buf[27]);
	datum_coeffs[4] = bGetDouble (&buf[35]);
	return FALSE;
}


short rpt_0x8F17 (TSIPPKT *rpt,
	unsigned char *UTM_grid_letter,
	short *UTM_grid_number,
	float *UTM_northing_value,
	float *UTM_easting_value,
	float *UTM_altitude,
	float *clock_bias,
	float *time_of_fix)
/*  UTM Single precision position response  */
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 24) return TRUE;
	*UTM_grid_letter    = buf[1];
	*UTM_grid_number    = bGetShort (&buf[2]);
	*UTM_northing_value = bGetSingle (&buf[ 4]);
	*UTM_easting_value  = bGetSingle (&buf[ 8]);
	*UTM_altitude       = bGetSingle (&buf[12]);
	*clock_bias         = bGetSingle (&buf[16]);
	*time_of_fix        = bGetSingle (&buf[20]);
	return FALSE;
}


short rpt_0x8F18 (TSIPPKT *rpt,
	unsigned char *UTM_grid_letter,
	short *UTM_grid_number,
	double *UTM_northing_value,
	double *UTM_easting_value,
	double *UTM_altitude,
	double *clock_bias,
	float *time_of_fix)
/*  UTM double precision position response  */
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 40) return TRUE;
	*UTM_grid_letter 	  = buf[1];
	*UTM_grid_number    = bGetShort (&buf[2]);
	*UTM_northing_value = bGetDouble (&buf[ 4]);
	*UTM_easting_value  = bGetDouble (&buf[12]);
	*UTM_altitude       = bGetDouble (&buf[20]);
	*clock_bias         = bGetDouble (&buf[28]);
	*time_of_fix        = bGetSingle (&buf[36]);
	return FALSE;
}


short rpt_0x8F19 (TSIPPKT *rpt,
   short *UTM_enabled)
/*  UTM position message enabled/disabled response  */
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 3) return TRUE;
	*UTM_enabled = (buf[2] == 'E') ? TRUE : FALSE;
	return FALSE;
}

#define MAX_LONG  (2147483648.)   /* 2**31 */

short rpt_0x8F20 (TSIPPKT *rpt,
	unsigned char *subpacket_id,
	unsigned char *info,
	double *lat,
	double *lon,
	double *alt,
	double vel_enu[],
	double *time_of_fix,
	short *week_num,
	unsigned char *nsvs,
	unsigned char sv_prn[],
	short sv_IODC[],
	short *datum_index)
{
	short
		isv;
	unsigned char
		*buf, prnx;
	unsigned long
		ulongtemp;
	long
		longtemp;
	double
		vel_scale;

	buf = rpt->buf;

	if (rpt->len != 56) return TRUE;
	*subpacket_id = buf[0];

	vel_scale = (buf[24]&1)? 0.020 : 0.005;
	vel_enu[0] = bGetShort (buf+2)*vel_scale;
	vel_enu[1] = bGetShort (buf+4)*vel_scale;
	vel_enu[2] = bGetShort (buf+6)*vel_scale;

	*time_of_fix = bGetULong (buf+8)*.001;

	longtemp = bGetLong (buf+12);
	*lat = longtemp*(GPS_PI/MAX_LONG);

	ulongtemp = bGetULong (buf+16);
	*lon = ulongtemp*(GPS_PI/MAX_LONG);
	if (*lon > GPS_PI) *lon -= 2.0*GPS_PI;

	*alt = bGetLong (buf+20)*.001;
	/* 25 blank; 29 = UTC */
	*datum_index = (short)buf[26] - 1;
	*info = buf[27];
	*nsvs = buf[28];
	*week_num = bGetShort (&buf[30]);
	for (isv = 0; isv < 8; isv++) {
		prnx = buf[32+2*isv];
		sv_prn[isv] = prnx&0x3F;
		sv_IODC[isv] = buf[32 + 2 * isv + 1] + (short)(prnx - sv_prn[isv]) * 4;
	}
	return FALSE;
}

short rpt_0x8F26(TSIPPKT *rpt,
		  unsigned long *see_write)
{
	unsigned char
		*buf;
	
	buf = rpt->buf;
   if (rpt->len != 5) return TRUE;
   *see_write = bGetLong(&buf[1]);

   return FALSE;
}

short rpt_0x8F40 (TSIPPKT *rpt,
   	unsigned char *Flags,
      unsigned char *HeartBeatSentence,
      short *TopOfHourOffset,
      short *Frequency,
      char *VehicleID)
/* TAIP configuration &*/
{
	unsigned char *buf;
	buf = rpt->buf;

	if (rpt->len != 11) return TRUE;
	*Flags    			= buf[1];
	*HeartBeatSentence= buf[2];
	*TopOfHourOffset 	= bGetShort (&buf[3]);
	*Frequency  		= bGetShort (&buf[5]);
   VehicleID[0] = buf[7];
   VehicleID[1] = buf[8];
   VehicleID[2] = buf[9];
   VehicleID[3] = buf[10];
	return FALSE;
}