KenanZhu/KNZ_GeoTrackLab
Archived
1
0 Fork 0
Code Issues Actions Packages Projects Releases Wiki Activity

Update: 2024/10/28

Browse Source 
1.  KNZ_GeoTrackLab: Fix some bug.
2.  KNZ_GeoTrackLab: Optimize code structure.
This commit is contained in:
KenanZhu111 committed 2024-10-28 14:37:23 +08:00
1 parent ccd8d27d1c
commit f895b31a8e
12 files changed
+1579 -1537
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

README.md
+4
View File
@@ -36,6 +36,10 @@ Features ：
## Project Update
Update: 2024/10/28
1. KNZ_GeoTrackLab: Fix some bug.
2. KNZ_GeoTrackLab: Optimize code structure.
Rename the Project & Update: 2024/10/26
1. First release KNZ_GeoTrackLab.
2. KNZ_Calculate: Merged with KNZ_Plot.
READMECN.md
+8 -5
View File
@@ -36,12 +36,15 @@
## 项目更新
重命名项目并更新：2024/10/26
更新：2024/10/28
1. KNZ_GeoTrackLab：修复一些bug。
2. KNZ_GeoTrackLab：优化代码结构。
首次发布 KNZ_GeoTrackLab。
1. KNZ_Calculate：与 KNZ_Plot 合并。
2. KNZ_Plot：与 KNZ_Calculate 合并。
3. KNZ_GeoTrackLab：支持 GPS、北斗、伽利略和 GLONASS。
重命名项目并更新：2024/10/26
1. 首次发布 KNZ_GeoTrackLab。
2. KNZ_Calculate：与 KNZ_Plot 合并。
3. KNZ_Plot：与 KNZ_Calculate 合并。
4. KNZ_GeoTrackLab：支持 GPS、北斗、伽利略和 GLONASS。
更新：2024/10/14
1. KNZ_Calculate：支持 GPS、BDS 和伽利略。
app/Mainui.py
+47 -19
View File
@@ -22,8 +22,6 @@ import matplotlib.pyplot as plt
from matplotlib.backends.backend_tkagg import (
FigureCanvasTkAgg, NavigationToolbar2Tk)
dll = cdll.LoadLibrary('./brdm2pos.dll')
dll.Start.restype = c_double
#
#
#
@@ -46,8 +44,13 @@ satsysdict = {1: "G", 4:"R", 2:"C", 3:"E", 5:"S"}
satellite_system = 1
##Combox Count-er
ComboxObsCount = 0
Obsvalue = ('',)
ComboxNavCount = 0
Navvalue = ('',)
ComboxOutCount = 0
Outvalue = ('',)
#
version = 'ver 1.5.3.1'
#
#
#
@@ -68,7 +71,7 @@ def Abo_windows():
abo.minsize(200, 100)
abo.maxsize(200, 100)
About = tk.Label(abo, text="\nKNZ_GeoTrackLab ver1.5.1\n\nCopyright (c) 2024 by KenanZhu\nAll Right Reserved.")
About = tk.Label(abo, text="\nKNZ_GeoTrackLab %s\n\nCopyright (c) 2024 by KenanZhu\nAll Right Reserved."%version)
About.pack(side=tk.TOP, expand=tk.YES)
class FUNCTION:
@@ -204,10 +207,12 @@ class FUNCTION:
@staticmethod
def GetObsFilePath(ObsFileSelectBox):
global ComboxObsCount
global Obsvalue
path = filedialog.askopenfilename(title='RINEX OBS File',
filetypes=[('RINEX OBS File(*.o*.*.*obs.*.*d)', '*.*o;*.*obs;*.*d'),
('All Files', '*.*')])
if path and path not in ObsFileSelectBox['value']:
Obsvalue += (path,)
ObsFileSelectBox['value'] += (path,)
ComboxObsCount += 1
ObsFileSelectBox.current(ComboxObsCount)
@@ -215,11 +220,13 @@ class FUNCTION:
@staticmethod
def GetNavFilePath(NavFileSelectBox):
global ComboxNavCount
global Navvalue
path = filedialog.askopenfilename(title='RINEX NAV File',
filetypes=[('RINEX NAV File(*.*nav.*.hnav.*.gnav.*.qnav.*.*n.*.*g.*.*h.*.*q.*.*p)',
'*.*nav;*.hnav;*.gnav;*.qnav;*.*n;*.*g;*.*h;*.*q;*.*p'),
('All Files', '*.*')])
if path and path not in NavFileSelectBox['value']:
Navvalue += (path,)
NavFileSelectBox['value'] += (path,)
ComboxNavCount += 1
NavFileSelectBox.current(ComboxNavCount)
@@ -227,8 +234,10 @@ class FUNCTION:
@staticmethod
def AskDirectory(AskDirectorySelectBox):
global ComboxOutCount
global Outvalue
path = filedialog.askdirectory()
if path and path not in AskDirectorySelectBox['value']:
Outvalue += (path,)
AskDirectorySelectBox['value'] += (path + "/*.sp",)
ComboxOutCount += 1
AskDirectorySelectBox.current(ComboxOutCount)
@@ -293,8 +302,28 @@ class FUNCTION:
# -----------------------------------------------#
ExecuteState.config(text='Processing...... file : %s' % obs_path[obspath:])
dll.Start(nav_path.encode(), obs_path.encode(), res_path.encode(), satellite_system)
ExecuteState.config(text='Complete generate file : %s!' % obsfile + ".sp")
dll = cdll.LoadLibrary('./brdm2pos.dll')
dll.brdm2pos.restype = c_double
match dll.brdm2pos(nav_path.encode(), obs_path.encode(), res_path.encode(), satellite_system):
case 0.0:
ExecuteState.config(text='Generated successfully ! file : %s' % obsfile + ".sp")
case -1.0:
ExecuteState.config(text='Error ! cant open nav file')
case -2.0:
ExecuteState.config(text='Error ! cant open obs file')
case -3.0:
ExecuteState.config(text='Error ! unsupported nav file')
case -4.0:
ExecuteState.config(text='Error ! no nav data')
case -5.0:
ExecuteState.config(text='Error ! unsupported obs file')
case -6.0:
ExecuteState.config(text='Error ! no obs data')
windll.kernel32.FreeLibrary('./brdm2pos.dll')
def _ExecuteFile(self,ExecuteState,
ObsSelectBoxVar,
@@ -347,7 +376,7 @@ class MAINGUI:
hwnd.mainloop()
def initGUI(self, hwnd):
hwnd.title("KNZ_GeoTrackLab ver1.5.1")
hwnd.title("KNZ_GeoTrackLab %s"%version)
hwnd.geometry('1200x700')
hwnd.minsize(1200, 700)
@@ -598,7 +627,6 @@ class MAINGUI:
def SPP_calculat(self, hwnd):
# Brief # read .sp file & draw in the canvas & output the .po file
# Return# none
# Const define
self.sattrack.clear()
C_V = 299792458
@@ -666,13 +694,14 @@ class MAINGUI:
with open(o_path, 'w+') as of: # Initialize-------#
of.close()
of = open(o_path, 'a+')
print("@ GENERATE PROGRAM : KNZ_GeoTrackLab ver1.5.1\n"
print("@ GENERATE PROGRAM : KNZ_GeoTrackLab %s\n"
"@ GENERATE TYPE : Receiver Station Position\n"
"@ GENERATE TIME : %s\n"
"@ GENERATE SYS : %s\n"
"@ IONOS OPT : %s\n"
"@ TROPO OPT : %s\n"
"@\n\n" % (time.asctime(time.localtime())
"@\n\n" % ( version
, time.asctime(time.localtime())
, satsysdict.get(satellite_system)
, ioncormode[ioncorstate]
, trocormode[trocorstate]), file=of)
@@ -718,23 +747,19 @@ class MAINGUI:
epoch = int(line[2:6])
count = 0
# Check the lost epoch
while count < epoch - epoch_last - 1:
ENU_O = np.hstack((ENU_O, [[0], [0], [0]]))
satnumnarry = np.hstack((satnumnarry, [[0]]))
print("------------ MISSING EPOCH DATA", file=of)
count += 1
epoch_last = epoch
# Calculate start
while 1:
line = f.readline()
if line == "\n":
# The sampling rate of 3D draw
if epoch % 15 == 0:
self.sattrack.scatter(satX[0,:], satY[0,:], satZ[0,:], s=5, zdir='z', c=SatColor[6] )
# Get the viald satellite number
satnumnarry = np.hstack((satnumnarry, [[satnum]]))
match satsysdict.get(satellite_system):
@@ -765,7 +790,6 @@ class MAINGUI:
B = np.empty((0, 4))
L = np.empty((0, 1))
while 1:
# Linear the observation equation
R = math.sqrt(
pow((satX[0, count] - ap_X), 2) + pow((satY[0, count] - ap_Y), 2) + pow(
(satZ[0, count] - ap_Z), 2))
@@ -804,7 +828,7 @@ class MAINGUI:
Pl = C1[0, count]
case 2:
pass
# Omc matrix
L = np.vstack(
( L, [Pl - R + C_V * Dt[0, count] - Tro] )
)
@@ -813,7 +837,6 @@ class MAINGUI:
if count == satnum:
break
# Construct the Least Squares
P = np.diag(Ptem)
BTP = np.dot(np.transpose(B), P)
BTPB = np.dot(BTP, B)
@@ -1271,8 +1294,10 @@ class EXECUDUI:
# -----------------------------------------------------------------------------------
ObsInFrame3 = ttk.LabelFrame(TopFrame1, text='RINEX Obs File')
ObsInFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='2px', fill=tk.X)
ObsFileSelectBox = ttk.Combobox(ObsInFrame3, width=46, height=4, values=('',),
ObsFileSelectBox = ttk.Combobox(ObsInFrame3, width=46, height=4, values=Obsvalue,
textvariable=self.ObsSelectBoxVar)
ObsFileSelectBox.current(ComboxObsCount)
ObsFileSelectBox.pack(side=tk.LEFT, anchor='w', padx='1px', pady='1px')
ObsFileSelectButton = ttk.Button(ObsInFrame3, text='...', width=3,
@@ -1283,8 +1308,10 @@ class EXECUDUI:
#-----------------------------------------------------------------------------------
NavInFrame3 = ttk.LabelFrame(TopFrame1, text='RINEX Nav File')
NavInFrame3.pack(side=tk.TOP, expand=tk.YES, padx='1px', pady='2px', fill=tk.X)
NavFileSelectBox = ttk.Combobox(NavInFrame3, width=46, height=4, values=('',),
NavFileSelectBox = ttk.Combobox(NavInFrame3, width=46, height=4, values=Navvalue,
textvariable=self.NavSelectBoxVar)
NavFileSelectBox.current(ComboxNavCount)
NavFileSelectBox.pack(side=tk.LEFT, anchor='w', padx='1px', pady='1px')
NavFileSelectButton = ttk.Button(NavInFrame3, text='...', width=3,
@@ -1308,8 +1335,9 @@ class EXECUDUI:
AskOrNotCheckButton.pack(side=tk.LEFT, anchor='w', padx='1px', pady='0px')
## SelectBox Initialize
AskDirectorySelectBox = ttk.Combobox(FileOutFrame3, width=36, height=4, values=('',),
AskDirectorySelectBox = ttk.Combobox(FileOutFrame3, width=36, height=4, values=Outvalue,
state=tk.DISABLED, textvariable=self.DirectorySelectBoxVar)
AskDirectorySelectBox.current(ComboxOutCount)
AskDirectorySelectBox.pack(side=tk.LEFT, anchor='w', padx='1px', pady='0px')
## SelectButton Initialize
doc/KNZ_GeoTrackLab ver 1.5.1.docx
BIN
View File
Binary file not shown.
src/pch.h → src/brdm2pos.h
+95 -88
View File
@@ -1,37 +1,33 @@
// pch.h: 这是预编译标头文件。
// 下方列出的文件仅编译一次，提高了将来生成的生成性能。
// 这还将影响 IntelliSense 性能，包括代码完成和许多代码浏览功能。
// 但是，如果此处列出的文件中的任何一个在生成之间有更新，它们全部都将被重新编译。
// 请勿在此处添加要频繁更新的文件，这将使得性能优势无效。
#pragma once
#ifndef PCH_H
#define PCH_H
#ifndef _BRDM2POS_H_
#define _BRDM2POS_H_
// 添加要在此处预编译的标头
#include "framework.h"
#include <iostream>
#include <math.h>
#include <time.h>
#endif
/* ------------------------------ CALCU CONST ------------------------------- */
#endif // _BRDM2POS_H_
/* CALCU CONST*/
#define C_V 299792458
#define GM 398600500000000
#define math_e 2.718281828459
#define PI 3.141592653589793
#define Earth_e 7.2921151467e-5
#define C20 1.0826257e-3
/* ------------------------------ WGS-84 CONST ------------------------------ */
#define C20 1.0826257e-3
/*WGS-84 CONST*/
#define a 6378137.0
#define e2 (0.0033528106647475*(2 - 0.0033528106647475))
/* ------------------------------- PZ-90 CONST ------------------------------ */
//
/*PZ-90 CONST*/
#define TSTEP 30.0
/* ----------------------------- GNSS OBSERV TYPE --------------------------- */
/*GNSS OBSERV TYPE*/
//frequency 1
#define C1 100
#define C1 100
#define C1A 100
#define C1B 101
#define C1C 102
#define C1D 103
#define C1D 103
#define C1E
#define C1F
#define C1G
@@ -39,24 +35,23 @@
#define C1I
#define C1J
#define C1K
#define C1L 104
#define C1M 105
#define C1L 104
#define C1M 105
#define C1N
#define C1O
#define C1P 106
#define C1P 106
#define C1Q
#define C1R
#define C1S 107
#define C1S 107
#define C1T
#define C1U
#define C1V
#define C1W
#define C1X 108
#define C1Y 109
#define C1Z 110
#define C1X 108
#define C1Y 109
#define C1Z 110
//frequency 2
#define C2 200
#define C2 200
#define C2A
#define C2B
#define C2C 200
@@ -65,7 +60,7 @@
#define C2F
#define C2G
#define C2H
#define C2I 202
#define C2I 202
#define C2J
#define C2K
#define C2L 203
@@ -80,24 +75,21 @@
#define C2U
#define C2V
#define C2W
#define C2X 208
#define C2X 208
#define C2Y 209
#define C2Z 210
//frequency 3
#define C3 300
#define C3I 300
#define C3Q 301
#define C3X 302
#define C3I 300
#define C3Q 301
#define C3X 302
//frequency 4
#define C4 400
#define C4A 400
#define C4B 401
#define C4X 402
#define C4 400
#define C4A 400
#define C4B 401
#define C4X 402
//frequency 5
#define C5 500
#define C5 500
#define C5A
#define C5B
#define C5C
@@ -124,12 +116,11 @@
#define C5X 502
#define C5Y
#define C5Z
//frequency 6
#define C6 600
#define C6A 600
#define C6B 601
#define C6C 602
#define C6 600
#define C6A 600
#define C6B 601
#define C6C 602
#define C6D
#define C6E
#define C6F
@@ -150,12 +141,11 @@
#define C6U
#define C6V
#define C6W
#define C6X 603
#define C6X 603
#define C6Y
#define C6Z 604
#define C6Z 604
//frequency 7
#define C7 700
#define C7 700
#define C7A
#define C7B
#define C7C
@@ -164,7 +154,7 @@
#define C7F
#define C7G
#define C7H
#define C7I 700
#define C7I 700
#define C7J
#define C7K
#define C7L
@@ -172,34 +162,30 @@
#define C7N
#define C7O
#define C7P
#define C7Q 701
#define C7Q 701
#define C7R
#define C7S
#define C7T
#define C7U
#define C7V
#define C7W
#define C7X 702
#define C7X 702
#define C7Y
#define C7Z
//frequency 8
#define C8 800
#define C8I 800
#define C8Q 801
#define C8X 802
#define C8 800
#define C8I 800
#define C8Q 801
#define C8X 802
/*GNSS CODE*/
#define GPS 01
#define BDS 02
#define GAL 03
#define GLO 04
#define SBAS 05
/* ------------------------------- GNSS CODE -------------------------------- */
#define GPS 01
#define BDS 02
#define GAL 03
#define GLO 04
#define SBAS 05
/* ------------------------------ FOR CALMOD.H ------------------------------ */
#define MAXRINEX 1000
#define DAYSEC 86400
//satellite position & clock bias structure
typedef struct
{
@@ -248,14 +234,13 @@ typedef struct
double N;
double U;
}blh2enu;
//longitude & latitude structure
typedef struct
{
double B;
double L;
double H;
}xyz2blh;
/* ------------------------------- FOR READ.H ------------------------------- */
//header data of file of nav
typedef struct nav_head
{
@@ -290,22 +275,22 @@ typedef struct nav_body
int TOC_Sec;
double sa0;
double sa1;
double sa2; double Dos;
double sa2; double Dos;
double IODE; double AODE; double SatX;
double Crs; double SatXv;
double deltan; double SatXa;
double IODE; double AODE; double SatX;
double Crs; double SatXv;
double deltan; double SatXa;
double M0;
double Cuc; double SatY;
double e; double SatYv;
double Cus; double SatYa;
double sqrtA; int FreN;
double Cuc; double SatY;
double e; double SatYv;
double Cus; double SatYa;
double sqrtA; int FreN;
double TOE; double SatZ;
double Cic; double SatZv;
double OMEGA; double SatZa;
double Cis; int AOO;
double TOE; double SatZ;
double Cic; double SatZv;
double OMEGA; double SatZa;
double Cis; int AOO;
double i0;
double Crc;
@@ -313,17 +298,17 @@ typedef struct nav_body
double deltaomega;
double IDOT;
double L2code; int Datasource;
double L2code; int Datasource;
double GPSweek; double BDTweek; double GALweek;
double L2Pflag;
double sACC;
double sHEA;
double TGD; double TGD1; double BGDa;
double TGD; double TGD1; double BGDa;
double IODC; double TGD2; double BGDb;
double TTN;
double fit; double AODC;
double fit; double AODC;
double spare1;
double spare2;
@@ -391,8 +376,30 @@ typedef struct obs_body
double obs_gal[20][26];//GALILEO obs data
double obs_glo[20][26];//GLONASS obs data
}obs_body, * pobs_body;
extern "C" _declspec(dllexport) double Start(char nav_path[260], char obs_path[260], char res_path[260], int gnsscode);
//
extern double JDUTC2GPST(double);
extern double Time2GPST(int, int, int, double, int, double);
extern double UTCTime2JD(int, int, int, double, int, double);
//
extern double deg2rad(double);
extern double rad2deg(double);
//
extern rahcal RAHCAL(rahcal, double, double, double);
extern blh2enu BLH2ENU(blh2enu, double, double, double, double, double);
extern xyz2blh XYZ2BLH(xyz2blh, double, double, double);
//
extern int getsatnum(FILE*);
extern int get_epochnum(FILE*);
extern void initobs_e(pobs_epoch, pobs_body);
extern void freeobs_e(pobs_epoch, pobs_body);
extern int Type2Code(int, char buff[MAXRINEX]);
extern int Code2Type(int, int, int typearr[36]);
extern int select_epoch(double, int, pnav_body, int, int);
//
extern void read_n_h(FILE*, pnav_head);
extern void read_n_b(FILE*, pnav_body);
extern void read_o_h(FILE*, pobs_head);
extern void read_o_eb(FILE*, pobs_head, pobs_epoch, pobs_body);
//
extern "C" _declspec(dllexport) double brdm2pos(char nav_path[260], char obs_path[260], char res_path[260], int gnsscode);
src/coordtrans.cpp
+99
View File
@@ -0,0 +1,99 @@
#include "brdm2pos.h"
using namespace std;
/* -------------------------------------------------------------------------- */
/// @brief degree to radian
/// @param deg
/// @return return radian
/* -------------------------------------------------------------------------- */
double deg2rad(double deg)
{
double rad = (PI / 180) * deg;
return rad;
}
/* -------------------------------------------------------------------------- */
/// @brief radian to degree
/// @param rad
/// @return return degree
/* -------------------------------------------------------------------------- */
double rad2deg(double rad)
{
double deg = (180 / PI) * rad;
return deg;
}
/* -------------------------------------------------------------------------- */
/// @brief satellite azimuth angle calculation
/// @param rahcal parameter transfer structure
/// @param E east
/// @param N north
/// @param U up
/// @return return structure contain R, A, H members
/* -------------------------------------------------------------------------- */
rahcal RAHCAL(rahcal rahcal,
double E, double N, double U)
{
rahcal.H = atan2(U, sqrt(E * E + N * N));
rahcal.A = atan2(E, U);
if (rahcal.A < 0)
rahcal.A += 2 * PI;
if (rahcal.A > 2 * PI)
rahcal.A -= 2 * PI;
rahcal.R = sqrt(E * E + N * N + U * U);
return rahcal;
}
/* -------------------------------------------------------------------------- */
/// @brief convert BLH coordinates to ENU coordinates
/// @param blh2enu parameter transfer structure
/// @param stationB latitude of receiver station
/// @param stationL longitude of receiver station
/// @param deltax X-coordinate difference of satellite arrival center
/// @param deltay Y-coordinate difference of satellite arrival center
/// @param deltaz Z-coordinate difference of satellite arrival center
/// @return return type of structure contain E, N, U members
/* -------------------------------------------------------------------------- */
blh2enu BLH2ENU(blh2enu blh2enu,
double stationB, double stationL,
double deltax, double deltay, double deltaz)
{
double sinB = sin(stationB);
double cosB = cos(stationB);
double sinL = sin(stationL);
double cosL = cos(stationL);
blh2enu.E = -sinL * (deltax)+cosL * (deltay);
blh2enu.N = -sinB * cosL * (deltax)-sinB * sinL * (deltay)+cosB * (deltaz);
blh2enu.U = cosB * cosL * (deltax)+cosB * sinL * (deltay)+sinB * (deltaz);
return blh2enu;
}
/* -------------------------------------------------------------------------- */
/// @brief Convert ECEF geostationary coordinates to BLH coordinates
/// @param xyz2blh parameter transfer structure
/// @param X X coordinates of ECEF
/// @param Y Y coordinates of ECEF
/// @param Z Z coordinates of ECEF
/// @param a major semiaxis of WGS-84 ellipsoid
/// @param e2 the square of the first eccentricity of WGS-84 ellipsoid
/// @return return type of structure contain B, L, H members
/* -------------------------------------------------------------------------- */
xyz2blh XYZ2BLH(xyz2blh xyz2blh,
double X, double Y, double Z)
{
double B = 0.0, N = 0.0, H = 0.0, R0, R1, deltaH, deltaB;
R0 = sqrt(pow(X, 2) + pow(Y, 2));
R1 = sqrt(pow(X, 2) + pow(Y, 2) + pow(Z, 2));
xyz2blh.L = atan2(Y, X);
N = a;
H = R1 - a;
B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
do{
deltaH = N;
deltaB = B;
N = a / sqrt(1 - e2 * pow(sin(B), 2));
H = R0 / cos(B) - N;
B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
} while (fabs(deltaH - H) > 1.0e-3 && fabs(deltaB - B) > 1.0e-9);
xyz2blh.B = B;
xyz2blh.H = H;
return xyz2blh;
}
src/dllmain.cpp
-1415
View File
File diff suppressed because it is too large. Load diff
src/framework.h
-5
View File
@@ -1,5 +0,0 @@
#pragma once
#define WIN32_LEAN_AND_MEAN // 从 Windows 头文件中排除极少使用的内容
// Windows 头文件
#include <windows.h>
src/pch.cpp
-5
View File
@@ -1,5 +0,0 @@
// pch.cpp: 与预编译标头对应的源文件
#include "pch.h"
// 当使用预编译的头时，需要使用此源文件，编译才能成功。
src/readrnx.cpp
+847
View File
@@ -0,0 +1,847 @@
#include "brdm2pos.h"
using namespace std;
/* -------------------------------------------------------------------------- */
/// @brief find the nearest obs epoch with boardcast
/// @param SecofWeek second of week of obs epoch time
/// @param sPRN the satellite PRN which need to match
/// @param nav_b nav data file structure
/// @param satnum total number of boardcast satellite
/// @param syscode GNSS code, more on PPUBLIC.H
/// @return Return the absolute epoch number of the best epoch
/// When there is no matching satellite number, the function returns -1
/* -------------------------------------------------------------------------- */
int select_epoch(double SecofWeek, int sPRN, pnav_body nav_b, int satnum, int syscode)
{
int best_epoch = -1;
double min = 10000;//Initialize the minimum value
double Min;
if (syscode == GPS){//Be judged as GPS
for (int i = 0; i < satnum; i++){
if (sPRN == nav_b[i].sPRN_GPS){
Min = fabs(SecofWeek - nav_b[i].TOE);
if (Min <= min){
best_epoch = i;
min = Min;
}
}
}return best_epoch;
}
if (syscode == BDS){//Be judged as BeiDou
for (int i = 0; i < satnum; i++){
if (sPRN == nav_b[i].sPRN_BDS){
Min = fabs(SecofWeek - nav_b[i].TOE);
if (Min <= min){
best_epoch = i;
min = Min;
}
}
}return best_epoch;
}
if (syscode == GAL){//Be judged as Galileo
for (int i = 0; i < satnum; i++){
if (sPRN == nav_b[i].sPRN_GAL){
Min = fabs(SecofWeek - nav_b[i].TOE);
if (Min <= min){
best_epoch = i;
min = Min;
}
}
}return best_epoch;
}
if (syscode == GLO){
for (int i = 0; i < satnum; i++){
if (sPRN == nav_b[i].sPRN_GLO){
Min = fabs(SecofWeek - nav_b[i].TOE);
if (Min <= min)
{
best_epoch = i;
min = Min;
}
}
}return best_epoch;
}return best_epoch;
}
/* -------------------------------------------------------------------------- */
/// @brief converts the observation type to the numeric code defined in PUBLIC.H
/// @param i reads the number of incoming loops in the loop
/// @param buff type of incoming observation
/// @return Returns the obs code
/* -------------------------------------------------------------------------- */
int Type2Code(int i, char buff[MAXRINEX])
{
int code_return = 0;
char code_contrast[4];
char code_original[4];
const char L1codeindex[] = "C1AC1BC1CC1DC1LC1MC1PC1SC1XC1YC1Z";//obs type of different frequency
const char L2codeindex[] = "C2CC2DC2IC2LC2MC2PC2QC2SC2XC2YC2Z";
const char L3codeindex[] = "C3IC3QC3X";
const char L4codeindex[] = "C5IC5QC5X";
const char L5codeindex[] = "C5IC5QC5X";
const char L6codeindex[] = "C6AC6BC6CC6XC6Z";
const char L7codeindex[] = "C7IC7QC7XC7DC7PC7Z";
const char L8codeindex[] = "C8IC8QC8X";
memset(code_contrast, 0, sizeof(code_contrast));
memset(code_original, 0, sizeof(code_original));
strncpy(code_original, buff + 7 + 4 * i, 3);
if (strstr(code_original, "1")) {
for (int j = 0; j < (strlen(L1codeindex) / 3); j++) {
strncpy(code_contrast, L1codeindex + 0 + j * 3, 3);
if (strcmp(code_contrast, code_original) == 0) {
code_return = j + 100;
return code_return;
break;
}
}
}
if (strstr(code_original, "2")) {
for (int j = 0; j < (strlen(L2codeindex) / 3); j++) {
strncpy(code_contrast, L2codeindex + 0 + j * 3, 3);
strncpy(code_original, buff + 7 + 4 * i, 3);
if (strcmp(code_contrast, code_original) == 0) {
code_return = j + 200;
return code_return;
break;
}
}
}
if (strstr(code_original, "3")) {
for (int j = 0; j < (strlen(L3codeindex) / 3); j++) {
strncpy(code_contrast, L3codeindex + 0 + j * 3, 3);
strncpy(code_original, buff + 7 + 4 * i, 3);
if (strcmp(code_contrast, code_original) == 0) {
code_return = j + 500;
return code_return;
break;
}
}
}
if (strstr(code_original, "4")) {
for (int j = 0; j < (strlen(L3codeindex) / 3); j++) {
strncpy(code_contrast, L3codeindex + 0 + j * 3, 3);
strncpy(code_original, buff + 7 + 4 * i, 3);
if (strcmp(code_contrast, code_original) == 0) {
code_return = j + 500;
return code_return;
break;
}
}
}
if (strstr(code_original, "5")) {
for (int j = 0; j < (strlen(L5codeindex) / 3); j++) {
strncpy(code_contrast, L5codeindex + 0 + j * 3, 3);
strncpy(code_original, buff + 7 + 4 * i, 3);
if (strcmp(code_contrast, code_original) == 0) {
code_return = j + 500;
return code_return;
break;
}
}
}
if (strstr(code_original, "6")) {
for (int j = 0; j < (strlen(L6codeindex) / 3); j++) {
strncpy(code_contrast, L6codeindex + 0 + j * 3, 3);
strncpy(code_original, buff + 7 + 4 * i, 3);
if (strcmp(code_contrast, code_original) == 0) {
code_return = j + 600;
return code_return;
break;
}
}
}
if (strstr(code_original, "7")) {
for (int j = 0; j < (strlen(L7codeindex) / 3); j++) {
strncpy(code_contrast, L7codeindex + 0 + j * 3, 3);
strncpy(code_original, buff + 7 + 4 * i, 3);
if (strcmp(code_contrast, code_original) == 0) {
code_return = j + 700;
return code_return;
break;
}
}
}
if (strstr(code_original, "8")) {
for (int j = 0; j < (strlen(L8codeindex) / 3); j++) {
strncpy(code_contrast, L8codeindex + 0 + j * 3, 3);
strncpy(code_original, buff + 7 + 4 * i, 3);
if (strcmp(code_contrast, code_original) == 0) {
code_return = j + 800;
return code_return;
break;
}
}
}return code_return;
}
/* -------------------------------------------------------------------------- */
/// @brief the type of the obs value read matches the type code stored in an array
/// @param typecode obs type that you want to match
/// @param typenum the total number of obs types
/// @param typearr type codes store arrays
/// @return returns the array position of the corresponding obs type code
/* -------------------------------------------------------------------------- */
int Code2Type(int typecode, int typenum, int typearr[36])
{
int pos;
for (pos = 0; pos < typenum; pos++) {
if ((typearr[pos] - typearr[pos] % 100) == typecode) {
return pos;
}
}return pos;
}
/* -------------------------- Read the file of rnx -------------------------- */
/* -------------------------------------------------------------------------- */
/// @brief Convert the string to number
/// @param buff
/// @param i Position of begin to read
/// @param n The number of character will to capture
/// @return Number of string you captured
/* -------------------------------------------------------------------------- */
static double strtonum(const char* buff, int i, int n)
{
double value = 0.0;
char str[256] = { 0 };
char* p = str;
/* ---------------------------------- */
/// Function will return 0.0 as error code when three occasion are below:
/// 1.Begining position < 0
/// 2.The number of Byte of read < i
/// 3.The Byte size of string < n
/* ---------------------------------- */
if (i < 0 || (int)strlen(buff) < i || (int)sizeof(str) - 1 < n) {
return 0.0;
}
for (buff += i; *buff && --n >= 0; buff++) {
*p++ = ((*buff == 'D' || *buff == 'd') ? 'e' : *buff);
}
*p = '\0';
return sscanf(str, "%lf", &value) == 1 ? value : 0.0;
}
/*Allocate the memory of observation structure*/
void initobs_e(pobs_epoch obs_e, pobs_body obs_b)
{
obs_e = (pobs_epoch)malloc(sizeof(obs_epoch));
obs_b = (pobs_body)malloc(sizeof(obs_body));
}
/*Free the memory of observation structure*/
void freeobs_e(pobs_epoch obs_e, pobs_body obs_b)
{
free(obs_e); free(obs_b);
}
/*Read the file of nav*/
/* -------------------------------------------------------------------------- */
/// @brief get the total number of satellite boardcast data of nav file
/// @param fp_nav the file pointer of nav file
/// @return total number of satellite boardcast data
/* -------------------------------------------------------------------------- */
int getsatnum(FILE* fp_nav)
{
int satnum = 0;
char buff[MAXRINEX];
char satvar;
char* lable = buff + 60;
while (fgets(buff, MAXRINEX, fp_nav)) {
if (strstr(lable, "RINEX VERSION / TYPE")) {
if (strtonum(buff, 0, 9) < 3 || strtonum(buff, 0, 9) >= 4) return -1;
}
if (strstr(lable, "END OF HEADER")) {
while (fgets(buff, MAXRINEX, fp_nav)) {
strncpy(&satvar, buff + 0, 1);
if (satvar == 'G' || satvar == 'E' || satvar == 'R' || satvar == 'S' || satvar == 'C' || satvar == 'I' || satvar == 'J') {
satnum++;
}
else {
continue;
}
}
}
}return satnum;
}
/*Read the header data of file of nav*/
void read_n_h(FILE* fp_nav, pnav_head nav_h)
{
char buff[MAXRINEX] = { 0 };
char* lable = buff;
int i = 0;
int j = 0;
while (fgets(buff, MAXRINEX, fp_nav)) {
if (strstr(lable, "RINEX VERSION / TYPE")) {
nav_h->ver = strtonum(buff, 0, 9);
strncpy((nav_h->type), buff + 20, 15);
continue;
}
else if (strstr(lable, "GPSA")) {
nav_h->ION_GPSA[0] = strtonum(buff, 6, 12);
nav_h->ION_GPSA[1] = strtonum(buff, 6 + 12, 12);
nav_h->ION_GPSA[2] = strtonum(buff, 6 + 12 + 12, 12);
nav_h->ION_GPSA[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
continue;
}
else if (strstr(lable, "GPSB")) {
nav_h->ION_GPSB[0] = strtonum(buff, 6, 12);
nav_h->ION_GPSB[1] = strtonum(buff, 6 + 12, 12);
nav_h->ION_GPSB[2] = strtonum(buff, 6 + 12 + 12, 12);
nav_h->ION_GPSB[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
continue;
}
else if (strstr(lable, "BDSA")) {
nav_h->ION_BDSA[0] = strtonum(buff, 6, 12);
nav_h->ION_BDSA[1] = strtonum(buff, 6 + 12, 12);
nav_h->ION_BDSA[2] = strtonum(buff, 6 + 12 + 12, 12);
nav_h->ION_BDSA[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
continue;
}
else if (strstr(lable, "BDSB")) {
nav_h->ION_BDSB[0] = strtonum(buff, 6, 12);
nav_h->ION_BDSB[1] = strtonum(buff, 6 + 12, 12);
nav_h->ION_BDSB[2] = strtonum(buff, 6 + 12 + 12, 12);
nav_h->ION_BDSB[3] = strtonum(buff, 6 + 12 + 12 + 12, 12);
continue;
}
else if (strstr(lable, "BDUT")) {
nav_h->BDUT[0] = strtonum(buff, 5, 17);
nav_h->BDUT[1] = strtonum(buff, 5 + 17, 16);
nav_h->BDUT[2] = strtonum(buff, 5 + 17 + 16, 7);
nav_h->BDUT[3] = strtonum(buff, 5 + 17 + 16 + 7, 5);
continue;
}
else if (strstr(lable, "GPUT")) {
nav_h->GPUT[0] = strtonum(buff, 5, 17);
nav_h->GPUT[1] = strtonum(buff, 5 + 17, 16);
nav_h->GPUT[2] = strtonum(buff, 5 + 17 + 16, 7);
nav_h->GPUT[3] = strtonum(buff, 5 + 17 + 16 + 7, 5);
continue;
}
else if (strstr(lable, "LEAP SECONDS")) {
nav_h->leap = (int)strtonum(buff, 4, 2);
}
else if (strstr(lable, "END OF HEADER"))
break;
}
}
/*Read the body data of file of nav*/
void read_n_b(FILE* fp_nav, pnav_body nav_b)
{
int i_g = 0;
char buff[MAXRINEX] = { 0 };
char flag = { 0 };
while (fgets(buff, MAXRINEX, fp_nav)) {
int j_g = 0;
int j_c = 0;
int j_ga = 0;
int j_gl = 0;
strncpy(&flag, buff + 0, 1);
//GPS SATELLITE
if (flag == 'G') {
for (j_g = 0; j_g < 8; j_g++) {
switch (j_g) {
case 0:
nav_b[i_g].sPRN_BDS = -1;
nav_b[i_g].sPRN_GAL = -1;
nav_b[i_g].sPRN_GPS = (int)strtonum(buff, 1, 2);
nav_b[i_g].TOC_Y = (int)strtonum(buff, 4, 4);
nav_b[i_g].TOC_M = (int)strtonum(buff, 9, 2);
nav_b[i_g].TOC_D = (int)strtonum(buff, 12, 2);
nav_b[i_g].TOC_H = (int)strtonum(buff, 15, 2);
nav_b[i_g].TOC_Min = (int)strtonum(buff, 18, 2);
nav_b[i_g].TOC_Sec = strtonum(buff, 21, 2);
nav_b[i_g].sa0 = strtonum(buff, 23, 19);
nav_b[i_g].sa1 = strtonum(buff, 23 + 19, 19);
nav_b[i_g].sa2 = strtonum(buff, 23 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 1:
nav_b[i_g].IODE = strtonum(buff, 4, 19);
nav_b[i_g].Crs = strtonum(buff, 4 + 19, 19);
nav_b[i_g].deltan = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].M0 = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 2:
nav_b[i_g].Cuc = strtonum(buff, 4, 19);
nav_b[i_g].e = strtonum(buff, 4 + 19, 19);
nav_b[i_g].Cus = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].sqrtA = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 3:
nav_b[i_g].TOE = strtonum(buff, 4, 19);
nav_b[i_g].Cic = strtonum(buff, 4 + 19, 19);
nav_b[i_g].OMEGA = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].Cis = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 4:
nav_b[i_g].i0 = strtonum(buff, 4, 19);
nav_b[i_g].Crc = strtonum(buff, 4 + 19, 19);
nav_b[i_g].omega = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].deltaomega = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 5:
nav_b[i_g].IDOT = strtonum(buff, 4, 19);
nav_b[i_g].L2code = strtonum(buff, 4 + 19, 19);
nav_b[i_g].GPSweek = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].L2Pflag = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 6:
nav_b[i_g].sACC = strtonum(buff, 4, 19);
nav_b[i_g].sHEA = strtonum(buff, 4 + 19, 19);
nav_b[i_g].TGD = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].IODC = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 7:
nav_b[i_g].TTN = strtonum(buff, 4, 19);
nav_b[i_g].fit = strtonum(buff, 4 + 19, 19);
//nav_b[i_g].spare1 = strtonum(buff, 4 + 19 + 19, 19);
//nav_b[i_g].spare2 = strtonum(buff, 4 + 19 + 19 + 19, 19);
break;
}
}
i_g++;
continue;
}
//BDS SATELLITE
if (flag == 'C') {
for (j_c = 0; j_c < 8; j_c++) {
switch (j_c) {
case 0:
nav_b[i_g].sPRN_GPS = -1;
nav_b[i_g].sPRN_GAL = -1;
nav_b[i_g].sPRN_BDS = (int)strtonum(buff, 1, 2);
nav_b[i_g].TOC_Y = (int)strtonum(buff, 4, 4);
nav_b[i_g].TOC_M = (int)strtonum(buff, 9, 2);
nav_b[i_g].TOC_D = (int)strtonum(buff, 12, 2);
nav_b[i_g].TOC_H = (int)strtonum(buff, 15, 2);
nav_b[i_g].TOC_Min = (int)strtonum(buff, 18, 2);
nav_b[i_g].TOC_Sec = strtonum(buff, 21, 2);
nav_b[i_g].sa0 = strtonum(buff, 23, 19);
nav_b[i_g].sa1 = strtonum(buff, 23 + 19, 19);
nav_b[i_g].sa2 = strtonum(buff, 23 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 1:
nav_b[i_g].AODE = strtonum(buff, 4, 19);
nav_b[i_g].Crs = strtonum(buff, 4 + 19, 19);
nav_b[i_g].deltan = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].M0 = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 2:
nav_b[i_g].Cuc = strtonum(buff, 4, 19);
nav_b[i_g].e = strtonum(buff, 4 + 19, 19);
nav_b[i_g].Cus = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].sqrtA = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 3:
nav_b[i_g].TOE = strtonum(buff, 4, 19);
nav_b[i_g].Cic = strtonum(buff, 4 + 19, 19);
nav_b[i_g].OMEGA = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].Cis = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 4:
nav_b[i_g].i0 = strtonum(buff, 4, 19);
nav_b[i_g].Crc = strtonum(buff, 4 + 19, 19);
nav_b[i_g].omega = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].deltaomega = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 5:
nav_b[i_g].IDOT = strtonum(buff, 4, 19);
//nav_b[i_g].L2code = strtonum(buff, 4 + 19, 19);
nav_b[i_g].BDTweek = strtonum(buff, 4 + 19 + 19, 19);
//nav_b[i_g].L2Pflag = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 6:
nav_b[i_g].sACC = strtonum(buff, 4, 19);
nav_b[i_g].sHEA = strtonum(buff, 4 + 19, 19);
nav_b[i_g].TGD1 = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].TGD2 = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 7:
//nav_b[i_g].TTN = strtonum(buff, 4, 19);
nav_b[i_g].AODC = strtonum(buff, 4 + 19, 19);
//nav_b[i_g].spare1 = strtonum(buff, 4 + 19 + 19, 19);
//nav_b[i_g].spare2 = strtonum(buff, 4 + 19 + 19 + 19, 19);
break;
}
}
i_g++;
continue;
}
//GAL SATELLITE
if (flag == 'E') {
for (j_ga = 0; j_ga < 8; j_ga++) {
switch (j_ga) {
case 0:
nav_b[i_g].sPRN_BDS = -1;
nav_b[i_g].sPRN_GPS = -1;
nav_b[i_g].sPRN_GAL = (int)strtonum(buff, 1, 2);
nav_b[i_g].TOC_Y = (int)strtonum(buff, 4, 4);
nav_b[i_g].TOC_M = (int)strtonum(buff, 9, 2);
nav_b[i_g].TOC_D = (int)strtonum(buff, 12, 2);
nav_b[i_g].TOC_H = (int)strtonum(buff, 15, 2);
nav_b[i_g].TOC_Min = (int)strtonum(buff, 18, 2);
nav_b[i_g].TOC_Sec = strtonum(buff, 21, 2);
nav_b[i_g].sa0 = strtonum(buff, 23, 19);
nav_b[i_g].sa1 = strtonum(buff, 23 + 19, 19);
nav_b[i_g].sa2 = strtonum(buff, 23 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 1:
nav_b[i_g].IODE = strtonum(buff, 4, 19);
nav_b[i_g].Crs = strtonum(buff, 4 + 19, 19);
nav_b[i_g].deltan = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].M0 = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 2:
nav_b[i_g].Cuc = strtonum(buff, 4, 19);
nav_b[i_g].e = strtonum(buff, 4 + 19, 19);
nav_b[i_g].Cus = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].sqrtA = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 3:
nav_b[i_g].TOE = strtonum(buff, 4, 19);
nav_b[i_g].Cic = strtonum(buff, 4 + 19, 19);
nav_b[i_g].OMEGA = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].Cis = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 4:
nav_b[i_g].i0 = strtonum(buff, 4, 19);
nav_b[i_g].Crc = strtonum(buff, 4 + 19, 19);
nav_b[i_g].omega = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].deltaomega = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 5:
nav_b[i_g].IDOT = strtonum(buff, 4, 19);
//nav_b[i_g].L2code = strtonum(buff, 4 + 19, 19);
nav_b[i_g].GALweek = strtonum(buff, 4 + 19 + 19, 19);
//nav_b[i_g].L2Pflag = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 6:
nav_b[i_g].sACC = strtonum(buff, 4, 19);
nav_b[i_g].sHEA = strtonum(buff, 4 + 19, 19);
nav_b[i_g].BGDa = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].BGDb = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 7:
//nav_b[i_g].TTN = strtonum(buff, 4, 19);
//nav_b[i_g].fit = strtonum(buff, 4 + 19, 19);
//nav_b[i_g].spare1 = strtonum(buff, 4 + 19 + 19, 19);
//nav_b[i_g].spare2 = strtonum(buff, 4 + 19 + 19 + 19, 19);
break;
}
}
i_g++;
continue;
}
//GLO SATELLITE
if (flag == 'R') {
for (j_gl = 0; j_gl < 4; j_gl++) {
switch (j_gl) {
case 0:
nav_b[i_g].sPRN_BDS = -1;
nav_b[i_g].sPRN_GPS = -1;
nav_b[i_g].sPRN_GAL = -1;
nav_b[i_g].sPRN_GLO = (int)strtonum(buff, 1, 2);
nav_b[i_g].TOC_Y = (int)strtonum(buff, 4, 4);
nav_b[i_g].TOC_M = (int)strtonum(buff, 9, 2);
nav_b[i_g].TOC_D = (int)strtonum(buff, 12, 2);
nav_b[i_g].TOC_H = (int)strtonum(buff, 15, 2);
nav_b[i_g].TOC_Min = (int)strtonum(buff, 18, 2);
nav_b[i_g].TOC_Sec = strtonum(buff, 21, 2);
nav_b[i_g].sa0 = strtonum(buff, 23, 19);
nav_b[i_g].sa1 = strtonum(buff, 23 + 19, 19);
nav_b[i_g].Dos = strtonum(buff, 23 + 19 + 19, 19);
nav_b[i_g].TOE = JDUTC2GPST(
UTCTime2JD(nav_b[i_g].TOC_Y, nav_b[i_g].TOC_M, nav_b[i_g].TOC_D,
nav_b[i_g].TOC_H, nav_b[i_g].TOC_Min, nav_b[i_g].TOC_Sec)
);
fgets(buff, MAXRINEX, fp_nav);
break;
case 1:
nav_b[i_g].SatX = strtonum(buff, 4, 19);
nav_b[i_g].SatXv = strtonum(buff, 4 + 19, 19);
nav_b[i_g].SatXa = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].sHEA = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 2:
nav_b[i_g].SatY = strtonum(buff, 4, 19);
nav_b[i_g].SatYv = strtonum(buff, 4 + 19, 19);
nav_b[i_g].SatYa = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].FreN = strtonum(buff, 4 + 19 + 19 + 19, 19);
fgets(buff, MAXRINEX, fp_nav);
break;
case 3:
nav_b[i_g].SatZ = strtonum(buff, 4, 19);
nav_b[i_g].SatZv = strtonum(buff, 4 + 19, 19);
nav_b[i_g].SatZa = strtonum(buff, 4 + 19 + 19, 19);
nav_b[i_g].AOO = strtonum(buff, 4 + 19 + 19 + 19, 19);
break;
}
}
i_g++;
continue;
}
}
}
/*Read the file of obs*/
/* -------------------------------------------------------------------------- */
/// @brief get the total number of epoch of obs
/// @param fp_obs the file pointer of obs
/// @return epoch number
/* -------------------------------------------------------------------------- */
int get_epochnum(FILE* fp_obs)
{
int n = 0;
char flag;
char buff[MAXRINEX];
char* lable = buff + 60;
while (fgets(buff, MAXRINEX, fp_obs)) {
if (strstr(lable, "RINEX VERSION / TYPE")) {
if (strtonum(buff, 0, 9) < 3) return -1;
}
if (strstr(lable, "END OF HEADER")) {
while (fgets(buff, MAXRINEX, fp_obs)) {
strncpy(&flag, buff + 0, 1);
if (flag == '>') {
n++;
}
}
}
}return n;
}
/*Read the header data of file of obs*/
void read_o_h(FILE* fp_obs, pobs_head obs_h)
{
char buff[MAXRINEX] = { 0 };
char flag = { 0 };
char* lable = buff + 60;
int i = 0;
int j = 0;
obs_h->interval = 30;
obs_h->f_y = obs_h->f_m = obs_h->f_d = obs_h->f_h = obs_h->f_min = obs_h->f_sec = 0;
obs_h->l_y = obs_h->l_m = obs_h->l_d = obs_h->l_h = obs_h->l_min = obs_h->l_sec = 0;
while (fgets(buff, MAXRINEX, fp_obs)) {
if (strstr(lable, "RINEX VERSION / TYPE")) {
obs_h->ver = strtonum(buff, 0, 9);
strncpy(obs_h->type, buff + 20, 30);
continue;
}
else if (strstr(lable, "APPROX POSITION XYZ")) {
obs_h->apX = strtonum(buff, 0, 14);
obs_h->apY = strtonum(buff, 0 + 14, 14);
obs_h->apZ = strtonum(buff, 0 + 14 + 14, 14);
continue;
}
else if (strstr(lable, "ANTENNA: DELTA H/E/N")) {
obs_h->ANTH = strtonum(buff, 0, 14);
obs_h->ANTdeltaE = strtonum(buff, 14, 14);
obs_h->ANTdeltaN = strtonum(buff, 14 + 14, 14);
continue;
}
else if (strstr(lable, "SYS / # / OBS TYPES")) {
char flag = { 0 };
strncpy(&flag, buff + 0, 1);
if (flag == 'G') {
obs_h->obstypenum_gps = (int)strtonum(buff, 4, 2);
if (obs_h->obstypenum_gps <= 13) {
for (i = 0; i < obs_h->obstypenum_gps; i++) {
obs_h->obscode_gps[i + 1] = Type2Code(i, buff);
}
}
else if (obs_h->obstypenum_gps > 13) {
for (i = 0; i < 13; i++) {
obs_h->obscode_gps[i] = Type2Code(i, buff);
}
fgets(buff, MAXRINEX, fp_obs);
for (i = 0; i < obs_h->obstypenum_gps - 13; i++) {
obs_h->obscode_gps[i + 13] = Type2Code(i, buff);
}
}continue;
}
else if (flag == 'C') {
obs_h->obstypenum_bds = (int)strtonum(buff, 4, 2);
if (obs_h->obstypenum_bds <= 13) {
for (i = 0; i < obs_h->obstypenum_bds; i++) {
obs_h->obscode_bds[i] = Type2Code(i, buff);
}
}
else if (obs_h->obstypenum_bds > 13) {
for (i = 0; i < 13; i++) {
obs_h->obscode_bds[i] = Type2Code(i, buff);
}
fgets(buff, MAXRINEX, fp_obs);
for (i = 0; i < obs_h->obstypenum_bds - 13; i++) {
obs_h->obscode_bds[i + 13] = Type2Code(i, buff);
}
}continue;
}
else if (flag == 'E') {
obs_h->obstypenum_gal = (int)strtonum(buff, 4, 2);
if (obs_h->obstypenum_gal <= 13) {
for (i = 0; i < obs_h->obstypenum_gal; i++) {
obs_h->obscode_gal[i] = Type2Code(i, buff);
}
}
else if (obs_h->obstypenum_gal > 13) {
for (i = 0; i < 13; i++) {
obs_h->obscode_gal[i] = Type2Code(i, buff);
}
fgets(buff, MAXRINEX, fp_obs);
for (i = 0; i < obs_h->obstypenum_gal - 13; i++) {
obs_h->obscode_gal[i + 13] = Type2Code(i, buff);
}
}continue;
}
else if (flag == 'R') {
obs_h->obstypenum_glo = (int)strtonum(buff, 4, 2);
if (obs_h->obstypenum_glo <= 13) {
for (i = 0; i < obs_h->obstypenum_glo; i++) {
obs_h->obscode_glo[i] = Type2Code(i, buff);
}
}
else if (obs_h->obstypenum_glo > 13) {
for (i = 0; i < 13; i++) {
obs_h->obscode_glo[i] = Type2Code(i, buff);
}
fgets(buff, MAXRINEX, fp_obs);
for (i = 0; i < obs_h->obstypenum_glo - 13; i++) {
obs_h->obscode_glo[i + 13] = Type2Code(i, buff);
}
}continue;
}continue;
}
else if (strstr(lable, "INTERVAL")) {
obs_h->interval = strtonum(buff, 0, 10);
continue;
}
else if (strstr(lable, "TIME OF FIRST OBS")) {
obs_h->f_y = (int)strtonum(buff, 2, 4);
obs_h->f_m = (int)strtonum(buff, 2 + 6, 4);
obs_h->f_d = (int)strtonum(buff, 2 + 6 + 6, 4);
obs_h->f_h = (int)strtonum(buff, 2 + 6 + 6 + 6, 4);
obs_h->f_min = (int)strtonum(buff, 2 + 6 + 6 + 6 + 6, 4);
obs_h->f_sec = strtonum(buff, 2 + 6 + 6 + 6 + 6 + 6, 9);
strncpy(obs_h->tsys, buff + 6 + 6 + 6 + 6 + 6 + 18, 3);
continue;
}
else if (strstr(lable, "TIME OF LAST OBS")) {
obs_h->l_y = (int)strtonum(buff, 2, 4);
obs_h->l_m = (int)strtonum(buff, 2 + 6, 4);
obs_h->l_d = (int)strtonum(buff, 2 + 6 + 6, 4);
obs_h->l_h = (int)strtonum(buff, 2 + 6 + 6 + 6, 4);
obs_h->l_min = (int)strtonum(buff, 2 + 6 + 6 + 6 + 6, 4);
obs_h->l_sec = strtonum(buff, 2 + 6 + 6 + 6 + 6 + 6, 9);
continue;
}
else if (strstr(lable, "END OF HEADER")) {
break;
}
}
}
/*Read the body data of file of obs*/
void read_o_eb(FILE* fp_obs, pobs_head obs_h, pobs_epoch obs_e, pobs_body obs_b)
{
initobs_e(obs_e, obs_b);
char buff[MAXRINEX] = { 0 };
char flag = { 0 };
while (fgets(buff, MAXRINEX, fp_obs)) {
strncpy(&flag, buff + 0, 1);
if (flag == '>') {
int j_g = 0; int k_g = 0;
int j_c = 0; int k_c = 0;
int j_ga = 0; int k_ga = 0;
int j_gl = 0; int k_gl = 0;
obs_e->gps_num = 0;
obs_e->bds_num = 0;
obs_e->gal_num = 0;
obs_e->glo_num = 0;
obs_e->y = (int)strtonum(buff, 2, 4);
obs_e->m = (int)strtonum(buff, 7, 2);
obs_e->d = (int)strtonum(buff, 10, 2);
obs_e->h = (int)strtonum(buff, 13, 2);
obs_e->min = (int)strtonum(buff, 16, 2);
obs_e->sec = strtonum(buff, 19, 10);
obs_e->p_flag = (int)strtonum(buff, 31, 1);
obs_e->sat_num = strtonum(buff, 33, 3);
strncpy(&flag, buff + 0, 1);
memset(obs_e->sPRN, 0, sizeof(obs_e->sPRN));
memset(obs_e->sPRN_GPS, 0, sizeof(obs_e->sPRN_GPS));
memset(obs_e->sPRN_BDS, 0, sizeof(obs_e->sPRN_BDS));
memset(obs_e->sPRN_GAL, 0, sizeof(obs_e->sPRN_GAL));
memset(obs_e->sPRN_GLO, 0, sizeof(obs_e->sPRN_GLO));
memset(obs_b->obs_gps, 0, sizeof(obs_b->obs_gps));
memset(obs_b->obs_bds, 0, sizeof(obs_b->obs_bds));
memset(obs_b->obs_gal, 0, sizeof(obs_b->obs_gal));
memset(obs_b->obs_glo, 0, sizeof(obs_b->obs_glo));
for (int i = 0; i < obs_e->sat_num; i++) {
if (flag == 'G') {
obs_e->sPRN[i] = strtonum(buff, 1, 2);
obs_e->sPRN_GPS[j_g] = obs_e->sPRN[i];
for (k_g = 0; k_g < obs_h->obstypenum_gps; k_g++) {
obs_b->obs_gps[j_g][k_g] = strtonum(buff, 3 + 16 * k_g, 14);
}
obs_e->gps_num++; j_g++;
fgets(buff, MAXRINEX, fp_obs);
strncpy(&flag, buff + 0, 1);
}
else if (flag == 'C') {
obs_e->sPRN[i] = strtonum(buff, 1, 2);
obs_e->sPRN_BDS[j_c] = obs_e->sPRN[i];
for (k_c = 0; k_c < obs_h->obstypenum_bds; k_c++) {
obs_b->obs_bds[j_c][k_c] = strtonum(buff, 3 + 16 * k_c, 14);
}
obs_e->bds_num++; j_c++;
fgets(buff, MAXRINEX, fp_obs);
strncpy(&flag, buff + 0, 1);
}
else if (flag == 'E') {
obs_e->sPRN[i] = strtonum(buff, 1, 2);
obs_e->sPRN_GAL[j_ga] = obs_e->sPRN[i];
for (k_ga = 0; k_ga < obs_h->obstypenum_gal; k_ga++) {
obs_b->obs_gal[j_ga][k_ga] = strtonum(buff, 3 + 16 * k_ga, 14);
}
obs_e->gal_num++; j_ga++;
fgets(buff, MAXRINEX, fp_obs);
strncpy(&flag, buff + 0, 1);
}
else if (flag == 'R') {
obs_e->sPRN[i] = strtonum(buff, 1, 2);
obs_e->sPRN_GLO[j_gl] = obs_e->sPRN[i];
for (k_gl = 0; k_gl < obs_h->obstypenum_glo; k_gl++) {
obs_b->obs_glo[j_gl][k_gl] = strtonum(buff, 3 + 16 * k_gl, 14);
}
obs_e->glo_num++; j_gl++;
fgets(buff, MAXRINEX, fp_obs);
strncpy(&flag, buff + 0, 1);
}
else {
fgets(buff, MAXRINEX, fp_obs);
strncpy(&flag, buff + 0, 1);
}
}
}break;
}
}
src/rnx2pos.cpp
+411
View File
@@ -0,0 +1,411 @@
#include "brdm2pos.h"
using namespace std;
/*Obatain the intergral term of acceleraction of each component*/
void pz90deq(double X[6], double K[6], double acc[3])
{
double r = sqrt(pow(X[0], 2) + pow(X[1], 2) + pow(X[2], 2));
double coef0 = -GM / pow(r, 3);
double coef1 = 1.5 * C20 * (GM * pow(a, 2) / pow(r, 5));
K[0] = X[3]; K[1] = X[4]; K[2] = X[5];
K[3] =
coef0 * X[0] +
coef1 * (1 - 5 * pow((X[2] / r), 2)) * X[0] +
pow(Earth_e, 2) * X[0] +
2 * Earth_e * X[4] +
acc[0];
K[4] =
coef0 * X[1] +
coef1 * (1 - 5 * pow((X[2] / r), 2)) * X[1] +
pow(Earth_e, 2) * X[1] -
2 * Earth_e * X[3] +
acc[1];
K[5] =
coef0 * X[2] +
coef1 * (3 - 5 * pow((X[2] / r), 2)) * X[2] +
acc[2];
}
/*Construct the RK-4 method*/
void pz90pos(double tstep, double X[6], double acc[3])
{
double K1[6], K2[6], K3[6], K4[6], W[6];
pz90deq(X, K1, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K1[i] * tstep / 2.0;
pz90deq(W, K2, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K2[i] * tstep / 2.0;
pz90deq(W, K3, acc); for (int i = 0; i < 6; i++) W[i] = X[i] + K3[i] * tstep;
pz90deq(W, K4, acc);
for (int i = 0; i < 6; i++) X[i] += (K1[i] + 2.0 * K2[i] + 2.0 * K3[i] + K4[i]) * tstep / 6.0;
}
/*Calculate the satellite position of GLONASS on this epoch*/
pos_ts glo_pos(int best_epoch, double Weeksec,
pnav_body nav_b, pobs_head obs_h, pos_ts pos_t)
{
double X[6], acc[3]; double R = 0.0;
double tdist = Weeksec - pos_t.delta_t - nav_b[best_epoch].TOE, tstep = 0.0;
pos_t.delta_clk = -nav_b[best_epoch].sa0 + nav_b[best_epoch].sa1 * tdist;
X[0] = nav_b[best_epoch].SatX * 1e3;
X[1] = nav_b[best_epoch].SatY * 1e3;
X[2] = nav_b[best_epoch].SatZ * 1e3;//Position
X[3] = nav_b[best_epoch].SatXv * 1e3;
X[4] = nav_b[best_epoch].SatYv * 1e3;
X[5] = nav_b[best_epoch].SatZv * 1e3;//Velocity
acc[0] = nav_b[best_epoch].SatXa * 1e3;
acc[1] = nav_b[best_epoch].SatYa * 1e3;
acc[2] = nav_b[best_epoch].SatZa * 1e3;//Acclerate
for (tstep = tdist < 0.0 ? TSTEP : -TSTEP; fabs(tdist) > 1.0e-9; tdist += tstep) {
if (fabs(tdist) < TSTEP) tstep = -tdist;
pz90pos(tstep, X, acc);
}
X[0] = -0.47 + (X[0] * 1 + X[1] * 1.728e-6 + X[2] * -0.017e-6) * (1 + 22e-9);
X[1] = -0.51 + (X[0] * 1.728e-6 + X[1] * 1 + X[2] * -0.076e-6) * (1 + 22e-9);
X[2] = -1.56 + (X[0] * 0.0178e-6 + X[1] * -0.076e-6 + X[2] * 1) * (1 + 22e-9);//PZ90 Convert to WGS-84
pos_t.X = X[0];
pos_t.Y = X[1];
pos_t.Z = X[2];
R = sqrt(pow(pos_t.X - obs_h->apX, 2) + pow(pos_t.Y - obs_h->apY, 2) + pow(pos_t.Z - obs_h->apZ, 2));
pos_t.deltat = pos_t.delta_t;
pos_t.delta_t = R / C_V + pos_t.delta_clk;
return pos_t;
}
/*Calculate the satellite position of other systems on this epoch*/
pos_ts sat_pos(int sPRN, int best_epoch, double Weeksec, int gnsscode,
pnav_body nav_b, pobs_head obs_h, pos_ts pos_t)
{
double T_S = Weeksec - pos_t.delta_t;
double n_0 = sqrt(GM) / pow(nav_b[best_epoch].sqrtA, 3);
double n = n_0 + nav_b[best_epoch].deltan;
double tk;
if (gnsscode == BDS) tk = T_S - nav_b[best_epoch].TOE - 14;
else tk = T_S - nav_b[best_epoch].TOE;
if (tk > 32400)tk -= 604800;
else if (tk < -32400)tk += 604800;
double Ms = nav_b[best_epoch].M0 + n * tk;
double Es = Ms, E0;
do
{
E0 = Es;
Es = Ms + nav_b[best_epoch].e * sin(Es);
} while (fabs(Es - E0) > 1.0e-10);
double cosfs = (cos(Es) - nav_b[best_epoch].e) / (1 - nav_b[best_epoch].e * cos(Es));
double sinfs = (sqrt(1 - pow(nav_b[best_epoch].e, 2)) * sin(Es)) / (1 - nav_b[best_epoch].e * cos(Es));
double fs = atan2(sinfs, cosfs);
double u0 = fs + nav_b[best_epoch].omega;
double epsilon_u = nav_b[best_epoch].Cus * sin(2 * u0) + nav_b[best_epoch].Cuc * cos(2 * u0);
double epsilon_r = nav_b[best_epoch].Crs * sin(2 * u0) + nav_b[best_epoch].Crc * cos(2 * u0);
double epsilon_i = nav_b[best_epoch].Cis * sin(2 * u0) + nav_b[best_epoch].Cic * cos(2 * u0);
double u = u0 + epsilon_u;
double r = pow(nav_b[best_epoch].sqrtA, 2) * (1 - nav_b[best_epoch].e * cos(Es)) + epsilon_r;
double i = nav_b[best_epoch].i0 + epsilon_i + nav_b[best_epoch].IDOT * tk;
double x = r * cos(u);
double y = r * sin(u);
double l, X, Y, Z;
if (gnsscode == BDS) {
if (sPRN > 5 && sPRN < 59) //BDS IGSO & MEO Satellites
{
l = nav_b[best_epoch].OMEGA + (nav_b[best_epoch].deltaomega - Earth_e) * tk - Earth_e * nav_b[best_epoch].TOE;
X = x * cos(l) - y * cos(i) * sin(l);
Y = x * sin(l) + y * cos(i) * cos(l);
Z = y * sin(i);
}
else //BDS GEO Satellites
{
l = nav_b[best_epoch].OMEGA + nav_b[best_epoch].deltaomega * tk - Earth_e * nav_b[best_epoch].TOE;
X = x * cos(l) - y * cos(i) * sin(l);
Y = x * sin(l) + y * cos(i) * cos(l);
Z = y * sin(i);
double f = deg2rad(-5);
double p = Earth_e * tk;
X = X * cos(p) + Y * sin(p) * cos(f) + Z * sin(p) * sin(f);
Y = X * -sin(p) + Y * cos(p) * cos(f) + Z * cos(p) * sin(f);
Z = Y * -sin(f) + Z * cos(f);
}
}
else {
l = nav_b[best_epoch].OMEGA + (nav_b[best_epoch].deltaomega - Earth_e) * tk - Earth_e * nav_b[best_epoch].TOE;
X = x * cos(l) - y * cos(i) * sin(l);
Y = x * sin(l) + y * cos(i) * cos(l);
Z = y * sin(i);
}
pos_t.X = cos(Earth_e * pos_t.delta_t) * X + sin(Earth_e * pos_t.delta_t) * Y;
pos_t.Y = -sin(Earth_e * pos_t.delta_t) * X + cos(Earth_e * pos_t.delta_t) * Y;
pos_t.Z = Z;//Correction of earth rotation
double TGD;
if (gnsscode == GPS) TGD = nav_b[best_epoch].TGD;
else if (gnsscode == GAL) TGD = nav_b[best_epoch].BGDa;
else if (gnsscode == BDS) TGD = 0;
double R = sqrt(pow(pos_t.X - obs_h->apX, 2) + pow(pos_t.Y - obs_h->apY, 2) + pow(pos_t.Z - obs_h->apZ, 2));
double rela = 2 * sqrt(GM) * nav_b[best_epoch].e * nav_b[best_epoch].sqrtA * sin(Es) / pow(C_V, 2);//Relativistic effect
pos_t.delta_clk = nav_b[best_epoch].sa0 + nav_b[best_epoch].sa1 * tk + nav_b[best_epoch].sa2 * pow(tk, 2) - rela - TGD;
pos_t.deltat = pos_t.delta_t;
pos_t.delta_t = R / C_V + pos_t.delta_clk;
return pos_t;
}
/* -------------------------------------------------------------------------- */
/// @brief Calculate the position of satellite in the ECEF coordinate system by the broadcast ephemeris and epoch information
/// @param obs_h Observation header data
/// @param obs_e Observation epoch data
/// @param obs_b Observation data
/// @param nav_b Navigation data
/// @param res_file The file path of result file of satellite position
/// @param station Prior information about station location
/// @param gnsscode GNSS system code
/// @param satnum The total number of satellite of designated satellite system
/// @return 1 refer OK
/* -------------------------------------------------------------------------- */
int sat_pos_cal(pobs_head obs_h, pobs_epoch obs_e, pobs_body obs_b,
pnav_body nav_b,
FILE* res_file, stations station,
int gnsscode, int satnum)
{
//the beginning second of observation
double beginsec = Time2GPST(obs_h->f_y, obs_h->f_m, obs_h->f_d, obs_h->f_h, obs_h->f_min, obs_h->f_sec);
//the ending second of observation
double endinsec = Time2GPST(obs_h->l_y, obs_h->l_m, obs_h->l_d, obs_h->l_h, obs_h->l_min, obs_h->l_sec);
blhs blh = { 0 }; blh2enu enu = { 0 };
rahcal rah = { 0 }; pos_ts pos_t = { 0 };
xyz2blh tem1 = { 0 }; blh2enu tem2 = { 0 }; rahcal tem3 = { 0 };
int y = obs_e->y; int m = obs_e->m; int d = obs_e->d;
double h = obs_e->h; int min = obs_e->min; double sec = obs_e->sec;
double Weeksec = Time2GPST(y, m, d, h, min, sec);
int epochcount = round((Weeksec - beginsec) / obs_h->interval) + 1;
fprintf(res_file, "\n>%04d %04d %02d %02d %02d %02d %07.04f"
, epochcount, obs_e->y, obs_e->m, obs_e->d, obs_e->h, obs_e->min, obs_e->sec);
int sat_num = 0, sPRN = 0, SIGN = 0, PERS = 0;
double detat_toc;
//Check the GNSS and assign the satellite number
if (gnsscode == GPS)sat_num = obs_e->gps_num;
else if (gnsscode == GAL)sat_num = obs_e->gal_num;
else if (gnsscode == BDS)sat_num = obs_e->bds_num;
else if (gnsscode == GLO)sat_num = obs_e->glo_num;
for (int j = 0; j < sat_num; j++) {
if (gnsscode == GPS) {
sPRN = obs_e->sPRN_GPS[j]; SIGN = GPS;
PERS = obs_b->obs_gps[j][Code2Type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)] / C_V;
}
else if (gnsscode == GAL) {
sPRN = obs_e->sPRN_GAL[j]; SIGN = GAL;
PERS = obs_b->obs_gal[j][Code2Type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)] / C_V;
}
else if (gnsscode == BDS) {
sPRN = obs_e->sPRN_BDS[j]; SIGN = BDS;
PERS = obs_b->obs_bds[j][Code2Type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)] / C_V;
}
else if (gnsscode == GLO) {
sPRN = obs_e->sPRN_GLO[j]; SIGN = GLO;
PERS = obs_b->obs_glo[j][Code2Type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)] / C_V;
}
int best_epoch = select_epoch(Weeksec, sPRN, nav_b, satnum, SIGN);
if (best_epoch == -1)//when the sPRN is not exist function will return -1
break;
if (SIGN == BDS) detat_toc = Weeksec - nav_b[best_epoch].TOE - 14;
else detat_toc = Weeksec - nav_b[best_epoch].TOE;
//Iterative approximate propagation time
//Exclude GLONASS
if (SIGN == GLO) {
pos_t.delta_t = PERS -
station.delta_TR +
nav_b[best_epoch].sa0 +
nav_b[best_epoch].sa1 * detat_toc;
pos_t.deltat = 0.0;
while (fabs(pos_t.delta_t - pos_t.deltat) > 1.0e-9) {
pos_t = glo_pos(best_epoch, Weeksec, nav_b, obs_h, pos_t);
}
}
else {
pos_t.delta_t = PERS -
station.delta_TR +
nav_b[best_epoch].sa0 +
nav_b[best_epoch].sa1 * detat_toc +
nav_b[best_epoch].sa2 * pow(detat_toc, 2);
pos_t.deltat = 0.0;
while (fabs(pos_t.delta_t - pos_t.deltat) > 1.0e-9) {
pos_t = sat_pos(sPRN, best_epoch, Weeksec, gnsscode, nav_b, obs_h, pos_t);
}
}
//Satellite blh position calculation
tem1 = XYZ2BLH(tem1, pos_t.X, pos_t.Y, pos_t.Z);
blh.B = rad2deg(tem1.B);
blh.L = rad2deg(tem1.L);
blh.H = tem1.H;
double deltax = pos_t.X - obs_h->apX;
double deltay = pos_t.Y - obs_h->apY;
double deltaz = pos_t.Z - obs_h->apZ;
tem2 = BLH2ENU(tem2, station.B, station.L, deltax, deltay, deltaz);
enu.E = tem2.E;
enu.N = tem2.N;
enu.U = tem2.U;
//Satellite elevation Angle calculation
tem3 = RAHCAL(tem3, enu.E, enu.N, enu.U);
rah.R = tem3.R;
rah.A = rad2deg(tem3.A);
rah.H = rad2deg(tem3.H);
if (blh.H < 0 || rah.H < 10 || nav_b[best_epoch].sHEA != 0) {
continue;
}
else {
if (SIGN == GPS) {
fprintf(res_file, "\nG %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, obs_b->obs_gps[j][Code2Type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)]
, obs_b->obs_gps[j][Code2Type(C2, obs_h->obstypenum_gps, obs_h->obscode_gps)]
, rah.H, pos_t.delta_clk);//GPS satellite pos output
}
else if (SIGN == GAL) {
fprintf(res_file, "\nE %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, obs_b->obs_gal[j][Code2Type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)]
, obs_b->obs_gal[j][Code2Type(C6, obs_h->obstypenum_gal, obs_h->obscode_gal)]
, rah.H, pos_t.delta_clk);//Galileo satellite pos output
}
else if (SIGN == BDS) {
if (sPRN >= 19) {
fprintf(res_file, "\nCC%02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, obs_b->obs_bds[j][Code2Type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)]
, obs_b->obs_bds[j][Code2Type(C7, obs_h->obstypenum_bds, obs_h->obscode_bds)]
, rah.H, pos_t.delta_clk);//BDS-3 satellite pos output
}
else {
fprintf(res_file, "\nCB%02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, obs_b->obs_bds[j][Code2Type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)]
, obs_b->obs_bds[j][Code2Type(C7, obs_h->obstypenum_bds, obs_h->obscode_bds)]
, rah.H, pos_t.delta_clk);//BDS-2 satellite pos output
}
}
else if (SIGN == GLO) {
fprintf(res_file, "\nR %02d| %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %16.13f"
, sPRN, pos_t.X, pos_t.Y, pos_t.Z
, obs_b->obs_glo[j][Code2Type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)]
, obs_b->obs_glo[j][Code2Type(C2, obs_h->obstypenum_glo, obs_h->obscode_glo)]
, rah.H, pos_t.delta_clk);//Galileo satellite pos output
}
}
}return 1;
}
/* -------------------------------------------------------------------------- */
/// @brief The entry function of full .dll package,
/// @param nav_path The RINEX navigation file's path
/// @param obs_path The RINEX observation file's path
/// @param res_path The Satellite output file's path
/// @param gnsscode The GNSS position you want to calculate
/// @return 0 refer complete solution; -1 refer fail to complete solution
/// @return > 0 refer partlt complete solution
/* -------------------------------------------------------------------------- */
double brdm2pos(char nav_path[260], char obs_path[260], char res_path[260], int gnsscode)
{
int satnum, epochunum, result = 0;
pos_ts pos_t = { 0 };
xyz2blh tem2 = { 0 };
stations station = { 0 };
time_t gen_time; time(&gen_time);
pnav_head nav_h = NULL;
pnav_body nav_b = NULL;
pobs_head obs_h = NULL;
pobs_body obs_b = NULL;
pobs_epoch obs_e = NULL;
FILE* fp_nav = fopen(nav_path, "r");
if (fp_nav == NULL) return -1;
satnum = getsatnum(fp_nav);
switch (satnum) {
case -1: return -3;//mismatch version or format
case 0: return -4;//no data
default:
break;
}
rewind(fp_nav);
nav_h = (pnav_head)malloc(sizeof(nav_head));
nav_b = (pnav_body)malloc(sizeof(nav_body) * (satnum));
if (nav_h && nav_b) {
read_n_h(fp_nav, nav_h); read_n_b(fp_nav, nav_b);
}
fclose(fp_nav);
FILE* fp_obs = fopen(obs_path, "r");
if (fp_obs == NULL) return -2;
epochunum = get_epochnum(fp_obs);
switch (epochunum) {
case -1: return -5;//mismatch version or format
case 0: return -6;//no data
default:
break;
}
rewind(fp_obs);
obs_h = (pobs_head)malloc(sizeof(obs_head));
obs_e = (pobs_epoch)malloc(sizeof(obs_epoch));
obs_b = (pobs_body)malloc(sizeof(obs_body));
if (obs_h && obs_e && obs_b) {
read_o_h(fp_obs, obs_h);
tem2 = XYZ2BLH(tem2, obs_h->apX, obs_h->apY, obs_h->apZ);
station.B = tem2.B; station.L = tem2.L; station.H = tem2.H;
//Out put the header of solution file
FILE* result_file_clear = fopen(res_path, "w");
fclose(result_file_clear);
FILE* result_file = fopen(res_path, "a+");
fprintf(result_file, "@ GENERATE PROGRAM : KNZ_GeoTrackLab ver1.5.3.1\n");
fprintf(result_file, "@ GENERATE TYPE : Satellite Position\n");
fprintf(result_file, "@ GENERATE TIME : %s", ctime(&gen_time));
fprintf(result_file, "@ OBS FILE PATH : %s\n", obs_path);
fprintf(result_file, "@ NAV FILE PATH : %s\n", nav_path);
fprintf(result_file, "@ TIME OF FIRST OBS : %4d %02d %02d %02d %02d %07.4f\n"
, obs_h->f_y, obs_h->f_m, obs_h->f_d, obs_h->f_h, obs_h->f_min, obs_h->f_sec);
fprintf(result_file, "@ TIME OF LAST OBS : %4d %02d %02d %02d %02d %07.4f\n"
, obs_h->l_y, obs_h->l_m, obs_h->l_d, obs_h->l_h, obs_h->l_min, obs_h->l_sec);
fprintf(result_file, "@ APPROX POSITION XYZ: %13.04f%13.04f%13.04f\n@ APPROX POSITION BLH: %12.07f %12.07f %12.07f\n"
, obs_h->apX, obs_h->apY, obs_h->apZ
, station.B, station.L, station.H);
fprintf(result_file, "END OF HEADER\n");
//Begin to calculate
for (int i = 0; i < epochunum; i++) {
read_o_eb(fp_obs, obs_h, obs_e, obs_b);
result += sat_pos_cal(obs_h, obs_e, obs_b, nav_b,
result_file, station,
gnsscode, satnum);
fprintf(result_file, "\n");
}
freeobs_e(obs_e, obs_b);
free(obs_h); free(nav_h); free(nav_b);
fprintf(result_file, "\nEND"); fclose(result_file);
if (epochunum == result) return 0;
}
}
src/timesys.cpp
+68
View File
@@ -0,0 +1,68 @@
#include "brdm2pos.h"
using namespace std;
/* -------------------------------------------------------------------------- */
/// @brief convert UTC to GPS Second of week
/// @param y year
/// @param m month
/// @param d day
/// @param h hour
/// @param min minute
/// @param sec second
/// @return return GPS Second of week
/* -------------------------------------------------------------------------- */
double Time2GPST(int y, int m, int d, double h, int min, double sec)
{
if (m > 2)
{
y = y;
m = m;
}
if (m <= 2)
{
y = y - 1;
m = m + 12;
}
h = h + min / 60.0 + sec / 3600.0;
double JD = (int)(365.25 * y) + (int)(30.6001 * (m + 1)) + d + h / 24.0 + 1720981.5;
double MJD = JD - 2400000.5;
int gpsweek = (int)((MJD - 44244) / 7);
double secofweek = (MJD - 44244.0 - gpsweek * 7.0) * 86400.0;
return secofweek;
}
double JDUTC2GPST(double JD_UTCSU)
{
double n;//Leap second
if (2451179.5000000 < JD_UTCSU && JD_UTCSU < 2453736.5000000) { n = 32.0; }
else if (2453736.5000000 < JD_UTCSU && JD_UTCSU < 2454832.5000000) { n = 33.0; }
else if (2454832.5000000 < JD_UTCSU && JD_UTCSU < 2455927.5000000) { n = 34.0; }
else if (2455927.5000000 < JD_UTCSU && JD_UTCSU < 2457023.5000000) { n = 35.0; }
else if (2457023.5000000 < JD_UTCSU && JD_UTCSU < 2457754.5000000) { n = 36.0; }
else if (2457754.5000000 < JD_UTCSU) { n = 37.0; }
JD_UTCSU += ((n * 1 - 19) / 3600.0) / 24.0;
double MJD = JD_UTCSU - 2400000.5;
int gpsweek = (int)((MJD - 44244) / 7);
double secofweek = (MJD - 44244.0 - gpsweek * 7.0) * 86400.0;
return secofweek;
}
double UTCTime2JD(int y, int m, int d, double h, int min, double sec)
{
if (m > 2)
{
y = y;
m = m;
}
if (m <= 2)
{
y = y - 1;
m = m + 12;
}
h = h + min / 60.0 + sec / 3600.0;
double JD = (int)(365.25 * y) + (int)(30.6001 * (m + 1)) + d + h / 24.0 + 1720981.5;
return JD;
}