diff --git a/README.md b/README.md index 4991a81..7cf7824 100644 --- a/README.md +++ b/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. diff --git a/READMECN.md b/READMECN.md index 39dcfe5..9b46c9e 100644 --- a/READMECN.md +++ b/READMECN.md @@ -36,12 +36,15 @@ ## 项目更新 -重命名项目并更新：2024/10/26 +更新：2024/10/28 +1. KNZ_GeoTrackLab：修复一些bug。 +2. KNZ_GeoTrackLab：优化代码结构。 -首次发布 KNZ_GeoTrackLab。 -1. KNZ_Calculate：与 KNZ_Plot 合并。 -2. KNZ_Plot：与 KNZ_Calculate 合并。 -3. KNZ_GeoTrackLab：支持 GPS、北斗、伽利略和 GLONASS。 +重命名项目并更新：2024/10/26 +1. 首次发布 KNZ_GeoTrackLab。 +2. KNZ_Calculate：与 KNZ_Plot 合并。 +3. KNZ_Plot：与 KNZ_Calculate 合并。 +4. KNZ_GeoTrackLab：支持 GPS、北斗、伽利略和 GLONASS。 更新：2024/10/14 1. KNZ_Calculate：支持 GPS、BDS 和伽利略。 diff --git a/app/Mainui.py b/app/Mainui.py index d0f9e99..34c2f5d 100644 --- a/app/Mainui.py +++ b/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 diff --git a/doc/KNZ_GeoTrackLab ver 1.5.1.docx b/doc/KNZ_GeoTrackLab ver 1.5.1.docx new file mode 100644 index 0000000..5f4d172 Binary files /dev/null and b/doc/KNZ_GeoTrackLab ver 1.5.1.docx differ diff --git a/src/pch.h b/src/brdm2pos.h similarity index 66% rename from src/pch.h rename to src/brdm2pos.h index 601b49d..1e45d4b 100644 --- a/src/pch.h +++ b/src/brdm2pos.h @@ -1,37 +1,33 @@ -// pch.h: 这是预编译标头文件。 -// 下方列出的文件仅编译一次，提高了将来生成的生成性能。 -// 这还将影响 IntelliSense 性能，包括代码完成和许多代码浏览功能。 -// 但是，如果此处列出的文件中的任何一个在生成之间有更新，它们全部都将被重新编译。 -// 请勿在此处添加要频繁更新的文件，这将使得性能优势无效。 +#pragma once -#ifndef PCH_H -#define PCH_H +#ifndef _BRDM2POS_H_ +#define _BRDM2POS_H_ -// 添加要在此处预编译的标头 -#include "framework.h" +#include +#include +#include -#endif -/* ------------------------------ CALCU CONST ------------------------------- */ +#endif // _BRDM2POS_H_ + +/* 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 - -/* ------------------------------ WGS-84 CONST ------------------------------ */ +#define C20 1.0826257e-3 +/*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 C1M 105 +#define C1L 104 +#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 C1Y 109 -#define C1Z 110 - +#define C1X 108 +#define C1Y 109 +#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 C3Q 301 -#define C3X 302 - +#define C3I 300 +#define C3Q 301 +#define C3X 302 //frequency 4 -#define C4 400 -#define C4A 400 -#define C4B 401 -#define C4X 402 - +#define C4 400 +#define C4A 400 +#define C4B 401 +#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 C6A 600 -#define C6B 601 -#define C6C 602 +#define C6 600 +#define C6A 600 +#define C6B 601 +#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 C8I 800 -#define C8Q 801 -#define C8X 802 +#define C8 800 +#define C8I 800 +#define C8Q 801 +#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 Crs; double SatXv; - double deltan; double SatXa; + double IODE; double AODE; double SatX; + double Crs; double SatXv; + double deltan; double SatXa; double M0; - double Cuc; double SatY; - double e; double SatYv; - double Cus; double SatYa; - double sqrtA; int FreN; + double Cuc; double SatY; + double e; double SatYv; + double Cus; double SatYa; + double sqrtA; int FreN; - double TOE; double SatZ; - double Cic; double SatZv; - double OMEGA; double SatZa; - double Cis; int AOO; + double TOE; double SatZ; + double Cic; double SatZv; + double OMEGA; double SatZa; + 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); diff --git a/src/coordtrans.cpp b/src/coordtrans.cpp new file mode 100644 index 0000000..ca02984 --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/src/dllmain.cpp b/src/dllmain.cpp deleted file mode 100644 index f592c0f..0000000 --- a/src/dllmain.cpp +++ /dev/null @@ -1,1415 +0,0 @@ -/* - * @Author: KenanZhu111 3471685733@qq.com - * @Date: 2024-09-20 21:47:22 - * @LastEditors: KenanZhu111 3471685733@qq.com - * @LastEditTime: 2024-10-26 10:13:40 -*/ -#include "pch.h" -#include -#include -#include -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; -} - -/* -------------------------------------------------------------------------- */ -/// @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; -} - -/* -------------------------------------------------------------------------- */ -/// @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){ //Be judged as GLONASS - 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[1000]) -{ - 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 + 300; - 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 + 400; - 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; -} - -/*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); -} - -/* -------------------------------------------------------------------------- */ -/// @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 -/* -------------------------------------------------------------------------- */ -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; -} - -/* -------------------------- 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; - int flag = 0; - char buff[MAXRINEX]; - char satvar; - char* lable = buff + 60; - - while (fgets(buff, MAXRINEX, fp_nav)){ - if (strstr(lable, "END OF HEADER")){ - flag = 1; - } - if (flag == 1){ - 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; - int satnum = 0; - char flag; - char buff[MAXRINEX]; - while (fgets(buff, MAXRINEX, fp_obs)){ - satnum = (int)strtonum(buff, 33, 2); - 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; - } -} - -/*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 Start(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"); - satnum = getsatnum(fp_nav); 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"); - epochunum = get_epochnum(fp_obs); - 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.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); - return epochunum - result; - } -} diff --git a/src/framework.h b/src/framework.h deleted file mode 100644 index 80cbbc9..0000000 --- a/src/framework.h +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once - -#define WIN32_LEAN_AND_MEAN // 从 Windows 头文件中排除极少使用的内容 -// Windows 头文件 -#include diff --git a/src/pch.cpp b/src/pch.cpp deleted file mode 100644 index b6fb8f4..0000000 --- a/src/pch.cpp +++ /dev/null @@ -1,5 +0,0 @@ -// pch.cpp: 与预编译标头对应的源文件 - -#include "pch.h" - -// 当使用预编译的头时，需要使用此源文件，编译才能成功。 diff --git a/src/readrnx.cpp b/src/readrnx.cpp new file mode 100644 index 0000000..8bff24a --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/src/rnx2pos.cpp b/src/rnx2pos.cpp new file mode 100644 index 0000000..7e03517 --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/src/timesys.cpp b/src/timesys.cpp new file mode 100644 index 0000000..b65d2a2 --- /dev/null +++ b/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; +} \ No newline at end of file