Archived
Modify
This commit is contained in:
KenanZhu111 committed
1 parent
1d5ab4cb1c
commit
dbecbe95cb
26 files changed
+255
-243
No files matched your search
@@ -0,0 +1,34 @@
|
||||
#ifndef _2GPST_H_
|
||||
#define _2GPST_H_
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/// @brief 将世界协调时转换为GPS周内秒
|
||||
/// @param y 年
|
||||
/// @param m 月
|
||||
/// @param d 日
|
||||
/// @param h 时
|
||||
/// @param min 分
|
||||
/// @param sec 秒
|
||||
/// @return 返回GPS周内秒
|
||||
/* -------------------------------------------------------------------------- */
|
||||
double Time2GPSsec(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;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,39 @@
|
||||
#ifndef _BLH2ENU_H_
|
||||
#define _BLH2ENU_H_
|
||||
|
||||
#include<stdlib.h>
|
||||
#include<math.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double E;
|
||||
double N;
|
||||
double U;
|
||||
}blh2enu;
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/// @brief 将大地高坐标转换为站心坐标
|
||||
/// @param blh2enu 自定义结构体类型用来传参
|
||||
/// @param stationB 站心纬度
|
||||
/// @param stationL 站心经度
|
||||
/// @param deltax 卫星到站心的x坐标差值
|
||||
/// @param deltay 卫星到站心的y坐标差值
|
||||
/// @param deltaz 卫星到站心的z坐标差值
|
||||
/// @return 返回自定义结构体类型包含E, N, U成员
|
||||
/* -------------------------------------------------------------------------- */
|
||||
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;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,213 @@
|
||||
#ifndef _CAL_MOD_H_
|
||||
#define _CAL_MOD_H_
|
||||
#include<stdio.h>
|
||||
#include<stdlib.h>
|
||||
#include<math.h>
|
||||
|
||||
#include"public.h"
|
||||
|
||||
#include"read.h"
|
||||
#include"2gpst.h"
|
||||
#include"select_epo.h"
|
||||
#include"xyz2blh.h"
|
||||
#include"degRrad.h"
|
||||
#include"deg2dms.h"
|
||||
#include"blh2enu.h"
|
||||
#include"rahcal.h"
|
||||
|
||||
/* ------------------------------- GPS卫星位置计算函数 ------------------------------ */
|
||||
int gps_pos(int eponum, int sPRN, int best_epoch, double GPSsec,
|
||||
pnav_body nav_b, pobs_head obs_h, ppos_t pos_t)
|
||||
{
|
||||
double T_S = GPSsec - pos_t[eponum].deltat[sPRN];//计算信号发射时刻
|
||||
double n_0 = sqrt(GM) / pow(nav_b[best_epoch].sqrtA,3);
|
||||
double n = n_0 + nav_b[best_epoch].deltan; //计算卫星运行平均角速度
|
||||
double 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;//计算卫星发射时卫星的平近角M
|
||||
double Es = Ms, E0;//迭代计算偏近点角
|
||||
do
|
||||
{
|
||||
E0 = Es;
|
||||
Es = Ms + nav_b[best_epoch].e * sin(Es);
|
||||
} while (fabs(Es - E0) > 1.0e-5);//迭代计算偏近点角
|
||||
//计算真近点角
|
||||
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 = nav_b[best_epoch].OMEGA + (nav_b[best_epoch].deltaomega - Earth_e) * tk - Earth_e * nav_b[best_epoch].TOE;
|
||||
//计算升交点经度
|
||||
double X = x * cos(l) - y * cos(i) * sin(l);
|
||||
double Y = x * sin(l) + y * cos(i) * cos(l);
|
||||
double Z = y * sin(i);//卫星在地固系下的直角坐标
|
||||
pos_t[eponum].X[sPRN] = cos(Earth_e * pos_t[eponum].delta_t[sPRN]) * X + sin(Earth_e * pos_t[eponum].delta_t[sPRN]) * Y;
|
||||
pos_t[eponum].Y[sPRN] = -sin(Earth_e * pos_t[eponum].delta_t[sPRN]) * X + cos(Earth_e * pos_t[eponum].delta_t[sPRN]) * Y;
|
||||
pos_t[eponum].Z[sPRN] = Z;
|
||||
double R = sqrt(pow(pos_t[eponum].X[sPRN] - obs_h->apX, 2) + pow(pos_t[eponum].Y[sPRN] - obs_h->apY, 2) + pow(pos_t[eponum].Z[sPRN] - obs_h->apY, 2));
|
||||
pos_t[eponum].deltat[sPRN] = pos_t[eponum].delta_t[sPRN];
|
||||
pos_t[eponum].delta_t[sPRN] = R / C_V;
|
||||
double rela = 2*sqrt(GM) * nav_b[best_epoch].e * nav_b[best_epoch].sqrtA * sin(Es)/pow(C_V,2);
|
||||
pos_t[eponum].delta_clk[sPRN] = nav_b[best_epoch].sa0 + nav_b[best_epoch].sa1 * tk + nav_b[best_epoch].sa2 * pow(tk,2) - rela;
|
||||
}
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
|
||||
/* ------------------------------- GPS卫星位置计算&输出 ------------------------------ */
|
||||
int sat_gps_pos_clac(FILE * result_file,
|
||||
pnav_body nav_b, pobs_epoch obs_e, pobs_body obs_b,
|
||||
pobs_head obs_h, pstation station, ppos_t pos_t,
|
||||
pblh blh, penu enu, prah rah,
|
||||
int o_epochnum, int gps_satnum)
|
||||
{ int progress = 0;
|
||||
int count = 0;
|
||||
xyz2blh tem1 = {0};
|
||||
xyz2blh tem2 = {0};
|
||||
blh2enu tem3 = {0};
|
||||
rahcal tem4 = {0};
|
||||
deg2dms temb = {0};
|
||||
deg2dms teml = {0};
|
||||
blh = (pblh)malloc(sizeof(pblh) * o_epochnum* 256);
|
||||
enu = (penu)malloc(sizeof(penu) * o_epochnum* 256);
|
||||
rah = (prah)malloc(sizeof(prah) * o_epochnum* 256);
|
||||
pos_t = (ppos_t)malloc(sizeof(ppos_t) * o_epochnum* 256);
|
||||
station = (pstation)malloc(sizeof(pstation) * o_epochnum* 256);
|
||||
for (int i = 0; i < o_epochnum; i++)//第i个历元
|
||||
{
|
||||
result_file = fopen(".\\Pos_out\\LLA_result.txt", "a+");
|
||||
fprintf(result_file, "\n>%04d %04d %02d %02d %02d %02d %07.04f",i + 1 ,obs_e[i].y ,obs_e[i].m ,obs_e[i].d ,obs_e[i].h ,obs_e[i].min ,obs_e[i].sec);
|
||||
fclose(result_file);
|
||||
result_file = fopen(".\\PyGMT_out\\LLA_result.txt", "a+");
|
||||
fprintf(result_file, "\n>%04d %04d %02d %02d %02d %02d %07.04f",i + 1 ,obs_e[i].y ,obs_e[i].m ,obs_e[i].d ,obs_e[i].h ,obs_e[i].min ,obs_e[i].sec);
|
||||
fclose(result_file);
|
||||
result_file = fopen(".\\Result_out\\LLA_result.txt", "a+");
|
||||
fprintf(result_file, "\n>%04d %04d %02d %02d %02d %02d %07.04f",i + 1 ,obs_e[i].y ,obs_e[i].m ,obs_e[i].d ,obs_e[i].h ,obs_e[i].min ,obs_e[i].sec);
|
||||
fclose(result_file);
|
||||
|
||||
|
||||
for (int j = 0; j < obs_e[i].gps_num; j++)//第j颗GPS卫星
|
||||
{
|
||||
int y = obs_e[i].y; int m = obs_e[i].m; int d = obs_e[i].d;
|
||||
double h = obs_e[i].h; int min = obs_e[i].min; double sec = obs_e[i].sec;
|
||||
//转换为GPS周内秒
|
||||
double GPSsec = Time2GPSsec(y, m, d, h, min, sec);
|
||||
int sPRN = obs_e[i].sPRN_GPS[j];
|
||||
//遍历N文件GPS卫星数据块，寻找最佳历元
|
||||
int best_epoch = select_epoch(GPSsec, sPRN, nav_b, gps_satnum);
|
||||
//观测时刻 - 参考时刻
|
||||
double detat_toc = GPSsec - nav_b[best_epoch].TOE;
|
||||
station[i].delta_TR == 0.0;
|
||||
|
||||
//计算近似的信号传播时间,接收机钟差已初始化为0(伪距/光速-接收机钟差+卫星钟差)
|
||||
pos_t[i].delta_t[sPRN] = (obs_b[i].obs_gps[j][C1] / C_V) - station[i].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[i].deltat[sPRN] = 0.0;
|
||||
while (fabs(pos_t[i].delta_t[sPRN] - pos_t[i].deltat[sPRN]) > 0.00001){
|
||||
|
||||
gps_pos( i, sPRN, best_epoch, GPSsec, nav_b, obs_h, pos_t);
|
||||
}
|
||||
|
||||
/* ----------------------------- 地固坐标系转经纬大地高坐标系 ----------------------------- */
|
||||
tem1 = XYZ2BLH( tem1, pos_t[i].X[sPRN], pos_t[i].Y[sPRN], pos_t[i].Z[sPRN]);
|
||||
blh->B = rad2deg(tem1.B);
|
||||
blh->L = rad2deg(tem1.L);
|
||||
blh->H = tem1.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;
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
/* ----------------------------- 经纬大地高坐标系转站心坐标系 ----------------------------- */
|
||||
double deltax = pos_t[i].X[sPRN] - obs_h->apX;
|
||||
double deltay = pos_t[i].Y[sPRN] - obs_h->apY;
|
||||
double deltaz = pos_t[i].Z[sPRN] - obs_h->apZ;
|
||||
|
||||
tem3 = BLH2ENU(tem3, station->B, station->L, deltax, deltay, deltaz);
|
||||
enu->E = tem3.E;
|
||||
enu->N = tem3.N;
|
||||
enu->U = tem3.U;
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
/* ---------------------------- 卫星方位角a，高度角h，向径r计算 --------------------------- */
|
||||
tem4 = RAHCAL(tem4, enu->E, enu->N, enu->U);
|
||||
rah->R = tem4.R;
|
||||
rah->A = rad2deg(tem4.A);
|
||||
rah->H = rad2deg(tem4.H);
|
||||
/* -------------------------------------------------------------------------- */
|
||||
if (blh->H < 0 || nav_b[best_epoch].sHEA != 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
temb = DEG2DMS(temb, blh->B);
|
||||
blh->B_d = temb.D;
|
||||
blh->B_m = temb.M;
|
||||
blh->B_s = temb.S;
|
||||
|
||||
teml = DEG2DMS(teml, blh->L);
|
||||
blh->L_d = teml.D;
|
||||
blh->L_m = teml.M;
|
||||
blh->L_s = teml.S;
|
||||
|
||||
result_file = fopen(".\\Pos_out\\LLA_result.txt", "a+");
|
||||
fprintf(result_file, "\nG%02d[sPRN] %4d° %02d′ %02d″ %4d° %02d′ %02d″ %15.05f",sPRN, blh->B_d, blh->B_m, blh->B_s, blh->L_d, blh->L_m, blh->L_s, blh->H);
|
||||
fclose(result_file);
|
||||
result_file = fopen(".\\PyGMT_out\\LLA_result.txt", "a+");
|
||||
fprintf(result_file, "\nG%02d[sPRN] %10.05f %10.05f %15.05f",sPRN, blh->B, blh->L, blh->H);
|
||||
fclose(result_file);
|
||||
result_file = fopen(".\\Result_out\\LLA_result.txt", "a+");
|
||||
fprintf(result_file, "\nG%02d[sPRN] %15.05f %15.05f %15.05f %15.05f %15.05f %15.05f %15.12f %16.13f"
|
||||
,sPRN, pos_t[i].X[sPRN], pos_t[i].Y[sPRN], pos_t[i].Z[sPRN], obs_b[i].obs_gps[j][C1], obs_b[i].obs_gps[j][10], rah->H, nav_b[best_epoch].TGD, pos_t[i].delta_clk[sPRN]);
|
||||
fclose(result_file);
|
||||
}
|
||||
|
||||
}
|
||||
result_file = fopen(".\\Result_out\\LLA_result.txt", "a+");
|
||||
fprintf(result_file, "\n");
|
||||
fclose(result_file);
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
//const char * progressstr = "[----------------------------------------------------]100%%";
|
||||
|
||||
int progress_now = (int)(((i+1)*100/2880));
|
||||
if ((progress_now - progress)>=2)
|
||||
{
|
||||
printf("=");
|
||||
count++;
|
||||
progress = progress_now;
|
||||
printf("=]%3d%%", progress);
|
||||
for(int countb = 0; countb < (5); countb++)
|
||||
{
|
||||
printf("\b");
|
||||
}
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
}
|
||||
|
||||
result_file = fopen(".\\Result_out\\LLA_result.txt", "a+");
|
||||
fprintf(result_file, "\nEND");
|
||||
fclose(result_file);
|
||||
free(blh);blh = NULL;
|
||||
free(enu);enu = NULL;
|
||||
free(rah);rah = NULL;
|
||||
free(pos_t);pos_t = NULL;
|
||||
free(station);station = NULL;
|
||||
return 1;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,33 @@
|
||||
#ifndef _DEG2DMS_H_
|
||||
#define _DEG2DMS_H_
|
||||
|
||||
#include<stdlib.h>
|
||||
#include<math.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double D;
|
||||
double M;
|
||||
double S;
|
||||
}deg2dms;
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/// @brief 将十进制的度数转换为度分秒形式
|
||||
/// @param deg2dms 结构体传递参数
|
||||
/// @param D 要转换度数
|
||||
/// @return 返回自定义结构体包含度，分，秒成员
|
||||
/* -------------------------------------------------------------------------- */
|
||||
deg2dms DEG2DMS(deg2dms deg2dms,
|
||||
double D)
|
||||
{
|
||||
int sig = D / fabs(D);//符号判断
|
||||
deg2dms.D = (int)(D);//转换为度分秒
|
||||
deg2dms.M = (int)((D - deg2dms.D) * 60);//分秒去除符号格式
|
||||
deg2dms.S = (int)(((D - deg2dms.D) * 60 - deg2dms.M) * 60);
|
||||
deg2dms.M = deg2dms.M * sig;
|
||||
deg2dms.S = deg2dms.S * sig;
|
||||
|
||||
return deg2dms;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,28 @@
|
||||
#ifndef _DEGRRAD_H_
|
||||
#define _DEGRRAD_H_
|
||||
|
||||
#include"public.h"
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/// @brief 角度转弧度
|
||||
/// @param deg
|
||||
/// @return 返回弧度
|
||||
/* -------------------------------------------------------------------------- */
|
||||
double deg2rad(double deg)
|
||||
{
|
||||
double rad = (PI / 180) * deg;
|
||||
return rad;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/// @brief 弧度转角度
|
||||
/// @param rad
|
||||
/// @return 返回角度
|
||||
/* -------------------------------------------------------------------------- */
|
||||
double rad2deg(double rad)
|
||||
{
|
||||
double deg = (180 / PI) * rad;
|
||||
return deg;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,227 @@
|
||||
#ifndef _PUBLIC_H_
|
||||
#define _PUBLIC_H_
|
||||
|
||||
/* ------------------------------ WGS-84 CONST ------------------------------ */
|
||||
#define a 6378137.0
|
||||
#define e2 (0.0033528106647475*(2 - 0.0033528106647475))
|
||||
#define C_V 299792458
|
||||
#define GM 398600500000000
|
||||
#define math_e 2.718281828459
|
||||
#define PI 3.141592653589793
|
||||
#define Earth_e 7.2921151467e-5
|
||||
#define C1 0
|
||||
/* ----------------------------- GPS OBSERV TYPE ---------------------------- */
|
||||
#define C1C 101
|
||||
#define C1S 102
|
||||
#define C1L 103
|
||||
#define C1X 104
|
||||
#define C1P 105
|
||||
#define C1W 106
|
||||
#define C1Y 107
|
||||
#define C1M 108
|
||||
|
||||
#define C2C 201
|
||||
#define C2S 202
|
||||
#define C2L 203
|
||||
#define C2X 204
|
||||
#define C2P 205
|
||||
#define C2W 206
|
||||
#define C2Y 207
|
||||
#define C2M 208
|
||||
|
||||
#define C5I 501
|
||||
#define C5Q 502
|
||||
#define C5X 503
|
||||
/* ------------------------------ FOR CALMOD.H ------------------------------ */
|
||||
#define MAXRINEX 1000
|
||||
//卫星位置结构体
|
||||
typedef struct
|
||||
{
|
||||
double X[36];
|
||||
double Y[36];
|
||||
double Z[36];
|
||||
double deltat[36];//改正前的信号传播时间
|
||||
double delta_t[36];//改正后的信号传播时间
|
||||
double delta_clk[36];
|
||||
}pos_t, *ppos_t;
|
||||
|
||||
//测站位置结构体
|
||||
typedef struct
|
||||
{
|
||||
double X;
|
||||
double Y;
|
||||
double Z;
|
||||
double B;
|
||||
double L;
|
||||
double H;
|
||||
double delta_TR;
|
||||
}station, *pstation;
|
||||
//经纬高结构体
|
||||
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;
|
||||
}blh, *pblh;
|
||||
//东北天结构体
|
||||
typedef struct enu
|
||||
{
|
||||
double E;
|
||||
double N;
|
||||
double U;
|
||||
}enu, *penu;
|
||||
//高度方位角结构体
|
||||
typedef struct rah
|
||||
{
|
||||
double R;
|
||||
double A;
|
||||
double H;
|
||||
}rah, *prah;
|
||||
/* ------------------------------- FOR READ.H ------------------------------- */
|
||||
//导航头文件结构体
|
||||
typedef struct nav_head
|
||||
{
|
||||
double ver;//rinex 版本号
|
||||
char type[20];//读取的数据类型
|
||||
double ION_GPSA[4];//8个电离层参数
|
||||
double ION_GPSB[4];
|
||||
double ION_BDSA[4];//8个电离层参数
|
||||
double ION_BDSB[4];
|
||||
double ION_QZSA[4];//8个电离层参数
|
||||
double ION_QZSB[4];
|
||||
double ION_IRNA[4];//8个电离层参数
|
||||
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;
|
||||
}nav_head, *pnav_head;
|
||||
|
||||
//导航数据结构体
|
||||
typedef struct nav_body
|
||||
{
|
||||
//数据块第一行内容：
|
||||
int sPRN;//卫星PRN号
|
||||
//历元：TOC中卫星钟的参考时刻
|
||||
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 IODE;//数据、星历发布时间(数据期龄)
|
||||
double Crs;//轨道半径的正弦调和改正项的振幅（单位：m）
|
||||
double deltan;//卫星平均运动速率与计算值之差(rad/s)
|
||||
double M0;//参考时间的平近点角(rad)
|
||||
|
||||
//数据块第三行内容：
|
||||
double Cuc;//维度幅角的余弦调和改正项的振幅(rad)
|
||||
double e;//轨道偏心率
|
||||
double Cus;//轨道幅角的正弦调和改正项的振幅(rad)
|
||||
double sqrtA;//长半轴平方根
|
||||
|
||||
//数据块第四行内容：
|
||||
double TOE;//星历的参考时刻(GPS周内秒)
|
||||
double Cic;//轨道倾角的余弦调和改正项的振幅(rad)
|
||||
double OMEGA;//参考时刻的升交点赤经
|
||||
double Cis;//维度倾角的正弦调和改正项的振幅(rad)
|
||||
|
||||
//数据块第五行内容：
|
||||
double i0;//参考时间的轨道倾角(rad)
|
||||
double Crc;//轨道平径的余弦调和改正项的振幅(m)
|
||||
double omega;//近地点角距
|
||||
double deltaomega;//升交点赤经变化率(rad)
|
||||
|
||||
//数据块第六行内容：
|
||||
double IDOT;//近地点角距(rad/s)
|
||||
double L2code;//L2上的码
|
||||
double GPSweek;//GPS周,于TOE一同表示
|
||||
double L2Pflag;//L2,p码数据标记
|
||||
|
||||
//数据块第七行内容
|
||||
double sACC;//卫星精度
|
||||
double sHEA;//卫星健康状态
|
||||
double TGD;//sec
|
||||
double IODC;//钟的数据龄期
|
||||
|
||||
//数据块第八行内容
|
||||
double TTN;//电文发送时间
|
||||
double fit;//拟合区间
|
||||
double spare1;//空
|
||||
double spare2;//空
|
||||
|
||||
}nav_body, *pnav_body;
|
||||
|
||||
//观测值头文件结构体
|
||||
typedef struct obs_head
|
||||
{
|
||||
double ver;//RINEX文件版本号
|
||||
char type[30];//文件类型
|
||||
double apX;//测站近似位置XYZ（WGS84）
|
||||
double apY;
|
||||
double apZ;
|
||||
double ANTH;//天线高
|
||||
double ANTdeltaE;//天线中心对于测站标志在东方向上的偏移量
|
||||
double ANTdeltaN;//天线中心对于测站标志在北方向上的偏移量
|
||||
int WAFL1;//缺省的L1载波的波长因子
|
||||
int WAFL2;//缺省的L2载波的波长因子
|
||||
int WAFflag;
|
||||
int obstypenum_gps;//GPS观测值类型数量
|
||||
int obstypenum_bds;
|
||||
char obstype_gps[36];//rinex3.04GPS观测值类型列表
|
||||
char obstype_bds[36];
|
||||
double interval;//观测值的历元间隔
|
||||
int f_y;//第一个观测记录的时刻
|
||||
int f_m;
|
||||
int f_d;
|
||||
int f_h;
|
||||
int f_min;
|
||||
double f_sec;
|
||||
char tsys[5];//时间系统
|
||||
}obs_head,*pobs_head;
|
||||
|
||||
|
||||
//观测值历元数据结构体
|
||||
typedef struct obs_epoch
|
||||
{
|
||||
//观测历元时刻
|
||||
int y;
|
||||
int m;
|
||||
int d;
|
||||
int h;
|
||||
int min;
|
||||
double sec;
|
||||
int p_flag;//历元标志
|
||||
int sat_num;//当前历元所观测到的卫星数量
|
||||
int gps_num;//当前历元所观测到的GPS卫星数量
|
||||
int bds_num;//当前历元所观测到的BDS卫星数量
|
||||
int sPRN[84];//当前历元所能观测到的卫星的PRN列表
|
||||
int sPRN_GPS[25];
|
||||
int sPRN_BDS[36];
|
||||
}obs_epoch,*pobs_epoch;
|
||||
|
||||
|
||||
//观测值数据结构体
|
||||
typedef struct obs_body
|
||||
{
|
||||
double obs_gps[100][24];//GPS观测值
|
||||
double obs_bds[100][24];//BDS观测值
|
||||
}obs_body,*pobs_body;
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,38 @@
|
||||
#ifndef _RAHCAL_H_
|
||||
#define _RAHCAL_H_
|
||||
|
||||
#include<stdlib.h>
|
||||
#include<math.h>
|
||||
|
||||
#include"public.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double R;
|
||||
double A;
|
||||
double H;
|
||||
}rahcal;
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/// @brief 计算站心坐标系下的卫星高度方位角
|
||||
/// @param rahcal 自定义结构体变量用于传参
|
||||
/// @param E 站心坐标东
|
||||
/// @param N 站心坐标北
|
||||
/// @param U 站心坐标天
|
||||
/// @return 返回自定义结构体变量包含R, A, H成员
|
||||
/* -------------------------------------------------------------------------- */
|
||||
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;
|
||||
}
|
||||
|
||||
#endif
|
||||
+450
@@ -0,0 +1,450 @@
|
||||
#ifndef _READ_H_
|
||||
#define _READ_H_
|
||||
|
||||
#include<stdio.h>
|
||||
#include<string.h>
|
||||
|
||||
#include"public.h"
|
||||
/* ------------------------ 将字符串转换为浮点数,i起始位置，n输入多少个字符 ----------------------- */
|
||||
static double strtonum(const char* buff, int i, int n)
|
||||
{
|
||||
double value = 0.0;
|
||||
char str[256] = { 0 };
|
||||
char* p = str;
|
||||
/************************************
|
||||
* 当出现以下三种情况报错，返回0.0
|
||||
* 1.起始位置<0
|
||||
* 2.读取字符串个数<i
|
||||
* 3.str里面存放的字节数<n
|
||||
*************************************/
|
||||
if (i < 0 || (int)strlen(buff) < i || (int)sizeof(str) - 1 < n)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
for (buff += i; *buff && --n >= 0; buff++)
|
||||
{
|
||||
//三目操作符：D和d为文件中科学计数法部分，将其转换成二进制能读懂的e
|
||||
*p++ = ((*buff == 'D' || *buff == 'd') ? 'e' : *buff);
|
||||
}
|
||||
*p = '\0';
|
||||
//三目操作符，将str中存放的数以格式化读取到value中。
|
||||
return sscanf(str, "%lf", &value) == 1 ? value : 0.0;
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/* 读取O文件
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
/* -------------------------------- 获取O文件历元数 -------------------------------- */
|
||||
extern int get_epochnum(FILE* fp_obs)
|
||||
{
|
||||
int n = 0;//记录历元数
|
||||
int satnum = 0;//记录每个历元的卫星数
|
||||
char flag;//存放标志符号'>'
|
||||
char buff[MAXRINEX];//存放读取的字符串
|
||||
while (fgets(buff, MAXRINEX, fp_obs))
|
||||
{
|
||||
satnum = (int)strtonum(buff, 33, 2);
|
||||
strncpy(&flag, buff + 0, 1);
|
||||
if (flag == '>')
|
||||
{
|
||||
n++;
|
||||
}
|
||||
}
|
||||
return n;
|
||||
}
|
||||
|
||||
/* -------------------------------- 读取O文件数据头 -------------------------------- */
|
||||
extern 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;
|
||||
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, "WAVELENGTH FACT L1/2"))
|
||||
{
|
||||
obs_h->WAFL1 = (int)strtonum(buff, 0, 6);
|
||||
obs_h->WAFL2 = (int)strtonum(buff, 6, 6);
|
||||
obs_h->WAFflag = (int)strtonum(buff, 6 + 6, 6);
|
||||
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++)
|
||||
{
|
||||
strncpy(&(obs_h->obstype_gps[i]), buff + 7 + 4 * i, 3);
|
||||
}
|
||||
}
|
||||
else if (obs_h->obstypenum_gps > 13)
|
||||
{
|
||||
for (i = 0; i < 13; i++)
|
||||
{
|
||||
strncpy(&(obs_h->obstype_gps[i]), buff + 7 + 4 * i, 3);
|
||||
}
|
||||
fgets(buff, MAXRINEX, fp_obs);
|
||||
for (i = 0; i < obs_h->obstypenum_gps - 13; i++)
|
||||
{
|
||||
strncpy(&(obs_h->obstype_gps[i + 13]), buff + 7 + 4 * i, 3);
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
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++)
|
||||
{
|
||||
strncpy(&(obs_h->obstype_bds[i]), buff + 7 + 4 * i, 3);
|
||||
}
|
||||
}
|
||||
else if (obs_h->obstypenum_bds > 13)
|
||||
{
|
||||
for (i = 0; i < 13; i++)
|
||||
{
|
||||
strncpy(&(obs_h->obstype_bds[i]), buff + 7 + 4 * i, 3);
|
||||
}
|
||||
fgets(buff, MAXRINEX, fp_obs);
|
||||
for (i = 0; i < obs_h->obstypenum_bds - 13; i++)
|
||||
{
|
||||
strncpy(&(obs_h->obstype_bds[i + 13]), buff + 7 + 4 * i, 3);
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
}
|
||||
else if (strstr(lable, "INTERVAL"))
|
||||
{
|
||||
obs_h->interval = strtonum(buff, 0, 11);
|
||||
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, "END OF HEADER"))
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* -------------------------------- 读取O文件数据块 -------------------------------- */
|
||||
extern void read_o_b(FILE* fp_obs, pobs_epoch obs_e, pobs_body obs_b, int type_gps, int type_bds)
|
||||
{
|
||||
int n = 0;//历元数
|
||||
char buff[MAXRINEX] = { 0 };
|
||||
char flag = { 0 };//判断符号
|
||||
while (fgets(buff, MAXRINEX, fp_obs))
|
||||
{
|
||||
int i = 0;//第i颗卫星
|
||||
int j_g = 0;//第j颗GPS卫星
|
||||
int k_g = 0;//第j颗GPS卫星的第k个观测值
|
||||
int j_c = 0;//第j颗BDS卫星
|
||||
int k_c = 0;//第j颗BDS卫星的第k个观测值
|
||||
obs_e[n].gps_num = 0;
|
||||
obs_e[n].bds_num = 0;
|
||||
|
||||
//按照格式将历元参考时间依次存入
|
||||
//除秒以外，均转换成整型
|
||||
obs_e[n].y = (int)strtonum(buff, 2, 4);
|
||||
obs_e[n].m = (int)strtonum(buff, 7, 2);
|
||||
obs_e[n].d = (int)strtonum(buff, 10, 2);
|
||||
obs_e[n].h = (int)strtonum(buff, 13, 2);
|
||||
obs_e[n].min = (int)strtonum(buff, 16, 2);
|
||||
obs_e[n].sec = strtonum(buff, 19, 10);
|
||||
obs_e[n].p_flag = (int)strtonum(buff, 31, 1);
|
||||
//输入卫星数
|
||||
obs_e[n].sat_num = strtonum(buff, 33, 3);
|
||||
strncpy(&flag, buff + 0, 1);
|
||||
|
||||
for (i = 0; i < obs_e[n].sat_num; i++)
|
||||
{
|
||||
//判断是否为GPS卫星
|
||||
if (flag == 'G')//获取flag标识，判断GPS卫星为真，读取该卫星PRN号
|
||||
{
|
||||
obs_e[n].sPRN[i] = strtonum(buff, 1, 2);//将卫星编号存储到第n个历元中，第i个PRN位置
|
||||
obs_e[n].sPRN_GPS[j_g] = obs_e[n].sPRN[i];//将GPS卫星编号与总观测卫星号关联
|
||||
for (k_g = 0; k_g < type_gps; k_g++)
|
||||
{
|
||||
obs_b[n].obs_gps[j_g][k_g] = strtonum(buff, 3 + 16 * k_g, 14);
|
||||
}
|
||||
obs_e[n].gps_num++; j_g++;
|
||||
fgets(buff, MAXRINEX, fp_obs);
|
||||
strncpy(&flag, buff + 0, 1);
|
||||
}
|
||||
|
||||
else if (flag != 'G')//获取flag标识，判断GPS卫星为否
|
||||
{
|
||||
//判断是否为BDS卫星
|
||||
if (flag == 'C')//获取flag标识，判断BDS卫星为真，读取该卫星PRN号
|
||||
{
|
||||
obs_e[n].sPRN[i] = strtonum(buff, 1, 2);//将卫星编号存储到第n个历元中，第i个PRN位置
|
||||
obs_e[n].sPRN_BDS[j_c] = obs_e[n].sPRN[i];//将BDS卫星编号与总观测卫星号关联
|
||||
for (k_c = 0; k_c < type_bds; k_c++)
|
||||
{
|
||||
obs_b[n].obs_bds[j_c][k_c] = strtonum(buff, 3 + 16 * k_c, 14);
|
||||
}
|
||||
obs_e[n].bds_num++; j_c++;
|
||||
fgets(buff, MAXRINEX, fp_obs);
|
||||
strncpy(&flag, buff + 0, 1);
|
||||
}
|
||||
|
||||
else if (flag != 'C')//获取flag标识，判断BDS卫星为真，读取该卫星PRN号
|
||||
{
|
||||
fgets(buff, MAXRINEX, fp_obs);
|
||||
strncpy(&flag, buff + 0, 1);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
n++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/* 读取N文件
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
/* ---------------------- 获取文件数据块行数，从END OF HEADER后开始起算 --------------------- */
|
||||
extern int getgpssatnum(FILE* fp_nav)
|
||||
{
|
||||
int gps_satnum = 0;
|
||||
int flag = 0;
|
||||
char buff[MAXRINEX];//用来存放读取到的字符串
|
||||
char satvar;
|
||||
char* lable = buff + 60;
|
||||
//fgets函数，读取一行，当读取结束后返回NULL指针，格式如下：
|
||||
//char * fgets ( char * str, int num, FILE * stream );
|
||||
while (fgets(buff, MAXRINEX, fp_nav))
|
||||
{
|
||||
if (flag == 1)
|
||||
{
|
||||
while (fgets(buff, MAXRINEX, fp_nav))
|
||||
{
|
||||
strncpy(&satvar, buff + 0, 1);
|
||||
if (satvar != 'G')
|
||||
{
|
||||
continue;
|
||||
}
|
||||
else if (satvar == 'G')
|
||||
{
|
||||
gps_satnum++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (strstr(lable, "END OF HEADER"))
|
||||
{
|
||||
flag = 1;
|
||||
}
|
||||
}
|
||||
return gps_satnum;
|
||||
}
|
||||
|
||||
/* -------------------------------- 读取N文件数据头 -------------------------------- */
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
/* -------------------------------- 读取N文件数据块 -------------------------------- */
|
||||
void read_n_b(FILE* fp_nav, pnav_body nav_b)
|
||||
{
|
||||
int i_g = 0;//第i_g个GPS卫星数据
|
||||
int i_c = 0;//第i_c个BDS卫星数据
|
||||
char buff[84] = { 0 };
|
||||
char flag = { 0 };//判断符号
|
||||
while (fgets(buff, MAXRINEX, fp_nav))
|
||||
{
|
||||
int j_g = 0;//数据块的第j_g行数据
|
||||
int j_c = 0;//数据块的第j_C行数据
|
||||
strncpy(&flag, buff + 0, 1);
|
||||
if(flag == 'G')
|
||||
{
|
||||
for (j_g = 0; j_g < 8; j_g++)//读取第i_g颗 GPS卫星广播星历("总共包含8行数据") 数据块的第j_g行数据
|
||||
{
|
||||
switch (j_g)
|
||||
{
|
||||
case 0:
|
||||
nav_b[i_g].sPRN = (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;
|
||||
}
|
||||
else if(flag != 'G')
|
||||
{
|
||||
fgets(buff, MAXRINEX, fp_nav);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
/* -------------------------------------------------------------------------- */
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,37 @@
|
||||
#ifndef _SELECT_EPO_H_
|
||||
#define _SELECT_EPO_H_
|
||||
|
||||
#include<math.h>
|
||||
|
||||
#include"public.h"
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/// @brief 寻找与观测历元最近的卫星广播历元
|
||||
/// @param SecofWeek 计算得到的观测历元的周内秒
|
||||
/// @param sPRN 需要匹配的该历元卫星号
|
||||
/// @param nav_b 导航文件数据块结构体，与卫星号匹配
|
||||
/// @param gps_satnum 该历元的GPS卫星数量
|
||||
/// @return 返回最佳历元的绝对历元序号
|
||||
/* -------------------------------------------------------------------------- */
|
||||
extern int select_epoch(double SecofWeek, int sPRN, pnav_body nav_b, int gps_satnum)
|
||||
{
|
||||
int best_epoch = 0;
|
||||
double min = 10000;//初始化最小值
|
||||
double Min;
|
||||
int i;
|
||||
for (i = 0; i < gps_satnum; i++)
|
||||
{
|
||||
if (sPRN == nav_b[i].sPRN)
|
||||
{
|
||||
Min = fabs(SecofWeek - nav_b[i].TOE);
|
||||
if (Min <= min)
|
||||
{
|
||||
best_epoch = i;
|
||||
min = Min;
|
||||
}
|
||||
}
|
||||
}
|
||||
return best_epoch;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,137 @@
|
||||
#ifndef _TERMINAL_H_
|
||||
#define _TREMINAL_H_
|
||||
|
||||
#include<stdio.h>
|
||||
#include<stdlib.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
char obs_file[20];
|
||||
char nav_file[20];
|
||||
}filename, *pfilename;
|
||||
|
||||
|
||||
int Begin_inTermianl()
|
||||
{
|
||||
int langop;
|
||||
printf("|**************SPP-pseudorange-positioning*****************************************|\n");
|
||||
printf("**@Author: KenanZhu\n**@Date: 2024/09/21\n**@Make by: C & Python & Matlab\n");
|
||||
printf("**@INTRO: \n**The main functions of the software include:\n");
|
||||
printf("**read RINEX3.04 files, and output three formats of document files,\n");
|
||||
printf("**please cooperate with the corresponding tools for processing.\n");
|
||||
printf("**Specific please see:\n https://github.com/KenanZhu111/SPP_-pseudorange-positioning\n");
|
||||
printf("|**********************************************************************************|\n\n");
|
||||
|
||||
printf("|>>> 请选择您的语言:/Please choose your language <<<|\n--------------------\n[1 | ENG][0 | 中文]\n--------------------\n");
|
||||
scanf("%d",&langop);
|
||||
|
||||
return langop;
|
||||
}
|
||||
|
||||
pfilename InO_inTermianl(int langop, pfilename filename)
|
||||
{
|
||||
filename = (pfilename)malloc(sizeof(pfilename));
|
||||
if (langop == 0)
|
||||
{
|
||||
printf("|>>> 本程序只支持 ver3.04 RINEX文件的读取 <<<|\n\n");
|
||||
printf("|>> 请输入观测数据文件的文件名（包含拓展名）：\n");
|
||||
scanf("%s", &filename->obs_file);
|
||||
}
|
||||
if (langop == 1)
|
||||
{
|
||||
printf("|>>> This program only supports reading ver3.04 RINEX files <<<|\n\n");
|
||||
printf("|>> Please enter the file name (including the extension) of the observation data file.:\n");
|
||||
scanf("%s", &filename->obs_file);
|
||||
}
|
||||
return filename;
|
||||
}
|
||||
|
||||
void ReadO_inTerminal(FILE *fp_obs, int langop, char obs_file[20])
|
||||
{
|
||||
if (langop == 0 && fp_obs == NULL)
|
||||
{
|
||||
printf("|>> 观测文件%s读取失败,请检查文件名或者确认文件是否被移动\n\n", obs_file);
|
||||
}
|
||||
else if (langop == 0 && fp_obs != NULL)
|
||||
{
|
||||
printf("|>> 观测文件%s读取成功,正在处理数据...\n\n", obs_file);
|
||||
}
|
||||
|
||||
if (langop == 1 && fp_obs == NULL)
|
||||
{
|
||||
printf("|>> Observation file %s failed to be read. Please check the file name or confirm whether the file has been moved\n\n", obs_file);
|
||||
}
|
||||
else if (langop == 1 && fp_obs != NULL)
|
||||
{
|
||||
printf("|>> The observation file %s has been read successfully and data is being processed...\n\n", obs_file);
|
||||
}
|
||||
}
|
||||
|
||||
pfilename OutO_InN_inTermianl(FILE *fp_obs, int langop, pfilename filename)
|
||||
{
|
||||
if (langop == 0 && fp_obs != NULL)
|
||||
{
|
||||
printf("|>>> 观测文件读取完成! <<<|\n\n\n");
|
||||
printf("|>> 请输入导航文件的文件名（包含拓展名）：\n\n");
|
||||
scanf("%s", &filename->nav_file);
|
||||
}
|
||||
if (langop == 1 && fp_obs != NULL)
|
||||
{
|
||||
printf("|>>> Observation file read complete! <<<|\n\n\n");
|
||||
printf("|>> Please enter the file name (including the extension) of the Navigation data file.:\n\n");
|
||||
scanf("%s", &filename->nav_file);
|
||||
}
|
||||
return filename;
|
||||
}
|
||||
|
||||
void ReadN_inTerminal(FILE *fp_nav, FILE *fp_obs, int langop, char nav_file[20])
|
||||
{
|
||||
if (langop == 0 && fp_obs != NULL && fp_nav == NULL)
|
||||
{
|
||||
printf("|>> 导航文件%s读取失败,请检查文件名或者确认文件是否被移动\n\n", nav_file);
|
||||
}
|
||||
else if (langop == 0 && fp_obs != NULL && fp_nav != NULL)
|
||||
{
|
||||
printf("|>> 观测文件%s读取成功,正在处理数据...\n\n", nav_file);
|
||||
}
|
||||
|
||||
if (langop == 1 && fp_obs != NULL && fp_nav == NULL)
|
||||
{
|
||||
printf("|>> Navigation file %s failed to be read. Please check the file name or confirm whether the file has been moved\n\n", nav_file);
|
||||
}
|
||||
else if (langop == 1 && fp_obs != NULL && fp_nav != NULL)
|
||||
{
|
||||
printf("|>> The Navigation file %s has been read successfully and data is being processed...\n\n", nav_file);
|
||||
}
|
||||
}
|
||||
|
||||
void OutN_inTerminal(FILE *fp_nav, FILE *fp_obs, int langop)
|
||||
{
|
||||
if (langop == 0 && fp_obs != NULL && fp_nav != NULL)
|
||||
{
|
||||
printf("|>>> 导航文件读取完成! <<<|\n\n");printf("|>> 正在计算数据:[=");
|
||||
}
|
||||
if (langop == 1 && fp_obs != NULL && fp_nav != NULL)
|
||||
{
|
||||
printf("|>>> Navigation file read complete! <<<|\n\n");printf("|>> Calculating data:[=");
|
||||
}
|
||||
}
|
||||
|
||||
void End_inTerminal(FILE *fp_nav, FILE *fp_obs, int langop, int over)
|
||||
{
|
||||
if (langop == 0 && fp_nav != NULL && fp_obs != NULL)
|
||||
{ if (over == 1)
|
||||
{printf("\n数据生成完毕!");}
|
||||
if (over != 1)
|
||||
{printf("\n数据处理发生错误!");}
|
||||
}
|
||||
if (langop == 1 && fp_nav != NULL && fp_obs != NULL)
|
||||
{
|
||||
if (over == 1)
|
||||
{printf("\nData generation completed!");}
|
||||
if (over != 1)
|
||||
{printf("\nData processing error!");}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,53 @@
|
||||
#ifndef _XYZ2BLH_H_
|
||||
#define _XYZ2BLH_H_
|
||||
|
||||
#include<stdlib.h>
|
||||
#include<math.h>
|
||||
|
||||
#include"public.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double B;
|
||||
double L;
|
||||
double H;
|
||||
}xyz2blh;
|
||||
|
||||
/* -------------------------------------------------------------------------- */
|
||||
/// @brief 将ECEF地心地固坐标转换为经纬高坐标
|
||||
/// @param xyz2blh 自定义结构体类型，用来传参
|
||||
/// @param X ECEF的X坐标
|
||||
/// @param Y ECEF的Y坐标
|
||||
/// @param Z ECEF的Z坐标
|
||||
/// @param a 椭球的长半轴
|
||||
/// @param e2 椭球的第一偏心率平方
|
||||
/// @return 返回自定义结构体类型包含B，L，H成员
|
||||
/* -------------------------------------------------------------------------- */
|
||||
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;
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in new issue
Block a user