📄 gpscx.c
字号:
#include <iom128.h>
#include <stdlib.h>
#include <string.h>
#include "stdio.h"
#include "math.h"
#include <ctype.h>
#include "htcprocdef.h"
#include "htcprocrom.h"
#include "biaoding.h"
/*保留最近三个GPS点*/
double SQRT(double y)
{
#define eps 1e-15
double x1,x2;
if(y<eps) return 0;
x2= y>1.0? y/2 : y*2;
do{
x1 = x2;
x2 = (x1 + y/x1)/2;
}while(fabs(x2-x1) > eps);
return x2;
#undef eps
}
float lcd_bit5dis(unsigned char lcd_ds,unsigned char lcd_dy,float fdata)
{ long j;
unsigned int i;
unsigned int p;
static unsigned char buf_lcd[5];
j=fdata;
buf_lcd[0]=j/10000;
i=j%10000;
p=i%1000;
buf_lcd[1]=i/1000;
buf_lcd[2]=p/100;
buf_lcd[3]=(p%100)/10;
buf_lcd[4]=(p%100)%10;
lcd_display(lcd_ds,lcd_dy,buf_lcd,5);
return (fdata-j);
}
/*由kcl和kch插值计算*/
void compass_in(float com_tem)
{
float c_newton;
if (kch!=kchN)
{
c_newton=(com_bd[kcl][1]-com_bd[kch][1])/(com_bd[kcl][0]-com_bd[kch][0]);
compass_m=com_bd[kcl][1]+c_newton*(com_tem-com_bd[kcl][0]);
}
else if (com_tem>com_bd[kch][0])/*com_bd[kcl0+1][0]+360后再插值计算*/
{
c_newton=(com_bd[kcl][1]-com_bd[kch][1])/(com_bd[kcl][0]-com_bd[kch][0]-360);
compass_m=com_bd[kcl][1]+c_newton*(com_tem-com_bd[kcl][0]);
}
else/*com_tem加360，com_bd[kcl0+1][0]+360后再插值计算,将得到的结果-360后，得到compass_m*/
{
c_newton=(com_bd[kcl][1]-com_bd[kch][1])/(com_bd[kcl][0]-com_bd[kch][0]-360);
compass_m=com_bd[kcl][1]+c_newton*(com_tem+360-com_bd[kcl][0]);
}
compass_m=ceil(compass_m*10);
compass_m=compass_m/10;/*去除小数点后面第二位*/
compass_m=compass_m-compen_th;
}
void compassdata_cal(float com_tem)
{
int kn,KCH=120;
float dc1,dc2;
kcl=0;kch=118;
kchN=kch+1;//int kcl,kchN,kcm,kch=116;插值时标定数据的序号*/
com_tem=ceil(com_tem*10);
com_tem=com_tem/10;/*去除小数点后面第二位*/
/*处理区间[10.9-357.5]*/
if (fabs(com_tem-com_bd[0][0])<0.08) /*判断是否相等*/
{
compass_m=com_bd[0][1];
compass_m=compass_m-compen_th;//double compen_th=0; 设置磁偏角
return;
}
if (fabs(com_tem-com_bd[kch][0])<0.08)/*判断是否相等*/
{
compass_m=com_bd[kcl-1][1];
compass_m=compass_m-compen_th;
return;
}
if (com_tem>com_bd[0][0]&&com_tem<com_bd[kch][0])
{
while (kch-kcl!=1)
{
kcm=(kcl+kch)/2;
dc1=com_tem-com_bd[kcl][0];
dc2=com_tem-com_bd[kcm][0];
if (fabs(dc2)<0.08)
{
compass_m=com_bd[kcm][1];
compass_m=compass_m-compen_th;
return;
}
if (dc1*dc2>0)
{
kcl=kcm;
}
else
{
kch=kcm;
}
}
/*调用插值公式，利用kcl和kch*/
compass_in(com_tem);
return;
}
/*处理[kchN，KCH]区间上的读数*/
if (com_tem>com_bd[kchN][0]&&com_tem<com_bd[KCH][0])
{
for (kn=kchN+1;kn<=KCH;kn++)
{
if (fabs(com_tem-com_bd[kn][0])<0.08)/*判断是否相等*/
{
compass_m=com_bd[kn][1];
compass_m=compass_m-compen_th;
return;
}
if (com_tem<com_bd[kn][0])
{
kcl=kn-1;
kch=kn;
break;
}
}/*for*/
/*插值计算,得到compass_m*/
compass_in(com_tem);
return;
}
/*处理区间[357.5-360]和[0-0.8]上的读数*/
if (fabs(com_tem-com_bd[kch][0])<0.08)/*判断是否相等*/
{
compass_m=com_bd[kch][1];
compass_m=compass_m-compen_th;
return;
}
if (fabs(com_tem-com_bd[kchN][0])<0.08)/*判断是否相等*/
{
compass_m=com_bd[kchN][1];
compass_m=compass_m-compen_th;
return;
}
if (com_tem>com_bd[kch][0]||com_tem<com_bd[kchN][0])
{
kcl=kch;
kch=kchN;
compass_in(com_tem);
return;
}
}
/*计算流速
**double move_velocity(int N_pulse,double time_velocity)；
**输入：脉冲个数N_pulse，时间time_velocity，
**返回：该段时间内的流速，并置位is_vel_new*/
void move_velocity( void)
{
if (move_time==0)
move_vel=0;
else
move_vel=C_vel+K_vel*5*move_pulse/move_time;
if (move_vel<5)
over_move_vel=move_vel;
else
move_vel=over_move_vel;
}
/*海流速度的计算
**void wave_vel_cal(struct location *p_loc_data,double *p_wave_vel);
**输入：指向海流速度的指针double *p_wave_vel，指向结构体数组的指针struct location *p_loc_data
**返回：空*/
void wave_vel_cal(void)
{
float a,b,c;
a=b=c=0;
a=location_data[2].GPS_x-location_data[1].GPS_x-location_data[2].Relative_x;
b=location_data[2].GPS_time-location_data[1].GPS_time;
b=b/10.0*10;
wave_vel[0]=a/b;
c=location_data[2].GPS_y-location_data[1].GPS_y-location_data[2].Relative_y;
wave_vel[1]=c/b;
}
void wave_vel_cal1(void)
{
float a,b,c;
a=b=c=0;
a=location_data[2].GPS_x-location_data[1].GPS_x-location_data[2].Relative_x;
b=location_data[2].GPS_time-location_data[1].GPS_time;
b=b/10.0*10;
wave_vel[0]=a/b;
c=location_data[2].GPS_y-location_data[1].GPS_y-location_data[2].Relative_y;
wave_vel[1]=c/b;
}
/*计算两个GPS点在x轴和y轴方向的坐标差
**void dist_cal(struct location *p_loc_data,double *p_dist)
**输入：
**返回：*/
void dist_cal(void)
{
int p_loc_tem;
p_loc_tem=p_current_loc-1;
dist[0]=dist[1];
dist[2]=dist[3];
dist[4]=dist[5];
dist[6]=dist[7];
dist[1]=location_data[p_loc_tem].GPS_x-location_data[p_loc_tem-1].GPS_x;
dist[3]=location_data[p_loc_tem].GPS_y-location_data[p_loc_tem-1].GPS_y;
dist[5]=location_data[p_loc_tem].GPS_time;
dist[7]=SQRT(dist[1]*dist[1]+dist[3]*dist[3]);
}
/*更新海流的速度
wave_vel_adjust(p_current_loc,dist,wave_vel);
** 返回：空 */
void wave_vel_adjust()
{
double a,b;
int p_loc_data;
p_loc_data=p_current_loc-1;
a=dist[0]+dist[1]-location_data[p_loc_data].Relative_x-location_data[p_loc_data-2].Relative_x;
b=dist[4]+dist[5];
b=b*10.0/10;
wave_vel[0]=a/b;
a=dist[2]+dist[3]-location_data[p_loc_data].Relative_y-location_data[p_loc_data-2].Relative_y;
wave_vel[1]=a/b;
}
/*相对轨迹的计算
**void relative_dis()
// relative_dis(compass_data,&Done_i,&Current_I,Relative_tem);
//void relative_dis(double *p_compass,char *p_Done_i,char *p_Current_I,double *p_rela_dis)
*/
void relative_dis(void)
{ double dir;
while (Done_i!=Current_i)
{
compassdata_cal(compass_data[Done_i]);
dir=compass_m;
Relative_tem[0]=Relative_tem[0]+sin(dir/180*PI)/f_compass;/*x轴方向*/
Relative_tem[1]=Relative_tem[1]+cos(dir/180*PI)/f_compass;
Done_i++;
N_compass++;
if (Done_i==50)
Done_i=0;
}
}
/*航行方向
**void navigate_direction()
// navigate_direction(wave_vel,move_vel,compass_data[49],&nav_dir);
*/
void navigate_direction(float comp_cur)
{
float a,b,theta;
a=move_vel*sin(comp_cur/180*PI)+wave_vel[0];/*x轴方向*/
b=move_vel*cos(comp_cur/180*PI)+wave_vel[1];/*y轴方向*/
theta=atan2(a,b)/PI*180;/*返回值在-180—180度之间*/
if (theta<0)
nav_dir=360+theta;
else
nav_dir=theta;
}
/*计算经度和纬度的正弦和余弦值*/
void sin_cos(double *p_LBH_ini,double *p_sin_cos_ini)
{
*p_sin_cos_ini=sin((*p_LBH_ini)/180.0*PI);
*(p_sin_cos_ini+1)=cos((*p_LBH_ini)/180.0*PI);
*(p_sin_cos_ini+2)=sin((*(p_LBH_ini+1))/180.0*PI);
*(p_sin_cos_ini+3)=cos((*(p_LBH_ini+1))/180.0*PI);
}
/*将大地坐标系（经度、纬度、高度）转化到直角坐标系（XYZ）
**void LBH2XYZ(double L,double B,double H,double *p_XYZ)
*/
void LBH2XYZ(double *p_LBH,double *p_XYZ)
{
double N,e2=0.00669437999013,a=6378137;//a：长轴
double sinL,cosL,sinB,cosB;
sinL=sin((*p_LBH)/180.0*PI);
cosL=cos((*p_LBH)/180.0*PI);
sinB=sin((*(p_LBH+1))/180.0*PI);
cosB=cos((*(p_LBH+1))/180.0*PI);
N=a/SQRT(1-e2*sinB*sinB);
*p_XYZ=(N+(*(p_LBH+2)))*cosB*cosL;//X
*(p_XYZ+1)=(N+(*(p_LBH+2)))*cosB*sinL;//Y
*(p_XYZ+2)=(N*(1-e2)+(*(p_LBH+2)))*sinB;
}
/*将直角坐标（XYZ）转化到东北天坐标（xyz）
*/
//void LBH2xyz(double *p_XYZ_ini,double *p_sin_cos_ini,double *p_LBH,double *p_xyz)
//LBH2xyz(XYZ_ini,sin_cos_ini,LBH_Targ,xyz_Targ);
void LBH2xyz(double *p_LBH,double *p_xyz)
{
double X1,Y1,Z1,XYZ[3];
LBH2XYZ(p_LBH,XYZ);/*当前GPS点的XYZ*/
//lcd_compass(-XYZ[0]-2613200);// 调试
//lcd_pressure(XYZ[1]-4739600);//调试
X1=XYZ[0]-XYZ_ini[0];/*正确-X1-1380*/
Y1=XYZ[1]-XYZ_ini[1];/*正确值应该是-760.5*/
Z1=XYZ[2]-XYZ_ini[2];/*正确值应该是0*/
*p_xyz=-sin_cos_ini[0]*X1+sin_cos_ini[1]*Y1;
*(p_xyz+1)=-sin_cos_ini[1]*X1*sin_cos_ini[2]-sin_cos_ini[0]*Y1*sin_cos_ini[2]+sin_cos_ini[3]*Z1;
*(p_xyz+2)=sin_cos_ini[1]*X1*sin_cos_ini[3]+sin_cos_ini[0]*Y1*sin_cos_ini[3]+sin_cos_ini[2]*Z1;
}
/*将东北天坐标xyz转化为直角坐标XYZ
void xyz2XYZ(double *p_xyz,double *p_sin_cos_ini,double *p_XYZ_ini,double *p_XYZ)
// xyz2XYZ(Relative_tem,sin_cos_ini,XYZ_ini,XYZ_cal);
*/
void xyz2XYZ(void)
{
double dX,dY,dZ;
dX=-sin_cos_ini[2]*Relative_tem[1]*sin_cos_ini[1]-sin_cos_ini[0]*Relative_tem[0]+sin_cos_ini[3]*Relative_tem[2]*sin_cos_ini[1];
dY=-sin_cos_ini[2]*Relative_tem[1]*sin_cos_ini[0]+sin_cos_ini[1]*Relative_tem[0]+sin_cos_ini[3]*Relative_tem[2]*sin_cos_ini[0];
dZ=sin_cos_ini[3]*Relative_tem[1]+sin_cos_ini[2]*Relative_tem[2];
XYZ_cal[0]=XYZ_ini[0]+dX;
XYZ_cal[1]=XYZ_ini[1]+dY;
XYZ_cal[2]=XYZ_ini[2]+dZ;
}
/*将直角坐标（XYZ）转换到大地坐标（LBH）*/
// XYZ2LBH(XYZ_cal,LBH_cal);
//void XYZ2LBH(double *p_XYZ,double *p_LBH_cal)
void XYZ2LBH(void)
{
double a=6378137,b=6356752.3142;
double e1_2=0.00669437999013,e2_2=0.00673949674227;
double B0,s_XY,u,sinu,cosu,N,T;/**/
s_XY=SQRT(XYZ_cal[0]*XYZ_cal[0]+XYZ_cal[1]*XYZ_cal[1]);
u=atan(a*XYZ_cal[2]/(s_XY*b));
sinu=sin(u);cosu=cos(u);
B0=atan((XYZ_cal[2]+b*e2_2*sinu*sinu*sinu)/(s_XY-a*e1_2*cosu*cosu*cosu));
N=a/SQRT(1-e1_2*sin(B0)*sin(B0));
LBH_cal[0]=atan(XYZ_cal[1]/XYZ_cal[0])+PI;
T=XYZ_cal[2]+N*e1_2*sin(B0);
LBH_cal[2]=SQRT(s_XY*s_XY+T*T)-N;
LBH_cal[1]=atan(XYZ_cal[2]*(N+LBH_cal[2])/s_XY/(N*(1-e1_2)+LBH_cal[2]));
}
/*计算显示时需要转动的角度
*/
void display_rotate(double *p_Targ_xyz,float *p_disp_rot)
{
float theta;
theta=atan2(*p_Targ_xyz,*(p_Targ_xyz+1))/PI*180;/*返回值在-180—180度之间*/
if (theta<0)
*p_disp_rot=360+theta;
else
*p_disp_rot=theta;
}
/*计算要在液晶屏上显示的航行方向
** p_disp_direction:要在液晶屏上显示的航行方向
*/
/*void disp_direction(double *p_disp_rot,double *p_nav_dir,double *p_disp_direction)
{
*p_disp_direction=*p_nav_dir-*p_disp_rot;
if (*p_disp_direction<0)
*p_disp_direction=360+*p_disp_direction;
}*/
/*当前方位的显示坐标
**void display_xy(double disp_rot,double *p_xyz,double *p_Disp_xy)
//display_xy(disp_rot,Relative_tem,Disp_xy);
*/
void display_xy(double *p_xyz)
{
double a,b;
a=(*p_xyz)*cos(disp_rot/180.0*PI);
b=(*(p_xyz+1))*sin(disp_rot/180.0*PI);
Disp_xy[0]=a-b;
//*p_Disp_xy=(*p_xyz)*cos(disp_rot/180*PI)-(*(p_xyz+1))*sin(disp_rot/180*PI);
a=(*(p_xyz+1))*cos(disp_rot/180.0*PI);
b=(*p_xyz)*sin(disp_rot/180.0*PI);
Disp_xy[1]=a+b;
}
/*判断当前东北天坐标是否要显示
**char is_xy_display(double total_dist,double *p_Disp_xy,double *p_Disp_xy_int)
//is_display=is_xy_display(total_dist,Disp_xy,Disp_xy_int);
**返回：“0”表示无需在液晶屏上显示，“1”表示需要在液晶屏上显示。
*/
char is_xy_display(void)
{
int x,y;
x=ceil(Disp_xy[0]/total_dist*N_pixel);
y=ceil(Disp_xy[1]/total_dist*N_pixel);
is_exceed_bound=0;
if (x>x_N_pixel)/*判断是否超出边界*/
{ x=x_N_pixel;
is_exceed_bound=1;
}
if (x<-x_N_pixel)
{ x=-x_N_pixel;
is_exceed_bound=1;
}
if (y<-15)/*也可以是0*/
{ y=-15;
is_exceed_bound=1;
}
if (y>N_pixel+15)
{ y=N_pixel+15;
is_exceed_bound=1;
}
if (Disp_xy_int[0]==x&&Disp_xy_int[1]==y) /*判断是否要显示*/
return (0);
else
{
Disp_xy_int[0]=x;
Disp_xy_int[1]=y;
return (1);
}
}
/*计算方向箭头的点*/
// is_new_point=point_arrow(compass_data[Current_i-1],Disp_xy_int,arrow_point);
char point_arrow(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] ;
y_arrow=ceil(arrow_leng*cos((comp_cur-disp_rot)/180*PI))+ref_point[1];
new_point=(x_arrow==arrow_point[0])&&(y_arrow==arrow_point[1]);
if(new_point==0)
{
past_arrow[0]=arrow_point[0];
past_arrow[1]=arrow_point[1];
past_arrow[2]=arrow_point[2];
past_arrow[3]=arrow_point[3];
past_arrow[4]=arrow_point[4];
past_arrow[5]=arrow_point[5];
arrow_point[0]=x_arrow;
arrow_point[1]=y_arrow;
arrow_point[2]=ceil(arrow_leng*0.6*sin((comp_cur-disp_rot)/180*PI+PI/6))+ref_point[0] ;
arrow_point[3]=ceil(arrow_leng*0.6*cos((comp_cur-disp_rot)/180*PI+PI/6))+ref_point[1];
arrow_point[4]=ceil(arrow_leng*0.6*sin((comp_cur-disp_rot)/180*PI-PI/6))+ref_point[0] ;
arrow_point[5]=ceil(arrow_leng*0.6*cos((comp_cur-disp_rot)/180*PI-PI/6))+ref_point[1];
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -