Update: 2024/10/28

1.  KNZ_GeoTrackLab: Fix some bug.
2.  KNZ_GeoTrackLab: Optimize code structure.

README.md

@@ -36,6 +36,10 @@ Features ：
   
 ## Project Update
 
+Update: 2024/10/28
+1.  KNZ_GeoTrackLab: Fix some bug.
+2.  KNZ_GeoTrackLab: Optimize code structure.
+
 Rename the Project & Update: 2024/10/26
 1.	First release KNZ_GeoTrackLab.
 2.	KNZ_Calculate: Merged with KNZ_Plot.

READMECN.md

@@ -36,12 +36,15 @@
 
 ## 项目更新
 
-重命名项目并更新：2024/10/26
+更新：2024/10/28
+1. KNZ_GeoTrackLab：修复一些bug。
+2. KNZ_GeoTrackLab：优化代码结构。
 
-首次发布 KNZ_GeoTrackLab。
+重命名项目并更新：2024/10/26
-1. KNZ_Calculate：与 KNZ_Plot 合并。
+1. 首次发布 KNZ_GeoTrackLab。
-2. KNZ_Plot：与 KNZ_Calculate 合并。
+2. KNZ_Calculate：与 KNZ_Plot 合并。
-3. KNZ_GeoTrackLab：支持 GPS、北斗、伽利略和 GLONASS。
+3. KNZ_Plot：与 KNZ_Calculate 合并。
+4. KNZ_GeoTrackLab：支持 GPS、北斗、伽利略和 GLONASS。
 
 更新：2024/10/14
 1. KNZ_Calculate：支持 GPS、BDS 和伽利略。

app/Mainui.py

@@ -22,8 +22,6 @@ import matplotlib.pyplot as plt
 from matplotlib.backends.backend_tkagg import (
     FigureCanvasTkAgg, NavigationToolbar2Tk)
 
-dll = cdll.LoadLibrary('./brdm2pos.dll')
-dll.Start.restype = c_double
 #
 #
 #
@@ -46,8 +44,13 @@ satsysdict = {1: "G", 4:"R", 2:"C", 3:"E", 5:"S"}
 satellite_system = 1
 ##Combox Count-er
 ComboxObsCount = 0
+Obsvalue = ('',)
 ComboxNavCount = 0
+Navvalue = ('',)
 ComboxOutCount = 0
+Outvalue = ('',)
+#
+version = 'ver 1.5.3.1'
 #
 #
 #
@@ -68,7 +71,7 @@ def Abo_windows():
     abo.minsize(200, 100)
     abo.maxsize(200, 100)
 
-    About = tk.Label(abo, text="\nKNZ_GeoTrackLab ver1.5.1\n\nCopyright (c) 2024 by KenanZhu\nAll Right Reserved.")
+    About = tk.Label(abo, text="\nKNZ_GeoTrackLab %s\n\nCopyright (c) 2024 by KenanZhu\nAll Right Reserved."%version)
     About.pack(side=tk.TOP, expand=tk.YES)
 
 class FUNCTION:
@@ -204,10 +207,12 @@ class FUNCTION:
     @staticmethod
     def GetObsFilePath(ObsFileSelectBox):
         global ComboxObsCount
+        global Obsvalue
         path = filedialog.askopenfilename(title='RINEX OBS File',
                                           filetypes=[('RINEX OBS File(*.o*.*.*obs.*.*d)', '*.*o;*.*obs;*.*d'),
                                                      ('All Files', '*.*')])
         if path and path not in ObsFileSelectBox['value']:
+            Obsvalue += (path,)
             ObsFileSelectBox['value'] += (path,)
             ComboxObsCount += 1
             ObsFileSelectBox.current(ComboxObsCount)
@@ -215,11 +220,13 @@ class FUNCTION:
     @staticmethod
     def GetNavFilePath(NavFileSelectBox):
         global ComboxNavCount
+        global Navvalue
         path = filedialog.askopenfilename(title='RINEX NAV File',
                                           filetypes=[('RINEX NAV File(*.*nav.*.hnav.*.gnav.*.qnav.*.*n.*.*g.*.*h.*.*q.*.*p)',
                                                       '*.*nav;*.hnav;*.gnav;*.qnav;*.*n;*.*g;*.*h;*.*q;*.*p'),
                                                      ('All Files', '*.*')])
         if path and path not in NavFileSelectBox['value']:
+            Navvalue += (path,)
             NavFileSelectBox['value'] += (path,)
             ComboxNavCount += 1
             NavFileSelectBox.current(ComboxNavCount)
@@ -227,8 +234,10 @@ class FUNCTION:
     @staticmethod
     def AskDirectory(AskDirectorySelectBox):
         global ComboxOutCount
+        global Outvalue
         path = filedialog.askdirectory()
         if path and path not in AskDirectorySelectBox['value']:
+            Outvalue += (path,)
             AskDirectorySelectBox['value'] += (path + "/*.sp",)
             ComboxOutCount += 1
             AskDirectorySelectBox.current(ComboxOutCount)
@@ -293,8 +302,28 @@ class FUNCTION:
         # -----------------------------------------------#
 
         ExecuteState.config(text='Processing...... file : %s' % obs_path[obspath:])
