garmin.c

这是经典的卫星编程应用程序

/*
 * setup buffer to send route ID record
 */
	if(LinkProtocol==1)
		message[0] = Pid_Rte_Hdr;
	else
		message[0] = Pid_Rte_Hdr2;
	strupr(route_comment);
	switch(RouteHdrType) {
		case 200:
				D200=*(SerialMessage+3);
				route_number=D200;
				strcpy(route_comment,"NONE");
				message[1] = 1;  // Just the route number
				message[2] = route_number;
				break;
		case 201:
				D201=(D201_Rte_Hdr_Type *)&message[2];
				memset(&message[3],' ',20);
				message[19]=0;
				D201->nmbr=route_number;
				strncpy(D201->cmnt,route_comment,20);
				message[1] = 21;  // 21 data bytes in the 201 record
				break;
		case 202:
				D202=(D202_Rte_Hdr_Type *)&message[2];
				strncpy(D202->rte_ident, route_comment,20);
				trim(D202->rte_ident);
				message[1] = strlen(D202->rte_ident)+1;
				break;
	} // switch(RouteHdrType)

	SendGarminMessage(message, (short)(message[1]+2), "Pid_Rte_Hdr record");
	getGarminMessage(1);	/* make sure GPS receiver responded */
} /* doSRouteHdr */

/* -------------------------------------------------------------------------- */
PROCX void doID(void)
{
	BYTE *x;  
	BYTE *q;
	char *p;

	INT i;

	Product_Data_Type *dd;

	if(gotID==1) return;
	dd=(Product_Data_Type *)&SerialMessage[3];

	x=&SerialMessage[2]; 			/* data length */

	Product_ID=dd->product_ID;
	SWVersion=dd->software_version;

	Ggps=0;
	switch(Product_ID) {
	    case GPS38_40_45_45XL_A:
	    case GPS38_40_45_45XL_B:
	    case GPSII:
	    case GPS38_C:
	    case GPS38_J:
	    case GPSIIP:
	    case GPS12_12XLA:
	    case GPS12:
	    case GPS12CX:
	    case GPS12_12XLB:
	    case GPSIIP_1:
	    case GPS12XL_J:
	    case GPS12XL_C:
	        Ggps=G12XL;
	        break;
	
	    case GPSIII_ID:
	    case GPSIIIPILOT:
	        Ggps=GPSIII;
	        break;
	
	    case GPS3P:
	        Ggps=GPSIIIP;
	        break;
	
	    case STREETPILOT:
	    case STREETPILOTCOLORMAP:
	        Ggps=SPILOT;
	        break;
	}
//
// decode null terminated strings
//
	q=dd->a;
	p=getstrings((char*)q,"",0);


	if(p != NULL) {
		strcpy(VersionString,p);
		trim(VersionString);
		q+=(strlen(p)+1);
	}

	i=0;
	while(q < (dd->a + *x - 4)) {
		p=getstrings(NULL,"",1);
		if(p != NULL) {
			VersionSubString[i]=strdup(p);
			q+=(strlen(p)+1);
		}
		i++;
		VersionSubString[i]=NULL;
	}
	VersionSubStrings=i;
	gotID=1;
} /* doID */

/* -------------------------------------------------------------------------- */
PROCX void doEOFRec(void)
{
	AlmanacEOF=RouteEOF=TrackEOF=ProxWaypointEOF=WaypointEOF=0;
	switch(SerialMessage[3]) {
		case 1: AlmanacEOF=1; break;
		case 3: ProxWaypointEOF=1;break;
		case 4: RouteEOF=1; break;
		case 6: TrackEOF=1; break;
		case 7: WaypointEOF=1; break;
		default: break;
	}
} /* doEOFRec */

/* -------------------------------------------------------------------------- */
PROCX void doGPSVolts(void)
{
	INT i;
	i=*((int*)(SerialMessage+3));
	InternalVolts=(float)i/100.0;
	i=*((int*)(SerialMessage+5));
	ExternalVolts=(float)i/100.0;
} /* doGPSVolts */

