gpscx.c

GPS调试程序

	
	//lcd_compass(arrow_point[2]);//
    //lcd_pressure(arrow_point[3]);//
    return (1);
  }
return (0);
}

char point_arrow1(float comp_cur)
{
  int x_arrow,y_arrow;
  char new_point=0;
  
    
      
  x_arrow=ceil(arrow_leng*sin((comp_cur-disp_rot)/180*PI))+ref_point[0]+1 ;
  y_arrow=ceil(arrow_leng*cos((comp_cur-disp_rot)/180*PI))+ref_point[1];
  new_point=(x_arrow==arrow_point1[0])&&(y_arrow==arrow_point1[1]);
  if(new_point==0)  
  {
    past_arrow1[0]=arrow_point1[0];
	past_arrow1[1]=arrow_point1[1];
	past_arrow1[2]=arrow_point1[2];
	past_arrow1[3]=arrow_point1[3];
	past_arrow1[4]=arrow_point1[4];
	past_arrow1[5]=arrow_point1[5];
  
    arrow_point1[0]=x_arrow;
    arrow_point1[1]=y_arrow;
    arrow_point1[2]=ceil(arrow_leng*0.6*sin((comp_cur-disp_rot)/180*PI+PI/6))+ref_point[0] ;
    arrow_point1[3]=ceil(arrow_leng*0.6*cos((comp_cur-disp_rot)/180*PI+PI/6))+ref_point[1];
    arrow_point1[4]=ceil(arrow_leng*0.6*sin((comp_cur-disp_rot)/180*PI-PI/6))+ref_point[0] ;
    arrow_point1[5]=ceil(arrow_leng*0.6*cos((comp_cur-disp_rot)/180*PI-PI/6))+ref_point[1];

	
	//lcd_compass(arrow_point[2]);//
    //lcd_pressure(arrow_point[3]);//
    return (1);
  }
return (0);
}

char point_arrow2(float comp_cur)
{
  int x_arrow,y_arrow;
  char new_point=0;
  
    
      
  x_arrow=ceil(arrow_leng*sin((comp_cur-disp_rot)/180*PI))+ref_point[0]-1 ;
  y_arrow=ceil(arrow_leng*cos((comp_cur-disp_rot)/180*PI))+ref_point[1];
  new_point=(x_arrow==arrow_point2[0])&&(y_arrow==arrow_point2[1]);
  if(new_point==0)  
  {
    past_arrow2[0]=arrow_point2[0];
	past_arrow2[1]=arrow_point2[1];
	past_arrow2[2]=arrow_point2[2];
	past_arrow2[3]=arrow_point2[3];
	past_arrow2[4]=arrow_point2[4];
	past_arrow2[5]=arrow_point2[5];
  
    arrow_point2[0]=x_arrow;
    arrow_point2[1]=y_arrow;
    arrow_point2[2]=ceil(arrow_leng*0.6*sin((comp_cur-disp_rot)/180*PI+PI/6))+ref_point[0] ;
    arrow_point2[3]=ceil(arrow_leng*0.6*cos((comp_cur-disp_rot)/180*PI+PI/6))+ref_point[1];
    arrow_point2[4]=ceil(arrow_leng*0.6*sin((comp_cur-disp_rot)/180*PI-PI/6))+ref_point[0] ;
    arrow_point2[5]=ceil(arrow_leng*0.6*cos((comp_cur-disp_rot)/180*PI-PI/6))+ref_point[1];

	
	//lcd_compass(arrow_point[2]);//
    //lcd_pressure(arrow_point[3]);//
    return (1);
  }
return (0);
}


/*流速系数的自适应更新*/
/*void adaptive_coe(void)
{
 	 float C_adap,K_adap,e0,e1,S0,S1;
	 long n;
	 C_adap=C_vel;K_adap=K_vel;
	 S0=dist[1]-wave_vel[0]*R_compass/f_compass;
	 S1=dist[3]-wave_vel[1]*R_compass/f_compass;
	 for (n=0;n<200;n++)
	 {
	  e0=S0-C_adap*V_coe[0]-K_adap*V_coe[2];
	  C_adap=C_adap+2*mu*e0*V_coe[0];
	  K_adap=K_adap+2*mu*e0*V_coe[2];
	  e1=S1-C_adap*V_coe[1]-K_adap*V_coe[3];
	  C_adap=C_adap+2*mu*e1*V_coe[1];
	  K_adap=K_adap+2*mu*e1*V_coe[3];
	  if (fabs(C_adap-C_vel)/C_vel>0.05 && fabs(K_adap-K_vel)/K_vel>0.05)  break;
	 }
	 C_vel=C_adap;
	 K_vel=K_adap;
}*/

/*获得过去的连续三个GPS*/
/*void get_pastxyz(void)
{
  signed char index,k_d,k_up;
  k_up=up_i;
  for (k_d=0;k_d<3;k_d++) 
  {
 	if (k_up==0) k_up=19;
	else k_up--;
    xyz_d[k_d][0]=xyz_cache[k_up][0];
	xyz_d[k_d][1]=xyz_cache[k_up][1];
	xyz_d[k_d][2]=xyz_cache[k_up][2];
  }
}*/


