tsip_rpt.c

gps开发专用的源代码

static void rpt_8F15 (TSIPPKT *rpt)
/* Datum parameters */
{
	double
		datum_coeffs[5];
	short
		datum_idx;

	/* unload rptbuf */
	if (rpt_0x8F15 (rpt, &datum_idx, datum_coeffs)) {
		parsed = BADLEN_PARSE;
		return;
	}

	xprintf ("Datum Index %2d ", datum_idx);
	show_crlf ();
	xprintf ("    dx     dy     dz       a-axis          e-squared");
	show_crlf ();
	xprintf("%6.1f %6.1f %6.1f    %10.3f    %16.14f", datum_coeffs[0], datum_coeffs[1],
			datum_coeffs[2], datum_coeffs[3],datum_coeffs[4]);
}

/* 0x8F17 */
static void rpt_8F17 (TSIPPKT *rpt)
/* UTM single precision position response */
{
	unsigned char UTM_grid_letter;
	short UTM_grid_number;
	float UTM_northing_value,
			   UTM_easting_value,
			   UTM_altitude,
			   clock_bias,
			time_of_fix;

	/* unload rptbuf */
	if (rpt_0x8F17 (rpt,
	  &UTM_grid_letter, &UTM_grid_number, &UTM_northing_value,
		&UTM_easting_value, &UTM_altitude, &clock_bias, &time_of_fix)) {
		parsed = BADLEN_PARSE;
		return;
	}

	xprintf ("UTM:%-3d%c Northing:%8.2f Easting:%8.2f UTM Alt:%8.2f",
		UTM_grid_number,
		UTM_grid_letter,
		UTM_northing_value,
		UTM_easting_value,
		UTM_altitude);
	show_crlf ();
	xprintf ("Clock bias:%11.2f", clock_bias);
	show_time (time_of_fix);
}

static void rpt_8F18 (TSIPPKT *rpt)
/* UTM double precision position response */
{
	unsigned char UTM_grid_letter;
	short UTM_grid_number;

	double UTM_northing_value,
			   UTM_easting_value,
			   UTM_altitude,
			   clock_bias;

	float time_of_fix;

	/* unload rptbuf */
	if (rpt_0x8F18 (rpt, &UTM_grid_letter, &UTM_grid_number, &UTM_northing_value,
					&UTM_easting_value, &UTM_altitude, &clock_bias,
					&time_of_fix)) {
		parsed = BADLEN_PARSE;
		return;
	}

	xprintf ("UTM:%-3d%c Northing:%8.2lf Easting:%8.2lf UTM Alt:%8.2lf",
		UTM_grid_number,
		UTM_grid_letter,
		UTM_northing_value,
		UTM_easting_value,
		UTM_altitude);
	show_crlf ();
	xprintf ("Clock bias:%11.2f", clock_bias);
	show_time (time_of_fix);
}

static void rpt_8F19 (TSIPPKT *rpt)
/* UTM message enable status response */
{
	short UTM_enabled;

	if (rpt_0x8F19 (rpt, &UTM_enabled)) {
		parsed = BADLEN_PARSE;
		return;
	}

	xprintf ("UTM Position Output is %s",
		UTM_enabled ? "Enabled" : "Disabled");
}