struct alm {
	INT wk;			/* Week       -1 if poor health */
	float ta;		/* Time of Applicability(s)		*/
	float af0;		/* Af0(s)						*/
	float af1;		/* Af1(s)						*/
	float ecc;		/* Eccentricity					*/
	float sqrta;	/* SQRT(A)  (m^1/2)				*/
	float meananom;	/* Mean Anom(rad)				*/
	float argper;	/* Argument of Perigee(rad)		*/
	float RA;		/* Right Ascen at TOA(rad)		*/
	float RRA;		/* Rate of Right Ascen(r/s)		*/
	float OI;		/* Orbital Inclination(rad)		*/
	byte hlth;		// added for type D501 protocol
} *almm;

struct alm *alm;

/* -------------------------------------------------------------------------- */
PROCX void doSAlmanac500()
{
/*
 * we assume all 32 satellites.  Some Garmin GPS units do not have a method
 *  for inputting the satellite number....
 */
	INT mlen;
	INT sending=0,id,hold,health;
 	char lb[150];

	memset(message,' ',MAX_LENGTH);

	bgnxfr[2]=32;  // Number of satellites to send

	SendGarminMessage(bgnxfr,4,"Send number of sats");  /* send # sats */
	getGarminMessage(1);	/* make sure GPS receiver responded */

	almm=(struct alm*)(&message[2]);  // ok for D500

	if(AlmanacDataRecord==500) mlen=0;else mlen=1;

	while(fgets((char*)lb,MAX_LENGTH,in) != NULL) { /* get first line */
		sending++;
		fgets(lb,MAX_LENGTH,in);	/* ID */
		id=atoi(&lb[25]);
		fgets(lb,MAX_LENGTH,in);	/* Health (also in week# for Garmin) */
		health=atoi(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* ecc */
		almm->ecc=(float)atof(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* ta */
		almm->ta=(float)atof(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* OI */
		almm->OI=(float)atof(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* rra */
		almm->RRA=(float)atof(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* sqrta */
		almm->sqrta=(float)atof(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* RA */
		almm->RA=(float)atof(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* argper */
		almm->argper=(float)atof(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* meananom */
		almm->meananom=(float)atof(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* af0 */
		almm->af0=(float)atof(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* af1 */
		almm->af1=(float)atof(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* wk */
		almm->wk=atoi(&lb[25]);

		fgets(lb,MAX_LENGTH,in);	/* blank line */

		if(health!=0)
			hold=-1;
		else 
			hold=almm->wk;

		almm->wk=-1;
		message[0]=0x1f; message[1]=0x2a+mlen;
		almm->hlth = 0;//0xe6;
		while(sending != id) { /* sending dummy */
			SendGarminMessage(message, (short)(44+mlen), "Almanac Record Dummy");
			getGarminMessage(1);
			sending++;
		}
		almm->wk=hold;
		almm->hlth=0;
		SendGarminMessage(message, (short)(44+mlen), "Almanac Record good");
		getGarminMessage(1);
	}
	while(sending!=32) {
		almm->wk=-1;
		almm->hlth=0;//0xe6;
		SendGarminMessage(message, (short)(44+mlen), "Almanac record rest");
		getGarminMessage(1);
		sending++;
	}
	doSTime();
	SendGarminMessage(almt,(short)4, "Almanac terminator");  /* almanac data terminator */
	getGarminMessage(1);   /* get ack/nak, assume ack */
} /* doSAlmanac500 */

D500_Almanac_Type *D500_Almanac;
D501_Almanac_Type *D501_Almanac;
D550_Almanac_Type *D550_Almanac;
D551_Almanac_Type *D551_Almanac;
/* -------------------------------------------------------------------------- */
PROCX void doAlmanac()
{
/**** Week 851 almanac for PRN-01 ***********
ID:                          001
Health:                      000
Eccentricity:                3.608226776e-03
Time of Applicability(s):    5.898240000e+05
Orbital Inclination(rad):    9.540965021e-01
Rate of Right Ascen(r/s):   -8.091765626e-09
SQRT(A) (m^1/2):             5.153528809e+03
Right Ascen at TOA(rad):    -7.805333171e-01
Argument of Perigee(rad):   -1.456979147e+00
Mean Anom(rad):              2.899803800e+00
Af0(s):                      6.866455078e-05
Af1(s/s):                    1.382431947e-10
week:                        851
*/
	if(AlmanacDataRecord == 500 || AlmanacDataRecord == 501) {
		alm = (void *)(SerialMessage+3);
		D501_Almanac = (void *)(SerialMessage+3);
		if(satnum>1) printf("\n");
		printf("**** Week %3d almanac for PRN-%02d ***********\n",alm->wk,satnum);
		printf("%-29s%03d\n","ID:",satnum);
		if(AlmanacDataRecord==500) 	printf("%-29s%3s\n","Health:",alm->wk<0?"111":"000");
		else printf("%-29s%03x\n","Health:",D501_Almanac->hlth);
		printf("%-25s%19.9e\n","Eccentricity:",alm->ecc);
		printf("%-25s%19.9e\n","Time of Applicability(s):",alm->ta);
		printf("%-25s%19.9e\n","Orbital Inclination(rad):",alm->OI);
		printf("%-25s%19.9e\n","Rate of Right Ascen(r/s):",alm->RRA);
		printf("%-25s%19.9e\n","SQRT(A) (m^1/2):",alm->sqrta);
		printf("%-25s%19.9e\n","Right Ascen at TOA(rad):",alm->RA);
		printf("%-25s%19.9e\n","Argument of Perigee(rad):",alm->argper);
		printf("%-25s%19.9e\n","Mean Anom(rad):",alm->meananom);
		printf("%-25s%19.9e\n","Af0(s):",alm->af0);
		printf("%-25s%19.9e\n","Af1(s/s):",alm->af1);
		printf("%-29s%d\n","week:",alm->wk);
	}
	if(AlmanacDataRecord==550) fprintf(stderr, "Almanac data record D550 not implemented\n");
	if(AlmanacDataRecord==551) fprintf(stderr, "Almanac data record D550 not implemented\n");
} /* doAlmanac */

/* -------------------------------------------------------------------------- */
PROCX void AsyncSend(BYTE *message, INT bytes)
{
	INT i;

#if __BORLANDC__
	for(i=0;i<bytes;i++) {
		if(WinNT==1) msdelay(2);
		AsyncOut(message[i]);
	}
#else
	AsyncOut(message,bytes);
#endif
} // AsyncSend()

/* -------------------------------------------------------------------------- */
PROCX void SendGarminMessage(BYTE *message, short bytes, char *msgtype)
{
	short c,i;
	INT n;
	BYTE qq,csum = 0;
	BYTE *p = SerialMessageOut;

	ok=-1;	
//
// Frame start byte
//
	*p++ = 0x10;    /* Start of message */
//
// Transfer message buffer to transmit buffer duplicating any 0x10 bytes
//
	for (i=0; i<bytes; i++) {
		*p++ = message[i];
		csum -= message[i];
		if (message[i] == 0x10)
			*p++ = 0x10;
	}
//
// Add in checksum
//
	*p++ = csum;
	if (csum == 0x10) *p++ = 0x10;
//
// Add frame trailer
//
	*p++ = 0x10;
	*p++ = 0x03;
//
// Message length in bytes
//
	n =(int)(p - SerialMessageOut);
//
// Dumping the send buffer to a file?
//
	if(Gdebug==1) {
		if(strcmp(msgtype,"ACK")!=0) fprintf(Gdebug_out,"\n");
		fprintf(Gdebug_out,"Sending: %s\n",msgtype);
		c=0;
		for(i=0;i<n;i++) {
			fprintf(Gdebug_out,outfmt,qq=*(SerialMessageOut+i));
			if(qq>=32 && qq<=126) {
				fprintf(Gdebug_out,"%c ",qq);
			}
			else {
				fprintf(Gdebug_out,"  ");
			}
			if(++c>15) {
				fprintf(Gdebug_out,"\n");
				c=0;
			}
	    }
		fprintf(Gdebug_out,"\n");
	}
//
// 'virus..check?  a 0x2b record apparently causes the erasure of
//  the GPS's program.
//
	if( ((SerialMessageOut[1] & 0x0f)==0x0b) &&
	    ((SerialMessageOut[1] & 0xf0)==0x20)       ) {
	        fprintf(stderr,"Error 141, aborting...\n");
			byebye(0);
	}
//
// Save message
//
	if(strcmp(msgtype,"ACK") != 0) {
		LastGarminLen=bytes;  		// Length w/o header, checksum and trailer
		memcpy(LastGarminGPSMsg,message,bytes);
	}
#if !DEBUGREAD
	AsyncSend(SerialMessageOut, n);
#endif
} /* SendGarminMessage() */

//-------------------------------------------------------------------------- 
//  The data is sent as scan lines, 100 lines with 160 pixels per line.
//
//  The data is stored in unsigned long format, startin