/*剔除GPS野值*/
/*void remove_errxyz(void)
{
 float dst;
 float dstx,dsty;
  char k_ture=10;
  dstx=xyz_d[0][0]-xyz_d[1][0];
  dsty=xyz_d[0][1]-xyz_d[1][1];
  dst=SQRT(dstx*dstx+dsty*dsty);
  if (dst<5)  
  {
    k_ture=0;
    dstx=xyz_d[0][0]-xyz_d[2][0];
    dsty=xyz_d[0][1]-xyz_d[2][1]; 
    dst=SQRT(dstx*dstx+dsty*dsty);
  }
  else if (dst<5)
  {
    k_ture=0;
	dstx=xyz_d[1][0]-xyz_d[2][0];
    dsty=xyz_d[1][1]-xyz_d[2][1]; 
    dst=SQRT(dstx*dstx+dsty*dsty);
  }
  else if (dst<5)
  {
  k_ture=1;
  }
  else 
  {
  xyz_m[0]=xyz_d[0][0]/3+xyz_d[1][0]/3+xyz_d[2][0]/3;
  xyz_m[1]=xyz_d[0][1]/3+xyz_d[1][1]/3+xyz_d[2][1]/3;
  xyz_m[2]=xyz_d[0][2]/3+xyz_d[1][2]/3+xyz_d[2][2]/3;
  }
  if (k_ture<10)
  { 
  xyz_m[0]=xyz_d[k_ture][0];
  xyz_m[1]=xyz_d[k_ture][1];
  xyz_m[2]=xyz_d[k_ture][2];  
  }
  
}*/








//
//```````````````````````````````````````````````````````````````````````````
/// 硬件子程序
///、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、
///

void p_double_char(float a)
{
	unsigned int temp;
	float temp1;
	temp1=a;
	temp=a;
	buf_double_char[0]=temp/100;
	buf_double_char[1]=temp%100;
	temp1 =(temp1-temp)*100;
	buf_double_char[2]=temp=temp1;		//浮点数转换成字符用于USB存储
	buf_double_char[3]=(temp1-temp)*100;		//浮点数转换成字符用于USB存储
}

 float lcd_bit4dis(unsigned char lcd_ds,unsigned char lcd_dy,float fdata)
{
	unsigned int i;
	unsigned int p;
	static unsigned char buf_lcd[4];
	i=fdata;
	p=i%1000;
	buf_lcd[0]=i/1000;
	buf_lcd[1]=p/100;
	buf_lcd[2]=(p%100)/10;
	buf_lcd[3]=(p%100)%10;

	lcd_display(lcd_ds,lcd_dy,buf_lcd,4);
	return (fdata-i);
}



void ADprocess(void)
{
  unsigned char f,g,num;
  unsigned int h;
  num=5;
   for(f=1;f<=num-1;f++)
   {
       for(g=0;g<=num-f;g++)
	   {
           if(AD_chushi[g]>AD_chushi[g+1]) 
		   {
		   h=AD_chushi[g];
		   AD_chushi[g]=AD_chushi[g+1];
		   AD_chushi[g+1]=h;
		   }
		}
	}
	  
ADval=(AD_chushi[1]+AD_chushi[2]+AD_chushi[3])/3;
if(!power_first_flag) power_first_flag=1;
else power_first_flag=2;

}

double GPSprocess(double *pro,unsigned char num)
{
  unsigned char f,g;
  double h;
   for(f=1;f<=num-1;f++)
   {
       for(g=0;g<=num-f-1;g++)
	   {
           if(pro[g]>pro[g+1]) 
		   {
		   h=pro[g];
		   pro[g]=pro[g+1];
		   pro[g+1]=h;
		   }
		}
	}
	  
return pro[4];
}




void Powerclr(void)
{
unsigned char z,x,y;
lcd_stateadd=d_lcdini_state_word|0x01;					//电源，先设置lcd写寄存器的状态值
	 
  lcd_xadd=4;								
  lcd_yadd=z=194;
	
	for (y=0;y<41;y++)
	{
	for (x=0;x<14;x++) lcd_dataadd=d_darkc;
	lcd_yadd=z=z-1;
	lcd_xadd=4;
	}

}

/*******************************************************/
void Powerdis(void)
{

unsigned char z,t,x,y;
lcd_stateadd=d_lcdini_state_word|0x01;					//电源，先设置lcd写寄存器的状态值
	
if (power==1){   
  lcd_xadd=4;								
  lcd_yadd=z=164;
	
	for (y=0;y<10;y++)
	{
	for (x=0;x<14;x++) lcd_dataadd=dianliang[y][x];
	lcd_yadd=z=z-1;
	lcd_xadd=4;
	}
	lcd_yadd=z=z-1;
  }
else if (power==2){
    lcd_xadd=4;								
    lcd_yadd=z=174;
	for (t=0;t<2;t++)
	{   for (y=0;y<10;y++)
		{
		for (x=0;x<14;x++) lcd_dataadd=dianliang[y][x];
		lcd_yadd=z=z-1;
		lcd_xadd=4;
		}
		lcd_yadd=z=z-1;
	}
}
	
else if (power==3){
    lcd_xadd=4;								
    lcd_yadd=z=184;
	for (t=0;t<3;t++)
	{
	for (y=0;y<10;y++)
		{
		for (x=0;x<14;x++) lcd_dataadd=dianliang[y][x];
		lcd_yadd=z=z-1;
		lcd_xadd=4;
		}
	lcd_yadd=z=z-1;	
   }
}

else if (power==4){

    lcd_xadd=4;								
    lcd_yadd=z=194;
	for (t=0;t<4;t++)                     //修改
	{
		for (y=0;y<10;y++)
		{
		for (x=0;x<14;x++) lcd_dataadd=dianliang[y][x];
		lcd_yadd=z=z-1;
		lcd_xadd=4;
		}
	lcd_yadd=z=z-1;
	}
}
}
/****************************************************/
//添加AD//
/****************************************************/

void dianxian(unsigned char power)
{
Powerclr();
Powerdis();
}



/****************************************************/
//添加AD//
/****************************************************/


/****************************************************/
//初始化GPS接收板
/****************************************************/

/*void put_char1(unsigned char C)
{
while(!(UCSR0A&(1<<UDRE0)));
UDR0=C;

}
void put_s1( unsigned char*ptr)

{

while(*ptr)
 {
  
  put_char1(*ptr++);

 }
put_char1(0x0D);
put_char1(0x0A);

}*/
void GPS_ini(void)
{
/*unsigned char flag_RETR=1;
unsigned char K;*/
UCSR0B=(1<<TXEN0);
/*while(flag_RETR) 
{*/
  
  
  //put_s(NMEB);
 // put_s1(JGG100_GGA);
  delay(100);
  //put_s(PMD);
  // delay(100);
  //put_s(NMEA);
   //delay(100);
 // put_s(SAV);
/*UCSR0B=(1<<RXEN0);
 flag_gpsrhead=false;
 while(!flag_gpsrhead) 
 {
  while(!(UCSR0A&(1<<RXC0)));
    {
   K=UDR0;
    if(K==0x24)
      { gps_ctdohao=4;}
    else
       {
         gps_ctdohao--;
        if(gps_ctdohao==0)
          {
           if(K=='G')
             {
              flag_gpsrhead=TRUE;
              flag_RETR=0;
             }
           else
             {
             flag_gpsrhead=TRUE;
             }
          }
        }
       }
        
     }
 }*/
 UCSR0B=(1<<RXEN0)|(1<<RXCIE0);
// flag_gpsrhead=false;

}

double filter1(double *pro,unsigned char num)  //东北天坐标滤波
{
  unsigned char f,g;
  double h;
  
   for(f=1;f<=num-1;f++)
   {
       for(g=0;g<=num-f-1;g++)
	   {
           if(pro[g]>pro[g+1]) 
		   {
		   h=pro[g];
		   pro[g]=pro[g+1];
		   pro[g+1]=h;
		   }
		}
	}
 
  return pro[5];
}


/*void filter2(void)  //东北天坐标滤波
{ 
  if (fabs(xyz[0]-x_temp[0])<20) xyz[0]=(xyz[0]+x_temp[0])*0.5;
  else xyz[0]=x_temp[0]=x_temp[0]*0.8+xyz[0]*0.2;
  if (fabs(xyz[1]-y_temp[0])<20) xyz[1]=(xyz[1]+y_temp[0])*0.5;
  else xyz[1]=y_temp[0]=y_temp[0]*0.8+xyz[1]*0.2;
  if (fabs(xyz[2]-z_temp[0])<20) xyz[2]=(xyz[2]+z_temp[0])*0.5;
  else xyz[2]=z_temp[0]=z_temp[0]*0.8+xyz[2]*0.2;
}*/



void main(void)
{  
    unsigned char uch_power;   //电量暂存
	//unsigned char *point;
     char o;
   flag_check_gps=0;
	 SREG=0;
	 
//disp_x=20;
//disp_y=30;
	 
//calc_arrow ();	 
	 WDTCR=(1<<WDCE)|(1<<WDE);
	 WDTCR=(0<<WDE);
	 MCUCR|=(1<<SRE);		  			//|(1<<SRW10);
//	 XMCRA|=(1<<SRW11);//(1<<SRW01)|
	ini_rob();
	power=4;//电量显示
	ADval=830;
	dianxian(power);
	delay(5000);
	GPS_ini();
//	ptr_proc();
	SREG=0x80;
	
	//lcd_pwrbright();					//点亮LCD工作灯