Adding and modify many useful function

in their header file:
-blh2enu.h
-xyz2blh.h
-rahcal.h
-deg2dms.h
-degRrad.h

2gpst.h

@@ -11,7 +11,7 @@
 /// @param sec 秒
 /// @return 返回GPS周内秒
 /* -------------------------------------------------------------------------- */
-double TimetoGPSsec(int y, int m, int d, double h, int min, double sec)
+double Time2GPSsec(int y, int m, int d, double h, int min, double sec)
 {
 	if (m > 2)
 	{

blh2enu.h

@@ -0,0 +1,40 @@
+#ifndef _BLH2ENU_H_
+#define _BLH2ENU_H_
+
+#include<stdlib.h>
+#include<math.h>
+
+typedef struct
+{
+   double E;
+   double N;
+   double U;
+}blh2enu, *pblh2enu;
+
+/* -------------------------------------------------------------------------- */
+/// @brief 将大地高坐标转换为站心坐标
+/// @param blh2enu 自定义结构体类型用来传参
+/// @param stationB 站心纬度
+/// @param stationL 站心经度
+/// @param deltax 卫星到站心的x坐标差值
+/// @param deltay 卫星到站心的y坐标差值
+/// @param deltaz 卫星到站心的z坐标差值
+/// @return 返回自定义结构体类型包含E, N, U成员
+/* -------------------------------------------------------------------------- */
+pblh2enu BLH2ENU(pblh2enu blh2enu, 
+                 double stationB, double stationL,
+                 double deltax, double deltay, double deltaz)
+{
+    double sinB = sin(stationB);
+    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);
+
+    return blh2enu;
+}
+
+#endif

cal_mod.h

@@ -8,6 +8,10 @@
 #include"2gpst.h"
 #include"select_epo.h"
 #include"xyz2blh.h"
+#include"degRrad.h"
+#include"deg2dms.h"
+#include"blh2enu.h"
+#include"rahcal.h"
 
 /* -------------------------------------------------------------------------- */
 #define a       6378137.0//长半轴
@@ -143,7 +147,7 @@
                 int y = obs_e[i].y; int m = obs_e[i].m; int d = obs_e[i].d; 
                 double h = obs_e[i].h; int min = obs_e[i].min; double sec = obs_e[i].sec;
                 //转换为GPS周内秒
-                double GPSsec = TimetoGPSsec(y, m, d, h, min, sec);
+                double GPSsec = Time2GPSsec(y, m, d, h, min, sec);
                 int sPRN = obs_e[i].sPRN_GPS[j];
                 //遍历N文件GPS卫星数据块，寻找最佳历元
                 int best_epoch = select_epoch(GPSsec, sPRN, nav_b, gps_satnum);
@@ -164,35 +168,38 @@
                 /* ----------------------------- 地固坐标系转经纬大地高坐标系 ----------------------------- */
                 pxyz2blh tem1 = NULL;
                 tem1 = (pxyz2blh)malloc(sizeof(pxyz2blh));
-                tem1 = XYZtoBLH( tem1, pos_t[i].X[sPRN], pos_t[i].Y[sPRN], pos_t[i].Z[sPRN], a, e2);
+                tem1 = XYZ2BLH( tem1, pos_t[i].X[sPRN], pos_t[i].Y[sPRN], pos_t[i].Z[sPRN], a, e2);
-                blh->B = tem1->B;
+                blh->B = rad2deg(tem1->B);
-                blh->L = tem1->L;
+                blh->L = rad2deg(tem1->L);
-                blh->H = tem1->H;
+                blh->H = rad2deg(tem1->H);
 
                 pxyz2blh tem2 = NULL;
                 tem2 = (pxyz2blh)malloc(sizeof(pxyz2blh));
-                tem2 = XYZtoBLH( tem2, obs_h->apX, obs_h->apY, obs_h->apZ, a, e2);
+                tem2 = XYZ2BLH( tem2, obs_h->apX, obs_h->apY, obs_h->apZ, a, e2);
-                station->B = tem2->B / 57.295779513;
+                station->B = tem2->B;
-                station->L = tem2->L / 57.295779513;
+                station->L = tem2->L;
                 station->H = tem2->H;
+                /* -------------------------------------------------------------------------- */
                 
+                /* ----------------------------- 经纬大地高坐标系转站心坐标系 ----------------------------- */
+                double deltax = pos_t[i].X[sPRN] - obs_h->apX;
+                double deltay = pos_t[i].Y[sPRN] - obs_h->apY;
+                double deltaz = pos_t[i].Z[sPRN] - obs_h->apZ;
+                pblh2enu tem3 = NULL;
+                tem3 = (pblh2enu)malloc(sizeof(pblh2enu));
+                tem3 = BLH2ENU(tem3, station->B, station->L, deltax, deltay, deltaz);
+                enu->E = tem3->E;
+                enu->N = tem3->N;
+                enu->U = tem3->U;
                 /* -------------------------------------------------------------------------- */
-                double sinL = sin(station->L);
+
-                double cosL = cos(station->L);
-                double sinB = sin(station->B);
-                double cosB = cos(station->B);
-                enu->E = -sinL*(pos_t[i].X[sPRN] - obs_h->apX) + cosL*(pos_t[i].Y[sPRN] - obs_h->apY);
-                enu->N = -sinB*cosL*(pos_t[i].X[sPRN] - obs_h->apX) - sinB*sinL*(pos_t[i].Y[sPRN] - obs_h->apY) + cosB*(pos_t[i].Z[sPRN] - obs_h->apZ);
-                enu->U = cosB*cosL*(pos_t[i].X[sPRN] - obs_h->apX) + cosB*sinL*(pos_t[i].Y[sPRN] - obs_h->apY) + sinB*(pos_t[i].Z[sPRN] - obs_h->apZ);
-                /* -------------------------------------------------------------------------- */
                 /* ---------------------------- 卫星方位角a，高度角h，向径r计算 --------------------------- */
-                rah->H = atan2(enu->U, sqrt(enu->E * enu->E + enu->N * enu->N)) * 57.295779513;
+                prahcal tem4 = NULL;
-                rah->A = atan2(enu->E, enu->U) * 57.295779513;
+                tem4 = (prahcal)malloc(sizeof(prahcal));
-                    if (rah->A < 0)
+                tem4 = RAHCAL(tem4, enu->E, enu->N, enu->U);
-                        rah->A += 2 * PI;
+                rah->R = tem4->R;
-                    if (rah->A > 2 * PI)
+                rah->A = rad2deg(tem4->A);
-                        rah->A -= 2 * PI;
+                rah->H = rad2deg(tem4->H);
-                rah->R = sqrt(enu->E * enu->E + enu->N * enu->N + enu->U * enu->U);
                 /* -------------------------------------------------------------------------- */
 
                 if (blh->H < 0 || nav_b[best_epoch].sHEA != 0)
@@ -200,19 +207,20 @@
                     break;
                 }
                 else
