KenanZhu/KNZ_GeoTrackLab
Archived
1
0 Fork 0
Code Issues Actions Packages Projects Releases Wiki Activity

modified the crash

Browse Source
This commit is contained in:
KenanZhu111 committed 2024-09-23 09:59:40 +08:00
1 parent 54547f9d3e
commit b8a6cf4e6e
6 files changed
+50 -59
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

blh2enu.h
+5 -6
View File
@@ -9,7 +9,7 @@ typedef struct
double E;
double N;
double U;
}blh2enu, *pblh2enu;
}blh2enu;
/* -------------------------------------------------------------------------- */
/// @brief 将大地高坐标转换为站心坐标
@@ -21,7 +21,7 @@ typedef struct
/// @param deltaz 卫星到站心的z坐标差值
/// @return 返回自定义结构体类型包含E, N, U成员
/* -------------------------------------------------------------------------- */
pblh2enu BLH2ENU(pblh2enu blh2enu,
blh2enu BLH2ENU(blh2enu blh2enu,
double stationB, double stationL,
double deltax, double deltay, double deltaz)
{
@@ -29,10 +29,9 @@ pblh2enu BLH2ENU(pblh2enu blh2enu,
double cosB = cos(stationB);
double sinL = sin(stationL);
double cosL = cos(stationL);
blh2enu = (pblh2enu)malloc(sizeof(pblh2enu));
blh2enu->E = - sinL*(deltax) + cosL*(deltay);
blh2enu->N = -sinB*cosL*(deltax) - sinB*sinL*(deltay) + cosB*(deltaz);
blh2enu->U = cosB*cosL*(deltax) + cosB*sinL*(deltay) + sinB*(deltaz);
blh2enu.E = - sinL*(deltax) + cosL*(deltay);
blh2enu.N = -sinB*cosL*(deltax) - sinB*sinL*(deltay) + cosB*(deltaz);
blh2enu.U = cosB*cosL*(deltax) + cosB*sinL*(deltay) + sinB*(deltaz);
return blh2enu;
}
cal_mod.h
+24 -29
View File
@@ -69,22 +69,17 @@ int sat_gps_pos_clac(FILE * result_file,
int o_epochnum, int gps_satnum)
{ int progress = 0;
int count = 0;
pxyz2blh tem1 = NULL;
pxyz2blh tem2 = NULL;
pblh2enu tem3 = NULL;
prahcal tem4 = NULL;
pdeg2dms temb = NULL;
pdeg2dms teml = NULL;
xyz2blh tem1 = {0};
xyz2blh tem2 = {0};
blh2enu tem3 = {0};
rahcal tem4 = {0};
deg2dms temb = {0};
deg2dms teml = {0};
blh = (pblh)malloc(sizeof(pblh) * o_epochnum* 256);
enu = (penu)malloc(sizeof(penu) * o_epochnum* 256);
rah = (prah)malloc(sizeof(prah) * o_epochnum* 256);
pos_t = (ppos_t)malloc(sizeof(ppos_t) * o_epochnum* 256);
station = (pstation)malloc(sizeof(pstation) * o_epochnum* 256);
tem1 = (pxyz2blh)malloc(sizeof(pxyz2blh)* 512);
tem2 = (pxyz2blh)malloc(sizeof(pxyz2blh)* 512);
tem3 = (pblh2enu)malloc(sizeof(pblh2enu)* 512);
temb = (pdeg2dms)malloc(sizeof(pdeg2dms)* 512);
teml = (pdeg2dms)malloc(sizeof(pdeg2dms)* 512);
for (int i = 0; i < o_epochnum; i++)//第i个历元
{
result_file = fopen(".\\Pos_out\\LLA_result_for_read.txt", "a+");
@@ -123,14 +118,14 @@ int sat_gps_pos_clac(FILE * result_file,
/* ----------------------------- 地固坐标系转经纬大地高坐标系 ----------------------------- */
tem1 = XYZ2BLH( tem1, pos_t[i].X[sPRN], pos_t[i].Y[sPRN], pos_t[i].Z[sPRN]);
blh->B = rad2deg(tem1->B);
blh->L = rad2deg(tem1->L);
blh->H = tem1->H;
blh->B = rad2deg(tem1.B);
blh->L = rad2deg(tem1.L);
blh->H = tem1.H;
tem2 = XYZ2BLH( tem2, obs_h->apX, obs_h->apY, obs_h->apZ);
station->B = tem2->B;
station->L = tem2->L;
station->H = tem2->H;
station->B = tem2.B;
station->L = tem2.L;
station->H = tem2.H;
/* -------------------------------------------------------------------------- */
/* ----------------------------- 经纬大地高坐标系转站心坐标系 ----------------------------- */
@@ -139,16 +134,16 @@ int sat_gps_pos_clac(FILE * result_file,
double deltaz = pos_t[i].Z[sPRN] - obs_h->apZ;
tem3 = BLH2ENU(tem3, station->B, station->L, deltax, deltay, deltaz);
enu->E = tem3->E;
enu->N = tem3->N;
enu->U = tem3->U;
enu->E = tem3.E;
enu->N = tem3.N;
enu->U = tem3.U;
/* -------------------------------------------------------------------------- */
/* ---------------------------- 卫星方位角a，高度角h，向径r计算 --------------------------- */
tem4 = RAHCAL(tem4, enu->E, enu->N, enu->U);
rah->R = tem4->R;
rah->A = rad2deg(tem4->A);
rah->H = rad2deg(tem4->H);
rah->R = tem4.R;
rah->A = rad2deg(tem4.A);
rah->H = rad2deg(tem4.H);
/* -------------------------------------------------------------------------- */
if (blh->H < 0 || nav_b[best_epoch].sHEA != 0)
{
@@ -157,14 +152,14 @@ int sat_gps_pos_clac(FILE * result_file,
else
{
temb = DEG2DMS(temb, blh->B);
blh->B_d = temb->D;
blh->B_m = temb->M;
blh->B_s = temb->S;
blh->B_d = temb.D;
blh->B_m = temb.M;
blh->B_s = temb.S;
teml = DEG2DMS(teml, blh->L);
blh->L_d = teml->D;
blh->L_m = teml->M;
blh->L_s = teml->S;
blh->L_d = teml.D;
blh->L_m = teml.M;
blh->L_s = teml.S;
result_file = fopen(".\\Pos_out\\LLA_result_for_read.txt", "a+");
fprintf(result_file, "\nG%02d[sPRN] %4d° %02d′ %02d″ %4d° %02d′ %02d″ %15.05f",sPRN, blh->B_d, blh->B_m, blh->B_s, blh->L_d, blh->L_m, blh->L_s, blh->H);
deg2dms.h
+7 -8
View File
@@ -9,7 +9,7 @@ typedef struct
double D;
double M;
double S;
}deg2dms, *pdeg2dms;
}deg2dms;
/* -------------------------------------------------------------------------- */
/// @brief 将十进制的度数转换为度分秒形式
@@ -17,16 +17,15 @@ typedef struct
/// @param D 要转换度数
/// @return 返回自定义结构体包含度，分，秒成员
/* -------------------------------------------------------------------------- */
pdeg2dms DEG2DMS(pdeg2dms deg2dms,
deg2dms DEG2DMS(deg2dms deg2dms,
double D)
{
deg2dms = (pdeg2dms)malloc(sizeof(pdeg2dms));
int sig = D / fabs(D);//符号判断
deg2dms->D = (int)(D);//转换为度分秒
deg2dms->M = (int)((D - deg2dms->D) * 60);//分秒去除符号格式
deg2dms->S = (int)(((D - deg2dms->D) * 60 - deg2dms->M) * 60);
deg2dms->M = deg2dms->M * sig;
deg2dms->S = deg2dms->S * sig;
deg2dms.D = (int)(D);//转换为度分秒
deg2dms.M = (int)((D - deg2dms.D) * 60);//分秒去除符号格式
deg2dms.S = (int)(((D - deg2dms.D) * 60 - deg2dms.M) * 60);
deg2dms.M = deg2dms.M * sig;
deg2dms.S = deg2dms.S * sig;
return deg2dms;
}
main.exe
BIN
View File
Binary file not shown.
rahcal.h
+9 -10
View File
@@ -11,7 +11,7 @@ typedef struct
double R;
double A;
double H;
}rahcal, *prahcal;
}rahcal;
/* -------------------------------------------------------------------------- */
/// @brief 计算站心坐标系下的卫星高度方位角
@@ -21,17 +21,16 @@ typedef struct
/// @param U 站心坐标天
/// @return 返回自定义结构体变量包含R, A, H成员
/* -------------------------------------------------------------------------- */
prahcal RAHCAL(prahcal rahcal,
rahcal RAHCAL(rahcal rahcal,
double E, double N, double U)
{
rahcal = (prahcal)malloc(sizeof(prahcal));
rahcal->H = atan2(U, sqrt(E * E + N * N));
rahcal->A = atan2(E, U);
if (rahcal->A < 0)
rahcal->A += 2 * PI;
if (rahcal->A > 2 * PI)
rahcal->A -= 2 * PI;
rahcal->R = sqrt(E * E + N * N + U * U);
rahcal.H = atan2(U, sqrt(E * E + N * N));
rahcal.A = atan2(E, U);
if (rahcal.A < 0)
rahcal.A += 2 * PI;
if (rahcal.A > 2 * PI)
rahcal.A -= 2 * PI;
rahcal.R = sqrt(E * E + N * N + U * U);
return rahcal;
}
xyz2blh.h
+5 -6
View File
@@ -11,7 +11,7 @@ typedef struct
double B;
double L;
double H;
}xyz2blh, *pxyz2blh;
}xyz2blh;
/* -------------------------------------------------------------------------- */
/// @brief 将ECEF地心地固坐标转换为经纬高坐标
@@ -23,16 +23,15 @@ typedef struct
/// @param e2 椭球的第一偏心率平方
/// @return 返回自定义结构体类型包含B，L，H成员
/* -------------------------------------------------------------------------- */
pxyz2blh XYZ2BLH(pxyz2blh xyz2blh,
xyz2blh XYZ2BLH(xyz2blh xyz2blh,
double X, double Y, double Z)
{
xyz2blh = (pxyz2blh)malloc(sizeof(pxyz2blh));
double B = 0.0, N = 0.0, H = 0.0, R0, R1, deltaH, deltaB;
R0 = sqrt(pow(X, 2) + pow(Y, 2));
R1 = sqrt(pow(X, 2) + pow(Y, 2) + pow(Z, 2));
//经度直接求解
xyz2blh->L = atan2(Y, X);
xyz2blh.L = atan2(Y, X);
//迭代求大地维度和大地高
N = a;
H = R1 - a;
@@ -45,8 +44,8 @@ pxyz2blh XYZ2BLH(pxyz2blh xyz2blh,
H = R0 / cos(B) - N;
B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
} while (fabs(deltaH - H) > 1.0e-3 && fabs(deltaB - B) > 1.0e-9);
xyz2blh->B = B;
xyz2blh->H = H;
xyz2blh.B = B;
xyz2blh.H = H;
return xyz2blh;
}