#define INFO_DGPS       0x02
#define INFO_2D         0x04
#define INFO_ALTSET     0x08
#define INFO_FILTERED   0x10
static void rpt_8F20 (TSIPPKT *rpt)
{
	unsigned char
		subpacket_id, info, nsvs, *buf, sv_prn[32];
	short
		week_num, datum_index, sv_IODC[32];
	double
		lat, lon, alt, time_of_fix;
	double
		londeg, latdeg, vel[3];
	short
		isv;

	buf = rpt->buf;
	/* unload rptbuf */
	if (rpt_0x8F20 (rpt,
		&subpacket_id, &info, &lat, &lon, &alt, vel,
		&time_of_fix,
		&week_num, &nsvs, sv_prn, sv_IODC, &datum_index))
	{
		parsed = BADLEN_PARSE;
		return;
	}
	/* convert from radians to degrees */
	latdeg = R2D * fabs(lat);
	londeg = R2D * fabs(lon);
	xprintf ("SPkt 8F%c20: %02d:%02d:%05.2f %4d:%09.6f %c %5d:%09.6f %c %10.2f ",
		 buf[1]?'*':'-',
		(short)fmod(time_of_fix/3600., 24.),
		(short)fmod(time_of_fix/60., 60.),
		fmod(time_of_fix, 60.),
		(short)latdeg, fmod (latdeg, 1.)*60.0,
		(lat<0.0)?'S':'N',
		(short)londeg, fmod (londeg, 1.)*60.0,
		(lon<0.0)?'W':'E',
		alt);
	show_crlf ();

	xprintf ("*   *%10.3f E %10.3f N %10.3f U         %s%s%s    Datum%3d",
		vel[0], vel[1], vel[2],
		(info & INFO_DGPS)?"Diff-":"",
		(info & INFO_2D)?((info&INFO_ALTSET)?"2D-AltSet":"2D-AltHold"):"3D",
		(info & INFO_FILTERED)?"-Filt":"",
		datum_index);
	show_crlf ();

	xprintf ("*   *SVs");
	for (isv = 0; isv < nsvs; isv++) {
		xprintf (" %02d", sv_prn[isv]);
	}
	xprintf ("          ( IODEs ");
	for (isv = 0; isv < nsvs; isv++) {
		xprintf ("%02X ", sv_IODC[isv]&0xFF);
	}
	xprintf (")");
}

void print_stinger_rcvr_config(TSIP_RCVR_CFG_03 *TsipxBB )
{

	show_crlf();
	xprintf (" operating mode:      %s",
		(TsipxBB->operating_mode < POS_FIX_TEXT_LEN)?
		pos_fix_text[TsipxBB->operating_mode]:
		"unknown operating mode");
	show_crlf ();
	xprintf (" DGPS mode:           %s",
		(TsipxBB->dgps_mode<DGPS_MODE_TEXT_LEN)?
		dgps_mode_text2[TsipxBB->dgps_mode]:
		"unknown DGPS mode");
	show_crlf ();
	xprintf (" tracking mode:       %s",
		(TsipxBB->trackmode<TRACKMODE_TEXT_LEN)?
		trackmode_text3[TsipxBB->trackmode]:
		"unknown tracking mode");
	show_crlf();
	xprintf (" dynamics:            %s",
		(TsipxBB->dyn_code<DYN_TEXT_LEN)?
		dyn_text2[TsipxBB->dyn_code]:
		"unknown dynamics code");
	show_crlf ();
	xprintf (" elev angle mask:     %g deg",
		TsipxBB->elev_mask * R2D);
	show_crlf ();
	xprintf (" SNR mask:            %g AMU",
		TsipxBB->cno_mask);
	show_crlf ();
	xprintf (" PDOP mask:           %g",
		TsipxBB->pdop_mask);
	show_crlf ();
	xprintf (" PDOP switch:         %g",
		TsipxBB->pdop_switch);
	show_crlf ();
	xprintf (" dgps age limit:      %u sec",
		TsipxBB->dgps_age_limit);
}

/* 0xBB */
static void rpt_stinger_rcvr_config (TSIPPKT *rpt)
{
	TSIP_RCVR_CFG_03 TsipxBB ;
	/* unload rptbuf */
	if (rpt_0xBB (rpt, &TsipxBB)){
		parsed = BADLEN_PARSE;
		return;
	}

	xprintf ("Stinger receiver configuration parameters:");
	print_stinger_rcvr_config(&TsipxBB);
	return ;
}

void print_rcvr_serial_port_config(
	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) {

	unsigned char known;

	xprintf ("   RECEIVER serial port %s config:",
		(port_num<RCVR_PORT_TEXT_LEN-1)?rcvr_port_text[port_num]:"unknown port number");

	show_crlf();
	xprintf("             Input baud %s, Output baud %s, %s - %s - %s",
		(in_baud<BAUD_TEXT_LEN)?st_baud_text_app[in_baud]:"unknown input baud rate",
		(out_baud<BAUD_TEXT_LEN)?st_baud_text_app[out_baud]:"unknown output baud rate",
		(data_bits<DATABIT_TEXT_LEN)?databit_text_app[data_bits]:"unknown data bits",
		(parity<PARITY_TEXT_LEN)?parity_text[parity]:"unknown parity",
		(stop_bits<STOPBIT_TEXT_LEN)?stopbit_text[stop_bits]:"unknown stop bits");
	show_crlf ();
	xprintf("             Flow control: ");
	if (flow_control == PORT_HS_NONE) xprintf("%s ", flow_cntrl_text[0]);
	else {
		known = FALSE;
		if (flow_control&PORT_HS_RTS_CTS) {
			xprintf("%s ", flow_cntrl_text[1]);
			known = TRUE;
		}
		if (flow_control&PORT_HS_TX_XON_XOFF) {
			xprintf("%s ", flow_cntrl_text[2]);
			known = TRUE;
		}
		if (flow_control&PORT_HS_TX_XANY) {
			xprintf("%s ", flow_cntrl_text[3]);
			known = TRUE;
		}
		if (flow_control&PORT_HS_RX_XON_XOFF){
			xprintf("%s ", flow_cntrl_text[4]);
			known = TRUE;
		}
		if (known == FALSE) xprintf("Unknown flow control setting");
	}
	show_crlf ();
	xprintf ("             Input protocols: ");
	known = FALSE;
	if (protocols_in&TAIP) {
		xprintf("%s ", protocols_text[0]);
		known = TRUE;
	}
	if (protocols_in&TSIP) {
		xprintf("%s ", protocols_text[1]);
		known = TRUE;
	}
	if (protocols_in&NMEA) {
		xprintf("%s ", protocols_text[2]);
		known = TRUE;
	}
	if (protocols_in&RTCM){
		xprintf("%s ", protocols_text[3]);
		known = TRUE;
	}
	if (protocols_in&DCOL){
		xprintf("%s ", protocols_text[4]);
		known = TRUE;
	}
	if (known == FALSE) xprintf("No known");

	show_crlf ();
	xprintf ("             Output protocols: ");
	known = FALSE;
	if (protocols_out&TAIP) {
		xprintf("%s ", protocols_text[0]);
		known = TRUE;
	}
	if (protocols_out&TSIP) {
		xprintf("%s ", protocols_text[1]);
		known = TRUE;
	}
	if (protocols_out&NMEA) {
		xprintf("%s ", protocols_text[2]);
		known = TRUE;
	}
	if (protocols_out&RTCM){
		xprintf("%s ", protocols_text[3]);
		known = TRUE;
	}
	if (protocols_out&DCOL){
		xprintf("%s ", protocols_text[4]);
		known = TRUE;
	}
	if (known == FALSE) xprintf("No known");
	reserved = reserved ;
	show_crlf ();
}

/* 0xBC */
static void rpt_rcvr_serial_port_config (TSIPPKT *rpt)
{
	unsigned char
		port_num, in_baud, out_baud, data_bits, parity, stop_bits, flow_control,
		protocols_in, protocols_out, reserved;

	/* unload rptbuf */
	if (rpt_0xBC (rpt, &port_num, &in_baud, &out_baud, &data_bits, &parity,
			&stop_bits, &flow_control, &protocols_in, &protocols_out, &reserved)) {
		parsed = BADLEN_PARSE;
		return;
	}
	/* rptbuf unloaded */

	print_rcvr_serial_port_config(port_num, in_baud, out_baud, data_bits,
		parity, stop_bits, flow_control, protocols_in, protocols_out, reserved);

 }

static void rpt_8F (TSIPPKT *rpt)
{
	switch (rpt->buf[0]) {
	case 0x15:
		rpt_8F15 (rpt);
		break;

	case 0x17:
		rpt_8F17 (rpt);
		break;

	case 0x18:
		rpt_8F18 (rpt);
		break;

	case 0x19:
		rpt_8F19 (rpt);
		break;

	case 0x20:
		rpt_8F20 (rpt);
		break;

	default:
		parsed = BADID_PARSE;
		break;
	}
}