-                {  int B_sig = blh->B / fabs(blh->B);//符号判断
+                {   
-                    blh->B_d = (int)(blh->B);//转换为度分秒
+                    pdeg2dms temb = NULL;
-                    blh->B_m = (int)((blh->B - blh->B_d) * 60);//分秒去除符号格式
+                    temb = (pdeg2dms)malloc(sizeof(pdeg2dms));
-                    blh->B_s = (int)(((blh->B - blh->B_d) * 60 - blh->B_m) * 60);
+                    temb = DEG2DMS(temb, blh->B);
-                    blh->B_m = blh->B_m * B_sig;
+                    blh->B_d = temb->D;
-                    blh->B_s = blh->B_s * B_sig;
+                    blh->B_m = temb->M;
+                    blh->B_s = temb->S;
 
-                    int L_sig = blh->L / fabs(blh->L);
+                    pdeg2dms teml = NULL;
-                    blh->L_d = (int)(blh->L);//转换为度分秒
+                    teml = (pdeg2dms)malloc(sizeof(pdeg2dms));
-                    blh->L_m = (int)((blh->L - blh->L_d) * 60);
+                    teml = DEG2DMS(teml, blh->L);
-                    blh->L_s = (int)(((blh->L - blh->L_d) * 60 - blh->L_m) * 60);
+                    blh->L_d = teml->D;
-                    blh->L_m = blh->L_m * L_sig;
+                    blh->L_m = teml->M;
-                    blh->L_s = blh->L_s * L_sig;
+                    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

@@ -0,0 +1,34 @@
+#ifndef _DEG2DMS_H_
+#define _DEG2DMS_H_
+
+#include<stdlib.h>
+#include<math.h>
+
+typedef struct
+{
+    double D;
+    double M;
+    double S;    
+}deg2dms, *pdeg2dms;
+
+/* -------------------------------------------------------------------------- */
+/// @brief 将十进制的度数转换为度分秒形式
+/// @param deg2dms 结构体传递参数
+/// @param D 要转换度数
+/// @return 返回自定义结构体包含度，分，秒成员                      
+/* -------------------------------------------------------------------------- */
+pdeg2dms DEG2DMS(pdeg2dms 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;
+
+    return deg2dms;
+}
+
+#endif

degRrad.h

@@ -0,0 +1,27 @@
+#ifndef _DEGRRAD_H_
+#define _DEGRRAD_H_
+#define PI 3.141592653589793
+
+/* -------------------------------------------------------------------------- */
+/// @brief 角度转弧度
+/// @param deg 
+/// @return 返回弧度
+/* -------------------------------------------------------------------------- */
+double deg2rad(double deg)
+{
+    double rad = (PI / 180) * deg;
+    return rad;
+}
+
+/* -------------------------------------------------------------------------- */
+/// @brief 弧度转角度                       
+/// @param rad 
+/// @return 返回角度
+/* -------------------------------------------------------------------------- */
+double rad2deg(double rad)
+{
+    double deg = (180 / PI) * rad;
+    return deg;
+}
+
+#endif

rahcal.h

@@ -0,0 +1,39 @@
+#ifndef _RAHCAL_H_
+#define _RAHCAL_H_
+#define PI 3.141592653589793
+
+
+#include<stdlib.h>
+#include<math.h>
+
+typedef struct 
+{
+    double R;
+    double A;
+    double H;
+}rahcal, *prahcal;
+
+/* -------------------------------------------------------------------------- */
+/// @brief 计算站心坐标系下的卫星高度方位角
+/// @param rahcal 自定义结构体变量用于传参
+/// @param E 站心坐标东
+/// @param N 站心坐标北
+/// @param U 站心坐标天
+/// @return 返回自定义结构体变量包含R, A, H成员                        
+/* -------------------------------------------------------------------------- */
+prahcal RAHCAL(prahcal 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);
+
+    return rahcal;
+}
+
+#endif

xyz2blh.h

@@ -23,7 +23,7 @@ typedef struct
 /// @param e2 椭球的第一偏心率平方
 /// @return 返回自定义结构体类型包含B，L，H成员
 /* -------------------------------------------------------------------------- */
-pxyz2blh XYZtoBLH(pxyz2blh xyz2blh, 
+pxyz2blh XYZ2BLH(pxyz2blh xyz2blh, 
                   double X, double Y, double Z, double a, double e2)
 
 {
@@ -32,7 +32,7 @@ pxyz2blh XYZtoBLH(pxyz2blh xyz2blh,
     R0 = sqrt(pow(X, 2) + pow(Y, 2));
     R1 = sqrt(pow(X, 2) + pow(Y, 2) + pow(Z, 2));
     //经度直接求解
-    xyz2blh->L = atan2(Y, X) * 57.295779513;
+    xyz2blh->L = atan2(Y, X);
     //迭代求大地维度和大地高
     N = a;
     H = R1 - a;
@@ -45,7 +45,7 @@ pxyz2blh XYZtoBLH(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 * 57.295779513;
+	xyz2blh->B = B;
 	xyz2blh->H = H;
 
     return xyz2blh;