From b28dd3e28d28caa2a664de7cb19142336ff6a5f7 Mon Sep 17 00:00:00 2001 From: KenanZhu111 <3471685733@qq.com> Date: Thu, 31 Oct 2024 14:20:09 +0800 Subject: [PATCH] See update notes --- README.md | 6 + READMECN.md | 6 + app/Mainui.py | 278 ++++---- src/brdm2pos.h | 638 ++++++++---------- src/coordtrans.cpp | 117 ++-- src/readrnx.cpp | 1525 +++++++++++++++++++++---------------------- src/rnx2pos.cpp | 677 ++++++++++--------- src/solutionout.cpp | 103 +++ src/timesys.cpp | 120 ++-- 9 files changed, 1732 insertions(+), 1738 deletions(-) create mode 100644 src/solutionout.cpp diff --git a/README.md b/README.md index 7cf7824..225ec49 100644 --- a/README.md +++ b/README.md @@ -36,6 +36,12 @@ Features ： ## Project Update +Update: 2024/10/31: +1. KNZ_GeoTrackLab: Fix some bug, Modify some UI logic. +2. KNZ_GeoTrackLab: Optimize code structure. +3. KNZ_GeoTrackLab: New added: + Elevation angle option. + Update: 2024/10/28 1. KNZ_GeoTrackLab: Fix some bug. 2. KNZ_GeoTrackLab: Optimize code structure. diff --git a/READMECN.md b/READMECN.md index 9b46c9e..28d3ebd 100644 --- a/READMECN.md +++ b/READMECN.md @@ -36,6 +36,12 @@ ## 项目更新 +更新: 2024/10/31: +1. KNZ_GeoTrackLab: 修复一些bug, 优化UI逻辑. +2. KNZ_GeoTrackLab: 优化代码结构. +3. KNZ_GeoTrackLab: 新增: + 截止高度角选项。 + 更新：2024/10/28 1. KNZ_GeoTrackLab：修复一些bug。 2. KNZ_GeoTrackLab：优化代码结构。 diff --git a/app/Mainui.py b/app/Mainui.py index 34c2f5d..48cf962 100644 --- a/app/Mainui.py +++ b/app/Mainui.py @@ -39,8 +39,10 @@ ioncormode = ['OFF', 'Ion-Free LC', 'Broadcast'] ioncorstate = 0 trocormode = ['OFF', 'Hopfield', 'Saastamoinen'] trocorstate = 0 +h_anglemode = ['0','5','10','15','20','25','30','35','40','45','50'] +h_anglestate = 0 #Sat system register part -satsysdict = {1: "G", 4:"R", 2:"C", 3:"E", 5:"S"} +satsysdict = {1: "G", 2:"C", 3:"E", 4:"R", 5:"S"} satellite_system = 1 ##Combox Count-er ComboxObsCount = 0 @@ -50,7 +52,7 @@ Navvalue = ('',) ComboxOutCount = 0 Outvalue = ('',) # -version = 'ver 1.5.3.1' +version = 'ver 1.5.4.1' # # # @@ -67,45 +69,60 @@ def Abo_windows(): abo = tk.Toplevel(root) abo.title("About") abo.attributes('-toolwindow', 2) - abo.geometry('200x100') - abo.minsize(200, 100) - abo.maxsize(200, 100) + Move_center(abo,200,100) + abo.minsize(200,100) + abo.maxsize(200,100) 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) +def Move_center(hwnd, win_x, win_y): + + position_x = int((hwnd.winfo_screenwidth() - win_x) / 2) + position_y = int((hwnd.winfo_screenheight() - win_y) / 2) + + hwnd.geometry(f'{win_x}x{win_y}+{position_x}+{position_y}') + class FUNCTION: @staticmethod - def Colorchoose(i, label, mode): + def Colorchoose(hwnd,i, label, mode): # Brief # choice to change the color of draw - # Param # i : only for mode 1 + # Param # i : for mode 0 & 1 # Param # label : match the label with the button # Param # mode : color register type - # undo choice == 0 & - # middle choice store == 1 & - # final confirm == 3 & - # satn color == 4 + # undo choice == 0 & + # middle choice store == 1 & + # final confirm == 3 & + # satn color == 4 & + # satn undo == 5 # Return# none global enucolor if mode == 0: - enucolor = ["#0080FF", "#0080FF", "#0080FF"] - satncolor[0] = "#0080FF" - label.config(bg=enucolor[0]) + #enucolor = ["#0080FF", "#0080FF", "#0080FF"] + #satncolor[0] = "#0080FF" + label.config(bg=enucolor[i]) elif mode == 1: - colorvalue = tk.colorchooser.askcolor() - enucolormid[i] = str(colorvalue[1]) - label.config(bg=str(colorvalue[1])) + colorvalue = tk.colorchooser.askcolor(parent=hwnd) + if colorvalue[1] is not None: + enucolormid[i] = str(colorvalue[1]) + label.config(bg=str(colorvalue[1])) + else: + return elif mode == 3: enucolor[0] = enucolormid[0] enucolor[1] = enucolormid[1] enucolor[2] = enucolormid[2] satncolor[0] = satncolormid[0] elif mode == 4: - colorvalue = tk.colorchooser.askcolor() - satncolormid[0] = str(colorvalue[1]) - label.config(bg=str(colorvalue[1])) - + colorvalue = tk.colorchooser.askcolor(parent=hwnd) + if colorvalue[1] is not None: + satncolormid[0] = str(colorvalue[1]) + label.config(bg=str(colorvalue[1])) + else: + return + elif mode==5: + label.config(bg=satncolor[0]) @staticmethod def Linewchoose(entry0, entry1, entry2, mode): # Brief # input to change the width of line @@ -159,7 +176,7 @@ class FUNCTION: enulinew[1] = entry1.get() enulinew[2] = entry2.get() - def Calcu_confirm(self, v, cboxi, cboxt): + def Calcu_confirm(self, v, cboxi, cboxt, cboxangle): # Brief # choice to change the ion & tro correction && confirm the sat system # Param # v sat system radio button value # Param # cboxi the combo box of ion correction @@ -167,10 +184,12 @@ class FUNCTION: # Return# none global ioncorstate global trocorstate + global h_anglestate global satellite_system - ioncorstate = self.Ion_and_Trocbox(cboxi, 0) - trocorstate = self.Ion_and_Trocbox(cboxt, 1) + ioncorstate = self.ITA_cbox(cboxi, 0) + trocorstate = self.ITA_cbox(cboxt, 1) + h_anglestate = self.ITA_cbox(cboxangle, 2) satellite_system = self.Satsys_button(v) print(ioncorstate, trocorstate, satellite_system) @@ -184,11 +203,12 @@ class FUNCTION: return int(Systemdict.get(v.get())) @staticmethod - def Ion_and_Trocbox(cbox, cortype): + def ITA_cbox(cbox, cortype): # Brief # get the value of combo box # Param # cortype: correction type of # ion == 0 & - # tro == 1 + # tro == 1 & + # angle == 2 # Param # cbox: # Return# ion | tro state code count = -1 @@ -204,11 +224,18 @@ class FUNCTION: if cbox.get() == tromode: return count + elif cortype == 2: + for h_angle in h_anglemode: + count += 1 + if cbox.get() == h_angle: + return count + @staticmethod - def GetObsFilePath(ObsFileSelectBox): + def GetObsFilePath(hwnd, ObsFileSelectBox): global ComboxObsCount global Obsvalue - path = filedialog.askopenfilename(title='RINEX OBS File', + path = filedialog.askopenfilename(parent=hwnd, + title='RINEX OBS File', filetypes=[('RINEX OBS File(*.o*.*.*obs.*.*d)', '*.*o;*.*obs;*.*d'), ('All Files', '*.*')]) if path and path not in ObsFileSelectBox['value']: @@ -218,10 +245,11 @@ class FUNCTION: ObsFileSelectBox.current(ComboxObsCount) @staticmethod - def GetNavFilePath(NavFileSelectBox): + def GetNavFilePath(hwnd, NavFileSelectBox): global ComboxNavCount global Navvalue - path = filedialog.askopenfilename(title='RINEX NAV File', + path = filedialog.askopenfilename(parent=hwnd, + 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', '*.*')]) @@ -232,10 +260,10 @@ class FUNCTION: NavFileSelectBox.current(ComboxNavCount) @staticmethod - def AskDirectory(AskDirectorySelectBox): + def AskDirectory(hwnd, AskDirectorySelectBox): global ComboxOutCount global Outvalue - path = filedialog.askdirectory() + path = filedialog.askdirectory(parent=hwnd) if path and path not in AskDirectorySelectBox['value']: Outvalue += (path,) AskDirectorySelectBox['value'] += (path + "/*.sp",) @@ -305,7 +333,8 @@ class FUNCTION: dll = cdll.LoadLibrary('./brdm2pos.dll') dll.brdm2pos.restype = c_double - match dll.brdm2pos(nav_path.encode(), obs_path.encode(), res_path.encode(), satellite_system): + match dll.brdm2pos(nav_path.encode(), obs_path.encode(), res_path.encode(), + satellite_system, int(h_anglemode[h_anglestate])): case 0.0: ExecuteState.config(text='Generated successfully ! file : %s' % obsfile + ".sp") case -1.0: @@ -322,9 +351,6 @@ class FUNCTION: ExecuteState.config(text='Error ! no obs data') windll.kernel32.FreeLibrary('./brdm2pos.dll') - - - def _ExecuteFile(self,ExecuteState, ObsSelectBoxVar, NavSelectBoxVar, @@ -377,7 +403,7 @@ class MAINGUI: def initGUI(self, hwnd): hwnd.title("KNZ_GeoTrackLab %s"%version) - hwnd.geometry('1200x700') + Move_center(hwnd,1200,700) hwnd.minsize(1200, 700) icon_img = (b'AAABAAEAICAAAAEAIACoEAAAFgAAACgAAAAgAAAAQAAAAAEAIAAAAAAAgBAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA' @@ -614,7 +640,7 @@ class MAINGUI: self.canvas0.draw() self.satn.clear() - self.satn.plot(satnumnarry[0, :], marker='_', linestyle=None, color=satncolor[0], linewidth=0) + self.satn.plot(satnumnarry[0, :], marker='|', ms=5, linestyle=None, color=satncolor[0], linewidth=0) self.satn.set_title('Valid Sat Numbers', loc='left', fontsize=8) self.satn.set_xlabel('Epochs') self.satn.grid(True, linestyle='--', alpha=0.7) @@ -630,19 +656,6 @@ class MAINGUI: # Const define self.sattrack.clear() C_V = 299792458 - f1 = 1575.42 - f2 = 1227.60 - - match satsysdict.get(satellite_system): - case 'G': - f1 = 1575.42 - f2 = 1227.60 - case 'E': - f1 = 1575.42 - f2 = 1278.75 - case 'C': - f1 = 1561.098 - f2 = 1207.140 ENU_O = np.empty((3, 0)) GDOP = np.empty((1, 0)) @@ -676,7 +689,7 @@ class MAINGUI: obspath = len(Obs) while obspath > 0: flag = Obs[obspath - 1:obspath] - if flag == "/": + if flag == "/" or flag == "\\": obsfile = satsysdict.get(satellite_system) + "-" + Obs[obspath:-4] break obspath -= 1 @@ -700,7 +713,7 @@ class MAINGUI: "@ GENERATE SYS : %s\n" "@ IONOS OPT : %s\n" "@ TROPO OPT : %s\n" - "@\n\n" % ( version + "@\n\n" % ( version , time.asctime(time.localtime()) , satsysdict.get(satellite_system) , ioncormode[ioncorstate] @@ -711,9 +724,9 @@ class MAINGUI: # Coordinate transformation matrix---------------# S = np.array([ - [-math.sin(apL), math.cos(apL), 0], + [-math.sin(apL) , math.cos(apL) , 0 ], [-math.sin(apB) * math.cos(apL), -math.sin(apB) * math.sin(apL), math.cos(apB)], - [math.cos(apB) * math.cos(apL), math.cos(apB) * math.sin(apL), math.sin(apB)] + [ math.cos(apB) * math.cos(apL), math.cos(apB) * math.sin(apL), math.sin(apB)] ]) # -----------------------------------------------# self.plotstate.config(text="Reading...") @@ -733,9 +746,9 @@ class MAINGUI: satY = np.empty((1, 0)) satZ = np.empty((1, 0)) C1 = np.empty((1, 0)) - C2 = np.empty((1, 0)) + ma = np.empty((1, 0)) - np.empty((1, 0)) + Dt = np.empty((1, 0)) year = int(line[6:10]) @@ -798,39 +811,14 @@ class MAINGUI: n = -((satZ[0, count] - ap_Z) / R) B = np.vstack((B, [l, m, n, 1])) Ptem.append(pow(math.sin(math.radians(ma[0, count])), 2)) - Pl = 0 # Get the tropsphere correction match trocorstate: - case 0: - Tro = 0 - case 1: - Tro = Trocor.Hop_tro_cor(apH, ma[0, count]) - case 2: - Tro = Trocor.Sas_tro_cor(apB, apH, ma[0, count]) - pass - # Get the ionsphere correction - match ioncorstate: - case 0: - if C1[0, count] == 0 and C2[0, count] != 0: - Pl = C2[0, count] - - elif C1[0, count] != 0 and C2[0, count] == 0: - Pl = C1[0, count] - case 1: - if C2[0, count] != 0 and C1[0, count] != 0: - Pl = (f1 * f1 * C1[0, count]) / (f1 * f1 - f2 * f2) - ( - f2 * f2 * C2[0, count]) / (f1 * f1 - f2 * f2) - - elif C1[0, count] == 0 and C2[0, count] != 0: - Pl = C2[0, count] - - elif C1[0, count] != 0 and C2[0, count] == 0: - Pl = C1[0, count] - case 2: - pass + case 0:Tro = 0 + case 1:Tro = Trocor.Hop_tro_cor(apH, ma[0, count]) + case 2:Tro = Trocor.Sas_tro_cor(apB, apH, ma[0, count]) L = np.vstack( - ( L, [Pl - R + C_V * Dt[0, count] - Tro] ) + ( L, [C1[0, count] - R + C_V * Dt[0, count] - Tro] ) ) count += 1 @@ -850,19 +838,18 @@ class MAINGUI: DeltaXYZ = np.array([[X - apX], [Y - apY], [Z - apZ]]) ENU = S @ DeltaXYZ #if math.fabs(ENU[0, 0]) > 25 or math.fabs(ENU[1, 0]) > 25 or math.fabs(ENU[2, 0]) > 50: - # ENU_O = np.hstack((ENU_O, [[0], [0], [0]])) - # print("------------ ANOMALY RESOLUTION", file=of) + # ENU_O = np.hstack((ENU_O, [[0], [0], [0]])) + # print("------------ ANOMALY RESOLUTION", file=of) #else: - ENU_O = np.hstack((ENU_O, [[ENU[0, 0]], [ENU[1, 0]], [ENU[2, 0]]])) GDOP = np.hstack((GDOP, [ [math.sqrt(Qtrace)] ])) print( "%4d\\%02d\\%02d\\%02d\\%02d\\%04.1f %12.4f %12.4f %12.4f %3d %10.5f %10.5f %10.5f" % (year, month, day, hour, min, sec, X, Y, Z, satnum, - float(ENU[0, 0]), - float(ENU[1, 0]), - float(ENU[2, 0]) ), file=of) + float(ENU[0, 0]), + float(ENU[1, 0]), + float(ENU[2, 0]) ), file=of) break else: break @@ -875,9 +862,8 @@ class MAINGUI: satY = np.hstack((satY, [[float(line[22:37])]])) satZ = np.hstack((satZ, [[float(line[38:53])]])) C1 = np.hstack((C1, [[float(line[55:69])]])) - C2 = np.hstack((C2, [[float(line[71:85])]])) - ma = np.hstack((ma, [[float(line[86:101])]])) - Dt = np.hstack((Dt, [[float(line[102:118])]])) + ma = np.hstack((ma, [[float(line[71:85])]])) + Dt = np.hstack((Dt, [[float(line[86:102])]])) elif line.find("END") >= 0: self.ex.clear() @@ -904,7 +890,7 @@ class MAINGUI: self.canvas0.draw() self.satn.clear() - self.satn.plot(satnumnarry[0, :], marker='_', ms=10, linestyle=None, color=satncolor[0], linewidth=0) + self.satn.plot(satnumnarry[0, :], marker='|', ms=5, linestyle=None, color=satncolor[0], linewidth=0) self.satn.set_title('Valid Satellite Numbers',loc='left', fontsize=8) self.satn.set_xlabel('Epochs') self.satn.grid(True, linestyle='--', alpha=0.7) @@ -939,10 +925,10 @@ class OPTGUI: def initGUI(self,hwnd): self.opthwnd = tk.Toplevel(hwnd) self.opthwnd.title("Options") - self.opthwnd.resizable(0, 0) - self.opthwnd.geometry('320x250') - self.opthwnd.minsize(320, 250) - self.opthwnd.maxsize(320, 250) + self.opthwnd.resizable(0,0) + Move_center(self.opthwnd,320,250) + self.opthwnd.minsize(320,250) + self.opthwnd.maxsize(320,250) icon_img2 = (b'AAABAAEAICAAAAAAIACoEAAAFgAAACgAAAAgAAAAQAAAAAEAIAAAAAAAABAAADjsAAA47AAAAAA' b'AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA' @@ -1066,6 +1052,22 @@ class OPTGUI: self.ctrobox.current(trocorstate) self.ctrobox.pack(side=tk.RIGHT, anchor='e') + + TOP3Frame0 = ttk.Frame(self.Main1Frame0) + TOP3Frame0.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='2px', fill=tk.X) + + h_angle = ttk.Label(TOP3Frame0, text='Satellite Elevation(°) / #') + h_angle.pack(side=tk.LEFT, anchor='w', padx='1px') + + self.angleentry = ttk.Entry(TOP3Frame0, width=8) + self.angleentry.pack(side=tk.RIGHT, anchor='w') + + self.canglebox = ttk.Combobox(TOP3Frame0, width=6, height=10) + self.canglebox['state'] = 'readonly' + self.canglebox['value'] = ('0','5','10','15','20','25','30','35','40','45','50') + self.canglebox.current(h_anglestate) + self.canglebox.pack(side=tk.RIGHT, anchor='e') + # GNSS Options# TOP5Frame0 = ttk.LabelFrame(self.Main1Frame0, text='Satellite system', labelanchor='nw') TOP5Frame0.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='2px', fill=tk.X) @@ -1132,15 +1134,15 @@ class OPTGUI: SatnFrameB1 = tk.Frame(Buttom1Frame1) SatnFrameB1.pack(side=tk.LEFT, expand=tk.YES, padx='5px', pady='0px') satncolopt = ttk.Button(SatnFrameB1, text='Satn Color', width=10, - command=lambda: self.Cmd.Colorchoose(0, satncollab, 4)) + command=lambda: self.Cmd.Colorchoose(self.opthwnd,0, satncollab, 4)) satncolopt.pack(side=tk.LEFT, expand=tk.YES, padx='5px', pady='0px') - satncollab = tk.Label(SatnFrameB1, width=5, height=1, bg=satncolormid[0]) + satncollab = tk.Label(SatnFrameB1, width=5, height=1, bg=satncolor[0]) satncollab.pack(side=tk.LEFT, expand=tk.YES, padx='5px', pady='0px') LineConB1 = tk.Frame(Buttom1Frame1) LineConB1.pack(side=tk.RIGHT, expand=tk.YES, padx='5px', pady='0px') linewre = ttk.Button(LineConB1, text='Undo', width=10, - command=lambda: self.Cmd.Colorchoose(0, satncollab, 0)) + command=lambda: self.Cmd.Colorchoose(self.opthwnd,0, satncollab, 5)) linewre.pack(side=tk.LEFT, expand=tk.YES, padx='1px') # Line's colors options# @@ -1150,7 +1152,7 @@ class OPTGUI: EFrameL2 = tk.Frame(Left2Frame1) EFrameL2.pack(side=tk.TOP, expand=tk.YES, padx='5px', pady='0px') ecolopt = ttk.Button(EFrameL2, text='E Color', width=10, - command=lambda: self.Cmd.Colorchoose(0, ecollab, 1)) + command=lambda: self.Cmd.Colorchoose(self.opthwnd,0, ecollab, 1)) ecolopt.pack(side=tk.LEFT, expand=tk.YES, padx='5px', pady='0px') ecollab = tk.Label(EFrameL2, width=5, height=1, bg=enucolor[0]) ecollab.pack(side=tk.LEFT, expand=tk.YES, padx='5px', pady='0px') @@ -1158,7 +1160,7 @@ class OPTGUI: NFrameL2 = tk.Frame(Left2Frame1) NFrameL2.pack(side=tk.TOP, expand=tk.YES, padx='5px', pady='0px') ncolopt = ttk.Button(NFrameL2, text='N Color', width=10, - command=lambda: self.Cmd.Colorchoose(1, ncollab, 1)) + command=lambda: self.Cmd.Colorchoose(self.opthwnd,1, ncollab, 1)) ncolopt.pack(side=tk.LEFT, expand=tk.YES, padx='5px', pady='0px') ncollab = tk.Label(NFrameL2, width=5, height=1, bg=enucolor[1]) ncollab.pack(side=tk.LEFT, expand=tk.YES, padx='5px', pady='0px') @@ -1166,7 +1168,7 @@ class OPTGUI: UFrameL2 = tk.Frame(Left2Frame1) UFrameL2.pack(side=tk.TOP, expand=tk.YES, padx='5px', pady='0px') ucolopt = ttk.Button(UFrameL2, text='U Color', width=10, - command=lambda: self.Cmd.Colorchoose(2, ucollab, 1)) + command=lambda: self.Cmd.Colorchoose(self.opthwnd,2, ucollab, 1)) ucolopt.pack(side=tk.LEFT, expand=tk.YES, padx='5px', pady='0px') ucollab = tk.Label(UFrameL2, width=5, height=1, bg=enucolor[2]) ucollab.pack(side=tk.LEFT, expand=tk.YES, padx='5px', pady='0px') @@ -1174,9 +1176,9 @@ class OPTGUI: ColorConL2 = tk.Frame(Left2Frame1) ColorConL2.pack(side=tk.TOP, expand=tk.YES, padx='5px', pady='0px') colorcon = ttk.Button(ColorConL2, text='Undo', width=10, - command=lambda: [self.Cmd.Colorchoose(0, ecollab, 0) - , self.Cmd.Colorchoose(0, ncollab, 0) - , self.Cmd.Colorchoose(0, ucollab, 0)]) + command=lambda: [self.Cmd.Colorchoose(self.opthwnd,0, ecollab, 0) + , self.Cmd.Colorchoose(self.opthwnd,1, ncollab, 0) + , self.Cmd.Colorchoose(self.opthwnd,2, ucollab, 0)]) colorcon.pack(side=tk.LEFT, expand=tk.YES, padx='5px', pady='0px') def initCONFIRM(self): @@ -1187,12 +1189,11 @@ class OPTGUI: command=lambda: [self.opthwnd.destroy()]) TotalCancel.pack(side=tk.RIGHT, padx='1px', anchor='e') TotalCon = ttk.Button(Bottom0Frame0, text='Confirm', width=10, - command=lambda: [self.Cmd.Colorchoose(0, None, 3), + command=lambda: [self.Cmd.Colorchoose(self.opthwnd,0, None, 3), self.Cmd.Linewchoose(self.elinewidth, self.nlinewidth, self.ulinewidth, 1), - self.Cmd.Calcu_confirm(self.v, self.cionbox, self.ctrobox), - self.opthwnd.destroy()]) + self.Cmd.Calcu_confirm(self.v, self.cionbox, self.ctrobox, self.canglebox)]) TotalCon.pack(side=tk.RIGHT, padx='1px', anchor='e') class EXECUDUI: @@ -1213,10 +1214,10 @@ class EXECUDUI: def initGUI(self,hwnd): self.exehwnd = tk.Toplevel(hwnd) self.exehwnd.title("RINEX Processor") - self.exehwnd.resizable(0, 0) - self.exehwnd.geometry('400x215') - self.exehwnd.minsize(400, 215) - self.exehwnd.maxsize(400, 215) + self.exehwnd.resizable(0,0) + Move_center(self.exehwnd,550,180) + self.exehwnd.minsize(550,180) + self.exehwnd.maxsize(550,180) global icon_img3 icon_img3 = (b'AAABAAEAICAAAAEAIACoEAAAFgAAACgAAAAgAAAAQAAAAAEAIAAAAAAAABAAADjsAAA47AAAAAAAAAAAAAAAAAAAAAAAAAA' @@ -1289,41 +1290,41 @@ class EXECUDUI: MainFrame0 = tk.Frame(self.exehwnd) MainFrame0.pack() TopFrame1 = ttk.Frame(MainFrame0) - TopFrame1.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='2px', fill=tk.X) + TopFrame1.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='1px', fill=tk.X) # ----------------------------------------------------------------------------------- ObsInFrame3 = ttk.LabelFrame(TopFrame1, text='RINEX Obs File') - ObsInFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='2px', fill=tk.X) + ObsInFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='0px', fill=tk.X) - ObsFileSelectBox = ttk.Combobox(ObsInFrame3, width=46, height=4, values=Obsvalue, + ObsFileSelectBox = ttk.Combobox(ObsInFrame3, width=70, height=4, values=Obsvalue, textvariable=self.ObsSelectBoxVar) ObsFileSelectBox.current(ComboxObsCount) - ObsFileSelectBox.pack(side=tk.LEFT, anchor='w', padx='1px', pady='1px') + ObsFileSelectBox.pack(side=tk.LEFT, anchor='w', padx='0px', pady='0px') ObsFileSelectButton = ttk.Button(ObsInFrame3, text='...', width=3, - command=lambda: self.Cmd.GetObsFilePath(ObsFileSelectBox)) - ObsFileSelectButton.pack(side=tk.RIGHT, anchor='e', padx='1px', pady='1px') + command=lambda: self.Cmd.GetObsFilePath(self.exehwnd,ObsFileSelectBox)) + ObsFileSelectButton.pack(side=tk.RIGHT, anchor='e', padx='1px', pady='0px') # ----------------------------------------------------------------------------------- #----------------------------------------------------------------------------------- NavInFrame3 = ttk.LabelFrame(TopFrame1, text='RINEX Nav File') - NavInFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='2px', fill=tk.X) + NavInFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='0px', fill=tk.X) - NavFileSelectBox = ttk.Combobox(NavInFrame3, width=46, height=4, values=Navvalue, + NavFileSelectBox = ttk.Combobox(NavInFrame3, width=70, height=4, values=Navvalue, textvariable=self.NavSelectBoxVar) NavFileSelectBox.current(ComboxNavCount) - NavFileSelectBox.pack(side=tk.LEFT, anchor='w', padx='1px', pady='1px') + NavFileSelectBox.pack(side=tk.LEFT, anchor='w', padx='0px', pady='0px') NavFileSelectButton = ttk.Button(NavInFrame3, text='...', width=3, - command=lambda: self.Cmd.GetNavFilePath(NavFileSelectBox)) - NavFileSelectButton.pack(side=tk.RIGHT, anchor='e', padx='1px', pady='1px') + command=lambda: self.Cmd.GetNavFilePath(self.exehwnd,NavFileSelectBox)) + NavFileSelectButton.pack(side=tk.RIGHT, anchor='e', padx='1px', pady='0px') # ----------------------------------------------------------------------------------- BottomFrame1 = ttk.Frame(MainFrame0) - BottomFrame1.pack(side=tk.BOTTOM, expand=tk.YES, padx='1px', pady='2px', fill=tk.X) + BottomFrame1.pack(side=tk.BOTTOM, expand=tk.YES, padx='1px', pady='0px', fill=tk.X) FileOutFrame3 = ttk.LabelFrame(BottomFrame1, text='Output Directory') - FileOutFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='2px', fill=tk.X) + FileOutFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='0px', fill=tk.X) ## CheckButton Initialize AskOrNotCheckButton = ttk.Checkbutton(FileOutFrame3, text='Directory', @@ -1332,21 +1333,21 @@ class EXECUDUI: self.AskOrNotCheckButtonVar, AskDirectorySelectBox, AskDirectorySelectButton)) - AskOrNotCheckButton.pack(side=tk.LEFT, anchor='w', padx='1px', pady='0px') + AskOrNotCheckButton.pack(side=tk.LEFT, anchor='w', padx='0px', pady='0px') ## SelectBox Initialize - AskDirectorySelectBox = ttk.Combobox(FileOutFrame3, width=36, height=4, values=Outvalue, + AskDirectorySelectBox = ttk.Combobox(FileOutFrame3, width=60, height=4, values=Outvalue, state=tk.DISABLED, textvariable=self.DirectorySelectBoxVar) AskDirectorySelectBox.current(ComboxOutCount) - AskDirectorySelectBox.pack(side=tk.LEFT, anchor='w', padx='1px', pady='0px') + AskDirectorySelectBox.pack(side=tk.LEFT, anchor='w', padx='0px', pady='0px') ## SelectButton Initialize - AskDirectorySelectButton = ttk.Button(FileOutFrame3, text='...', width=3, - state=tk.DISABLED, command=lambda: self.Cmd.AskDirectory(AskDirectorySelectBox)) - AskDirectorySelectButton.pack(side=tk.RIGHT, anchor='e', padx='2px', pady='0px') + AskDirectorySelectButton = ttk.Button(FileOutFrame3, text='...', width=3, state=tk.DISABLED, + command=lambda: self.Cmd.AskDirectory(self.exehwnd, AskDirectorySelectBox)) + AskDirectorySelectButton.pack(side=tk.RIGHT, anchor='e', padx='1px', pady='0px') ExeFrame3 = ttk.Frame(BottomFrame1) - ExeFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='2px', fill=tk.X) + ExeFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='0px', fill=tk.X) ExitButton = ttk.Button(ExeFrame3, text='Exit', width=10, command=self.exehwnd.destroy) @@ -1372,7 +1373,8 @@ def View_windowshow(text, view): # Param # text : the show box of file of choice # Param # view : the father window of text ctr box # Return# none - f_path = filedialog.askopenfilename() + f_path = filedialog.askopenfilename(parent=view) + if not f_path: return 0 view.title('Viewing: %s' % f_path) text.delete('1.0', 'end') with open(f_path, 'r') as f: @@ -1388,7 +1390,7 @@ class VIEWGUI: def initGUI(hwnd): view = tk.Toplevel(hwnd) view.title("View") - view.geometry('900x600') + Move_center(view,1000,600) view.minsize(1000, 600) global icon_img1 diff --git a/src/brdm2pos.h b/src/brdm2pos.h index 33a7d88..1cae3d9 100644 --- a/src/brdm2pos.h +++ b/src/brdm2pos.h @@ -8,398 +8,330 @@ #include #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 +#define C_V 299792458 /* Speed of light (m/s) */ +#define GM 398600500000000 /* */ +#define math_e 2.718281828459 /* Natural constant e */ +#define PI 3.141592653589793 /* Ratio of circumference to diameter */ +#define Earth_e 7.2921151467e-5 /* Rotational angular velocity of the earths (omega/s) */ +#define C20 1.0826257e-3 /* */ +#define DAYSEC 86400 /* The total second of a day */ /*WGS-84 CONST*/ #define a 6378137.0 #define e2 (0.0033528106647475*(2 - 0.0033528106647475)) /*PZ-90 CONST*/ -#define TSTEP 30.0 +#define TSTEP 30.0 /* RK-4 step */ /*GNSS OBSERV TYPE*/ +#define FREQ1 1.57542E9 /* L1/E1/B1C frequency (Hz) */ +#define FREQ2 1.22760E9 /* L2 frequency (Hz) */ +#define FREQ5 1.17645E9 /* L5/E5a/B2a frequency (Hz) */ +#define FREQ6 1.27875E9 /* E6/L6 frequency (Hz) */ +#define FREQ7 1.20714E9 /* E5b frequency (Hz) */ +#define FREQ8 1.191795E9 /* E5a+b frequency (Hz) */ +#define FREQ9 2.492028E9 /* S frequency (Hz) */ +#define FREQ1_GLO 1.60200E9 /* GLONASS G1 base frequency (Hz) */ +#define DFRQ1_GLO 0.56250E6 /* GLONASS G1 bias frequency (Hz/n) */ +#define FREQ2_GLO 1.24600E9 /* GLONASS G2 base frequency (Hz) */ +#define DFRQ2_GLO 0.43750E6 /* GLONASS G2 bias frequency (Hz/n) */ +#define FREQ3_GLO 1.202025E9 /* GLONASS G3 frequency (Hz) */ +#define FREQ1a_GLO 1.600995E9 /* GLONASS G1a frequency (Hz) */ +#define FREQ2a_GLO 1.248060E9 /* GLONASS G2a frequency (Hz) */ +#define FREQ1_BDS 1.561098E9 /* BDS B1I frequency (Hz) */ +#define FREQ2_BDS 1.20714E9 /* BDS B2I/B2b frequency (Hz) */ +#define FREQ3_BDS 1.26852E9 /* BDS B3 frequency (Hz) */ //frequency 1 -#define C1 100 -#define C1A 100 -#define C1B 101 -#define C1C 102 -#define C1D 103 -#define C1E -#define C1F -#define C1G -#define C1H -#define C1I -#define C1J -#define C1K -#define C1L 104 -#define C1M 105 -#define C1N -#define C1O -#define C1P 106 -#define C1Q -#define C1R -#define C1S 107 -#define C1T -#define C1U -#define C1V -#define C1W -#define C1X 108 -#define C1Y 109 -#define C1Z 110 +#define C1 100 +#define C1A 100 +#define C1B 101 +#define C1C 102 +#define C1D 103 +#define C1L 104 +#define C1M 105 +#define C1P 106 +#define C1S 107 +#define C1X 108 +#define C1Y 109 +#define C1Z 110 //frequency 2 -#define C2 200 -#define C2A -#define C2B -#define C2C 200 -#define C2D 201 -#define C2E -#define C2F -#define C2G -#define C2H -#define C2I 202 -#define C2J -#define C2K -#define C2L 203 -#define C2M 204 -#define C2N -#define C2O -#define C2P 205 -#define C2Q 206 +#define C2 200 +#define C2C 200 +#define C2D 201 +#define C2I 202 +#define C2L 203 +#define C2M 204 +#define C2P 205 +#define C2Q 206 #define C2R -#define C2S 207 -#define C2T -#define C2U -#define C2V -#define C2W -#define C2X 208 -#define C2Y 209 -#define C2Z 210 +#define C2S 207 +#define C2X 208 +#define C2Y 209 +#define C2Z 210 //frequency 3 -#define C3 300 -#define C3I 300 -#define C3Q 301 -#define C3X 302 +#define C3 300 +#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 C5A -#define C5B -#define C5C -#define C5D -#define C5E -#define C5F -#define C5G -#define C5H -#define C5I 500 -#define C5J -#define C5K -#define C5L -#define C5M -#define C5N -#define C5O -#define C5P -#define C5Q 501 -#define C5R -#define C5S -#define C5T -#define C5U -#define C5V -#define C5W -#define C5X 502 -#define C5Y -#define C5Z +#define C5 500 +#define C5I 500 +#define C5Q 501 +#define C5X 502 //frequency 6 -#define C6 600 -#define C6A 600 -#define C6B 601 -#define C6C 602 -#define C6D -#define C6E -#define C6F -#define C6G -#define C6H -#define C6I -#define C6J -#define C6K -#define C6L -#define C6M -#define C6N -#define C6O -#define C6P -#define C6Q -#define C6R -#define C6S -#define C6T -#define C6U -#define C6V -#define C6W -#define C6X 603 -#define C6Y -#define C6Z 604 +#define C6 600 +#define C6A 600 +#define C6B 601 +#define C6C 602 +#define C6X 603 +#define C6Z 604 //frequency 7 -#define C7 700 -#define C7A -#define C7B -#define C7C -#define C7D -#define C7E -#define C7F -#define C7G -#define C7H -#define C7I 700 -#define C7J -#define C7K -#define C7L -#define C7M -#define C7N -#define C7O -#define C7P -#define C7Q 701 -#define C7R -#define C7S -#define C7T -#define C7U -#define C7V -#define C7W -#define C7X 702 -#define C7Y -#define C7Z +#define C7 700 +#define C7I 700 +#define C7Q 701 +#define C7X 702 //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 +#define GPS 01 +#define BDS 02 +#define GAL 03 +#define GLO 04 +#define SBAS 05 -#define MAXRINEX 1000 -#define DAYSEC 86400 -//satellite position & clock bias structure -typedef struct -{ - double X; - double Y; - double Z; - double deltat; - double delta_t; - double delta_clk; +#define MAXRINEX 4096 /* Max length of line of rinex file */ +#define MAXPATH 260 /* Max length of file path */ + +typedef struct pos_ts{//Satellite position + double X; + double Y; + double Z; + double deltat; + double delta_t; + double delta_clk; }pos_ts; -//receiver station position & clock bias structure -typedef struct -{ - double X; - double Y; - double Z; - double B; - double L; - double H; - double delta_TR; + +typedef struct stations{// + double X; + double Y; + double Z; + double B; + double L; + double H; + double delta_TR; }stations; -//longitude & latitude structure -typedef struct -{ - double B; - int B_d; - int B_m; - int B_s; - double L; - int L_d; - int L_m; - int L_s; - double H; -}blhs; -//satellite azimuth angle structure -typedef struct -{ - double R; - double A; - double H; -}rahcal; -//local Cartesian coordinates coordinate system structure -typedef struct -{ - double E; - double N; - double U; -}blh2enu; -typedef struct -{ - double B; - double L; - double H; -}xyz2blh; -//header data of file of nav -typedef struct nav_head -{ - double ver;//rinex - char type[20]; - double ION_GPSA[4]; - double ION_GPSB[4]; - double ION_BDSA[4]; - double ION_BDSB[4]; - double ION_QZSA[4]; - double ION_QZSB[4]; - double ION_IRNA[4]; - double ION_IRNB[4]; - double GPUT[4]; - double GLUT[4]; - double GAUT[4]; - double BDUT[4]; - double QZUT[4]; - double IRUT[4]; - double SBUT[4]; - int leap; +typedef struct rah{// + double R; + double A; + double H; +}rah; + +typedef struct enu{//East/North/Up coordinate system + double E; + double N; + double U; +}enu; + +typedef struct blh{//Longitude/Latitude/Altitude coordinate system + double B; + double L; + double H; +}blh; + +typedef struct blhdms{//Longitude/Latitude/Altitude coordinate system + //With Degree/Minute/Second + double B; + int B_d; + int B_m; + int B_s; + double L; + int L_d; + int L_m; + int L_s; + double H; +}blhdms; + + +typedef struct nav_head{//Header data of file of nav + double ver; /* RINEX version */ + char type[20]; /* RINEX file format */ + + double ION_GPSA[4]; /* Ionosphere correction parameters alpha GPS */ + double ION_GPSB[4]; /* Ionosphere correction parameters belta GPS */ + double ION_BDSA[4]; /* Ionosphere correction parameters alpha BeiDou */ + double ION_BDSB[4]; /* Ionosphere correction parameters belta BeiDou */ + double ION_QZSA[4]; /* Ionosphere correction parameters alpha QZSS */ + double ION_QZSB[4]; /* Ionosphere correction parameters belta QZSS */ + double ION_IRNA[4]; /* Ionosphere correction parameters alpha */ + double ION_IRNB[4]; /* Ionosphere correction parameters belta */ + + double GPUT[4]; /* Time system correction of GPST */ + double GLUT[4]; /* Time system correction of GLONASST */ + double GAUT[4]; /* Time system correction of GALT */ + double BDUT[4]; /* Time system correction of BDT */ + double QZUT[4]; /* Time system correction of QZST */ + double IRUT[4]; /* Time system correction of */ + double SBUT[4]; /* Time system correction of SBAST */ + + int leap; /* Leap second */ }nav_head, * pnav_head; -//body data of file of nav: GPS BeiDou Galileo -typedef struct nav_body -{ - int sPRN_GPS; int sPRN_BDS; int sPRN_GAL; int sPRN_GLO; - int TOC_Y; - int TOC_M; - int TOC_D; - int TOC_H; - int TOC_Min; - int TOC_Sec; - double sa0; - double sa1; - double sa2; double Dos; - double IODE; double AODE; double SatX; - double Crs; double SatXv; - double deltan; double SatXa; - double M0; +typedef struct nav_body{//Body data of file of nav: GPS BeiDou Galileo GLONASS - double Cuc; double SatY; - double e; double SatYv; - double Cus; double SatYa; - double sqrtA; int FreN; + /* GPS BeiDou Galileo GLONASS*/ + int sPRN_GPS; int sPRN_BDS; int sPRN_GAL; int sPRN_GLO;/* System */ + int TOC_Y; + int TOC_M; + int TOC_D; + int TOC_H; + int TOC_Min; + int TOC_Sec; + double sa0; + double sa1; + double sa2; double Dos; - double TOE; double SatZ; - double Cic; double SatZv; - double OMEGA; double SatZa; - double Cis; int AOO; + double IODE; double AODE; double SatX; + double Crs; double SatXv; + double deltan; double SatXa; + double M0; - double i0; - double Crc; - double omega; - double deltaomega; + double Cuc; double SatY; + double e; double SatYv; + double Cus; double SatYa; + double sqrtA; int FreN; - double IDOT; - double L2code; int Datasource; - double GPSweek; double BDTweek; double GALweek; - double L2Pflag; + double TOE; double SatZ; + double Cic; double SatZv; + double OMEGA; double SatZa; + double Cis; int AOO; - double sACC; - double sHEA; - double TGD; double TGD1; double BGDa; - double IODC; double TGD2; double BGDb; + double i0; + double Crc; + double omega; + double deltaomega; - double TTN; - double fit; double AODC; - double spare1; - double spare2; + double IDOT; + double L2code; int Datasource; + double GPSweek; double BDTweek; double GALweek; + double L2Pflag; + + double sACC; + double sHEA; + double TGD; double TGD1; double BGDa; + double IODC; double TGD2; double BGDb; + + double TTN; + double fit; double AODC; + double spare1; + double spare2; }nav_body, * pnav_body; -//header data of file of obs -typedef struct obs_head -{ - double ver; - char type[30]; - double apX; - double apY; - double apZ; - double ANTH; - double ANTdeltaE; - double ANTdeltaN; - int obstypenum_gps; - int obstypenum_bds; - int obstypenum_gal; - int obstypenum_glo; - int obscode_gps[40]; - int obscode_bds[40]; - int obscode_gal[40]; - int obscode_glo[40]; - double interval; - int f_y; - int f_m; - int f_d; - int f_h; - int f_min; - double f_sec; - int l_y; - int l_m; - int l_d; - int l_h; - int l_min; - double l_sec; - char tsys[5];//time system + +typedef struct obs_head{//Header data of file of obs + double ver; + char type[30]; + + double apX; + double apY; + double apZ; + + double apB; + double apL; + double apH; + + double ANTH; + double ANTdeltaE; + double ANTdeltaN; + + int obstypenum_gps; /* Observation amount of GPS */ + int obstypenum_bds; /* Observation amount of BeiDou */ + int obstypenum_gal; /* Observation amount of Galilro */ + int obstypenum_glo; /* Observation amount of GLONASS */ + + int obscode_gps[40];/* Observation code of GPS */ + int obscode_bds[40];/* Observation code of BeiDou */ + int obscode_gal[40];/* Observation code of Galileo */ + int obscode_glo[40];/* Observation code of GLONASS */ + + double interval; /* */ + + int f_y; /* Year of first observe */ + int f_m; /* Month of first observe */ + int f_d; /* Day of first observe */ + int f_h; /* Hour of first observe */ + int f_min; /* Minute of first observe */ + double f_sec; /* Second of first observe */ + + int l_y; /* Year of last observe */ + int l_m; /* Month of last observe */ + int l_d; /* Day of last observe */ + int l_h; /* Hour of last observe */ + int l_min; /* Minute of last observe */ + double l_sec; /* Second of last observe */ + + char tsys[5]; /* Time system of obs */ }obs_head, * pobs_head; -//epoch data of file of obs -typedef struct obs_epoch -{ - int y; - int m; - int d; - int h; - int min; - double sec;//epoch time - int p_flag;//epoch flag - int sat_num;//the total number of obsable sat - int gps_num;//the total gps sat - int bds_num;//the total bds sat - int gal_num;//the total galileo sat - int glo_num;//the total glonass sat - int sPRN[84];//total sprn of obs sat - int sPRN_GPS[32]; - int sPRN_BDS[60]; - int sPRN_GAL[30]; - int sPRN_GLO[24]; + +typedef struct obs_epoch{//Epoch data of file of obs + int y; /* Epoch year */ + int m; /* Epoch month */ + int d; /* Epoch day */ + int h; /* Epoch hour */ + int min; /* Epoch minute */ + double sec; /* Epoch second */ + int p_flag; /* Epoch flag */ + int sat_num; /* The total number of obsable sat */ + int gps_num; /* The total GPS satellite */ + int bds_num; /* The total BeiDou satellite */ + int gal_num; /* The total Galileo satellite */ + int glo_num; /* The total GLONASS satellite */ + int sPRN[84]; /* Total sprn of obs satellite */ + int sPRN_GPS[32];/* Total sprn of GPS satellite in this epoch */ + int sPRN_BDS[60];/* Total sprn of BeiDou satellite in this epoch */ + int sPRN_GAL[30];/* Total sprn of Galileo satellite in this epoch */ + int sPRN_GLO[24];/* Total sprn of GLONASS satellite in this epoch */ }obs_epoch, * pobs_epoch; -//body data of file of obs -typedef struct obs_body -{ - double obs_gps[20][26];//GPS obs data - double obs_bds[30][26];//BDS obs data - double obs_gal[20][26];//GALILEO obs data - double obs_glo[20][26];//GLONASS obs data + +typedef struct obs_body{//Body data of file of obs + double obs_gps[30][26];/* GPS obs data */ + double obs_bds[40][26];/* BeiDou obs data */ + double obs_gal[30][26];/* Galileo obs data */ + double obs_glo[30][26];/* GLONASS obs data */ }obs_body, * pobs_body; -// + +//Contain in timesys.cpp 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); +//Contain in coordtrans.cpp +extern double deg2rad(double); +extern double rad2deg(double); +extern rah rahcal(rah, double, double, double); +extern enu blh2enu(enu, double, double, double, double, double); +extern blh xyz2blh(blh, double, double, double); +//Contain in readrnx.cpp +extern int getsatnum(FILE*); +extern int get_epochnum(FILE*); 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 int type2code(int, char buff[MAXRINEX]); +extern int code2type(int, int, int typearr[36]); +extern int select_epoch(double, int, pnav_body, int, int); +//Contain in readrnx.cpp/read 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); +//Contain in solutionout.cpp +extern FILE* outputheader(pobs_head, char obs_path[MAXPATH], char nav_path[MAXPATH], char res_path[MAXPATH]); +extern void outputbodydata(pobs_head, pobs_body, pos_ts, rah, FILE*, int, int, int); +//Entry function +extern "C" _declspec(dllexport) double brdm2pos(char nav_path[MAXPATH], char obs_path[MAXPATH], char res_path[MAXPATH], + int GNSS, int Hangle); diff --git a/src/coordtrans.cpp b/src/coordtrans.cpp index ca02984..7b840ac 100644 --- a/src/coordtrans.cpp +++ b/src/coordtrans.cpp @@ -1,57 +1,61 @@ #include "brdm2pos.h" using namespace std; -/* -------------------------------------------------------------------------- */ -/// @brief degree to radian -/// @param deg -/// @return return radian -/* -------------------------------------------------------------------------- */ +/** + * Degree to radian. + * + * \param deg: Degree + * \return Radian + */ double deg2rad(double deg) { double rad = (PI / 180) * deg; return rad; } -/* -------------------------------------------------------------------------- */ -/// @brief radian to degree -/// @param rad -/// @return return degree -/* -------------------------------------------------------------------------- */ +/** + * Radian to degree . + * + * \param rad: Radian + * \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, +/** + * Azimuth/Calculate. + * + * \param rah: Parameter transfer structure + * \param E: East + * \param N: North + * \param U: Up + * \return structure contain R, A, H members + */ +rah rahcal(rah rah, 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); + rah.H = atan2(U, sqrt(E * E + N * N)); + rah.A = atan2(E, U); + if (rah.A < 0) + rah.A += 2 * PI; + if (rah.A > 2 * PI) + rah.A -= 2 * PI; + rah.R = sqrt(E * E + N * N + U * U); - return rahcal; + return rah; } -/* -------------------------------------------------------------------------- */ -/// @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, +/** + * Convert BLH coordinates to ENU coordinates. + * + * \param enu: Parameter transfer structure + * \param stationB: Latitude of receiver station + * \param stationL: Longitude of receiver station + * \param deltax: Deltax X-coordinate difference of satellite arrival center + * \param deltay: Deltax Y-coordinate difference of satellite arrival center + * \param deltaz: Deltax Z-coordinate difference of satellite arrival center + * \return + */ +enu blh2enu(enu enu, double stationB, double stationL, double deltax, double deltay, double deltaz) { @@ -59,29 +63,28 @@ blh2enu BLH2ENU(blh2enu blh2enu, 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); + enu.E = -sinL * (deltax) +cosL * (deltay); + enu.N = -sinB * cosL * (deltax)-sinB * sinL * (deltay)+cosB * (deltaz); + enu.U = cosB * cosL * (deltax)+cosB * sinL * (deltay)+sinB * (deltaz); - return blh2enu; + return enu; } -/* -------------------------------------------------------------------------- */ -/// @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, +/** + * Convert ECEF geostationary coordinates to BLH coordinates. + * + * \param blh: Parameter transfer structure + * \param X: X coordinates of ECEF + * \param Y; Y coordinates of ECEF + * \param Z: Z coordinates of ECEF + * \return type of structure contain B, L, H members + */ +blh xyz2blh(blh blh, 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); + blh.L = atan2(Y, X); N = a; H = R1 - a; B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H)); @@ -92,8 +95,8 @@ xyz2blh XYZ2BLH(xyz2blh xyz2blh, H = R0 / cos(B) - N; B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H)); } while (fabs(deltaH - H) > 1.0e-3 && fabs(deltaB - B) > 1.0e-9); - xyz2blh.B = B; - xyz2blh.H = H; + blh.B = B; + blh.H = H; - return xyz2blh; + return blh; } \ No newline at end of file diff --git a/src/readrnx.cpp b/src/readrnx.cpp index a3373fd..ab51313 100644 --- a/src/readrnx.cpp +++ b/src/readrnx.cpp @@ -1,840 +1,781 @@ #include "brdm2pos.h" + +#define MAXTOE_GPS 7200.0 +#define MAXTOE_BDS 21600.0 +#define MAXTOE_GAL 14400.0 +#define MAXTOE_GLO 1800.0 +#define MAXTOE_SBS 360.0 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 -/* -------------------------------------------------------------------------- */ +/** + * Find the nearest obs epoch with boardcast. + * + * \param SecofWeek: SecofWeek second of week of obs epoch time + * \param sPRN: 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 + * \return the absolute epoch number of the best epoch + * -1 ,no match satellite + */ 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; + int best_epoch = -1; + double min;//Initialize the minimum value + double Min; + + if (syscode == GPS){//GPS + min = MAXTOE_GPS; + for (int i = 0; i < satnum; i++){ + if (sPRN == nav_b[i].sPRN_GPS && nav_b[i].sHEA == 0) { + Min = fabs(SecofWeek - nav_b[i].TOE); + if (Min <= min){ + best_epoch = i; + min = Min; + } + } + }return best_epoch; + } + else if (syscode == BDS){//BeiDou + min = MAXTOE_BDS; + 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; + } + else if (syscode == GAL){//Galileo + min = MAXTOE_GAL; + 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; + } + else if (syscode == GLO){//GLONASS + min = MAXTOE_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]) +/** + * Match the observation type(char*) with the code type(int). + * + * \param Lxcodeindex: Frequency X observation type(char*) + * \param code_original: The observation will matched + * \param freq: Observation frequency + * \return code type + */ +static int findtype(const char Lxcodeindex[60], char code_original[4], int freq) { - int code_return = 0; - char code_contrast[4]; - char code_original[4]; + int code_return = 0; + char code_contrast[4] = "/0"; - 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; + for (int j = 0; j < (strlen(Lxcodeindex) / 3); j++) { + strncpy(code_contrast, Lxcodeindex + 0 + j * 3, 3); + if (strcmp(code_contrast, code_original) == 0) { + code_return = j + freq; + } + }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]) +/** + * Converts the observation type to the numeric code defined. + * + * \param i: Reads the number of incoming loops in the loop + * \param buff: Type of incoming observation + * \return + */ +int type2code(int i, char buff[MAXRINEX]) { - int pos; - for (pos = 0; pos < typenum; pos++) { - if ((typearr[pos] - typearr[pos] % 100) == typecode) { - return pos; - } - }return pos; + int code_return = 0; + char code_original[4] = "/0"; + + 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"; + + strncpy(code_original, buff + 7 + 4 * i, 3); + + if (strstr(code_original, "1")) return findtype(L1codeindex, code_original, C1); + else if (strstr(code_original, "2")) return findtype(L2codeindex, code_original, C2); + else if (strstr(code_original, "3")) return findtype(L3codeindex, code_original, C3); + else if (strstr(code_original, "4")) return findtype(L4codeindex, code_original, C4); + else if (strstr(code_original, "5")) return findtype(L5codeindex, code_original, C5); + else if (strstr(code_original, "6")) return findtype(L6codeindex, code_original, C6); + else if (strstr(code_original, "7")) return findtype(L7codeindex, code_original, C7); + else if (strstr(code_original, "8")) return findtype(L8codeindex, code_original, C8); + else return 0; } -/* -------------------------- 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 -/* -------------------------------------------------------------------------- */ +/** + * Match the obs code and convert into type. + * + * \param type: Obs type that you want to match + * \param typenum: The total number of obs types + * \param typearr: Type codes store arrays + * \return the array position of the corresponding obs type code + */ +int code2type(int type, int typenum, int typearr[36]) +{ + int pos; + for (pos = 0; pos < typenum; pos++) { + if ((typearr[pos] - typearr[pos] % 100) == type) { + return pos; + } + }return pos; +} +/** + * Broad form of function code2type. + * + * \param exclude: The excluded type + * \param typenum: The total number of obs types + * \param typearr: Type codes store arrays + * \return the array position of the corresponding obs type code + */ +int code2type_broad(int exclude, int freq, int typenum, int typearr[36]) +{ + int pos; + for (pos = 0; pos < typenum; pos++) { + if (typearr[pos] == 0 ? (0) : ((typearr[pos] - typearr[pos] % 100) == exclude ? 0 : 1)) { + + return pos; + } + }return pos; +} +/*Read the file of rnx*/ +/** + * 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 + * + * @Author: T. TAKASU + * + * 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 + */ 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; + double value = 0.0;char str[256] = { 0 };char* p = str; + 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; } /*Free the memory of observation structure*/ void freeobs_e(pobs_epoch obs_e, pobs_body obs_b) { - free(obs_e); free(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 -/* -------------------------------------------------------------------------- */ +/** + * Get the total amount of satellite boardcast data of nav file . + * + * \param fp_nav: The file pointer of nav file + * \return amount of satellite + */ 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; + 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; - } + 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); + 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; - } - } + 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 -/* -------------------------------------------------------------------------- */ +/** + * Get the total amount of epoch of obs. + * + * \param fp_obs: The file pointer of obs + * \return amount of epoch + */ 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; + 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; + 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; + 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] = 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; - } - } + 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); + for (i = 0; i < obs_h->obstypenum_gps;) { + for (int j = 0; j < 13; i++, j++) { + if (i == obs_h->obstypenum_gps) break; + else obs_h->obscode_gps[i] = type2code(i, buff); + }i == obs_h->obstypenum_gps? 0 : fgets(buff, MAXRINEX, fp_obs); + }continue; + } + else if (flag == 'C') { + obs_h->obstypenum_bds = (int)strtonum(buff, 4, 2); + for (i = 0; i < obs_h->obstypenum_bds;) { + for (int j = 0; j < 13; i++, j++) { + if (i == obs_h->obstypenum_bds) break; + else obs_h->obscode_bds[i] = type2code(i, buff); + }i == obs_h->obstypenum_bds ? 0 : fgets(buff, MAXRINEX, fp_obs); + }continue; + } + else if (flag == 'E') { + obs_h->obstypenum_gal = (int)strtonum(buff, 4, 2); + for (i = 0; i < obs_h->obstypenum_gal;) { + for (int j = 0; j < 13; i++, j++) { + if (i == obs_h->obstypenum_gal) break; + else obs_h->obscode_gal[i] = type2code(i, buff); + }i == obs_h->obstypenum_gal ? 0 : fgets(buff, MAXRINEX, fp_obs); + }continue; + } + else if (flag == 'R') { + obs_h->obstypenum_glo = (int)strtonum(buff, 4, 2); + for (i = 0; i < obs_h->obstypenum_glo;) { + for (int j = 0; j < 13; i++, j++) { + if (i == obs_h->obstypenum_glo) break; + else obs_h->obscode_glo[i] = type2code(i, buff); + }i == obs_h->obstypenum_glo ? 0 : fgets(buff, MAXRINEX, fp_obs); + }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) { - 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; + 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->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); + 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)); + 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; - } + 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 index 517eb91..7874827 100644 --- a/src/rnx2pos.cpp +++ b/src/rnx2pos.cpp @@ -3,411 +3,394 @@ 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)); + 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]; + 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]; + 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; + 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) + 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; + 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[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 + 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 + 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); - } + 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 + 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]; + 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; + 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; + 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) +pos_ts sat_pos(int sPRN, int best_epoch, double Weeksec, int GNSS, + 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; + 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 (tk > 32400)tk -= 604800; - else if (tk < -32400)tk += 604800; + if (GNSS == BDS) tk = T_S - nav_b[best_epoch].TOE - 14;//BDST = GPST + 14 + else tk = T_S - nav_b[best_epoch].TOE; - double Ms = nav_b[best_epoch].M0 + n * tk; + if (tk > 32400)tk -= 604800; + else if (tk < -32400)tk += 604800; - double Es = Ms, E0; - do - { - E0 = Es; - Es = Ms + nav_b[best_epoch].e * sin(Es); - } while (fabs(Es - E0) > 1.0e-10); + double Ms = nav_b[best_epoch].M0 + n * tk; - 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 Es = Ms, E0; + do + { + E0 = Es; + Es = Ms + nav_b[best_epoch].e * sin(Es); + } while (fabs(Es - E0) > 1.0e-10); - 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 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 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 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 x = r * cos(u); - double y = r * sin(u); + 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 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 x = r * cos(u); + double y = r * sin(u); - double f = deg2rad(-5); - double p = Earth_e * tk; + double l, X, Y, Z; + if (GNSS == BDS && (sPRN <= 5 || sPRN >= 59)) {//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); - 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 f = deg2rad(-5); + double p = Earth_e * tk; - 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; + 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 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; + double TGD=0.0; + if (GNSS == GPS) TGD = nav_b[best_epoch].TGD; + else if (GNSS == GAL) TGD = nav_b[best_epoch].BGDa; + else if (GNSS == BDS) TGD = nav_b[best_epoch].TGD1; - return pos_t; + //Recalculate the diatance of satellite-receiver + 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)); + //Get relativistic effect correction + double rela = 2 * sqrt(GM) * nav_b[best_epoch].e * nav_b[best_epoch].sqrtA * sin(Es) / pow(C_V, 2); + + 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 -/* -------------------------------------------------------------------------- */ +/** + * . + * + * \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 GNSS: GNSS system code + * \param satnum: The total number of satellite of designated satellite system + * \return + */ 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) + pnav_body nav_b, + FILE* res_file, + int GNSS, int satnum, int Hangle) { - //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); + //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); - blhs blh = { 0 }; blh2enu enu = { 0 }; - rahcal rah = { 0 }; pos_ts pos_t = { 0 }; - xyz2blh tem1 = { 0 }; blh2enu tem2 = { 0 }; rahcal tem3 = { 0 }; + //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); - 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; + rah in_rah = { 0 }; rah tem_rah = { 0 }; + enu in_enu = { 0 }; enu tem_enu = { 0 }; + blh in_blh = { 0 }; blh tem_blh = { 0 }; + pos_ts pos_t = { 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; + 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, PERS = 0; + double detat_toc; - 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; - } + //Match the system and assign the satellite number + if (GNSS == GPS)sat_num = obs_e->gps_num; + else if (GNSS == GAL)sat_num = obs_e->gal_num; + else if (GNSS == BDS)sat_num = obs_e->bds_num; + else if (GNSS == GLO)sat_num = obs_e->glo_num; - 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; + for (int j = 0; j < sat_num; j++) { - //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; + //Match system and assign the sPRN + if (GNSS == GPS) { + sPRN = obs_e->sPRN_GPS[j]; + PERS = obs_b->obs_gps[j][code2type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)] / C_V; + } + else if (GNSS == GAL) { + sPRN = obs_e->sPRN_GAL[j]; + PERS = obs_b->obs_gal[j][code2type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)] / C_V; + } + else if (GNSS == BDS) { + sPRN = obs_e->sPRN_BDS[j]; + PERS = obs_b->obs_bds[j][code2type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)] / C_V; + } + else if (GNSS == GLO) { + sPRN = obs_e->sPRN_GLO[j]; + PERS = obs_b->obs_glo[j][code2type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)] / C_V; + } - 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; + //Match the nearest satellite of nav with this epoch + int best_epoch = select_epoch(Weeksec, sPRN, nav_b, satnum, GNSS); - double deltax = pos_t.X - obs_h->apX; - double deltay = pos_t.Y - obs_h->apY; - double deltaz = pos_t.Z - obs_h->apZ; + if (best_epoch == -1)//when the sPRN is not exist function will return -1 + break; - 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; + if (GNSS == BDS) detat_toc = Weeksec - nav_b[best_epoch].TOE - 14;//BDST = GPST + 14s + + else detat_toc = Weeksec - nav_b[best_epoch].TOE; + + //Iterative approximate propagation time + if (GNSS == GLO) { + + //Get the approximately transit of signal + pos_t.delta_t = PERS - 0 + + nav_b[best_epoch].sa0 + + nav_b[best_epoch].sa1 * detat_toc; + + pos_t.deltat = 0.0; + //Satellite position calculation + 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); + } + } + //Exclude GLONASS + else { + + //Get the approximately transit of signal + pos_t.delta_t = PERS - 0 + + 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; + //Satellite position calculation + while (fabs(pos_t.delta_t - pos_t.deltat) > 1.0e-9) { + pos_t = sat_pos(sPRN, best_epoch, Weeksec, GNSS, nav_b, obs_h, pos_t); + } + } + //Satellite blh position calculation + tem_blh = xyz2blh(tem_blh, pos_t.X, pos_t.Y, pos_t.Z); + in_blh.B = rad2deg(tem_blh.B); + in_blh.L = rad2deg(tem_blh.L); + in_blh.H = tem_blh.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; + + tem_enu = blh2enu(tem_enu, obs_h->apB, obs_h->apL, deltax, deltay, deltaz); + in_enu.E = tem_enu.E; + in_enu.N = tem_enu.N; + in_enu.U = tem_enu.U; + + //Satellite elevation Angle calculation + tem_rah = rahcal(tem_rah, in_enu.E, in_enu.N, in_enu.U); + in_rah.R = tem_rah.R; + in_rah.A = rad2deg(tem_rah.A); + in_rah.H = rad2deg(tem_rah.H); + if (in_blh.H < 0 || in_rah.H < Hangle || nav_b[best_epoch].sHEA != 0) { + continue; + } + else outputbodydata(obs_h, obs_b, pos_t, in_rah, res_file, sPRN, j, GNSS); + }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) +/** + * The entrance of function. + * + * \param nav_path: The RINEX navigation file's path + * \param obs_path: The RINEX observation file's path + * \param res_path: The satellite position solution output path + * \param GNSS: GNSS code + * \return + * -1 ,Can not open the nav file + * -2 ,Can not open the obs file + * -3 ,Mismatch version or format of nav + * -4 ,No data of nav + * -5 ,Mismatch version or format of obs + * -6 ,No data of obs + * 0 ,Soulution generated completely + * >0 ,Some epoch cant be calculate + */ +double brdm2pos(char nav_path[MAXPATH], char obs_path[MAXPATH], char res_path[MAXPATH], + int GNSS, int Hangle) { - int satnum, epochunum, result = 0; - pos_ts pos_t = { 0 }; - xyz2blh tem2 = { 0 }; - stations station = { 0 }; - time_t gen_time; time(&gen_time); + int satnum, epochnum, result = 0; + pos_ts pos_t = { 0 }; + blh apblh = { 0 }; + + 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; - 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;//cant read nav file - FILE* fp_nav = fopen(nav_path, "r"); - if (fp_nav == NULL) return -1; - satnum = getsatnum(fp_nav); + //Get the amount of satellite + satnum = getsatnum(fp_nav); - switch (satnum) { - case -1: return -3;//mismatch version or format - case 0: return -4;//no data - default: - break; - } + 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); + rewind(fp_nav); + nav_h = (pnav_head)malloc(sizeof(nav_head)); + nav_b = (pnav_body)malloc(sizeof(nav_body) * (satnum)); - FILE* fp_obs = fopen(obs_path, "r"); - if (fp_obs == NULL) return -2; - epochunum = get_epochnum(fp_obs); + if (nav_h && nav_b) { read_n_h(fp_nav, nav_h); read_n_b(fp_nav, nav_b); } + + fclose(fp_nav); - switch (epochunum) { - case -1: return -5;//mismatch version or format - case 0: return -6;//no data - default: - break; - } + FILE* fp_obs = fopen(obs_path, "r"); + if (fp_obs == NULL) return -2;//cant read obs file - rewind(fp_obs); - obs_h = (pobs_head)malloc(sizeof(obs_head)); - if (obs_h) { - read_o_h(fp_obs, obs_h); + //Get the amonut of obs epoch + epochnum = get_epochnum(fp_obs); - tem2 = XYZ2BLH(tem2, obs_h->apX, obs_h->apY, obs_h->apZ); - station.B = tem2.B; station.L = tem2.L; station.H = tem2.H; + switch (epochnum) { + 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)); - //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"); + if (obs_h) { - //Begin to calculate - for (int i = 0; i < epochunum; i++) { - //Allocate memory for obs epoch and body data - obs_e = (pobs_epoch)malloc(sizeof(obs_epoch)); - obs_b = (pobs_body)malloc(sizeof(obs_body)); + //Read the header of obs + read_o_h(fp_obs, obs_h); - 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); + //Convert the approximately position (ECEF > LLA) + apblh = xyz2blh(apblh, obs_h->apX, obs_h->apY, obs_h->apZ); + obs_h->apB = apblh.B; + obs_h->apL = apblh.L; + obs_h->apH = apblh.H; - 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; - } + //Output header of solution + FILE* result_file = outputheader(obs_h, obs_path, nav_path, res_path); + + //Begin to calculate + for (int i = 0; i < epochnum; i++) { + + //Allocate memory for obs epoch and body data + obs_e = (pobs_epoch)malloc(sizeof(obs_epoch)); + obs_b = (pobs_body)malloc(sizeof(obs_body)); + + //Read epoch data + read_o_eb(fp_obs, obs_h, obs_e, obs_b); + + //Calculate the position of satellite + result += sat_pos_cal(obs_h, obs_e, obs_b, nav_b, + result_file, + GNSS, satnum, Hangle); + + + fprintf(result_file, "\n"); + + //Free epoch data + freeobs_e(obs_e, obs_b); + } + //Output the signal of ending + fprintf(result_file, "\nEND"); fclose(result_file); + + //free nav & obs memory of data + free(obs_h); free(nav_h); free(nav_b); + + if (epochnum == result) return 0; + else return epochnum - result; + } } \ No newline at end of file diff --git a/src/solutionout.cpp b/src/solutionout.cpp new file mode 100644 index 0000000..ddb4179 --- /dev/null +++ b/src/solutionout.cpp @@ -0,0 +1,103 @@ +#include "brdm2pos.h" +using namespace std; + +/*Output the header data of satllite position solution*/ +FILE* outputheader(pobs_head obs_h, + char obs_path[MAXPATH], char nav_path[MAXPATH], char res_path[MAXPATH]) +{ + time_t gen_time; + time(&gen_time); + + 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.4.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 + , obs_h->apB, obs_h->apL, obs_h->apH); + fprintf(result_file, "END OF HEADER\n"); + + return result_file; +} +/*Ion correction by IF-IC*/ +static double ionofree(pobs_body obs_b, pobs_head obs_h, int j, int GNSS) +{ + double P1=0.0, P2=0.0, freq1=0.0, freq2=0.0; + //Match the code with system + switch (GNSS) { + case GPS: + P1 = obs_b->obs_gps[j][code2type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)]; + P2 = obs_b->obs_gps[j][code2type(C2, obs_h->obstypenum_gps, obs_h->obscode_gps)]; + freq1 = FREQ1; freq2 = FREQ2; + break; + case BDS: + P1 = obs_b->obs_bds[j][code2type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)]; + P2 = obs_b->obs_bds[j][code2type(C7, obs_h->obstypenum_bds, obs_h->obscode_bds)]; + freq1 = FREQ1_BDS; freq2 = FREQ2_BDS; + break; + case GAL: + P1 = obs_b->obs_gal[j][code2type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)]; + P2 = obs_b->obs_gal[j][code2type(C5, obs_h->obstypenum_gal, obs_h->obscode_gal)]; + freq1 = FREQ1; freq2 = FREQ6; + break; + case GLO: + P1 = obs_b->obs_glo[j][code2type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)]; + P2 = obs_b->obs_glo[j][code2type(C2, obs_h->obstypenum_glo, obs_h->obscode_glo)]; + freq1 = FREQ1_GLO; freq2 = FREQ2_GLO; + break; + } + //IF-IC + if (P1 != 0.0 && P2 != 0.0) { + P1 = (pow(freq1, 2) * P1) / (pow(freq1, 2) - pow(freq2, 2)) + - (pow(freq2, 2) * P2) / (pow(freq1, 2) - pow(freq2, 2)); + + return P1; + } + if (P1 == 0.0 || P2 == 0.0) { + if (P1 == 0.0 && P2 != 0.0) return P2; + if (P1 != 0.0 && P2 == 0.0) return P1; + return 0.0; + } +} +/*Output the satellite solution data*/ +void outputbodydata(pobs_head obs_h, pobs_body obs_b, + pos_ts pos_t, rah in_rah, FILE* res_file, + int sPRN, int j, int GNSS) +{ + double P = ionofree(obs_b, obs_h, j, GNSS); + + if (P == 0) return; + + switch (GNSS) { + case GPS: + fprintf(res_file, "\nG %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f" + , sPRN, pos_t.X, pos_t.Y, pos_t.Z + , P, in_rah.H, pos_t.delta_clk); + break; + case BDS: + if (sPRN <= 5 || sPRN >= 59) break; + else; + fprintf(res_file, "\nC %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f" + , sPRN, pos_t.X, pos_t.Y, pos_t.Z + , P, in_rah.H, pos_t.delta_clk); + break; + case GAL: + fprintf(res_file, "\nE %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f" + , sPRN, pos_t.X, pos_t.Y, pos_t.Z + , P, in_rah.H, pos_t.delta_clk); + break; + case GLO: + fprintf(res_file, "\nR %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f" + , sPRN, pos_t.X, pos_t.Y, pos_t.Z + , P, in_rah.H, pos_t.delta_clk); + break; + } +} \ No newline at end of file diff --git a/src/timesys.cpp b/src/timesys.cpp index b65d2a2..4f14069 100644 --- a/src/timesys.cpp +++ b/src/timesys.cpp @@ -1,68 +1,86 @@ #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 -/* -------------------------------------------------------------------------- */ +/** + * Convert Time format to GPS second of week. + * + * \param y: Year of + * \param m: Month of + * \param d: Day of + * \param h: Hour of + * \param min: Minute of + * \param sec: Second of + * \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; + m = m; + } - if (m <= 2) - { - y = y - 1; - m = m + 12; - } + 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; + 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; } +/** + * Convert UTC(SU) time to GPS second of week. + * + * \param JD_UTCSU: JD of UTC(SU) time + * \return GPS: Second of week + */ double JDUTC2GPST(double JD_UTCSU) { - double n;//Leap second + 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; } + 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; + 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; } +/** + * Convert UTC time to JD format. + * + * \param y: Year of UTC time + * \param m: Month of UTC time + * \param d: Day of UTC time + * \param h: Hour of UTC time + * \param min: Minute of UTC time + * \param sec: Second of UTC time + * \return JD: + */ 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; + m = m; + } - if (m <= 2) - { - y = y - 1; - m = m + 12; - } + 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; + 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