nmea.c

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

#define VER	1.0.6
#define COM_PORT	2	// 2=com2 here only!
/*
GPS25 spits out: (by default, others can be tured on)
PGRME
GPRMC
GPGSA
GPGSV up to 3 lines
	
$PGRMV,-2.0,1.3,3*72
        east speed m/s, north speed, up speed
$PGRME,64.6,M,108.3M,126.1M*14
	horiz pos err in meters, vert pos in meters, est post in meters.
$GPRMC,032228,A,3723.0949,N,07910.0004,W,4.2,302.8,220396,008.4,W*7A
	time,A=val v=err,lat,lon,speed kts,course,date,mag var
$GPGSA,A,3,04,05,06,16,,20,24,26,,,,,2.3,1.2,1.9*38
	mode a=auto m=man,2=2d 3=3d 1=none,12 SV nums, DOP,HDOP, VDOP
$GPGSV,2,1,08,04,22,084,34,05,23,228,43,06,34,313,41,16,84,267,46*79
	no records,rec no,total SV, SV#, elevation, azimuth,S/N ratio,...
$GPGSV,2,2,08,18,08,037,36,20,26,261,41,24,55,055,46,26,12,179,35*7D
...
*/

//#define debug 01	// dump the hex values of TX and RX
#define debug 0
/*  search for XXX for things that need fixed */
// GRMN/GRMN protocol display program
//
// This works under MSC v7.0.  good luck with anything else.
// Written by Tim Hogard  9/20/95
//    This code was developed based on info from:
//	Bernard Greening <[e-mail]>  (rs-232 code)
//	Others that wish to remain anonymous.
// This program is copy right 1996 by Tim Hogard
//	You may not make copies of this.
//	I don't want this early release being widespread.
//	You can use this for personal use (or ask me).
// Latter versions of this program will be in the public domain.
// 	If you wish to help with this please let me know.
// Find a new version at http://www.abnormal.com/~thogard
// or http://www.inmind.com/~thogard
// email:  [e-mail]
// Ver: Mon May 05 1.0.6
//    Added better sat status, dop parsing
//    Inproved nmea_parse()
// Ver: Mon Apr 29 1.0.5
//    Added Com1,3,4 support -- untested! XXX
// Ver: Tue Mar 26
//    Fixed MPH speed bug
// Ver: Mon Mar 25
//    Made seperate display and parse routines
// Ver: Wed Mar 21
//    Converted to NMEA from GRMN protocol
// Ver: Mon Mar 04 22:35:16 EDT 1996
//    Added new screen layout for fields
//    added a few new packets
//    added coments for work to do.
// Ver: Wed Sep 20 09:39:59 EDT 1995
//    Inital release
//
// To exit program, type Alt-F (Windows style for Alt-F,x)
// Program works fine in 40 column mode (type "mode co40" at
//	the dos prompt; mode co80 to get back to normal). This
//	mode works nice for laptops in cars so it's readable.
// Program only works on COM2: right now.
//
// Build using:
// cl n.c graphics.lib

#include <stdio.h>
#include <stdlib.h>
#include <dos.h>
#include <graph.h>		// requires graphics.lib
#include <signal.h>		// requires graphics.lib
#include <math.h>		// for trig

double atof(const char *);

/************************ Serial drivers *********************************/

#ifdef BORLAND  // adjust a few things for Borloand... 
/**** untested XXX on borland ****/
#define _enable() enable()
#define _disable() disable()
#define _inp(x) inportb(x)
#define _outp(x,y) outportb(x,y)
#endif	// #def BORLAND

//----------------------------
/*      Offsets to various 8250 registers.  Taken from IBM Technical         */
/*      Reference Manual, p. 1-225                                           */

#define TXBUF   0                       /* Transmit buffer register */
#define RXBUF   0                       /* Receive buffer register */
#define DLLSB   0                       /* Divisor latch LS byte */
#define DLMSB   1                       /* Divisor latch MS byte */
#define IER     1                       /* Interrupt enable register */
#define IIR     2                       /* Interrupt ID register */
#define LCR     3                       /* Line control register */
#define MCR     4                       /* Modem control register */
#define LSR     5                       /* Line status register */
#define MSR     6                       /* Modem status register */

#define INT_CTRL 0x20                    /* 8259A control port address */
#define INT_MASK 0x21                    /* 8259A interupt enable port addr */
#define EOI     0x20                    /* 8259A EOI command */
/// stuff above here should go in a .h file

// code below works on a standard com2 only.
// INT is the dos interrupr vector number
#define INT	0xb

void break_fn(int);
void (cdecl interrupt far *old_int[4])();
int __cdecl nmea_parse(const char *, const char *, ...);
int count=0;
int auto_repeat=0;
int display=0x80;	// which screen to display
			// 0x8? clears and repaints page
			// 2 basic display
			// 3 satellite screen

//port settings:  com1: com2  com3  com4
// com1 is 0x3f8; com2 is 0x2f8; com3 0x3e8; com4 0x2e8
//int port_addr[4]={0x2f8,0x2f8,0x2f8,0x2f8};	// all com2 for now
int port_addr[4]={0x3f8,0x2f8,0x3e8,0x2e8};	//com1,2,3,4
//????1011	XXX why only low nibble?
// com2/4: 11110111(f7)  com1/3: 11111011(fb)
//int port_bit[4]= { 0xf7, 0xf7, 0xf7, 0xf7};
//int port_bit[4]= { 0xf7, 0xfb, 0xf7, 0xfb};
int port_bit[4]= { 0xfb, 0xf7, 0xfb, 0xf7};
enum port_baud {b150=0x300,b300=0x1c0,b600=0xc0,b1200=0x60,b2400=0x30,
b4800=0x18,b9600=0xc,b192=0x7fe9};

char rx_buf[4][256],*rx_ptr[4];
parse_gpwpl(char *r);
void nmea_decode(unsigned char *r,int rxcount);
void nmea_display(unsigned char *r,int rxcount);

/* globals for all displays */
int svs;	// number of space vehicles
long gmt_time,date;
double latitude=0,longitude=0;
float speed=0,hdg=0,mag=0;
float dop,hdop,vdop;
int used[12];
int  rcv_status;	// 1=none, 2=2d, 3=3d
com_port=COM_PORT-1;	// 0=com1, 1=com2, 2=com3, 4=com4

struct SV_t {
	int sv;		// sat number
	int alt;	// altitude in degrees
	int brg;	// bearing
	int sn;		// signal to noise
	int used;	// is it used in current solution
} sv[32];

// This is the basis for the ISR code.
#define ISR_PROTOTYPE(y)  { 					\
	int x=y; 						\
	int s;							\
	s=inp(port_addr[x]+IIR);				\
	*rx_ptr[x]++=inp(port_addr[x]+RXBUF);			\
	outp(INT_CTRL,EOI);					\
	if(rx_ptr[x]>rx_buf[x]+sizeof(rx_buf[x])-1)		\
		rx_ptr[x]=rx_buf[x];				\
	_chain_intr(old_int[x]);				\
}		

// We must have one ISR for each interrupt since there is no way
// to pass parameters into the ISR function.
// This should be 4 functions that call one common function but
// this does not work reliably on all PCs with all compilers so
// its done the hard way.
void interrupt far isr_0() ISR_PROTOTYPE(0)
void interrupt far isr_1() ISR_PROTOTYPE(1)
void interrupt far isr_2() ISR_PROTOTYPE(2)
void interrupt far isr_3() ISR_PROTOTYPE(3)

// codes hiding in the arrays:
// EOM: end of message
// CHK_SUM: put check sum here
// START_CHK_SUM: start checksum here
// USE_ARGV: argv[1]
enum { EOM=-4, CHK_SUM, START_CHK_SUM, USE_ARGV };  // Must not be 0..255

int out_init_value[]={EOM};
int send_wpt[]={'$','P','G','W','P','L',',','3','7','2','2','.','3','7',
'7',',','N',',','0','7','9','1','0','.','4','9','5',',','W',',','B','*',
'1','B',0xa,0xd,EOM};

//char *months[]={"no0","Jan","Feb","Mar","Apr","May","Jun","Jul","Aug",
//"Sep","Oct", "Nov","Dec"};

char rxbuf[256]; int rxcount;
int *out=out_init_value;// set this var to output a message
int exit_now=0;		// set to 1 to end
//int columns;		// number of text columns

ser_open(int x)	{	// x will be 1 for com 2; for now ignore
	union REGS reg;
reg.h.al=0xe3;	// 1110 0011  E3
reg.h.ah=0;
reg.x.dx=1;
int86(0x14, &reg, &reg); // this may help windows know were using the port
	_disable();
	old_int[com_port]=_dos_getvect(INT);
	switch(com_port) {
	case 0: _dos_setvect(INT,isr_0); break;
	case 1: _dos_setvect(INT,isr_1); break;
	case 2: _dos_setvect(INT,isr_2); break;
	case 3: _dos_setvect(INT,isr_3); break;
	}
	outp(port_addr[com_port]+LCR,0x80);	// get at baud rate
	outp(port_addr[com_port]+DLLSB,b4800);
	outp(port_addr[com_port]+DLMSB,b4800>>8);
	outp(port_addr[com_port]+LCR,0x3);	// 8 bits, 1 st, np
	outp(port_addr[com_port]+IER,0x1);	// turn on Rx Interrupt
/****** there is a second 8259A for irq's 2, 8->15  XXX*******/
	outp(INT_MASK,inp(INT_MASK)&port_bit[com_port]);	// turn on 8259A irq
	outp(port_addr[com_port]+MCR,inp(port_addr[com_port]+MCR)|0x9);// Modem control register
	inp(port_addr[com_port]+RXBUF);	// flush buffer
	inp(port_addr[com_port]+RXBUF);	// flush buffer
	_enable();
}

bail() {
	_disable();
	outp(port_addr[com_port]+IER,0x0);	// turn off interrupts
	outp(INT_MASK,inp(INT_MASK)|~port_bit[com_port]);	// turn off 8259A irq
	_dos_setvect(INT,old_int[com_port]);
	_enable();
	_settextposition(24,1);
	exit(0);
}

/* GraphicsLib ******************************************************/
/*  This is here to replace microsoft's graphics.lib 
int get_cols() {		// returns graphics.lib number of columns
	struct _videoconfig vc;
	_getvideoconfig(&vc);
	return(vc.numtextcols);
}
enum {_GCLEARSCREEN} clrscrnenum;
_clearscreen(enum clrscrnenum) {
	//_GCLEARSCREEN);
}
_settextposition(y,x) {
	// y=1->25
	// x=1->80 or 1->40
}
*/

main(int argc, char **argv) {
	int c;
	int chksum=0;
	int rxcount=0;
	char *rptr;
_clearscreen(_GCLEARSCREEN);
_settextposition(20,1);
printf("Opened Com%d:\nHit Alt-F to exit\nHit F1 for help\n",com_port+1);
	rptr=rx_ptr[com_port]=rx_buf[com_port];

	signal(SIGINT,break_fn);
	signal(SIGILL,break_fn);
	signal(SIGSEGV,break_fn);
	signal(SIGTERM,break_fn);
	signal(SIGABRT,break_fn);

	ser_open(2);	// init com2:
/**** this loop is a simple multi-task loop *****/
/**** 1st dump out the output buffer        *****/
/**** 2nd get keyboard input                *****/
/**** 3rd parse input buffer                *****/
	while(1) {
		if(*out!=EOM) {
			if(inp(port_addr[com_port]+LSR)&0x20) {
			//***** need check here to see if last Tx is done
			// Check bit 0x20 of port 0x2fd before Tx
			if(*out==USE_ARGV) {	// atoi number...
				if(debug) printf("argv(%x)\n",atoi(argv[1]));
				chksum+=atoi(argv[1]);
				outp(port_addr[com_port]+TXBUF,atoi(argv[1]));
			} else if(*out==CHK_SUM) { // GRMN Checksum
				if(debug) printf("chk(%02x)\n",256-(chksum&0xff));
				outp(port_addr[com_port]+TXBUF,256-(chksum&0xff));
			} else if(*out==START_CHK_SUM) {
				chksum=0;
			} else {
				chksum+=*out;
				outp(port_addr[com_port]+TXBUF,*out);
				if(debug) printf("%02x ",*out);
			}
			out++;
			}
		}
		if(exit_now) {		// set by control-C (ctrl-break)
			bail();
		}
		while(kbhit()) {
			c=getch();
			if(c==0) {
				int x;
				c=getch();
				/* case #'s are key press codes not ascii */
				switch(c) {
				case 46: 	// alt-C (clear screen)
					display|=0x80;//set bit to clear page
					break;
				case 33: 	// alt-F (File menu)
					bail(); break;
				case 134:	// F12 Toggle repeat
					auto_repeat=!auto_repeat; break;
				case 59:	// F1 - help
					help(); break;
				case 60:	// F2 - init
					_clearscreen(_GCLEARSCREEN);
					display=2;
					break;
				case 61:	// F3 - init
					_clearscreen(_GCLEARSCREEN);
					display=0x83;
					break;
				case 62:	// F4 - 
				case 63:	// F5 - 
					_clearscreen(_GCLEARSCREEN);
					display=0x85;
					break;
				case 64:	// F6 -
				case 65:	// F7 -
				case 66:	// F8 - waypoint list (route)
					_clearscreen(_GCLEARSCREEN);
					display=0x88; break;
				case 67:	// F9 - nack
				default:
					_settextposition(24,1);
					printf("Key 0,%d pressed\n",c);
				}
			} else
			if(c==27) {	// ESC
				if(display!=0) {
					display=0x80;
					//_clearscreen(_GCLEARSCREEN); break;
				}
			} else
				outp(port_addr[com_port]+TXBUF,c);
		}
		while(rptr!=rx_ptr[com_port]) {
			c=*rptr++;
			if(rptr>rx_buf[com_port]+sizeof(rx_buf[com_port])-1)
				rptr=rx_buf[com_port];	// overflow -- reset
//			printf("%c",c);
			if (debug) printf("%02x %c ",c,c>=' '?c:'?',count);
			if(c=='$') {
				rxcount=0;
//				printf("Start:\n");
			}
			if(c!=0xa && c!=0xd) {
				rxbuf[255&rxcount++]=c;
//				printf("%c",c);
			}
			if(c==0xd) {
				rxbuf[(255&rxcount)]='\0';
				// decode data
				nmea_decode(rxbuf,rxcount);
				// display -- sometimes realtime data
				nmea_display(rxbuf,rxcount);
//				printf(": End\n");
			}
		}
	}
}

// code to decode the NMEA data.
//
void nmea_decode(unsigned char *r,int rxcount) {
	if(!strncmp("$GPGSV",r,6)) {
		int lines, ln;