-        dll.Start(nav_path.encode(), obs_path.encode(), res_path.encode(), satellite_system)
+
-        ExecuteState.config(text='Complete generate file : %s!' % obsfile + ".sp")
+        dll = cdll.LoadLibrary('./brdm2pos.dll')
+        dll.brdm2pos.restype = c_double
+        match dll.brdm2pos(nav_path.encode(), obs_path.encode(), res_path.encode(), satellite_system):
+            case  0.0:
+                ExecuteState.config(text='Generated successfully ! file : %s' % obsfile + ".sp")
+            case -1.0:
+                ExecuteState.config(text='Error ! cant open nav file')
+            case -2.0:
+                ExecuteState.config(text='Error ! cant open obs file')
+            case -3.0:
+                ExecuteState.config(text='Error ! unsupported nav file')
+            case -4.0:
+                ExecuteState.config(text='Error ! no nav data')
+            case -5.0:
+                ExecuteState.config(text='Error ! unsupported obs file')
+            case -6.0:
+                ExecuteState.config(text='Error ! no obs data')
+        windll.kernel32.FreeLibrary('./brdm2pos.dll')
+
+
+
 
     def _ExecuteFile(self,ExecuteState,
                      ObsSelectBoxVar,
@@ -347,7 +376,7 @@ class MAINGUI:
         hwnd.mainloop()
 
     def initGUI(self, hwnd):
-        hwnd.title("KNZ_GeoTrackLab ver1.5.1")
+        hwnd.title("KNZ_GeoTrackLab %s"%version)
         hwnd.geometry('1200x700')
         hwnd.minsize(1200, 700)
 
@@ -598,7 +627,6 @@ class MAINGUI:
     def SPP_calculat(self, hwnd):
         # Brief # read .sp file & draw in the canvas & output the .po file
         # Return# none
-
         # Const define
         self.sattrack.clear()
         C_V = 299792458
@@ -666,13 +694,14 @@ class MAINGUI:
             with open(o_path, 'w+') as of:  # Initialize-------#
                 of.close()
             of = open(o_path, 'a+')
-            print("@ GENERATE PROGRAM   : KNZ_GeoTrackLab ver1.5.1\n"
+            print("@ GENERATE PROGRAM   : KNZ_GeoTrackLab %s\n"
                   "@ GENERATE TYPE      : Receiver Station Position\n"
                   "@ GENERATE TIME      : %s\n"
                   "@ GENERATE SYS       : %s\n"
                   "@ IONOS OPT          : %s\n"
                   "@ TROPO OPT          : %s\n"
-                  "@\n\n" % (time.asctime(time.localtime())
+                  "@\n\n" % ( version
+                             , time.asctime(time.localtime())
                              , satsysdict.get(satellite_system)
                              , ioncormode[ioncorstate]
                              , trocormode[trocorstate]), file=of)
@@ -718,23 +747,19 @@ class MAINGUI:
                     epoch = int(line[2:6])
 
                     count = 0
-                    # Check the lost epoch
                     while count < epoch - epoch_last - 1:
                         ENU_O = np.hstack((ENU_O, [[0], [0], [0]]))
                         satnumnarry = np.hstack((satnumnarry, [[0]]))
                         print("------------ MISSING EPOCH DATA", file=of)
                         count += 1
                     epoch_last = epoch
-                    # Calculate start
                     while 1:
                         line = f.readline()
                         if line == "\n":
-                            
+
-                            # The sampling rate of 3D draw  
                             if epoch % 15 == 0:
                                 self.sattrack.scatter(satX[0,:], satY[0,:], satZ[0,:], s=5, zdir='z', c=SatColor[6] )
 
-                            # Get the viald satellite number
                             satnumnarry = np.hstack((satnumnarry, [[satnum]]))
 
                             match satsysdict.get(satellite_system):
@@ -765,7 +790,6 @@ class MAINGUI:
                                     B = np.empty((0, 4))
                                     L = np.empty((0, 1))
                                     while 1:
-                                        # Linear the observation equation
                                         R = math.sqrt(
                                             pow((satX[0, count] - ap_X), 2) + pow((satY[0, count] - ap_Y), 2) + pow(
                                                 (satZ[0, count] - ap_Z), 2))
@@ -804,7 +828,7 @@ class MAINGUI:
                                                     Pl = C1[0, count]
                                             case 2:
                                                 pass
-                                        # Omc matrix
+
                                         L = np.vstack(
                                             ( L, [Pl - R + C_V * Dt[0, count] - Tro] )
                                         )
@@ -812,8 +836,7 @@ class MAINGUI:
                                         count += 1
                                         if count == satnum:
                                             break
-                                    
+
-                                    # Construct the Least Squares
                                     P = np.diag(Ptem)
                                     BTP = np.dot(np.transpose(B), P)
                                     BTPB = np.dot(BTP, B)
@@ -1271,8 +1294,10 @@ class EXECUDUI:
         # -----------------------------------------------------------------------------------
         ObsInFrame3 = ttk.LabelFrame(TopFrame1, text='RINEX Obs File')
         ObsInFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='2px', fill=tk.X)
-        ObsFileSelectBox = ttk.Combobox(ObsInFrame3, width=46, height=4, values=('',),
+
+        ObsFileSelectBox = ttk.Combobox(ObsInFrame3, width=46, height=4, values=Obsvalue,
                                         textvariable=self.ObsSelectBoxVar)
+        ObsFileSelectBox.current(ComboxObsCount)
         ObsFileSelectBox.pack(side=tk.LEFT, anchor='w', padx='1px', pady='1px')
 
         ObsFileSelectButton = ttk.Button(ObsInFrame3, text='...', width=3,
@@ -1283,8 +1308,10 @@ class EXECUDUI:
         #-----------------------------------------------------------------------------------
         NavInFrame3 = ttk.LabelFrame(TopFrame1, text='RINEX Nav File')
         NavInFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='2px', fill=tk.X)
-        NavFileSelectBox = ttk.Combobox(NavInFrame3, width=46, height=4, values=('',),
+
+        NavFileSelectBox = ttk.Combobox(NavInFrame3, width=46, height=4, values=Navvalue,
                                         textvariable=self.NavSelectBoxVar)
+        NavFileSelectBox.current(ComboxNavCount)
         NavFileSelectBox.pack(side=tk.LEFT, anchor='w', padx='1px', pady='1px')
 
         NavFileSelectButton = ttk.Button(NavInFrame3, text='...', width=3,
@@ -1308,8 +1335,9 @@ class EXECUDUI:
         AskOrNotCheckButton.pack(side=tk.LEFT, anchor='w', padx='1px', pady='0px')
 
         ## SelectBox Initialize
-        AskDirectorySelectBox = ttk.Combobox(FileOutFrame3, width=36, height=4, values=('',),
+        AskDirectorySelectBox = ttk.Combobox(FileOutFrame3, width=36, height=4, values=Outvalue,
                                              state=tk.DISABLED, textvariable=self.DirectorySelectBoxVar)
+        AskDirectorySelectBox.current(ComboxOutCount)
         AskDirectorySelectBox.pack(side=tk.LEFT, anchor='w', padx='1px', pady='0px')
 
         ## SelectButton Initialize

src/brdm2pos.h

@@ -1,37 +1,33 @@
-// pch.h: 这是预编译标头文件。
+#pragma once
-// 下方列出的文件仅编译一次，提高了将来生成的生成性能。
-// 这还将影响 IntelliSense 性能，包括代码完成和许多代码浏览功能。
-// 但是，如果此处列出的文件中的任何一个在生成之间有更新，它们全部都将被重新编译。
-// 请勿在此处添加要频繁更新的文件，这将使得性能优势无效。
 
-#ifndef PCH_H
+#ifndef _BRDM2POS_H_
-#define PCH_H
+#define _BRDM2POS_H_
 
-// 添加要在此处预编译的标头
+#include <iostream>
-#include "framework.h"
+#include <math.h>
+#include <time.h>
 
-#endif
+#endif // _BRDM2POS_H_
-/* ------------------------------ CALCU CONST ------------------------------- */
+
+/* CALCU CONST*/
 #define C_V     299792458
 #define GM      398600500000000
 #define math_e  2.718281828459
 #define PI      3.141592653589793
 #define Earth_e 7.2921151467e-5
-#define C20	1.0826257e-3
+#define C20		1.0826257e-3
-
+/*WGS-84 CONST*/
-/* ------------------------------ WGS-84 CONST ------------------------------ */
 #define a       6378137.0
 #define e2      (0.0033528106647475*(2 - 0.0033528106647475))
-/* ------------------------------- PZ-90 CONST ------------------------------ */
+/*PZ-90 CONST*/
-//
 #define TSTEP	30.0
-/* ----------------------------- GNSS OBSERV TYPE --------------------------- */
+/*GNSS OBSERV TYPE*/
 //frequency 1
-#define C1	100
+#define C1		100
 #define C1A     100
 #define C1B     101
 #define C1C     102
-#define C1D	103
+#define C1D		103
 #define C1E
 #define C1F
 #define C1G
@@ -39,24 +35,23 @@
 #define C1I
 #define C1J
 #define C1K
-#define C1L	104
+#define C1L		104
-#define C1M	105
+#define C1M		105
 #define C1N
 #define C1O
-#define C1P	106
+#define C1P		106
 #define C1Q
 #define C1R
-#define C1S	107
+#define C1S		107
 #define C1T
 #define C1U
 #define C1V
 #define C1W
-#define C1X	108
+#define C1X		108
-#define C1Y	109
+#define C1Y		109
-#define C1Z	110
+#define C1Z		110
-
 //frequency 2
-#define C2	200
+#define C2		200
 #define C2A
 #define C2B
 #define C2C     200
@@ -65,7 +60,7 @@
 #define C2F
 #define C2G
 #define C2H
-#define C2I	202
+#define C2I		202
 #define C2J
 #define C2K
 #define C2L     203
@@ -80,24 +75,21 @@
 #define C2U
 #define C2V
 #define C2W
-#define C2X	208
+#define C2X		208
 #define C2Y     209
 #define C2Z     210
-
 //frequency 3
 #define C3      300
-#define C3I	300
+#define C3I		300
-#define C3Q	301
+#define C3Q		301
-#define C3X	302
+#define C3X		302
-
 //frequency 4
-#define C4	400
+#define C4		400
-#define C4A	400
+#define C4A		400
-#define C4B	401
+#define C4B		401
-#define C4X	402
+#define C4X		402
-
 //frequency 5
-#define C5	500
+#define C5		500
 #define C5A
 #define C5B
 #define C5C
@@ -124,12 +116,11 @@
 #define C5X     502
 #define C5Y
 #define C5Z
-
 //frequency 6
-#define C6	600
+#define C6		600
-#define C6A	600
+#define C6A		600
-#define C6B	601
+#define C6B		601
-#define C6C	602
+#define C6C		602
 #define C6D
 #define C6E
 #define C6F
@@ -150,12 +141,11 @@
 #define C6U
 #define C6V
 #define C6W
-#define C6X	603
+#define C6X		603
 #define C6Y
-#define C6Z	604
+#define C6Z		604
-
 //frequency 7
-#define C7	700
+#define C7		700
 #define C7A
 #define C7B
 #define C7C
@@ -164,7 +154,7 @@
 #define C7F
 #define C7G
 #define C7H
-#define C7I	700
+#define C7I		700
 #define C7J
 #define C7K
 #define C7L
@@ -172,34 +162,30 @@
 #define C7N
 #define C7O
 #define C7P
-#define C7Q	701
+#define C7Q		701
 #define C7R
 #define C7S
 #define C7T
 #define C7U
 #define C7V
 #define C7W
-#define C7X	702
+#define C7X		702
 #define C7Y
 #define C7Z
-
 //frequency 8
-#define	C8	800
+#define	C8		800
-#define C8I	800
+#define C8I		800
-#define C8Q	801
+#define C8Q		801
-#define C8X	802
+#define C8X		802
+/*GNSS CODE*/
+#define GPS		 01
+#define BDS		 02
+#define GAL		 03
+#define GLO		 04
+#define SBAS	 05
 
-/* ------------------------------- GNSS CODE -------------------------------- */
-#define GPS	01
-#define BDS	02
-#define GAL	03
-#define GLO	04
-#define SBAS	05
-
-/* ------------------------------ FOR CALMOD.H ------------------------------ */
 #define MAXRINEX 1000
 #define DAYSEC	 86400
-
 //satellite position & clock bias structure
 typedef struct
 {
@@ -248,14 +234,13 @@ typedef struct
 	double N;
 	double U;
 }blh2enu;
-//longitude & latitude structure
+
 typedef struct
 {
 	double B;
 	double L;
 	double H;
 }xyz2blh;
-/* ------------------------------- FOR READ.H ------------------------------- */
 //header data of file of nav
 typedef struct nav_head
 {
@@ -290,22 +275,22 @@ typedef struct nav_body
 	int TOC_Sec;
 	double sa0;
 	double sa1;
-	double sa2;					double Dos;
+	double sa2;										double Dos;
 
-	double IODE;	double AODE;			double SatX;
+	double IODE;	double AODE;					double SatX;
-	double Crs;					double SatXv;
+	double Crs;										double SatXv;
-	double deltan;					double SatXa;
+	double deltan;									double SatXa;
 	double M0;
 
-	double Cuc;					double SatY;
+	double Cuc;										double SatY;
-	double e;					double SatYv;
+	double e;										double SatYv;
-	double Cus;					double SatYa;
+	double Cus;										double SatYa;
-	double sqrtA;					int FreN;
+	double sqrtA;									int FreN;
 
-	double TOE;					double SatZ;
+	double TOE;										double SatZ;
-	double Cic;					double SatZv;
+	double Cic;										double SatZv;
-	double OMEGA;					double SatZa;
+	double OMEGA;									double SatZa;
-	double Cis;					int AOO;
+	double Cis;										int AOO;
 
 	double i0;
 	double Crc;
@@ -313,17 +298,17 @@ typedef struct nav_body
 	double deltaomega;
 
 	double IDOT;
-	double L2code;			int Datasource;
+	double L2code;				    int Datasource;
 	double GPSweek; double BDTweek; double GALweek;
 	double L2Pflag;
 
 	double sACC;
 	double sHEA;
-	double TGD;	double TGD1;	double BGDa;
+	double TGD;		double TGD1;	double BGDa;
 	double IODC;	double TGD2;	double BGDb;
 
 	double TTN;
-	double fit;	double AODC;
+	double fit;		double AODC;
 	double spare1;
 	double spare2;
 
@@ -391,8 +376,30 @@ typedef struct obs_body
 	double obs_gal[20][26];//GALILEO obs data
 	double obs_glo[20][26];//GLONASS obs data
 }obs_body, * pobs_body;
-
+//
-extern "C" _declspec(dllexport) double Start(char nav_path[260], char obs_path[260], char res_path[260], int gnsscode);
+extern double JDUTC2GPST(double);
-
+extern double Time2GPST(int, int, int, double, int, double);
-
+extern double UTCTime2JD(int, int, int, double, int, double);
+//
+extern double deg2rad(double);
+extern double rad2deg(double);
+//
+extern rahcal RAHCAL(rahcal, double, double, double);
+extern blh2enu BLH2ENU(blh2enu, double, double, double, double, double);
+extern xyz2blh XYZ2BLH(xyz2blh, double, double, double);
+//
+extern int getsatnum(FILE*);
+extern int get_epochnum(FILE*);
+extern void initobs_e(pobs_epoch, pobs_body);
+extern void freeobs_e(pobs_epoch, pobs_body);
+extern int Type2Code(int, char buff[MAXRINEX]);
+extern int Code2Type(int, int, int typearr[36]);
+extern int select_epoch(double, int, pnav_body, int, int);
+//
+extern void read_n_h(FILE*, pnav_head);
+extern void read_n_b(FILE*, pnav_body);
+extern void read_o_h(FILE*, pobs_head);
+extern void read_o_eb(FILE*, pobs_head, pobs_epoch, pobs_body);
+//
+extern "C" _declspec(dllexport) double brdm2pos(char nav_path[260], char obs_path[260], char res_path[260], int gnsscode);
 

src/coordtrans.cpp

@@ -0,0 +1,99 @@
+#include "brdm2pos.h"
+using namespace std;
+/* -------------------------------------------------------------------------- */
+/// @brief degree to radian
+/// @param deg 
+/// @return return radian
+/* -------------------------------------------------------------------------- */
+double deg2rad(double deg)
+{
+	double rad = (PI / 180) * deg;
+	return rad;
+}
+/* -------------------------------------------------------------------------- */
+/// @brief radian to degree                      
+/// @param rad 
+/// @return return degree
+/* -------------------------------------------------------------------------- */
+double rad2deg(double rad)
+{
+	double deg = (180 / PI) * rad;
+	return deg;
+}
+/* -------------------------------------------------------------------------- */
+/// @brief satellite azimuth angle calculation
+/// @param rahcal parameter transfer structure
+/// @param E east
+/// @param N north
+/// @param U up
+/// @return return structure contain R, A, H members                      
+/* -------------------------------------------------------------------------- */
+rahcal RAHCAL(rahcal rahcal,
+	double E, double N, double 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;
+}
+/* -------------------------------------------------------------------------- */
+/// @brief convert BLH coordinates to ENU coordinates
+/// @param blh2enu parameter transfer structure
+/// @param stationB latitude of receiver station
+/// @param stationL longitude of receiver station
+/// @param deltax X-coordinate difference of satellite arrival center
+/// @param deltay Y-coordinate difference of satellite arrival center
+/// @param deltaz Z-coordinate difference of satellite arrival center
+/// @return return type of structure contain E, N, U members
+/* -------------------------------------------------------------------------- */
+blh2enu BLH2ENU(blh2enu 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.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;
+}
+/* -------------------------------------------------------------------------- */
+/// @brief Convert ECEF geostationary coordinates to BLH coordinates
+/// @param xyz2blh parameter transfer structure
+/// @param X X coordinates of ECEF
+/// @param Y Y coordinates of ECEF
+/// @param Z Z coordinates of ECEF
+/// @param a major semiaxis of WGS-84 ellipsoid
+/// @param e2 the square of the first eccentricity of WGS-84 ellipsoid
+/// @return return type of structure contain B, L, H members
+/* -------------------------------------------------------------------------- */
+xyz2blh XYZ2BLH(xyz2blh xyz2blh,
+	double X, double Y, double Z)
+{
+	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);
+	N = a;
+	H = R1 - a;
+	B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
+	do{
+		deltaH = N;
+		deltaB = B;
+		N = a / sqrt(1 - e2 * pow(sin(B), 2));
+		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;
+
+	return xyz2blh;
+}

src/framework.h

@@ -1,5 +0,0 @@
-#pragma once
-
-#define WIN32_LEAN_AND_MEAN             // 从 Windows 头文件中排除极少使用的内容
-// Windows 头文件
-#include <windows.h>

src/pch.cpp

@@ -1,5 +0,0 @@
-// pch.cpp: 与预编译标头对应的源文件
-
-#include "pch.h"
-
-// 当使用预编译的头时，需要使用此源文件，编译才能成功。

src/readrnx.cpp

@@ -0,0 +1,847 @@
+#include "brdm2pos.h"
+using namespace std;
+/* -------------------------------------------------------------------------- */
+/// @brief find the nearest obs epoch with boardcast
+/// @param SecofWeek second of week of obs epoch time
+/// @param sPRN the satellite PRN which need to match
+/// @param nav_b nav data file structure
+/// @param satnum total number of boardcast satellite 
+/// @param syscode GNSS code, more on PPUBLIC.H
+/// @return Return the absolute epoch number of the best epoch 
+///			When there is no matching satellite number, the function returns -1
+/* -------------------------------------------------------------------------- */
+int select_epoch(double SecofWeek, int sPRN, pnav_body nav_b, int satnum, int syscode)
+{
+	int best_epoch = -1;
+	double min = 10000;//Initialize the minimum value
+	double Min;
+	if (syscode == GPS){//Be judged as GPS
+		for (int i = 0; i < satnum; i++){
+			if (sPRN == nav_b[i].sPRN_GPS){
+				Min = fabs(SecofWeek - nav_b[i].TOE);
+				if (Min <= min){
+					best_epoch = i;
+					min = Min;
+				}
+			}
+		}return best_epoch;
+	}
+	if (syscode == BDS){//Be judged as BeiDou
+		for (int i = 0; i < satnum; i++){
+			if (sPRN == nav_b[i].sPRN_BDS){
+				Min = fabs(SecofWeek - nav_b[i].TOE);
+				if (Min <= min){
+					best_epoch = i;
+					min = Min;
+				}
+			}
+		}return best_epoch;
+	}
+	if (syscode == GAL){//Be judged as Galileo
+		for (int i = 0; i < satnum; i++){
+			if (sPRN == nav_b[i].sPRN_GAL){
+				Min = fabs(SecofWeek - nav_b[i].TOE);
+				if (Min <= min){
+					best_epoch = i;
+					min = Min;
+				}
+			}
+		}return best_epoch;
+	}
+	if (syscode == GLO){
+		for (int i = 0; i < satnum; i++){
+			if (sPRN == nav_b[i].sPRN_GLO){
+				Min = fabs(SecofWeek - nav_b[i].TOE);
+				if (Min <= min)
+				{
+					best_epoch = i;
+					min = Min;
+				}
+			}
+		}return best_epoch;
+	}return best_epoch;
+}
+/* -------------------------------------------------------------------------- */
+/// @brief converts the observation type to the numeric code defined in PUBLIC.H
+/// @param i reads the number of incoming loops in the loop
+/// @param buff type of incoming observation
+/// @return Returns the obs code
+/* -------------------------------------------------------------------------- */
+int Type2Code(int i, char buff[MAXRINEX])
+{
+	int  code_return = 0;
+	char code_contrast[4];
+	char code_original[4];
+
+	const char L1codeindex[] = "C1AC1BC1CC1DC1LC1MC1PC1SC1XC1YC1Z";//obs type of different frequency 
+	const char L2codeindex[] = "C2CC2DC2IC2LC2MC2PC2QC2SC2XC2YC2Z";
+	const char L3codeindex[] = "C3IC3QC3X";
+	const char L4codeindex[] = "C5IC5QC5X";
+	const char L5codeindex[] = "C5IC5QC5X";
+	const char L6codeindex[] = "C6AC6BC6CC6XC6Z";
+	const char L7codeindex[] = "C7IC7QC7XC7DC7PC7Z";
+	const char L8codeindex[] = "C8IC8QC8X";
+
+	memset(code_contrast, 0, sizeof(code_contrast));
+	memset(code_original, 0, sizeof(code_original));
+	strncpy(code_original, buff + 7 + 4 * i, 3);
+
+	if (strstr(code_original, "1")) {
+		for (int j = 0; j < (strlen(L1codeindex) / 3); j++) {
+			strncpy(code_contrast, L1codeindex + 0 + j * 3, 3);
+			if (strcmp(code_contrast, code_original) == 0) {
+				code_return = j + 100;
+				return code_return;
+				break;
+			}
+		}
+	}
+	if (strstr(code_original, "2")) {
+		for (int j = 0; j < (strlen(L2codeindex) / 3); j++) {
+			strncpy(code_contrast, L2codeindex + 0 + j * 3, 3);
+			strncpy(code_original, buff + 7 + 4 * i, 3);
+			if (strcmp(code_contrast, code_original) == 0) {
+				code_return = j + 200;
+				return code_return;
+				break;
+			}
+		}
+	}
+	if (strstr(code_original, "3")) {
+		for (int j = 0; j < (strlen(L3codeindex) / 3); j++) {
+			strncpy(code_contrast, L3codeindex + 0 + j * 3, 3);
+			strncpy(code_original, buff + 7 + 4 * i, 3);
+			if (strcmp(code_contrast, code_original) == 0) {
+				code_return = j + 500;
+				return code_return;
+				break;
+			}
+		}
+	}
+	if (strstr(code_original, "4")) {
+		for (int j = 0; j < (strlen(L3codeindex) / 3); j++) {
+			strncpy(code_contrast, L3codeindex + 0 + j * 3, 3);
+			strncpy(code_original, buff + 7 + 4 * i, 3);
+			if (strcmp(code_contrast, code_original) == 0) {
+				code_return = j + 500;
+				return code_return;
+				break;
+			}
+		}
+	}
+	if (strstr(code_original, "5")) {
+		for (int j = 0; j < (strlen(L5codeindex) / 3); j++) {
+			strncpy(code_contrast, L5codeindex + 0 + j * 3, 3);
+			strncpy(code_original, buff + 7 + 4 * i, 3);
+			if (strcmp(code_contrast, code_original) == 0) {
+				code_return = j + 500;
+				return code_return;
+				break;
+			}
+		}
+	}
+	if (strstr(code_original, "6")) {
+		for (int j = 0; j < (strlen(L6codeindex) / 3); j++) {
+			strncpy(code_contrast, L6codeindex + 0 + j * 3, 3);
+			strncpy(code_original, buff + 7 + 4 * i, 3);
+			if (strcmp(code_contrast, code_original) == 0) {
+				code_return = j + 600;
+				return code_return;
+				break;
+			}
+		}
+	}
+	if (strstr(code_original, "7")) {
+		for (int j = 0; j < (strlen(L7codeindex) / 3); j++) {
+			strncpy(code_contrast, L7codeindex + 0 + j * 3, 3);
+			strncpy(code_original, buff + 7 + 4 * i, 3);
+			if (strcmp(code_contrast, code_original) == 0) {
+				code_return = j + 700;
+				return code_return;
+				break;
+			}
+		}
+	}
+	if (strstr(code_original, "8")) {
+		for (int j = 0; j < (strlen(L8codeindex) / 3); j++) {
+			strncpy(code_contrast, L8codeindex + 0 + j * 3, 3);
+			strncpy(code_original, buff + 7 + 4 * i, 3);
+			if (strcmp(code_contrast, code_original) == 0) {
+				code_return = j + 800;
+				return code_return;
+				break;
+			}
+		}
+	}return code_return;
+}
+/* -------------------------------------------------------------------------- */
+/// @brief the type of the obs value read matches the type code stored in an array
+/// @param typecode obs type that you want to match
+/// @param typenum the total number of obs types
+/// @param typearr type codes store arrays
+/// @return returns the array position of the corresponding obs type code
+/* -------------------------------------------------------------------------- */
+int Code2Type(int typecode, int typenum, int typearr[36])
+{
+	int pos;
+	for (pos = 0; pos < typenum; pos++) {
+		if ((typearr[pos] - typearr[pos] % 100) == typecode) {
+			return pos;
+		}
+	}return pos;
+}
+/* -------------------------- Read the file of rnx -------------------------- */
+/* -------------------------------------------------------------------------- */
+/// @brief Convert the string to number
+/// @param buff 
+/// @param i Position of begin to read
+/// @param n The number of character will to capture
+/// @return Number of string you captured
+/* -------------------------------------------------------------------------- */
+static double strtonum(const char* buff, int i, int n)
+{
+	double value = 0.0;
+	char str[256] = { 0 };
+	char* p = str;
+	/* ---------------------------------- */
+	/// Function will return 0.0 as error code when three occasion are below:
+	/// 1.Begining position < 0
+	/// 2.The number of Byte of read < i
+	/// 3.The Byte size of string < n
+	/* ---------------------------------- */
+	if (i < 0 || (int)strlen(buff) < i || (int)sizeof(str) - 1 < n) {
+		return 0.0;
+	}
+	for (buff += i; *buff && --n >= 0; buff++) {
+		*p++ = ((*buff == 'D' || *buff == 'd') ? 'e' : *buff);
+	}
+	*p = '\0';
+	return sscanf(str, "%lf", &value) == 1 ? value : 0.0;
+}
+/*Allocate the memory of observation structure*/
+void initobs_e(pobs_epoch obs_e, pobs_body obs_b)
+{
+	obs_e = (pobs_epoch)malloc(sizeof(obs_epoch));
+	obs_b = (pobs_body)malloc(sizeof(obs_body));
+}
+/*Free the memory of observation structure*/
+void freeobs_e(pobs_epoch obs_e, pobs_body obs_b)
+{
+	free(obs_e); free(obs_b);
+}
+/*Read the file of nav*/
+/* -------------------------------------------------------------------------- */
+/// @brief get the total number of satellite boardcast data of nav file 
+/// @param fp_nav the file pointer of nav file
+/// @return total number of satellite boardcast data
+/* -------------------------------------------------------------------------- */
+int getsatnum(FILE* fp_nav)
+{
+	int satnum = 0;
+	char buff[MAXRINEX];
+	char satvar;
+	char* lable = buff + 60;
+	while (fgets(buff, MAXRINEX, fp_nav)) {	
+		if (strstr(lable, "RINEX VERSION / TYPE")) {
+			if (strtonum(buff, 0, 9) < 3 || strtonum(buff, 0, 9) >= 4) return -1;
+		}
+		if (strstr(lable, "END OF HEADER")) {
+			while (fgets(buff, MAXRINEX, fp_nav)) {
+				strncpy(&satvar, buff + 0, 1);
+				if (satvar == 'G' || satvar == 'E' || satvar == 'R' || satvar == 'S' || satvar == 'C' || satvar == 'I' || satvar == 'J') {
+					satnum++;
+				}
+				else {
+					continue;
+				}
+			}
+		}
+	}return satnum;
+}
+/*Read the header data of file of nav*/
+void read_n_h(FILE* fp_nav, pnav_head nav_h)
+{
+	char buff[MAXRINEX] = { 0 };
+	char* lable = buff;
+	int i = 0;
+	int j = 0;
+	while (fgets(buff, MAXRINEX, fp_nav)) {
+		if (strstr(lable, "RINEX VERSION / TYPE")) {
+			nav_h->ver = strtonum(buff, 0, 9);
+			strncpy((nav_h->type), buff + 20, 15);
+			continue;
+		}
+		else if (strstr(lable, "GPSA")) {
+			nav_h->ION_GPSA[0] = strtonum(buff, 6, 12);
+			nav_h->ION_GPSA[1] = strtonum(buff, 6 + 12, 12);
+			nav_h->ION_GPSA[2] = strtonum(buff, 6 + 12 + 12, 12);
+			nav_h->ION_GPSA[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
+			continue;
+		}
+		else if (strstr(lable, "GPSB")) {
+			nav_h->ION_GPSB[0] = strtonum(buff, 6, 12);
+			nav_h->ION_GPSB[1] = strtonum(buff, 6 + 12, 12);
+			nav_h->ION_GPSB[2] = strtonum(buff, 6 + 12 + 12, 12);
+			nav_h->ION_GPSB[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
+			continue;
+		}
+		else if (strstr(lable, "BDSA")) {
+			nav_h->ION_BDSA[0] = strtonum(buff, 6, 12);
+			nav_h->ION_BDSA[1] = strtonum(buff, 6 + 12, 12);
+			nav_h->ION_BDSA[2] = strtonum(buff, 6 + 12 + 12, 12);
+			nav_h->ION_BDSA[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
+			continue;
+		}
+		else if (strstr(lable, "BDSB")) {
+			nav_h->ION_BDSB[0] = strtonum(buff, 6, 12);
+			nav_h->ION_BDSB[1] = strtonum(buff, 6 + 12, 12);
+			nav_h->ION_BDSB[2] = strtonum(buff, 6 + 12 + 12, 12);
+			nav_h->ION_BDSB[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
+			continue;
+		}
+		else if (strstr(lable, "BDUT")) {
+			nav_h->BDUT[0] = strtonum(buff, 5, 17);
+			nav_h->BDUT[1] = strtonum(buff, 5 + 17, 16);
+			nav_h->BDUT[2] = strtonum(buff, 5 + 17 + 16, 7);
+			nav_h->BDUT[3] = strtonum(buff, 5 + 17 + 16 + 7, 5);
+			continue;
+		}
+		else if (strstr(lable, "GPUT")) {
+			nav_h->GPUT[0] = strtonum(buff, 5, 17);
+			nav_h->GPUT[1] = strtonum(buff, 5 + 17, 16);
+			nav_h->GPUT[2] = strtonum(buff, 5 + 17 + 16, 7);
+			nav_h->GPUT[3] = strtonum(buff, 5 + 17 + 16 + 7, 5);
+			continue;
+		}
+		else if (strstr(lable, "LEAP SECONDS")) {
+			nav_h->leap = (int)strtonum(buff, 4, 2);
+		}
+		else if (strstr(lable, "END OF HEADER"))
+			break;
+	}
+}
+/*Read the body data of file of nav*/
+void read_n_b(FILE* fp_nav, pnav_body nav_b)
+{
+	int i_g = 0;
+	char buff[MAXRINEX] = { 0 };
+	char flag = { 0 };
+	while (fgets(buff, MAXRINEX, fp_nav)) {
+		int j_g = 0;
+		int j_c = 0;
+		int j_ga = 0;
+		int j_gl = 0;
+		strncpy(&flag, buff + 0, 1);
+		//GPS SATELLITE
+		if (flag == 'G') {
+			for (j_g = 0; j_g < 8; j_g++) {
+				switch (j_g) {
+				case 0:
+					nav_b[i_g].sPRN_BDS = -1;
+					nav_b[i_g].sPRN_GAL = -1;
+					nav_b[i_g].sPRN_GPS = (int)strtonum(buff, 1, 2);
+					nav_b[i_g].TOC_Y = (int)strtonum(buff, 4, 4);
+					nav_b[i_g].TOC_M = (int)strtonum(buff, 9, 2);
+					nav_b[i_g].TOC_D = (int)strtonum(buff, 12, 2);
+					nav_b[i_g].TOC_H = (int)strtonum(buff, 15, 2);
+					nav_b[i_g].TOC_Min = (int)strtonum(buff, 18, 2);
+					nav_b[i_g].TOC_Sec = strtonum(buff, 21, 2);
+					nav_b[i_g].sa0 = strtonum(buff, 23, 19);
+					nav_b[i_g].sa1 = strtonum(buff, 23 + 19, 19);
+					nav_b[i_g].sa2 = strtonum(buff, 23 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 1:
+					nav_b[i_g].IODE = strtonum(buff, 4, 19);
+					nav_b[i_g].Crs = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].deltan = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].M0 = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 2:
+					nav_b[i_g].Cuc = strtonum(buff, 4, 19);
+					nav_b[i_g].e = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].Cus = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].sqrtA = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 3:
+					nav_b[i_g].TOE = strtonum(buff, 4, 19);
+					nav_b[i_g].Cic = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].OMEGA = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].Cis = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 4:
+					nav_b[i_g].i0 = strtonum(buff, 4, 19);
+					nav_b[i_g].Crc = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].omega = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].deltaomega = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 5:
+					nav_b[i_g].IDOT = strtonum(buff, 4, 19);
+					nav_b[i_g].L2code = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].GPSweek = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].L2Pflag = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 6:
+					nav_b[i_g].sACC = strtonum(buff, 4, 19);
+					nav_b[i_g].sHEA = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].TGD = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].IODC = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 7:
+					nav_b[i_g].TTN = strtonum(buff, 4, 19);
+					nav_b[i_g].fit = strtonum(buff, 4 + 19, 19);
+					//nav_b[i_g].spare1 = strtonum(buff, 4 + 19 + 19, 19);
+					//nav_b[i_g].spare2 = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					break;
+				}
+			}
+			i_g++;
+			continue;
+		}
+		//BDS SATELLITE
+		if (flag == 'C') {
+			for (j_c = 0; j_c < 8; j_c++) {
+				switch (j_c) {
+				case 0:
+					nav_b[i_g].sPRN_GPS = -1;
+					nav_b[i_g].sPRN_GAL = -1;
+					nav_b[i_g].sPRN_BDS = (int)strtonum(buff, 1, 2);
+					nav_b[i_g].TOC_Y = (int)strtonum(buff, 4, 4);
+					nav_b[i_g].TOC_M = (int)strtonum(buff, 9, 2);
+					nav_b[i_g].TOC_D = (int)strtonum(buff, 12, 2);
+					nav_b[i_g].TOC_H = (int)strtonum(buff, 15, 2);
+					nav_b[i_g].TOC_Min = (int)strtonum(buff, 18, 2);
+					nav_b[i_g].TOC_Sec = strtonum(buff, 21, 2);
+					nav_b[i_g].sa0 = strtonum(buff, 23, 19);
+					nav_b[i_g].sa1 = strtonum(buff, 23 + 19, 19);
+					nav_b[i_g].sa2 = strtonum(buff, 23 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 1:
+					nav_b[i_g].AODE = strtonum(buff, 4, 19);
+					nav_b[i_g].Crs = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].deltan = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].M0 = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 2:
+					nav_b[i_g].Cuc = strtonum(buff, 4, 19);
+					nav_b[i_g].e = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].Cus = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].sqrtA = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 3:
+					nav_b[i_g].TOE = strtonum(buff, 4, 19);
+					nav_b[i_g].Cic = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].OMEGA = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].Cis = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 4:
+					nav_b[i_g].i0 = strtonum(buff, 4, 19);
+					nav_b[i_g].Crc = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].omega = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].deltaomega = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 5:
+					nav_b[i_g].IDOT = strtonum(buff, 4, 19);
+					//nav_b[i_g].L2code = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].BDTweek = strtonum(buff, 4 + 19 + 19, 19);
+					//nav_b[i_g].L2Pflag = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 6:
+					nav_b[i_g].sACC = strtonum(buff, 4, 19);
+					nav_b[i_g].sHEA = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].TGD1 = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].TGD2 = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 7:
+					//nav_b[i_g].TTN = strtonum(buff, 4, 19);
+					nav_b[i_g].AODC = strtonum(buff, 4 + 19, 19);
+					//nav_b[i_g].spare1 = strtonum(buff, 4 + 19 + 19, 19);
+					//nav_b[i_g].spare2 = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					break;
+				}
+			}
+			i_g++;
+			continue;
+		}
+		//GAL SATELLITE
+		if (flag == 'E') {
+			for (j_ga = 0; j_ga < 8; j_ga++) {
+				switch (j_ga) {
+				case 0:
+					nav_b[i_g].sPRN_BDS = -1;
+					nav_b[i_g].sPRN_GPS = -1;
+					nav_b[i_g].sPRN_GAL = (int)strtonum(buff, 1, 2);
+					nav_b[i_g].TOC_Y = (int)strtonum(buff, 4, 4);
+					nav_b[i_g].TOC_M = (int)strtonum(buff, 9, 2);
+					nav_b[i_g].TOC_D = (int)strtonum(buff, 12, 2);
+					nav_b[i_g].TOC_H = (int)strtonum(buff, 15, 2);
+					nav_b[i_g].TOC_Min = (int)strtonum(buff, 18, 2);
+					nav_b[i_g].TOC_Sec = strtonum(buff, 21, 2);
+					nav_b[i_g].sa0 = strtonum(buff, 23, 19);
+					nav_b[i_g].sa1 = strtonum(buff, 23 + 19, 19);
+					nav_b[i_g].sa2 = strtonum(buff, 23 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 1:
+					nav_b[i_g].IODE = strtonum(buff, 4, 19);
+					nav_b[i_g].Crs = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].deltan = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].M0 = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 2:
+					nav_b[i_g].Cuc = strtonum(buff, 4, 19);
+					nav_b[i_g].e = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].Cus = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].sqrtA = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 3:
+					nav_b[i_g].TOE = strtonum(buff, 4, 19);
+					nav_b[i_g].Cic = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].OMEGA = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].Cis = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 4:
+					nav_b[i_g].i0 = strtonum(buff, 4, 19);
+					nav_b[i_g].Crc = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].omega = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].deltaomega = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 5:
+					nav_b[i_g].IDOT = strtonum(buff, 4, 19);
+					//nav_b[i_g].L2code = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].GALweek = strtonum(buff, 4 + 19 + 19, 19);
+					//nav_b[i_g].L2Pflag = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 6:
+					nav_b[i_g].sACC = strtonum(buff, 4, 19);
+					nav_b[i_g].sHEA = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].BGDa = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].BGDb = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 7:
+					//nav_b[i_g].TTN = strtonum(buff, 4, 19);
+					//nav_b[i_g].fit = strtonum(buff, 4 + 19, 19);
+					//nav_b[i_g].spare1 = strtonum(buff, 4 + 19 + 19, 19);
+					//nav_b[i_g].spare2 = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					break;
+				}
+			}
+			i_g++;
+			continue;
+		}
+		//GLO SATELLITE
+		if (flag == 'R') {
+			for (j_gl = 0; j_gl < 4; j_gl++) {
+				switch (j_gl) {
+				case 0:
+					nav_b[i_g].sPRN_BDS = -1;
+					nav_b[i_g].sPRN_GPS = -1;
+					nav_b[i_g].sPRN_GAL = -1;
+					nav_b[i_g].sPRN_GLO = (int)strtonum(buff, 1, 2);
+					nav_b[i_g].TOC_Y = (int)strtonum(buff, 4, 4);
+					nav_b[i_g].TOC_M = (int)strtonum(buff, 9, 2);
+					nav_b[i_g].TOC_D = (int)strtonum(buff, 12, 2);
+					nav_b[i_g].TOC_H = (int)strtonum(buff, 15, 2);
+					nav_b[i_g].TOC_Min = (int)strtonum(buff, 18, 2);
+					nav_b[i_g].TOC_Sec = strtonum(buff, 21, 2);
+					nav_b[i_g].sa0 = strtonum(buff, 23, 19);
+					nav_b[i_g].sa1 = strtonum(buff, 23 + 19, 19);
+					nav_b[i_g].Dos = strtonum(buff, 23 + 19 + 19, 19);
+
+					nav_b[i_g].TOE = JDUTC2GPST(
+						UTCTime2JD(nav_b[i_g].TOC_Y, nav_b[i_g].TOC_M, nav_b[i_g].TOC_D,
+							nav_b[i_g].TOC_H, nav_b[i_g].TOC_Min, nav_b[i_g].TOC_Sec)
+					);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 1:
+					nav_b[i_g].SatX = strtonum(buff, 4, 19);
+					nav_b[i_g].SatXv = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].SatXa = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].sHEA = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 2:
+					nav_b[i_g].SatY = strtonum(buff, 4, 19);
+					nav_b[i_g].SatYv = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].SatYa = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].FreN = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					fgets(buff, MAXRINEX, fp_nav);
+					break;
+				case 3:
+					nav_b[i_g].SatZ = strtonum(buff, 4, 19);
+					nav_b[i_g].SatZv = strtonum(buff, 4 + 19, 19);
+					nav_b[i_g].SatZa = strtonum(buff, 4 + 19 + 19, 19);
+					nav_b[i_g].AOO = strtonum(buff, 4 + 19 + 19 + 19, 19);
+					break;
+				}
+			}
+			i_g++;
+			continue;
+		}
+	}
+}
+/*Read the file of obs*/
+/* -------------------------------------------------------------------------- */
+/// @brief get the total number of epoch of obs
+/// @param fp_obs the file pointer of obs
+/// @return epoch number
+/* -------------------------------------------------------------------------- */
+int get_epochnum(FILE* fp_obs)
+{
+	int n = 0;
+	char flag;
+	char buff[MAXRINEX];
+	char* lable = buff + 60;
+	while (fgets(buff, MAXRINEX, fp_obs)) {
+		if (strstr(lable, "RINEX VERSION / TYPE")) {
+			if (strtonum(buff, 0, 9) < 3) return -1;
+		}
+		if (strstr(lable, "END OF HEADER")) {
+			while (fgets(buff, MAXRINEX, fp_obs)) {
+				strncpy(&flag, buff + 0, 1);
+				if (flag == '>') {
+					n++;
+				}
+			}
+		}
+	}return n;
+}
+/*Read the header data of file of obs*/
+void read_o_h(FILE* fp_obs, pobs_head obs_h)
+{
+	char buff[MAXRINEX] = { 0 };
+	char flag = { 0 };
+	char* lable = buff + 60;
+	int i = 0;
+	int j = 0;
+
+	obs_h->interval = 30;
+	obs_h->f_y = obs_h->f_m = obs_h->f_d = obs_h->f_h = obs_h->f_min = obs_h->f_sec = 0;
+	obs_h->l_y = obs_h->l_m = obs_h->l_d = obs_h->l_h = obs_h->l_min = obs_h->l_sec = 0;
+
+	while (fgets(buff, MAXRINEX, fp_obs)) {
+		if (strstr(lable, "RINEX VERSION / TYPE")) {
+			obs_h->ver = strtonum(buff, 0, 9);
+			strncpy(obs_h->type, buff + 20, 30);
+			continue;
+		}
+		else if (strstr(lable, "APPROX POSITION XYZ")) {
+			obs_h->apX = strtonum(buff, 0, 14);
+			obs_h->apY = strtonum(buff, 0 + 14, 14);
+			obs_h->apZ = strtonum(buff, 0 + 14 + 14, 14);
+			continue;
+		}
+		else if (strstr(lable, "ANTENNA: DELTA H/E/N")) {
+			obs_h->ANTH = strtonum(buff, 0, 14);
+			obs_h->ANTdeltaE = strtonum(buff, 14, 14);
+			obs_h->ANTdeltaN = strtonum(buff, 14 + 14, 14);
+			continue;
+		}
+		else if (strstr(lable, "SYS / # / OBS TYPES")) {
+			char flag = { 0 };
+			strncpy(&flag, buff + 0, 1);
+			if (flag == 'G') {
+				obs_h->obstypenum_gps = (int)strtonum(buff, 4, 2);
+				if (obs_h->obstypenum_gps <= 13) {
+					for (i = 0; i < obs_h->obstypenum_gps; i++) {
+						obs_h->obscode_gps[i + 1] = Type2Code(i, buff);
+					}
+				}
+				else if (obs_h->obstypenum_gps > 13) {
+					for (i = 0; i < 13; i++) {
+						obs_h->obscode_gps[i] = Type2Code(i, buff);
+					}
+					fgets(buff, MAXRINEX, fp_obs);
+					for (i = 0; i < obs_h->obstypenum_gps - 13; i++) {
+						obs_h->obscode_gps[i + 13] = Type2Code(i, buff);
+					}
+				}continue;
+			}
+			else if (flag == 'C') {
+				obs_h->obstypenum_bds = (int)strtonum(buff, 4, 2);
+				if (obs_h->obstypenum_bds <= 13) {
+					for (i = 0; i < obs_h->obstypenum_bds; i++) {
+						obs_h->obscode_bds[i] = Type2Code(i, buff);
+					}
+				}
+				else if (obs_h->obstypenum_bds > 13) {
+					for (i = 0; i < 13; i++) {
+						obs_h->obscode_bds[i] = Type2Code(i, buff);
+					}
+					fgets(buff, MAXRINEX, fp_obs);
+					for (i = 0; i < obs_h->obstypenum_bds - 13; i++) {
+						obs_h->obscode_bds[i + 13] = Type2Code(i, buff);
+					}
+				}continue;
+			}
+			else if (flag == 'E') {
+				obs_h->obstypenum_gal = (int)strtonum(buff, 4, 2);
+				if (obs_h->obstypenum_gal <= 13) {
+					for (i = 0; i < obs_h->obstypenum_gal; i++) {
+						obs_h->obscode_gal[i] = Type2Code(i, buff);
+					}
+				}
+				else if (obs_h->obstypenum_gal > 13) {
+					for (i = 0; i < 13; i++) {
+						obs_h->obscode_gal[i] = Type2Code(i, buff);
+					}
+					fgets(buff, MAXRINEX, fp_obs);
+					for (i = 0; i < obs_h->obstypenum_gal - 13; i++) {
+						obs_h->obscode_gal[i + 13] = Type2Code(i, buff);
+					}
+				}continue;
+			}
+			else if (flag == 'R') {
+				obs_h->obstypenum_glo = (int)strtonum(buff, 4, 2);
+				if (obs_h->obstypenum_glo <= 13) {
+					for (i = 0; i < obs_h->obstypenum_glo; i++) {
+						obs_h->obscode_glo[i] = Type2Code(i, buff);
+					}
+				}
+				else if (obs_h->obstypenum_glo > 13) {
+					for (i = 0; i < 13; i++) {
+						obs_h->obscode_glo[i] = Type2Code(i, buff);
+					}
+					fgets(buff, MAXRINEX, fp_obs);
+					for (i = 0; i < obs_h->obstypenum_glo - 13; i++) {
+						obs_h->obscode_glo[i + 13] = Type2Code(i, buff);
+					}
+				}continue;
+			}continue;
+		}
+		else if (strstr(lable, "INTERVAL")) {
+			obs_h->interval = strtonum(buff, 0, 10);
+			continue;
+		}
+		else if (strstr(lable, "TIME OF FIRST OBS")) {
+			obs_h->f_y = (int)strtonum(buff, 2, 4);
+			obs_h->f_m = (int)strtonum(buff, 2 + 6, 4);
+			obs_h->f_d = (int)strtonum(buff, 2 + 6 + 6, 4);
+			obs_h->f_h = (int)strtonum(buff, 2 + 6 + 6 + 6, 4);
+			obs_h->f_min = (int)strtonum(buff, 2 + 6 + 6 + 6 + 6, 4);
+			obs_h->f_sec = strtonum(buff, 2 + 6 + 6 + 6 + 6 + 6, 9);
+			strncpy(obs_h->tsys, buff + 6 + 6 + 6 + 6 + 6 + 18, 3);
+			continue;
+		}
+		else if (strstr(lable, "TIME OF LAST OBS")) {
+			obs_h->l_y = (int)strtonum(buff, 2, 4);
+			obs_h->l_m = (int)strtonum(buff, 2 + 6, 4);
+			obs_h->l_d = (int)strtonum(buff, 2 + 6 + 6, 4);
+			obs_h->l_h = (int)strtonum(buff, 2 + 6 + 6 + 6, 4);
+			obs_h->l_min = (int)strtonum(buff, 2 + 6 + 6 + 6 + 6, 4);
+			obs_h->l_sec = strtonum(buff, 2 + 6 + 6 + 6 + 6 + 6, 9);
+			continue;
+		}
+		else if (strstr(lable, "END OF HEADER")) {
+			break;
+		}
+	}
+}
+/*Read the body data of file of obs*/
+void read_o_eb(FILE* fp_obs, pobs_head obs_h, pobs_epoch obs_e, pobs_body obs_b)
+{
+	initobs_e(obs_e, obs_b);
+	char buff[MAXRINEX] = { 0 };
+	char flag = { 0 };
+	while (fgets(buff, MAXRINEX, fp_obs)) {
+		strncpy(&flag, buff + 0, 1);
+		if (flag == '>') {
+			int j_g = 0;   int k_g = 0;
+			int j_c = 0;   int k_c = 0;
+			int j_ga = 0;	int k_ga = 0;
+			int j_gl = 0;	int k_gl = 0;
+
+			obs_e->gps_num = 0;
+			obs_e->bds_num = 0;
+			obs_e->gal_num = 0;
+			obs_e->glo_num = 0;
+
+			obs_e->y = (int)strtonum(buff, 2, 4);
+			obs_e->m = (int)strtonum(buff, 7, 2);
+			obs_e->d = (int)strtonum(buff, 10, 2);
+			obs_e->h = (int)strtonum(buff, 13, 2);
+			obs_e->min = (int)strtonum(buff, 16, 2);
+			obs_e->sec = strtonum(buff, 19, 10);
+			obs_e->p_flag = (int)strtonum(buff, 31, 1);
+			obs_e->sat_num = strtonum(buff, 33, 3);
+			strncpy(&flag, buff + 0, 1);
+
+			memset(obs_e->sPRN, 0, sizeof(obs_e->sPRN));
+			memset(obs_e->sPRN_GPS, 0, sizeof(obs_e->sPRN_GPS));
+			memset(obs_e->sPRN_BDS, 0, sizeof(obs_e->sPRN_BDS));
+			memset(obs_e->sPRN_GAL, 0, sizeof(obs_e->sPRN_GAL));
+			memset(obs_e->sPRN_GLO, 0, sizeof(obs_e->sPRN_GLO));
+			memset(obs_b->obs_gps, 0, sizeof(obs_b->obs_gps));
+			memset(obs_b->obs_bds, 0, sizeof(obs_b->obs_bds));
+			memset(obs_b->obs_gal, 0, sizeof(obs_b->obs_gal));
+			memset(obs_b->obs_glo, 0, sizeof(obs_b->obs_glo));
+
+			for (int i = 0; i < obs_e->sat_num; i++) {
+				if (flag == 'G') {
+					obs_e->sPRN[i] = strtonum(buff, 1, 2);
+					obs_e->sPRN_GPS[j_g] = obs_e->sPRN[i];
+					for (k_g = 0; k_g < obs_h->obstypenum_gps; k_g++) {
+						obs_b->obs_gps[j_g][k_g] = strtonum(buff, 3 + 16 * k_g, 14);
+					}
+					obs_e->gps_num++; j_g++;
+					fgets(buff, MAXRINEX, fp_obs);
+					strncpy(&flag, buff + 0, 1);
+				}
+				else if (flag == 'C') {
+					obs_e->sPRN[i] = strtonum(buff, 1, 2);
+					obs_e->sPRN_BDS[j_c] = obs_e->sPRN[i];
+					for (k_c = 0; k_c < obs_h->obstypenum_bds; k_c++) {
+						obs_b->obs_bds[j_c][k_c] = strtonum(buff, 3 + 16 * k_c, 14);
+					}
+					obs_e->bds_num++; j_c++;
+					fgets(buff, MAXRINEX, fp_obs);
+					strncpy(&flag, buff + 0, 1);
+				}
+				else if (flag == 'E') {
+					obs_e->sPRN[i] = strtonum(buff, 1, 2);
+					obs_e->sPRN_GAL[j_ga] = obs_e->sPRN[i];
+					for (k_ga = 0; k_ga < obs_h->obstypenum_gal; k_ga++) {
+						obs_b->obs_gal[j_ga][k_ga] = strtonum(buff, 3 + 16 * k_ga, 14);
+					}
+					obs_e->gal_num++; j_ga++;
+					fgets(buff, MAXRINEX, fp_obs);
+					strncpy(&flag, buff + 0, 1);
+				}
+				else if (flag == 'R') {
+					obs_e->sPRN[i] = strtonum(buff, 1, 2);
+					obs_e->sPRN_GLO[j_gl] = obs_e->sPRN[i];
+					for (k_gl = 0; k_gl < obs_h->obstypenum_glo; k_gl++) {
+						obs_b->obs_glo[j_gl][k_gl] = strtonum(buff, 3 + 16 * k_gl, 14);
+					}
+					obs_e->glo_num++; j_gl++;
+					fgets(buff, MAXRINEX, fp_obs);
+					strncpy(&flag, buff + 0, 1);
+				}
+				else {
+					fgets(buff, MAXRINEX, fp_obs);
+					strncpy(&flag, buff + 0, 1);
+				}
+			}
+		}break;
+	}
+}

