📄 gpscx.c
字号:
location_data[1].GPS_time=0; /*时间*/
location_data[1].GPS_x=xyz_m[0];
location_data[1].GPS_y=xyz_m[1];
location_data[1].GPS_z=xyz_m[2];
location_data[1].state=1;/*下潜*/
location_data[1].Relative_x=0;
location_data[1].Relative_y=0;
flag_count=1;
// is_to_write=0;
}
if(filter_flag1 && flag_count==1)
{
//count_xyz++;
// if (count_xyz==4)
//{
//up_i=xyz_i;
//get_pastxyz();
//remove_errxyz();
/*注意当运动速度较快时,必须考虑精确的时间*/
xyz_m[0]=xyz[0];
xyz_m[1]=xyz[1];
xyz_m[2]=xyz[2];
location_data[2].GPS_time=R_compass/f_compass; /*当前时间*/
location_data[2].Relative_x=rela_dist[0];
location_data[2].Relative_y=rela_dist[1];
location_data[2].GPS_x=xyz_m[0];
location_data[2].GPS_y=xyz_m[1];
location_data[2].GPS_z=xyz_m[2];
location_data[2].state=0;/*上浮*/
p_current_loc=3;
dist_cal();/*初始化dist[8]*/
wave_vel_cal();/*调试通过*//*计算海流速度*/
flag_stage=3;
flag_count=0;
count_xyz=0;
display_xy(xyz);
is_display=is_xy_display();
///新加画线
// lcd_bit4dis(80,60,Disp_xy_int_past[0]);
// lcd_bit4dis(100,60,Disp_xy_int_past[1]);
lcd_drawline (Disp_xy_int_past[0],Disp_xy_int_past[1],0,250,d_darkc);
lcd_drawline (Disp_xy_int_past[0]+1,Disp_xy_int_past[1],1,250,d_darkc);
lcd_drawline (Disp_xy_int[0],Disp_xy_int[1],0,250,d_greencolor_lcd);
lcd_drawline (Disp_xy_int[0]+1,Disp_xy_int[1],1,250,d_greencolor_lcd);
Disp_xy_int_past[0]=Disp_xy_int[0];
Disp_xy_int_past[1]=Disp_xy_int[1];
lcd_mbcircle();
lcd_drawline (0,0,0,250,d_greencolor_lcd);
lcd_drawline (1,0,1,250,d_greencolor_lcd);
lcd_drawline (-1,0,-1,250,d_greencolor_lcd);
}
filter_flag1=0;
}/*is_new_gps==1*/
}/*flag_stage"2"*/
else if (flag_stage==3)
{
// lcd_pressure(SQRT(wave_vel[0]*wave_vel[0]+wave_vel[1]*wave_vel[1]));
/*在绘图区注明航行方向*/
N_circle++;
if (N_circle%300==0)
{
N_circle=0;
if (Current_i==0)
compassdata_cal(compass_data[49]);
//lcd_bit4dis(80,40,compass_data[49]);}
else
compassdata_cal(compass_data[Current_i-1]);
// lcd_bit4dis(120,40,compass_data[Current_i-1]);}
navigate_direction(compass_m);
// lcd_bit4dis(140,40,compass_data[Current_i-1]);
is_new_point=point_arrow(nav_dir);/*计算箭头的坐标*/
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(nav_dir);/*计算箭头的坐标*/
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(nav_dir);/*计算箭头的坐标*/
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();
if (new_move_vel==1)/*判断是否为新的流速*/
{
move_velocity();
//move_vel=1;//~~~~~~~~~~调试数据
Relative[0]=Relative[0]+Relative_tem[0]*move_vel;
Relative[1]=Relative[1]+Relative_tem[1]*move_vel;
//A_sin=A_sin+Relative_tem[0];
//A_cos=A_cos+Relative_tem[1];
if (move_time!=0)
{
//B_sin=B_sin+Relative_tem[0]*move_pulse/move_time/2;
//_cos=B_cos+Relative_tem[1]*move_pulse/move_time/2;
}
/*当前方位的东北天坐标,Relative_tem被用作其他用处*/
Relative_tem[0]=xyz[0]+Relative[0]+N_compass/f_compass*wave_vel[0];
Relative_tem[1]=xyz[1]+Relative[1]+N_compass/f_compass*wave_vel[1];
/*计算与目标的距离~~~~~~~~~~~~~~~~~*/
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(Relative_tem);/*计算显示坐标*/
disp_pianyi();
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)
{
/*显示Disp_xy_int[2],在航迹图上显示航行方向*/
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];//深度没法精确计算出来
//xyz2XYZ(Relative_tem,sin_cos_ini,XYZ_ini,XYZ_cal);
xyz2XYZ();//计算精度不高
//XYZ2LBH(XYZ_cal,LBH_cal);/*计算经度、纬度和高度*/
XYZ2LBH();
lcd_newloc(LBH_cal[0]/PI*180,LBH_cal[1]/PI*180);/*以文本显示经纬度LBH_cal[3]*/
is_display=0;
}
Relative_tem[0]=0;
Relative_tem[1]=0;
new_move_vel=0;
}//new_move_vel==1
if (is_new_gps==1)//如果接收到新的GPS数据,处理完以后置位is_new_gps
{
//relative_dis();
GPS_t[0]=GPS_t[1];
GPS_t[1]=LBH_Current[3];
dt_time=GPS_t[1]-GPS_t[0];
/*算出此时的相对轨迹*/
//Relative[0]=Relative[0]+Relative_tem[0]*move_vel; /*累加相对轨迹后清零*/
// Relative[1]=Relative[1]+Relative_tem[1]*move_vel;
//A_sin=A_sin+Relative_tem[0];
//A_cos=A_cos+Relative_tem[1];
//if (move_time!=0)
//{
// B_sin=B_sin+Relative_tem[0]*move_pulse/move_time/2;
// B_cos=B_cos+Relative_tem[1]*move_pulse/move_time/2;
//}
if (dt_time>Door_time)
{
// rela_dist[0]=Relative[0];
// rela_dist[1]=Relative[1];
// R_compass=N_compass;
// V_coe[0]=A_sin;V_coe[1]=A_cos;V_coe[2]=B_sin;V_coe[3]=B_cos;
filter_flag=0;
filter_num=0;
}
if (!filter_flag) //出水后第四个点起算
{
filter_num++;
if (filter_num>3)
{
filter_flag=1;
filter_flag2=1;
filter_num=0;
}
}
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;
filter_num=0;
lcd_upwbright();
//A_sin=0;A_cos=0;B_sin=0;B_cos=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();
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[0]+sd2300_clk[1]*60.0+sd2300_clk[2]*3600.0;
if (dt_time>Door_time)
{
//if (xyz_i==0) up_i=19;
//else up_i=xyz_i-1;
//get_pastxyz();
// remove_errxyz();
location_data[p_current_loc].GPS_time=0; /*当前时间*/
location_data[p_current_loc].GPS_x=xyz_m[0];
location_data[p_current_loc].GPS_y=xyz_m[1];
location_data[p_current_loc].GPS_z=xyz_m[2];
location_data[p_current_loc].state=1;/*下潜*/
location_data[p_current_loc].Relative_x=0;
location_data[p_current_loc].Relative_y=0;
p_current_loc++;
// is_to_write=0;
flag_count=1;
}
is_new_gps=0;/*是否为新的GPS数据*/
if(filter_flag1 && flag_count==1)
{
//up_i=xyz_i;
//get_pastxyz();
// remove_errxyz();
/*注意当运动速度较快时,必须考虑精确的时间*/
xyz_m[0]=xyz[0];
xyz_m[1]=xyz[1];
xyz_m[2]=xyz[2];
location_data[p_current_loc].GPS_time=R_compass/f_compass; /*当前时间*/
location_data[p_current_loc].Relative_x=rela_dist[0];
location_data[p_current_loc].Relative_y=rela_dist[1];
location_data[p_current_loc].GPS_x=xyz_m[0];
location_data[p_current_loc].GPS_y=xyz_m[1];
location_data[p_current_loc].GPS_z=xyz_m[2];
location_data[p_current_loc].state=0;/*上浮*/
p_current_loc++;
wave_need_new=1;/*在潜行一段时间上浮后置位“wave_need_new"*/
flag_count=0;
count_xyz=0;
dist_cal();/*初始化dist[8]*/
wave_vel_cal();/*调试通过*//*计算海流速度*/
display_xy(xyz);
is_display=is_xy_display();
///新加画线
lcd_drawline (Disp_xy_int_past[0],Disp_xy_int_past[1],0,250,d_darkc);
lcd_drawline (Disp_xy_int_past[0]+1,Disp_xy_int_past[1],1,250,d_darkc);
lcd_drawline (Disp_xy_int[0],Disp_xy_int[1],0,250,d_greencolor_lcd);
lcd_drawline (Disp_xy_int[0]+1,Disp_xy_int[1],1,250,d_greencolor_lcd);
Disp_xy_int_past[0]=Disp_xy_int[0];
Disp_xy_int_past[1]=Disp_xy_int[1];
lcd_mbcircle();
lcd_drawline (0,0,0,250,d_greencolor_lcd);
lcd_drawline (1,0,1,250,d_greencolor_lcd);
lcd_drawline (-1,0,-1,250,d_greencolor_lcd);
}
filter_flag1=0;
}/*is_new_gps==1*/
}//flag_stage==3
}
}
unsigned char clk_read(void ) //读板上时钟时间
{
unsigned char n,k,temp=7;
DDRD &=~((1<<PD0)|(1<<PD1));
I2CStart();
I2CWaitAck();
if (I2CChkAck()!=I2C_START ) return 0;
I2CSendByte(d_clk_w);
I2CWaitAck();
if (I2CChkAck()!=I2C_MT_SLA_ACK ) return 0;
I2CSendByte(d_clk_secondadd);
I2CWaitAck();
if (I2CChkAck()!=I2C_MT_DATA_ACK ) return 0;
I2CStart();
I2CWaitAck();
if (I2CChkAck()!=I2C_RESTART ) return 0;
I2CSendByte(d_clk_r);
I2CWaitAck();
if (I2CChkAck()!=I2C_MR_SLA_ACK ) return 0;
I2CRcvAckByte();
do{
I2CWaitAck();
if (I2CChkAck()!=I2C_MR_DATA_ACK ) return 0;
k=TWDR;
n=(k>>4)*10;
k=(k&0x0f);
sd2300_clk[7-temp]=n+k;
if (temp==1) I2CSendNoAck();//I2CRcvNckByte();
else I2CSendAck();//I2CRcvAckByte();
}while (--temp !=0);
return temp;
}
unsigned char clk_verify_proc(void) //GPS对时板上时钟
{
unsigned char temp=3,k,n;
DDRD &=~((1<<PD0)|(1<<PD1));
I2CStart();
I2CWaitAck();
if (I2CChkAck()!=I2C_START ) return 0;
I2CSendByte(d_clk_w);
I2CWaitAck();
if (I2CChkAck()!=I2C_MT_SLA_ACK ) return 0;
I2CSendByte(d_clk_secondadd);
I2CWaitAck();
if (I2CChkAck()!=I2C_MT_DATA_ACK ) return 0;
do {
k=sd2300_clk[3-temp];
k=(((k/10)<<4)&0xf0)+((k%10)&0x0f);
I2CSendByte(k)
I2CWaitAck();
if (I2CChkAck()!=I2C_MT_DATA_ACK ) return 0;
}while (--temp !=0);
// I2CStop();
return temp;
}
void twi_master_initialise(void) //use sd2300
{
TWBR=twi_twbr;
// TWDR=0xff;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -