📄 gpscx.c
字号:
//存储目标数据进USB
time_start(); //start speed check
flag_adcrxed=0;
flag_second_int=0;
while (1)
{
if (flag_gps_perrxdata)
{
flag_gps_perrxdata=0;
if (flag_gps_have) gps_rxproc(); //如受到GPS数据,转入处理
else
{
PORTF |=led_supwater;
LBH_Current[0]=LBH_Current[1]=0;
lcd_upwdark_led(); //如GPS不能定位,灭LCD的GPS指示灯,
}
}//flag_gps_perrxdata
//if (flag_adcrxed) chk_power ();
if (flag_second_int) //有板上时钟秒中断
{
flag_second_int=0;
//flag_storge_usb=1; //置每秒USB存储数据标志
o=clk_read(); //read clk_time //读板上时钟时间
AD_power=ADval/1024*15 ;
// lcd_bit3dis(100,135,AD_power); //测试用
// lcd_bit3dis(140,135,ADval);
if (AD_power>12) uch_power=4;
else if (AD_power>11.5) uch_power=3;
else if (AD_power>11) uch_power=2;
else if (AD_power>10.5) uch_power=1;
else if (AD_power<=10) uch_power=0;
if(power_first_flag==2)
{ if(power!=uch_power)
{
power=uch_power;
if (power==0) Powerclr();
else dianxian(uch_power);
}
}
lcd_stateadd=d_lcdini_state_word&0xfe;
// lcd_bit3dis(18,28,AD_power*10);
I2CStop();
TWCR=0;
ADCSRA |=0x40; //启动A/D
lcd_time();
if (Current_i==0) compassdata_cal(compass_data[49]);
else compassdata_cal(compass_data[Current_i-1]);
lcd_compass(compass_m);
} //每秒LCD上显示时间
//lcd_drawline (20,30,32,42,d_redc);
//lcd_drawline (32,42,28,40,d_redc);
//lcd_drawline (32,42,36,43,d_redc);
//disp_x=20;
//disp_y=30;
//calc_arrow();
//dx1=dx2;
//dy1=dy2;
//dy2 +=4;
/*
if (dx2>=70)
{
dx2=dx2-1;
f_dr=1;
}
if (f_dr) dx2 -=1;
else
*/
//dx2 -=1; //模拟画线
// ADCSRA|=0x40; //启动水深A/D转换
//flag_second_int
if (flag_speed_checked) //检测流速时间到
{
time_stop(); //计算出流速的脉冲个数及时间
time_start();
flag_speed_checked=0; //重新启动流速检测
}//flag_speed_checked
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//lcd_bit4dis(80,60,N_circle);
//
//lcd_bit3dis(140,100,Targ_load==1);
//lcd_stateadd=d_lcdini_state_word&0xfe;
//lcd_bit2disj(18,20,flag_stage);
if (flag_stage==1&&Targ_load==1)//赋初值为1
{
N_circle++;
if (N_circle%300==0)
{
N_circle=0;
if (Current_i==0)
// {lcd_bit4dis(180,100,compass_data[49]);
compassdata_cal(compass_data[49]);
else
//lcd_bit4dis(180,100,compass_data[Current_i-1]);
compassdata_cal(compass_data[Current_i-1]);
// lcd_bit4dis(220,100,compass_m);
is_new_point=point_arrow(compass_m);/*计算箭头的坐标*/
if (is_new_point)
{
/*将电子罗盘的读数以箭头形式显示到绘图区*/
lcd_drawline(ref_point[0],ref_point[1],past_arrow[0],past_arrow[1],0);
lcd_drawline(past_arrow[0],past_arrow[1],past_arrow[2],past_arrow[3],0);
lcd_drawline(past_arrow[0],past_arrow[1],past_arrow[4],past_arrow[5],0);
lcd_drawline(ref_point[0],ref_point[1],arrow_point[0],arrow_point[1],d_greencolor_lcd);
lcd_drawline(arrow_point[0],arrow_point[1],arrow_point[2],arrow_point[3],d_greencolor_lcd);
lcd_drawline(arrow_point[0],arrow_point[1],arrow_point[4],arrow_point[5],d_greencolor_lcd);
is_new_point=0;
}
is_new_point=point_arrow1(compass_m);/*计算箭头的坐标*/
if (is_new_point)
{
/*将电子罗盘的读数以箭头形式显示到绘图区*/
lcd_drawline(ref_point[0]+1,ref_point[1],past_arrow1[0],past_arrow1[1],0);
lcd_drawline(past_arrow1[0],past_arrow1[1],past_arrow1[2],past_arrow1[3],0);
lcd_drawline(past_arrow1[0],past_arrow1[1],past_arrow1[4],past_arrow1[5],0);
lcd_drawline(ref_point[0]+1,ref_point[1],arrow_point1[0],arrow_point1[1],d_greencolor_lcd);
lcd_drawline(arrow_point1[0],arrow_point1[1],arrow_point1[2],arrow_point1[3],d_greencolor_lcd);
lcd_drawline(arrow_point1[0],arrow_point1[1],arrow_point1[4],arrow_point1[5],d_greencolor_lcd);
is_new_point=0;
}
is_new_point=point_arrow2(compass_m);/*计算箭头的坐标*/
if (is_new_point)
{
/*将电子罗盘的读数以箭头形式显示到绘图区*/
lcd_drawline(ref_point[0]-1,ref_point[1],past_arrow2[0],past_arrow2[1],0);
lcd_drawline(past_arrow2[0],past_arrow2[1],past_arrow2[2],past_arrow2[3],0);
lcd_drawline(past_arrow2[0],past_arrow2[1],past_arrow2[4],past_arrow2[5],0);
lcd_drawline(ref_point[0]-1,ref_point[1],arrow_point2[0],arrow_point2[1],d_greencolor_lcd);
lcd_drawline(arrow_point2[0],arrow_point2[1],arrow_point2[2],arrow_point2[3],d_greencolor_lcd);
lcd_drawline(arrow_point2[0],arrow_point2[1],arrow_point2[4],arrow_point2[5],d_greencolor_lcd);
is_new_point=0;
}
}
/*初始化阶段，自电源打开至接收到第一个GPS信号*/
if (is_new_gps==1) /*判断是否接收到第一个GPS信号*/
{
Done_i=Current_i;/*将最近的电子罗盘序列号置为处理序列的起始号*/
LBH_ini[0]=LBH_Current[0];/*保存初始GPS信息，LBH_ini[0]为纬度,LBH_ini[1]为经度,LBH_ini[2]为高度,LBH_ini[3]为时间*/
LBH_ini[1]=LBH_Current[1];
LBH_ini[2]=LBH_Current[2];
LBH_ini[3]=LBH_Current[3];
GPS_t[1]=LBH_Current[3];
GPS_t[0]=LBH_Current[3];
sin_cos(LBH_ini,sin_cos_ini);/*初始点的正弦和余弦值最后两位有效数字有误*/
LBH2XYZ(LBH_ini,XYZ_ini); //经纬度坐标转换为东北天坐标
location_data[0].GPS_time=LBH_ini[3]; /*GPS时间*/
location_data[0].GPS_x=0;
location_data[0].GPS_y=0;
location_data[0].GPS_z=0;
location_data[0].state=0;
location_data[0].Relative_x=0;
location_data[0].Relative_y=0;
p_current_loc=1;
is_new_gps=0;
//LBH2xyz(XYZ_ini,sin_cos_ini,xyz_Targ);/*计算目标点的东北天坐标*/
LBH2xyz(LBH_Targ,xyz_Targ);
/*接收到第一个GPS信号后，计算出目标点与起始点的总距离total_dist*/
total_dist=SQRT(xyz_Targ[0]*xyz_Targ[0]+xyz_Targ[1]*xyz_Targ[1]);
/*计算显示转换的角度*/
display_rotate(xyz_Targ,&disp_rot);
//xyz_cache[0][0]=0;xyz_cache[0][1]=0;xyz_cache[0][2]=0;
//xyz_i=1;
flag_stage=2;
}
} //stage "1"
else if (flag_stage==2)
{
//lcd_pressure(SQRT(wave_vel[0]*wave_vel[0]+wave_vel[1]*wave_vel[1]));
//PORTD|=0x10;//亮红灯 //0xDF:亮绿灯
/*在绘图区注明电子罗盘的读数*/
N_circle++;
if (N_circle%300==0)
{
N_circle=0;
if (Current_i==0)
compassdata_cal(compass_data[49]);
else
compassdata_cal(compass_data[Current_i-1]);
is_new_point=point_arrow(compass_m);/*计算箭头的坐标*/
if (is_new_point)
{
/*先擦除原来的航行方向，在绘图区的方向显示区显示航行方向；*/
lcd_drawline(ref_point[0],ref_point[1],past_arrow[0],past_arrow[1],0);
lcd_drawline(past_arrow[0],past_arrow[1],past_arrow[2],past_arrow[3],0);
lcd_drawline(past_arrow[0],past_arrow[1],past_arrow[4],past_arrow[5],0);
lcd_drawline(ref_point[0],ref_point[1],arrow_point[0],arrow_point[1],d_greencolor_lcd);
lcd_drawline(arrow_point[0],arrow_point[1],arrow_point[2],arrow_point[3],d_greencolor_lcd);
lcd_drawline(arrow_point[0],arrow_point[1],arrow_point[4],arrow_point[5],d_greencolor_lcd);
is_new_point=0;
}
is_new_point=point_arrow1(compass_m);/*计算箭头的坐标*/
if (is_new_point)
{
/*将电子罗盘的读数以箭头形式显示到绘图区*/
lcd_drawline(ref_point[0]+1,ref_point[1],past_arrow1[0],past_arrow1[1],0);
lcd_drawline(past_arrow1[0],past_arrow1[1],past_arrow1[2],past_arrow1[3],0);
lcd_drawline(past_arrow1[0],past_arrow1[1],past_arrow1[4],past_arrow1[5],0);
lcd_drawline(ref_point[0]+1,ref_point[1],arrow_point1[0],arrow_point1[1],d_greencolor_lcd);
lcd_drawline(arrow_point1[0],arrow_point1[1],arrow_point1[2],arrow_point1[3],d_greencolor_lcd);
lcd_drawline(arrow_point1[0],arrow_point1[1],arrow_point1[4],arrow_point1[5],d_greencolor_lcd);
is_new_point=0;
}
is_new_point=point_arrow2(compass_m);/*计算箭头的坐标*/
if (is_new_point)
{
/*将电子罗盘的读数以箭头形式显示到绘图区*/
lcd_drawline(ref_point[0]-1,ref_point[1],past_arrow2[0],past_arrow2[1],0);
lcd_drawline(past_arrow2[0],past_arrow2[1],past_arrow2[2],past_arrow2[3],0);
lcd_drawline(past_arrow2[0],past_arrow2[1],past_arrow2[4],past_arrow2[5],0);
lcd_drawline(ref_point[0]-1,ref_point[1],arrow_point2[0],arrow_point2[1],d_greencolor_lcd);
lcd_drawline(arrow_point2[0],arrow_point2[1],arrow_point2[2],arrow_point2[3],d_greencolor_lcd);
lcd_drawline(arrow_point2[0],arrow_point2[1],arrow_point2[4],arrow_point2[5],d_greencolor_lcd);
is_new_point=0;
}
}
/*对电子罗盘数据进行处理，计算相对微小位移*/
relative_dis();
/*得到流速传感器的时间move_time和脉冲个数move_pulse，该中断的级别低于GPS中断
每10秒钟产生一次中断，计算相对航速move_vel,并产生一个标志位new_move_vel，来判断是否为新的流速*/
if (new_move_vel==1) /*判断是否为新的流速*/
{ //当螺旋桨不转时，move_time=0；
move_velocity();
//lcd_pressure(move_vel);
//move_vel=1;//~~~~~~~~~~调试数据
Relative[0]=Relative[0]+Relative_tem[0]*move_vel;
Relative[1]=Relative[1]+Relative_tem[1]*move_vel;
Relative_tem[0]=xyz[0]+Relative[0];/*加上GPS的偏移量*/
Relative_tem[1]=xyz[1]+Relative[1];/*加上GPS的偏移量*/
/*计算与目标的距离~~~~~~~~~~~~~~~~~~~~*/
d_s2tx=Relative_tem[0]-xyz_Targ[0];
d_s2ty=Relative_tem[1]-xyz_Targ[1];
dist_s2t=SQRT(d_s2tx*d_s2tx+d_s2ty*d_s2ty);
lcd_pressure(dist_s2t);
/*显示结果//display_xy(disp_rot,Relative_tem,Disp_xy);*/
display_xy(Relative_tem);/*计算当前显示坐标*/
disp_pianyi();
//is_display=is_xy_display(total_dist,Disp_xy,Disp_xy_int);
is_display=is_xy_display();/*量化显示坐标*/
if (is_exceed_bound==1) /*判断是否出界*/
{
lcd_overbright();/*出界指示*/
need_in_bound=1;
}
if (is_exceed_bound==0&&need_in_bound==1)
{
lcd_overdark_led();/*关闭出界指示*/
need_in_bound=0;
}
if (is_display==1)
{
//PORTD|=0x10;//亮红灯 //0xDF:亮绿灯
/*在航迹图上显示航行轨迹点；*/
HTC_point(Disp_xy_int[0],Disp_xy_int[1],d_yellowc);
HTC_point(Disp_xy_int[0]+1,Disp_xy_int[1],d_yellowc);
HTC_point(Disp_xy_int[0]-1,Disp_xy_int[1],d_yellowc);
HTC_point(Disp_xy_int[0],Disp_xy_int[1]+1,d_yellowc);
HTC_point(Disp_xy_int[0],Disp_xy_int[1]-1,d_yellowc);
/*Disp_past_point[0]=Disp_xy_int[0];
Disp_past_point[1]=Disp_xy_int[1];*/
/*更新经度和纬度*/
Relative_tem[2]=xyz[2];/*深度没法精确计算出来，只能以上一次GPS点的高度来代替*/
// xyz2XYZ(Relative_tem,sin_cos_ini,XYZ_ini,XYZ_cal);
xyz2XYZ();/*Y误差2m，Z误差1m，正确*/
// XYZ2LBH(XYZ_cal,LBH_cal);
XYZ2LBH();/*误差较小，正确*/
/*以文本显示经纬度*/
lcd_newloc(LBH_cal[0]/PI*180,LBH_cal[1]/PI*180);
is_display=0;
}/*is_display==1*/
Relative_tem[0]=0;
Relative_tem[1]=0;
new_move_vel=0;
}
/*处理GPS中断的结果*/
if (is_new_gps==1)
{
GPS_t[0]=GPS_t[1];
GPS_t[1]=LBH_Current[3];
/*算出此时的相对轨迹*/
//Relative[0]=Relative[0]+Relative_tem[0]*move_vel; /*累加相对轨迹后清零*/
//Relative[1]=Relative[1]+Relative_tem[1]*move_vel;
dt_time=GPS_t[1]-GPS_t[0];/*GPS时间差比较稳定*/
if (dt_time>Door_time)
{
// rela_dist[0]=Relative[0];
// rela_dist[1]=Relative[1];
// R_compass=N_compass;
filter_flag=0;
filter_num=0;
}
if (!filter_flag) //出水后第四个点起算
{
filter_num++;
if (filter_num>3)
{
filter_flag=1;
filter_num=0;
filter_flag2=1;
}
}
else
{LBH2xyz(LBH_Current,xyzM);/*计算当前点的东北天坐标xyz[3]*/
x_temp[filter_num]=xyzM[0]; //滤波
y_temp[filter_num]=xyzM[1];
z_temp[filter_num]=xyzM[2];
filter_num++;
if (filter_num>10)
{ //lcd_bit4dis(100,60,200);
xyz[0]=filter1(x_temp,11);
xyz[1]=filter1(y_temp,11);
xyz[2]=filter1(z_temp,11);
if (filter_flag2)
{
filter_flag2=0;
}
else
{
if (fabs(xyz[0]-xyz_t[0])>30) xyz[0]=xyz_t[0];
if (fabs(xyz[1]-xyz_t[1])>30) xyz[1]=xyz_t[1];
}
xyz_t[0]=xyz[0];
xyz_t[1]=xyz[1];
filter_flag1=1;
lcd_upwbright();
filter_num=0;
relative_dis();/*防止上面的计算时间过长*/
Relative[0]=Relative[0]+Relative_tem[0]*move_vel; /*累加相对轨迹后清零*/
Relative[1]=Relative[1]+Relative_tem[1]*move_vel;
rela_dist[0]=Relative[0];
rela_dist[1]=Relative[1];
R_compass=N_compass;
Relative_tem[0]=0;
Relative_tem[1]=0;
Relative[0]=0;
Relative[1]=0;
N_compass=0;
}
}
if(filter_flag && filter_flag1)
{
/*计算与目标的距离~~~~~~~~~~~~~~~~*/
d_s2tx=xyz[0]-xyz_Targ[0];
d_s2ty=xyz[1]-xyz_Targ[1];
dist_s2t=SQRT(d_s2tx*d_s2tx+d_s2ty*d_s2ty);
lcd_pressure(dist_s2t);
//xyz_cache[xyz_i][0]=xyz[0];xyz_cache[xyz_i][1]=xyz[1];xyz_cache[xyz_i][2]=xyz[2];
//xyz_i++;
//if (xyz_i==20) xyz_i=0;
display_xy(xyz);
disp_pianyi();
/*判断是否要显示该GPS点*/
is_display=is_xy_display();
if (is_exceed_bound==1) /*判断是否出界*/
{
lcd_overbright();/*出界指示*/
need_in_bound=1;
}
if (is_exceed_bound==0&&need_in_bound==1)
{
lcd_overdark_led();/*关闭出界指示*/
need_in_bound=0;
}
if (is_display==1)
{
//lcd_newloc(LBH_Current[0],LBH_Current[1]);/*以文本显示经纬度*/
/*在绘图区显示航行轨迹；*/
HTC_point(Disp_xy_int[0],Disp_xy_int[1],d_yellowc);
HTC_point(Disp_xy_int[0]+1,Disp_xy_int[1],d_yellowc);
HTC_point(Disp_xy_int[0]-1,Disp_xy_int[1],d_yellowc);
HTC_point(Disp_xy_int[0],Disp_xy_int[1]+1,d_yellowc);
HTC_point(Disp_xy_int[0],Disp_xy_int[1]-1,d_yellowc);
is_display=0;
}
// is_to_write=1;/*可以记录GPS读数*/
}
is_new_gps=0;/*是否为新的GPS数据*/
/*通过时间差来判断是否要记录*/
//Cur_time=sd2300_clk[2]*3600.0+sd2300_clk[1]*60.0+sd2300_clk[0];
if (dt_time>Door_time)
{
//if (xyz_i==0) up_i=19;
//else up_i=xyz_i-1;
//get_pastxyz();
//remove_errxyz();
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -