nmea.c

GPS的标准格式NMEA处理源程序

		int tmp;
		int x;
		nmea_parse(&r[7], "iii", &lines,&ln,&svs);
		ln=ln-1;
		ln=ln*4;
		nmea_parse(&r[7], "iiiiiiiiiiiiiiiiiii",
		&tmp,&tmp,&tmp,
		&sv[0+ln].sv, &sv[0+ln].alt, &sv[0+ln].brg, &sv[0+ln].sn,
		&sv[1+ln].sv, &sv[1+ln].alt, &sv[1+ln].brg, &sv[1+ln].sn,
		&sv[2+ln].sv, &sv[2+ln].alt, &sv[2+ln].brg, &sv[2+ln].sn,
		&sv[3+ln].sv, &sv[3+ln].alt, &sv[3+ln].brg, &sv[3+ln].sn);
		sv[0+ln].used=sv[1+ln].used=sv[2+ln].used=sv[3+ln].used=0;
	}
	if(!strncmp("$GPRMC",r,6)) {
		char ns,ew,mw,fix;
		nmea_parse(&r[7],"TcPPffDfc",
		&gmt_time,&fix,&latitude,&longitude,&speed,&hdg,&date,&mag,&mw);
	}
//		$GPGSA,A,3,04,05,06, 16,,20,24,26,,,,,2.3,1.2,1.9*38
	if(!strncmp("$GPGSA",r,6)) {
		int i;
		char auto_mode;
		for(i=0;i<12;i++)
			used[i]=0;
		dop=hdop=vdop=0;
		nmea_parse(&r[7], "ciiiiiiiiiiiiifff", &auto_mode,&rcv_status,
			&used[0], &used[1], &used[2], &used[3], &used[4],
			&used[5], &used[6], &used[7], &used[8], &used[9],
			&used[10], &used[11], &dop, &hdop, &vdop);
	}
}