void print_msg_table_header (unsigned char rptcode)
{
	/* force header is to help auto-output function */
	/* last_rptcode is to determine whether to print a header */
	/* for the first occurence of a series of reports */
	static unsigned char
		last_rptcode = 0;

	if (rptcode!=last_rptcode) switch (rptcode)
	{
	case 0x5A:
		/* supply a header in console output */
		xprintf ("Raw Measurement Data");
		show_crlf ();
		xprintf (
"   SV  Sample   SNR  Code Phase   Doppler    Seconds     Time of Meas");
		show_crlf();
		break;

	case 0x5C:
		xprintf ("Tracking Info");
		show_crlf ();
		xprintf (
"   SV  C S Acq Eph   SNR     Time of Meas      Elev   Azim   Old Msec BadD DCol");
		show_crlf();
		break;

	case 0x85:
		xprintf ("Differential Correction Status");
		show_crlf ();
		xprintf (
"    SV  Status  StaH  SV H  IODE1  IODE2  Z-count time        PRC    RRC    DRC");
		show_crlf();
		break;
	}
	last_rptcode = rptcode;
}

static void unknown_rpt (TSIPPKT *rpt)
{
	short i;

	/* app-specific rpt packets */
	xprintf ("TSIP report packet ID %2Xh, length %d", rpt->code, rpt->cnt);
	if (parsed == BADLEN_PARSE) xprintf (": Bad length");
	if (parsed == BADID_PARSE) xprintf (": translation not supported");
	show_crlf ();
	for (i = 0; i < rpt->cnt; i++) {
		if ((i % 20) == 0) show_crlf ();
		xprintf (" %02X", rpt->buf[i]);
	}
}
/**/
void rpt_packet (TSIPPKT *rpt)
{

	parsed = GOOD_PARSE;

	show_crlf();

	/* print a header if this is the first of a series of messages */
	print_msg_table_header (rpt->code);

	switch (rpt->code) {
	case 0x41:
		rpt_GPS_time (rpt);
		break;

	case 0x42:
		rpt_single_ECEF_position (rpt);
		break;

	case 0x43:
		rpt_single_ECEF_velocity (rpt);
		break;

	case 0x45:
		rpt_SW_version (rpt);
		break;

	case 0x46:
		rpt_rcvr_health (rpt);
		break;

	case 0x47:
		rpt_SNR_all_SVs (rpt);
		break;

	case 0x48:
		rpt_GPS_system_message (rpt);
		break;

	case 0x4A:
		if (rpt->cnt == 20)
		{
			rpt_single_lla_position (rpt);
		}
		else if (rpt->cnt == 9)

		{
			rpt_ref_alt (rpt);
		}
		break;

	case 0x4B:
		rpt_rcvr_id_and_status (rpt);
		break;

	case 0x4D:
		rpt_oscillator_offset (rpt);
		break;

	case 0x4E:
		rpt_GPS_time_set_response (rpt);
		break;

	case 0x55:
		rpt_io_opt (rpt);
		break;

	case 0x56:
		rpt_ENU_velocity (rpt);
		break;

	case 0x57:
		rpt_last_fix_info (rpt);
		break;

	case 0x58:
		rpt_GPS_system_data (rpt);
		break;

	case 0x59:
		rpt_SVs_enabled (rpt);
		break;

	case 0x5A:
		rpt_raw_msmt (rpt);
		break;

	case 0x5C:
		rpt_SV_tracking_status (rpt);
		break;

	case 0x6D:
		rpt_allSV_selection (rpt);
		break;

	case 0x6E:
		rpt_full_msmt_packet_controls (rpt);
		break;

	case 0x6F:
		rpt_full_msmt_packet (rpt);
		break;

	case 0x70:
		show_pv_operation (rpt);
		break;

	case 0x7B:
		rpt_nmea_parameters (rpt);
		break;

	case 0x82:
		rpt_DGPS_position_mode (rpt);
		break;

	case 0x83:
		rpt_double_ECEF_position (rpt);
		break;

	case 0x84:
		rpt_double_lla_position (rpt);
		break;

	case 0x85:
		rpt_dgps_info (rpt);
		break;

	case 0x8F:
		rpt_8F (rpt);
		break;

	case 0xBB: /* Stinger receiver configuration */
		rpt_stinger_rcvr_config (rpt);
		break;

	case 0xBC:  	/* Stinger receiver serial port config */
		rpt_rcvr_serial_port_config (rpt);
		break;

	default:
		parsed = BADID_PARSE;
		break;
	}

	if (parsed != GOOD_PARSE)
	{
		unknown_rpt (rpt);
	}
}