src/rnx2pos.cpp

@@ -0,0 +1,411 @@
+#include "brdm2pos.h"
+using namespace std;
+/*Obatain the intergral term of acceleraction of each component*/
+void pz90deq(double X[6], double K[6], double acc[3])
+{
+	double r = sqrt(pow(X[0], 2) + pow(X[1], 2) + pow(X[2], 2));
+	double coef0 = -GM / pow(r, 3);
+	double coef1 = 1.5 * C20 * (GM * pow(a, 2) / pow(r, 5));
+
+	K[0] = X[3]; K[1] = X[4]; K[2] = X[5];
+	K[3] =
+		coef0 * X[0] +
+		coef1 * (1 - 5 * pow((X[2] / r), 2)) * X[0] +
+		pow(Earth_e, 2) * X[0] +
+		2 * Earth_e * X[4] +
+		acc[0];
+	K[4] =
+		coef0 * X[1] +
+		coef1 * (1 - 5 * pow((X[2] / r), 2)) * X[1] +
+		pow(Earth_e, 2) * X[1] -
+		2 * Earth_e * X[3] +
+		acc[1];
+	K[5] =
+		coef0 * X[2] +
+		coef1 * (3 - 5 * pow((X[2] / r), 2)) * X[2] +
+		acc[2];
+}
+/*Construct the RK-4 method*/
+void pz90pos(double tstep, double X[6], double acc[3])
+{
+	double K1[6], K2[6], K3[6], K4[6], W[6];
+
+	pz90deq(X, K1, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K1[i] * tstep / 2.0;
+	pz90deq(W, K2, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K2[i] * tstep / 2.0;
+	pz90deq(W, K3, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K3[i] * tstep;
+	pz90deq(W, K4, acc);
+	for (int i = 0; i < 6; i++) X[i] += (K1[i] + 2.0 * K2[i] + 2.0 * K3[i] + K4[i]) * tstep / 6.0;
+}
+/*Calculate the satellite position of GLONASS on this epoch*/
+pos_ts glo_pos(int best_epoch, double Weeksec,
+	pnav_body nav_b, pobs_head obs_h, pos_ts pos_t)
+{
+	double X[6], acc[3]; double R = 0.0;
+	double tdist = Weeksec - pos_t.delta_t - nav_b[best_epoch].TOE, tstep = 0.0;
+	pos_t.delta_clk = -nav_b[best_epoch].sa0 + nav_b[best_epoch].sa1 * tdist;
+
+	X[0] = nav_b[best_epoch].SatX * 1e3;
+	X[1] = nav_b[best_epoch].SatY * 1e3;
+	X[2] = nav_b[best_epoch].SatZ * 1e3;//Position
+
+	X[3] = nav_b[best_epoch].SatXv * 1e3;
+	X[4] = nav_b[best_epoch].SatYv * 1e3;
+	X[5] = nav_b[best_epoch].SatZv * 1e3;//Velocity
+
+	acc[0] = nav_b[best_epoch].SatXa * 1e3;
+	acc[1] = nav_b[best_epoch].SatYa * 1e3;
+	acc[2] = nav_b[best_epoch].SatZa * 1e3;//Acclerate
+
+	for (tstep = tdist < 0.0 ? TSTEP : -TSTEP; fabs(tdist) > 1.0e-9; tdist += tstep) {
+		if (fabs(tdist) < TSTEP) tstep = -tdist;
+		pz90pos(tstep, X, acc);
+	}
+
+	X[0] = -0.47 + (X[0] * 1 + X[1] * 1.728e-6 + X[2] * -0.017e-6) * (1 + 22e-9);
+	X[1] = -0.51 + (X[0] * 1.728e-6 + X[1] * 1 + X[2] * -0.076e-6) * (1 + 22e-9);
+	X[2] = -1.56 + (X[0] * 0.0178e-6 + X[1] * -0.076e-6 + X[2] * 1) * (1 + 22e-9);//PZ90 Convert to WGS-84
+
+	pos_t.X = X[0];
+	pos_t.Y = X[1];
+	pos_t.Z = X[2];
+
+	R = sqrt(pow(pos_t.X - obs_h->apX, 2) + pow(pos_t.Y - obs_h->apY, 2) + pow(pos_t.Z - obs_h->apZ, 2));
+	pos_t.deltat = pos_t.delta_t;
+	pos_t.delta_t = R / C_V + pos_t.delta_clk;
+
+	return pos_t;
+}
+/*Calculate the satellite position of other systems on this epoch*/
+pos_ts sat_pos(int sPRN, int best_epoch, double	Weeksec, int gnsscode,
+	pnav_body nav_b, pobs_head obs_h, pos_ts pos_t)
+{
+	double T_S = Weeksec - pos_t.delta_t;
+	double n_0 = sqrt(GM) / pow(nav_b[best_epoch].sqrtA, 3);
+	double n = n_0 + nav_b[best_epoch].deltan;
+	double tk;
+	if (gnsscode == BDS) tk = T_S - nav_b[best_epoch].TOE - 14;
+	else tk = T_S - nav_b[best_epoch].TOE;
+
+	if (tk > 32400)tk -= 604800;
+	else if (tk < -32400)tk += 604800;
+
+	double Ms = nav_b[best_epoch].M0 + n * tk;
+
+	double Es = Ms, E0;
+	do
+	{
+		E0 = Es;
+		Es = Ms + nav_b[best_epoch].e * sin(Es);
+	} while (fabs(Es - E0) > 1.0e-10);
+
+	double cosfs = (cos(Es) - nav_b[best_epoch].e) / (1 - nav_b[best_epoch].e * cos(Es));
+	double sinfs = (sqrt(1 - pow(nav_b[best_epoch].e, 2)) * sin(Es)) / (1 - nav_b[best_epoch].e * cos(Es));
+	double fs = atan2(sinfs, cosfs);
+	double u0 = fs + nav_b[best_epoch].omega;
+
+	double epsilon_u = nav_b[best_epoch].Cus * sin(2 * u0) + nav_b[best_epoch].Cuc * cos(2 * u0);
+	double epsilon_r = nav_b[best_epoch].Crs * sin(2 * u0) + nav_b[best_epoch].Crc * cos(2 * u0);
+	double epsilon_i = nav_b[best_epoch].Cis * sin(2 * u0) + nav_b[best_epoch].Cic * cos(2 * u0);
+
+	double u = u0 + epsilon_u;
+	double r = pow(nav_b[best_epoch].sqrtA, 2) * (1 - nav_b[best_epoch].e * cos(Es)) + epsilon_r;
+	double i = nav_b[best_epoch].i0 + epsilon_i + nav_b[best_epoch].IDOT * tk;
+
+	double x = r * cos(u);
+	double y = r * sin(u);
+
+	double l, X, Y, Z;
+	if (gnsscode == BDS) {
+		if (sPRN > 5 && sPRN < 59) //BDS IGSO & MEO Satellites
+		{
+			l = nav_b[best_epoch].OMEGA + (nav_b[best_epoch].deltaomega - Earth_e) * tk - Earth_e * nav_b[best_epoch].TOE;
+			X = x * cos(l) - y * cos(i) * sin(l);
+			Y = x * sin(l) + y * cos(i) * cos(l);
+			Z = y * sin(i);
+		}
+		else //BDS GEO Satellites
+		{
+			l = nav_b[best_epoch].OMEGA + nav_b[best_epoch].deltaomega * tk - Earth_e * nav_b[best_epoch].TOE;
+			X = x * cos(l) - y * cos(i) * sin(l);
+			Y = x * sin(l) + y * cos(i) * cos(l);
+			Z = y * sin(i);
+
+			double f = deg2rad(-5);
+			double p = Earth_e * tk;
+
+			X = X * cos(p) + Y * sin(p) * cos(f) + Z * sin(p) * sin(f);
+			Y = X * -sin(p) + Y * cos(p) * cos(f) + Z * cos(p) * sin(f);
+			Z = Y * -sin(f) + Z * cos(f);
+		}
+	}
+	else {
+		l = nav_b[best_epoch].OMEGA + (nav_b[best_epoch].deltaomega - Earth_e) * tk - Earth_e * nav_b[best_epoch].TOE;
+		X = x * cos(l) - y * cos(i) * sin(l);
+		Y = x * sin(l) + y * cos(i) * cos(l);
+		Z = y * sin(i);
+	}
+	pos_t.X = cos(Earth_e * pos_t.delta_t) * X + sin(Earth_e * pos_t.delta_t) * Y;
+	pos_t.Y = -sin(Earth_e * pos_t.delta_t) * X + cos(Earth_e * pos_t.delta_t) * Y;
+	pos_t.Z = Z;//Correction of earth rotation
+
+	double TGD;
+	if (gnsscode == GPS) TGD = nav_b[best_epoch].TGD;
+	else if (gnsscode == GAL) TGD = nav_b[best_epoch].BGDa;
+	else if (gnsscode == BDS) TGD = 0;
+
+	double R = sqrt(pow(pos_t.X - obs_h->apX, 2) + pow(pos_t.Y - obs_h->apY, 2) + pow(pos_t.Z - obs_h->apZ, 2));
+	double rela = 2 * sqrt(GM) * nav_b[best_epoch].e * nav_b[best_epoch].sqrtA * sin(Es) / pow(C_V, 2);//Relativistic effect
+	pos_t.delta_clk = nav_b[best_epoch].sa0 + nav_b[best_epoch].sa1 * tk + nav_b[best_epoch].sa2 * pow(tk, 2) - rela - TGD;
+	pos_t.deltat = pos_t.delta_t;
+	pos_t.delta_t = R / C_V + pos_t.delta_clk;
+
+	return pos_t;
+}
+/* -------------------------------------------------------------------------- */
+/// @brief Calculate the position of satellite in the ECEF coordinate system by the broadcast ephemeris and epoch information
+/// @param obs_h Observation header data
+/// @param obs_e Observation epoch data
+/// @param obs_b Observation data
+/// @param nav_b Navigation data
+/// @param res_file The file path of result file of satellite position
+/// @param station Prior information about station location
+/// @param gnsscode GNSS system code
+/// @param satnum The total number of satellite of designated satellite system
+/// @return 1 refer OK 
+/* -------------------------------------------------------------------------- */
+int sat_pos_cal(pobs_head obs_h, pobs_epoch obs_e, pobs_body obs_b,
+	pnav_body nav_b,
+	FILE* res_file, stations station,
+	int gnsscode, int satnum)
+{
+	//the beginning second of observation
+	double beginsec = Time2GPST(obs_h->f_y, obs_h->f_m, obs_h->f_d, obs_h->f_h, obs_h->f_min, obs_h->f_sec);
+	//the ending second of observation
+	double endinsec = Time2GPST(obs_h->l_y, obs_h->l_m, obs_h->l_d, obs_h->l_h, obs_h->l_min, obs_h->l_sec);
+
+	blhs blh = { 0 }; blh2enu enu = { 0 };
+	rahcal rah = { 0 }; pos_ts pos_t = { 0 };
+	xyz2blh tem1 = { 0 }; blh2enu tem2 = { 0 }; rahcal tem3 = { 0 };
+
+	int y = obs_e->y; int m = obs_e->m; int d = obs_e->d;
+	double h = obs_e->h; int min = obs_e->min; double sec = obs_e->sec;
+
+	double Weeksec = Time2GPST(y, m, d, h, min, sec);
+	int epochcount = round((Weeksec - beginsec) / obs_h->interval) + 1;
+	fprintf(res_file, "\n>%04d %04d %02d %02d %02d %02d %07.04f"
+		, epochcount, obs_e->y, obs_e->m, obs_e->d, obs_e->h, obs_e->min, obs_e->sec);
+	int sat_num = 0, sPRN = 0, SIGN = 0, PERS = 0;
+	double detat_toc;
+	//Check the GNSS and assign the satellite number
+	if (gnsscode == GPS)sat_num = obs_e->gps_num;
+	else if (gnsscode == GAL)sat_num = obs_e->gal_num;
+	else if (gnsscode == BDS)sat_num = obs_e->bds_num;
+	else if (gnsscode == GLO)sat_num = obs_e->glo_num;
+
+	for (int j = 0; j < sat_num; j++) {
+		if (gnsscode == GPS) {
+			sPRN = obs_e->sPRN_GPS[j]; SIGN = GPS;
+			PERS = obs_b->obs_gps[j][Code2Type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)] / C_V;
+		}
+		else if (gnsscode == GAL) {
+			sPRN = obs_e->sPRN_GAL[j]; SIGN = GAL;
+			PERS = obs_b->obs_gal[j][Code2Type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)] / C_V;
+		}
+		else if (gnsscode == BDS) {
+			sPRN = obs_e->sPRN_BDS[j]; SIGN = BDS;
+			PERS = obs_b->obs_bds[j][Code2Type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)] / C_V;
+		}
+		else if (gnsscode == GLO) {
+			sPRN = obs_e->sPRN_GLO[j]; SIGN = GLO;
+			PERS = obs_b->obs_glo[j][Code2Type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)] / C_V;
+		}
+
+		int best_epoch = select_epoch(Weeksec, sPRN, nav_b, satnum, SIGN);
+		if (best_epoch == -1)//when the sPRN is not exist function will return -1
+			break;
+		if (SIGN == BDS) detat_toc = Weeksec - nav_b[best_epoch].TOE - 14;
+		else detat_toc = Weeksec - nav_b[best_epoch].TOE;
+
+		//Iterative approximate propagation time
+		//Exclude GLONASS
+		if (SIGN == GLO) {
+			pos_t.delta_t = PERS -
+				station.delta_TR +
+				nav_b[best_epoch].sa0 +
+				nav_b[best_epoch].sa1 * detat_toc;
+			pos_t.deltat = 0.0;
+
+			while (fabs(pos_t.delta_t - pos_t.deltat) > 1.0e-9) {
+				pos_t = glo_pos(best_epoch, Weeksec, nav_b, obs_h, pos_t);
+			}
+		}
+		else {
+			pos_t.delta_t = PERS -
+				station.delta_TR +
+				nav_b[best_epoch].sa0 +
+				nav_b[best_epoch].sa1 * detat_toc +
+				nav_b[best_epoch].sa2 * pow(detat_toc, 2);
+			pos_t.deltat = 0.0;
+			while (fabs(pos_t.delta_t - pos_t.deltat) > 1.0e-9) {
+				pos_t = sat_pos(sPRN, best_epoch, Weeksec, gnsscode, nav_b, obs_h, pos_t);
+			}
+		}
+		//Satellite blh position calculation
+		tem1 = XYZ2BLH(tem1, pos_t.X, pos_t.Y, pos_t.Z);
+		blh.B = rad2deg(tem1.B);
+		blh.L = rad2deg(tem1.L);
+		blh.H = tem1.H;
+
+		double deltax = pos_t.X - obs_h->apX;
+		double deltay = pos_t.Y - obs_h->apY;
+		double deltaz = pos_t.Z - obs_h->apZ;
+
+		tem2 = BLH2ENU(tem2, station.B, station.L, deltax, deltay, deltaz);
+		enu.E = tem2.E;
+		enu.N = tem2.N;
+		enu.U = tem2.U;
+		//Satellite elevation Angle calculation
+		tem3 = RAHCAL(tem3, enu.E, enu.N, enu.U);
+		rah.R = tem3.R;
+		rah.A = rad2deg(tem3.A);
+		rah.H = rad2deg(tem3.H);
+		if (blh.H < 0 || rah.H < 10 || nav_b[best_epoch].sHEA != 0) {
+			continue;
+		}
+		else {
+			if (SIGN == GPS) {
+				fprintf(res_file, "\nG %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
+					, sPRN, pos_t.X, pos_t.Y, pos_t.Z
+					, obs_b->obs_gps[j][Code2Type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)]
+					, obs_b->obs_gps[j][Code2Type(C2, obs_h->obstypenum_gps, obs_h->obscode_gps)]
+					, rah.H, pos_t.delta_clk);//GPS satellite pos output
+			}
+			else if (SIGN == GAL) {
+				fprintf(res_file, "\nE %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
+					, sPRN, pos_t.X, pos_t.Y, pos_t.Z
+					, obs_b->obs_gal[j][Code2Type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)]
+					, obs_b->obs_gal[j][Code2Type(C6, obs_h->obstypenum_gal, obs_h->obscode_gal)]
+					, rah.H, pos_t.delta_clk);//Galileo satellite pos output
+			}
+			else if (SIGN == BDS) {
+				if (sPRN >= 19) {
+					fprintf(res_file, "\nCC%02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
+						, sPRN, pos_t.X, pos_t.Y, pos_t.Z
+						, obs_b->obs_bds[j][Code2Type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)]
+						, obs_b->obs_bds[j][Code2Type(C7, obs_h->obstypenum_bds, obs_h->obscode_bds)]
+						, rah.H, pos_t.delta_clk);//BDS-3 satellite pos output
+				}
+				else {
+					fprintf(res_file, "\nCB%02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
+						, sPRN, pos_t.X, pos_t.Y, pos_t.Z
+						, obs_b->obs_bds[j][Code2Type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)]
+						, obs_b->obs_bds[j][Code2Type(C7, obs_h->obstypenum_bds, obs_h->obscode_bds)]
+						, rah.H, pos_t.delta_clk);//BDS-2 satellite pos output
+				}
+			}
+			else if (SIGN == GLO) {
+				fprintf(res_file, "\nR %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
+					, sPRN, pos_t.X, pos_t.Y, pos_t.Z
+					, obs_b->obs_glo[j][Code2Type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)]
+					, obs_b->obs_glo[j][Code2Type(C2, obs_h->obstypenum_glo, obs_h->obscode_glo)]
+					, rah.H, pos_t.delta_clk);//Galileo satellite pos output
+			}
+		}
+	}return 1;
+}
+/* -------------------------------------------------------------------------- */
+/// @brief The entry function of full .dll package,
+/// @param nav_path The RINEX navigation file's path
+/// @param obs_path The RINEX observation file's path
+/// @param res_path The Satellite output file's path
+/// @param gnsscode The GNSS position you want to calculate
+/// @return 0 refer complete solution; -1 refer fail to complete solution
+/// @return > 0 refer partlt complete solution
+/* -------------------------------------------------------------------------- */
+double brdm2pos(char nav_path[260], char obs_path[260], char res_path[260], int gnsscode)
+{
+	int satnum, epochunum, result = 0;
+	pos_ts pos_t = { 0 };
+	xyz2blh tem2 = { 0 };
+	stations station = { 0 };
+	time_t gen_time; time(&gen_time);
+
+	pnav_head nav_h = NULL;
+	pnav_body nav_b = NULL;
+	pobs_head obs_h = NULL;
+	pobs_body obs_b = NULL;
+	pobs_epoch obs_e = NULL;
+
+	FILE* fp_nav = fopen(nav_path, "r");
+	if (fp_nav == NULL) return -1;
+	satnum = getsatnum(fp_nav);
+
+	switch (satnum) {
+		case -1: return -3;//mismatch version or format
+		case  0: return -4;//no data
+		default:
+			break;
+	}
+
+	rewind(fp_nav);
+	nav_h = (pnav_head)malloc(sizeof(nav_head));
+	nav_b = (pnav_body)malloc(sizeof(nav_body) * (satnum));
+	if (nav_h && nav_b) {
+		read_n_h(fp_nav, nav_h); read_n_b(fp_nav, nav_b);
+	}
+	fclose(fp_nav);
+
+	FILE* fp_obs = fopen(obs_path, "r");
+	if (fp_obs == NULL) return -2;
+	epochunum = get_epochnum(fp_obs);
+
+	switch (epochunum) {
+		case -1: return -5;//mismatch version or format
+		case  0: return -6;//no data
+		default:
+			break;
+	}
+
+	rewind(fp_obs);
+	obs_h = (pobs_head)malloc(sizeof(obs_head));
+	obs_e = (pobs_epoch)malloc(sizeof(obs_epoch));
+	obs_b = (pobs_body)malloc(sizeof(obs_body));
+	if (obs_h && obs_e && obs_b) {
+		read_o_h(fp_obs, obs_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;
+
+		//Out put the header of solution file
+		FILE* result_file_clear = fopen(res_path, "w");
+		fclose(result_file_clear);
+		FILE* result_file = fopen(res_path, "a+");
+		fprintf(result_file, "@ GENERATE PROGRAM   : KNZ_GeoTrackLab ver1.5.3.1\n");
+		fprintf(result_file, "@ GENERATE TYPE      : Satellite  Position\n");
+		fprintf(result_file, "@ GENERATE TIME      : %s", ctime(&gen_time));
+		fprintf(result_file, "@ OBS FILE PATH      : %s\n", obs_path);
+		fprintf(result_file, "@ NAV FILE PATH      : %s\n", nav_path);
+		fprintf(result_file, "@ TIME OF FIRST OBS  : %4d %02d %02d %02d %02d %07.4f\n"
+			, obs_h->f_y, obs_h->f_m, obs_h->f_d, obs_h->f_h, obs_h->f_min, obs_h->f_sec);
+		fprintf(result_file, "@ TIME OF LAST OBS   : %4d %02d %02d %02d %02d %07.4f\n"
+			, obs_h->l_y, obs_h->l_m, obs_h->l_d, obs_h->l_h, obs_h->l_min, obs_h->l_sec);
+		fprintf(result_file, "@ APPROX POSITION XYZ: %13.04f%13.04f%13.04f\n@ APPROX POSITION BLH:  %12.07f %12.07f %12.07f\n"
+			, obs_h->apX, obs_h->apY, obs_h->apZ
+			, station.B, station.L, station.H);
+		fprintf(result_file, "END OF HEADER\n");
+
+		//Begin to calculate
+		for (int i = 0; i < epochunum; i++) {
+			read_o_eb(fp_obs, obs_h, obs_e, obs_b);
+			result += sat_pos_cal(obs_h, obs_e, obs_b, nav_b,
+				result_file, station,
+				gnsscode, satnum);
+			fprintf(result_file, "\n");
+		}
+
+		freeobs_e(obs_e, obs_b);
+		free(obs_h); free(nav_h); free(nav_b);
+		fprintf(result_file, "\nEND"); fclose(result_file);
+		if (epochunum == result) return 0;
+	}
+}

src/timesys.cpp

@@ -0,0 +1,68 @@
+#include "brdm2pos.h"
+using namespace std;
+/* -------------------------------------------------------------------------- */
+/// @brief convert UTC to GPS Second of week
+/// @param y year
+/// @param m month
+/// @param d day
+/// @param h hour
+/// @param min minute
+/// @param sec second
+/// @return return GPS Second of week
+/* -------------------------------------------------------------------------- */
+double Time2GPST(int y, int m, int d, double h, int min, double sec)
+{
+	if (m > 2)
+	{
+		y = y;
+		m = m;
+	}
+
+	if (m <= 2)
+	{
+		y = y - 1;
+		m = m + 12;
+	}
+
+	h = h + min / 60.0 + sec / 3600.0;
+	double JD = (int)(365.25 * y) + (int)(30.6001 * (m + 1)) + d + h / 24.0 + 1720981.5;
+	double MJD = JD - 2400000.5;
+	int gpsweek = (int)((MJD - 44244) / 7);
+	double secofweek = (MJD - 44244.0 - gpsweek * 7.0) * 86400.0;
+	return secofweek;
+}
+double JDUTC2GPST(double JD_UTCSU)
+{
+	double n;//Leap second
+
+	if (2451179.5000000 < JD_UTCSU && JD_UTCSU < 2453736.5000000) { n = 32.0; }
+	else if (2453736.5000000 < JD_UTCSU && JD_UTCSU < 2454832.5000000) { n = 33.0; }
+	else if (2454832.5000000 < JD_UTCSU && JD_UTCSU < 2455927.5000000) { n = 34.0; }
+	else if (2455927.5000000 < JD_UTCSU && JD_UTCSU < 2457023.5000000) { n = 35.0; }
+	else if (2457023.5000000 < JD_UTCSU && JD_UTCSU < 2457754.5000000) { n = 36.0; }
+	else if (2457754.5000000 < JD_UTCSU) { n = 37.0; }
+
+	JD_UTCSU += ((n * 1 - 19) / 3600.0) / 24.0;
+	double MJD = JD_UTCSU - 2400000.5;
+	int gpsweek = (int)((MJD - 44244) / 7);
+	double secofweek = (MJD - 44244.0 - gpsweek * 7.0) * 86400.0;
+	return secofweek;
+}
+double UTCTime2JD(int y, int m, int d, double h, int min, double sec)
+{
+	if (m > 2)
+	{
+		y = y;
+		m = m;
+	}
+
+	if (m <= 2)
+	{
+		y = y - 1;
+		m = m + 12;
+	}
+
+	h = h + min / 60.0 + sec / 3600.0;
+	double JD = (int)(365.25 * y) + (int)(30.6001 * (m + 1)) + d + h / 24.0 + 1720981.5;
+	return JD;
+}