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