complex.txt

通过此组合程序可以进行组合导航系统的设计。

#include <string.h>
#include <math.h> 

#define BUFF_SIZE 1024

char Buffer[BUFF_SIZE];
long BufferCounter = 0;
extern int GPSdata;  
int jjjj=0;

char strTmp[180];
char temp[200];
long i; 
	
	
struct GPSInfo{
	long NS;
	double Time;
	double H;
	double Heading;
	double Lat;
	double Long;
	double Ve;
	double Vn;
	double V;
	int fNavData;
	int fGPS;
	long invalidNum;
	int fReset;   
	int ResetNum;
} GPS;

extern void interrupt_NewCom();   
void GPSDecode();

    
extern void interrupt_NewCom()  
{   
	jjjj++;
	Buffer[BufferCounter++]=GPSdata;
	if(GPSdata== '\r')
	{    

		GPSDecode();
		BufferCounter=0;
		GPS.fGPS=1; 
		jjjj=0;
	}
 }

void GPSDecode()
{     
	int counter=0;
	double c1,c2;
	double angle;
	BufferCounter=0; 
	
	
	
	
	/*寻找起始点*/
	while(1)
	{    
		if(Buffer[BufferCounter++]=='$')
			break;
		counter++;
		if(counter>100)
			return;
	}  
	
	/*航向*/
	counter=0;
	while(counter<4)
	{
		if(Buffer[BufferCounter++]==',')
			counter++;
	}
	c1=(Buffer[BufferCounter++]-48)*100.0;
	c1+=(Buffer[BufferCounter++]-48)*10.0;
	c1+=(Buffer[BufferCounter++]-48);
	BufferCounter++;
	c2=(Buffer[BufferCounter++]-48)/10.0;
	c2+=(Buffer[BufferCounter++]-48)/100.0;
	GPS.Heading=c1+c2;
	
	/*航速*/
	counter=0;
	while(counter<1)
	{
		if(Buffer[BufferCounter++]==',')
			counter++;
	}
	c1=(Buffer[BufferCounter++]-48)*100.0;
	c1+=(Buffer[BufferCounter++]-48)*10.0;
	c1+=(Buffer[BufferCounter++]-48);
	BufferCounter++;
	c2=(Buffer[BufferCounter++]-48)/10.0;
	c2+=(Buffer[BufferCounter++]-48)/100.0;
	GPS.V=(c1+c2)*1.852/3.6;
	angle=GPS.Heading*0.0174532925;
	GPS.Vn = GPS.V*cos(angle);
	GPS.Ve = GPS.V*sin(angle);
	if(GPS.invalidNum>5)
	  {
		GPS.fReset=1; 
	
	  }
        if(GPS.ResetNum>10)
	  {
		GPS.fReset=0;
		GPS.ResetNum=0;
	  } 

	  	GPS.invalidNum=0;
	  
	 
}