// code to do the display
//
void nmea_display(unsigned char *r,int rxcount) {
	char *x=r;
/*	for(;*x;x++)	// should check checksum here! XXX
		;
*/
	switch(display) {
	case 0x80:
		_clearscreen(_GCLEARSCREEN);
		display&=0x7f;
	case 0:
		if(!strncmp("$GPBOD",r,6)) {
			_settextposition(3,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$GPGGA",r,6)) {
			_settextposition(4,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$GPGLL",r,6)) {
			_settextposition(5,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$GPGSA",r,6)) {
			_settextposition(6,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$GPGSV",r,6)) {
			_settextposition(-1+7+r[9]-'0',1);
			printf("F5  %s  \n",r);
		} else if(!strncmp("$GPRMB",r,6)) {
			_settextposition(10,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$GPRMC",r,6)) {
			_settextposition(11,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$PGRMV",r,6)) {
			_settextposition(12,1);
			printf("\"%s\"  \n",r);
		} else if(!strncmp("$GPWPL",r,6)) {
			_settextposition(12,1);
			printf("F8  %s  \n",r);
		} else if(!strncmp("$PGRME",r,6)) {
			_settextposition(13,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$PGRMM",r,6)) {
			_settextposition(14,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$PGRMZ",r,6)) {
			_settextposition(15,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$GPRTE",r,6)) {	// 15,16,17,18
			_settextposition(-1+16+r[9]-'0',1);
	//		_settextposition(16,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$GPVTG",r,6)) {	// 183 1.5
			_settextposition(20,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$GPBWC",r,6)) {	// 183 1.5
			_settextposition(21,1);
			printf("\"%s\"    \n",r);
		} else if(!strncmp("$GPXTE",r,6)) {	// 183 1.5
			_settextposition(22,1);
			printf("\"%s\"    \n",r);
		} else {
			_settextposition(23,1);
			printf("%-78.78s \n",r);
		}
		break;
	case 0x82:
		_clearscreen(_GCLEARSCREEN);
		display&=0x7f;
	case 2:
//xxxxxxxxxxxxxxxxxxxxxxxxxx
//$GPRMC,032228,A,3723.0949,N,07910.0004,W,4.2,302.8,220396,008.4,W*7A
		if(!strncmp("$GPRMC",r,6)) {
			int x;
			int y=date%100;
			int mo=(date/100)%100;
			int d=(date/10000)%100;
			int s=gmt_time%100;
			int m=(gmt_time/100)%100;
			int h=(gmt_time/10000)%100;
			_settextposition(2,1);
			printf("Speed: %.1f    %.1f\n",speed,speed*1.15);//*mph
			printf("%i/%2i/%2i %02d:%02d:%02d\n",mo,d,y,h,m,s);
			printf("%9.4f %9.4f\n",latitude,longitude);
			printf("Hdg: %.1f\n",hdg);
			printf("\"%s\"    \n",r);
			for(x=0;x<39;x++)
				if(x<speed/2)
					printf("*");
				else
					printf(" ");
			printf("\n");
			for(x=0;x<39;x++)
				if(x<speed/5)
					printf("*");
				else
					printf(" ");
			printf("\n");

printf("DOPs %f %f %f stat(%d)\n",dop,hdop,vdop,rcv_status);
		}
		break;
	case 0x85:
	case 0x83:
		_clearscreen(_GCLEARSCREEN);
		{
			int x;
			_settextposition(11,20);
			printf("*");
			for(x=0;x<360;x+=5) {
				_settextposition(
				11-(int)(cos(x*3.1415926*2/360)*7),
				20+(int)(sin(x*3.1415926*2/360)*10));
				printf(".");
			}
			for(x=0;x<360;x+=5) {
				_settextposition(
				11-(int)(.5*cos(x*3.1415926*2/360)*7),
				20+(int)(.5*sin(x*3.1415926*2/360)*10));
				printf(".");
			}
		}
		display&=0x7f;
	case 3:
	case 5:
		if(!strncmp("$GPGSV",r,6)) {
			int x;
				_settextposition(4,1);
//				for(x=0;x<svs;x++)
//					printf("Sv: %d @ (%d,%d) %d  \n",sv[x].sv,sv[x].alt,sv[x].brg,sv[x].sn);
				for(x=0;x<svs;x++) {
					int y;
					for(y=27;y<50;y+=3) {
						_settextposition(23-(y-27)/3,3*x+1);
						if(y<sv[x].sn)
							printf("*");
						else
							printf(" ");
					}
					_settextposition(23,3*x+1);
					printf("%d",sv[x].sv);
				}
				// display at (20,11)
				for(x=0;x<svs;x++) {
					_settextposition(
11-(int)(cos(sv[x].alt*3.1415926*2/360)*cos(sv[x].brg*3.1415926*2/360)*7),
20+(int)(cos(sv[x].alt*3.1415926*2/360)*sin(sv[x].brg*3.1415926*2/360)*10));
					printf("%d",sv[x].sv);
				}
				_settextposition(1,1);
				printf("DOPs %.1f h%.1f v%.1f stat(%d)\n",dop,hdop,vdop,rcv_status);
				_settextposition(24,1);
				for(x=0;x<12;x++)
					if(used[x])
						printf("%-2d ",used[x]);
					else
						printf("   ");
		}
		break;
	case 0x88:
		display&=0x7f;
		_clearscreen(_GCLEARSCREEN);
	case 8:
		if(!strncmp("$GPWPL",r,6)) {
			_settextposition(1,1);
			printf("%s   \n",r);
			parse_gpwpl(r);
		}
		break;
	default:
		break;
	}
}

struct WPT_t {
	char name[7];
	double lat,lon;
} wpt[100];
int wpts=0;
parse_gpwpl(char *r) {
	int x;
	char ns,ew;
	int found=0;
	struct WPT_t w;
//F8  $GPWPL,3816.484,N,08557.728,W,RAIN4W*28  
	sscanf(&r[7],"%lg,%c,%lg,%c,%s*",&w.lat,&ns,&w.lon,&ew,&w.name[0]);
	printf("Name: %s (%f,%f)  \n",w.name,w.lat,w.lon);
	for(x=0;x<wpts;x++) {
		if(!strcmp(w.name,wpt[x].name)) {
			wpt[x].lat=w.lat;
			wpt[x].lon=w.lon;
			found=1;
		}
		printf(" %12s  %4.4f %8.4f\n",wpt[x].name,wpt[x].lat,wpt[x].lon);
	}
	if(!found) {
		strncpy(wpt[x].name,w.name,sizeof(w.name));
		wpt[x].lat=w.lat;
		wpt[x].lon=w.lon;
		printf("ADD %12s  %4.4f %8.4f\n",wpt[x].name,wpt[x].lat,wpt[x].lon);
		printf("wpts=%d\n",wpts);
wpts++;
	}
}

void break_fn(int i) {
	exit_now=1;
}

help() {
	printf("Hit Alt-F to exit (from Alt-F X to exit like win programs)\n\n");
	printf("F1-Help\n");
	printf("F2-Basic Speed Display\n");
	printf("F3-View Sat screen\n");
	printf("F8-View Waypoints\n");
	printf("F12-Auto repeat\n");
	printf("Alt-C  Clear screen\n");
}

/* users manual:
At dos type:
c:\> n number
(the number is used by some commands for expirmental use)

Press F1 to get help.

Press Alt-F (or Ctrl-C or ctrl-break) to exit.

*/

#include <stdarg.h>
//sscanf(&r[7],"%ld,%c,%f,%c,%f,%c,%f,%f,%ld,%f,%c",
//sscan(in_buf,prototype,stuff...)
// Protocol:
// H: Header    $AAAAA
// P: Position  dddmm.mmmm,h	double   (- for south and west)
// T: Time	hhmmss		long
// D: Date	yymmdd		long
// i: integer	-32K->32K	int
// c: character	single char
// s: string
// f: float			float
// g: double			double
int __cdecl nmea_parse(const char *in_buf, const char *prototype, void *first,...) {
	int count=0;
	void *val;
	void far *val1;
	char *str;
	unsigned int u;
	va_list marker;
	va_start(marker,prototype);
	while(*prototype) {
		switch(*prototype++) {
		case 'H': // Header $AAAAA	//XXX
			val=va_arg(marker,void *);
			str=val;
			*str++='$';
			*str++=*in_buf++;	// $
			*str++=*in_buf++; *str++=*in_buf++;	// ch 1,2
			*str++=*in_buf++; *str++=*in_buf++;	// ch 3,4
			*str++=*in_buf++;			// ch 5
			*str++='\0';
			count++;
			break;
		case 'P': // Position  dddmm.mmmm  // a double
			val=va_arg(marker,void *);
			*(double *)val=atof(in_buf);
			while(*in_buf!=','&&*in_buf!='*'&&*in_buf!='\0')
				in_buf++;
			in_buf++;
//			in_buf++;
			if(*in_buf=='S'||*in_buf=='W')	// XXX is this correct?
				*(double *)val=-*(double *)val;
			break;
		case 'T': // Time	hhmmss	// a long
			val=va_arg(marker,void *);
			*(long *)val=atol(in_buf);
			break;
		case 'D': // Date	yymmdd	// a long
			val=va_arg(marker,void *);
			*(long *)val=atol(in_buf);
			break;
		case 'i': // integer	-32K->32K
			val=va_arg(marker,int *);
			*(int *)val=atoi(in_buf);
			break;
		case 'c': // character	single char
			val=va_arg(marker,void *);
			*(char *)val=*in_buf++;
			break;
		case 's': // string	//XXX
			val=va_arg(marker,void *);
			str=val;
			while(*in_buf!=','&&*in_buf!='*'&&*in_buf!='\0')
				*str++=*in_buf++;
			*str++='\0';
			break;
		case 'f': // float
			val=va_arg(marker,void *);
			*(float *)val=atof(in_buf);
			break;
		case 'g': // double
			val=va_arg(marker,void *);
			*(double *)val=atof(in_buf);
			break;
		default:
			printf("Bad value in nmea_parse\n");
			val=va_arg(marker,void *);
			break;
		}
		while(*in_buf!=','&&*in_buf!='*'&&*in_buf!='\0')
			in_buf++;
		if(*in_buf==',')	// next
			in_buf++;	// point to next char
		if(*in_buf=='*')	// checksum here
			break;		// return count
	}
	va_end(marker);
}


/*
$GPRMC,031158,V,,,,,,,240396,,*35    
$GPRMB,V,,,,,,,,,,,,V*66    
$GPGGA,031158,,,,,0,00,,,M,,M,,*68    
$GPGSA,A,1,,,,,,,,,,,,,,,*1E    
$PGRME,,M,,M,,M*00    
$GPGLL,,,,,031158,*5E    
$PGRMZ,,,*7E    
$PGRMM,WGS 84*06    
$GPBOD,,T,,M,,*47    
$GPRTE,1,1,c,0*07    
$GPRMC,031200,V,,,,,,,240396,,*3B    
$GPGSV,2,1,08,04,23,083,00,05,24,230,00,06,33,314,00,16,83,254,00*71
$GPGSV,2,2,08,18,08,038,00,20,27,263,00,24,55,055,00,26,11,179,00*76

$GPGSA,A,3,06,16,,,24,26,,,,,,,4.7,2.3,4.0*37    
$GPRMC,050739,A,3723.1073,N,07910.0890,W,0.0,326.3,240396,008.4,W*72    
$GPRMC,050747,A,3723.1028,N,07910.0874,W,0.0,326.3,240396,008.4,W*7F    
$PGRME,77.9,M,134.7,M,155.6,M*11    
$PGRME,97.7,M,,M,97.7,M*00    
$PGRMV,0.1,-0.0,-0.0*5D		//   -0.0?????
$PGRMV,0.1,0.0,-0.0*70
$PGRMV,0.2,-0.1,0.1*73
*/