Archived
515 lines
18 KiB
C++
515 lines
18 KiB
C++
#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);
|
|
}
|
|
|
|
pz90towgs84(X[0], X[1], X[2]);
|
|
|
|
pos_t.X = X[0];
|
|
pos_t.Y = X[1];
|
|
pos_t.Z = X[2];
|
|
|
|
R = sqrt(pow(pos_t.X - obs_h->apX, 2) +
|
|
pow(pos_t.Y - obs_h->apY, 2) +
|
|
pow(pos_t.Z - obs_h->apZ, 2));
|
|
pos_t.deltat = pos_t.delta_t;
|
|
pos_t.delta_t = R / C_V - pos_t.delta_clk;
|
|
|
|
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 GNSS,
|
|
pnav_body nav_b, pobs_head obs_h, pos_ts pos_t)
|
|
{
|
|
double T_S = Weeksec - pos_t.delta_t;
|
|
double n_0 = sqrt(GM) / pow(nav_b[best_epoch].sqrtA, 3);
|
|
double n = n_0 + nav_b[best_epoch].deltan;
|
|
double tk;
|
|
|
|
if (GNSS == BDS) tk = T_S - nav_b[best_epoch].TOE - 14;//BDST = GPST + 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 (GNSS == BDS && (sPRN <= 5 || sPRN >= 59)) {//BDS GEO Satellites
|
|
|
|
l = nav_b[best_epoch].OMEGA + nav_b[best_epoch].deltaomega * tk - Earth_e * nav_b[best_epoch].TOE;
|
|
X = x * cos(l) - y * cos(i) * sin(l);
|
|
Y = x * sin(l) + y * cos(i) * cos(l);
|
|
Z = y * sin(i);
|
|
|
|
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=0.0;
|
|
if (GNSS == GPS) TGD = nav_b[best_epoch].TGD;
|
|
else if (GNSS == GAL) TGD = nav_b[best_epoch].BGDa;
|
|
else if (GNSS == BDS) TGD = nav_b[best_epoch].TGD1;
|
|
|
|
//Recalculate the diatance of satellite-receiver
|
|
double R = sqrt(pow(pos_t.X - obs_h->apX, 2) +
|
|
pow(pos_t.Y - obs_h->apY, 2) +
|
|
pow(pos_t.Z - obs_h->apZ, 2));
|
|
//Get relativistic effect correction
|
|
double rela = 2 * sqrt(GM) * nav_b[best_epoch].e * nav_b[best_epoch].sqrtA * sin(Es) / pow(C_V, 2);
|
|
|
|
pos_t.delta_clk = nav_b[best_epoch].sa0 +
|
|
nav_b[best_epoch].sa1 * tk +
|
|
nav_b[best_epoch].sa2 * pow(tk, 2) -
|
|
rela + TGD;
|
|
pos_t.deltat = pos_t.delta_t;
|
|
pos_t.delta_t = R / C_V - pos_t.delta_clk;
|
|
|
|
return pos_t;
|
|
}
|
|
/**
|
|
* .
|
|
*
|
|
* \param obs_h: Observation header data
|
|
* \param obs_e: Observation epoch data
|
|
* \param obs_b: Observation data
|
|
* \param nav_b: Navigation data
|
|
* \param res_file: The file path of result file of satellite position
|
|
* \param GNSS: GNSS system code
|
|
* \param satnum: The total number of satellite of designated satellite system
|
|
* \return
|
|
*/
|
|
int sat_pos_cal(pobs_head obs_h, pobs_epoch obs_e, pobs_body obs_b,
|
|
pnav_body nav_b,
|
|
FILE* solu,
|
|
int GNSS, int satnum, int Hangle, int ionopt, int troopt)
|
|
{
|
|
int y, m, d, min, epochcount, sat_num = 0, sPRN = 0;
|
|
double h, sec, detat_toc, beginsec, endinsec, Weeksec, PERS = 0;
|
|
double apX, apY, apZ, ap_X, ap_Y, ap_Z;
|
|
|
|
stations sta = { 0 };rah in_rah = { 0 };
|
|
enu in_enu = { 0 };blh in_blh = { 0 };
|
|
lsq_part in_lsq = { 0 };pos_ts pos_t = { 0 };
|
|
acct_obs obs = { 0 };
|
|
|
|
//the beginning second of observation
|
|
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
|
|
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);
|
|
|
|
y = obs_e->y;m = obs_e->m;d = obs_e->d;
|
|
h = obs_e->h;min = obs_e->min;sec = obs_e->sec;
|
|
|
|
Weeksec = time2gpst(y, m, d, h, min, sec);
|
|
epochcount = round((Weeksec - beginsec) / obs_h->interval) + 1;
|
|
fprintf(solu, "\n>%05d lat/r lon/r alt/m", epochcount);
|
|
|
|
//Match the system and assign the satellite number
|
|
if (GNSS == GPS)sat_num = obs_e->gps_num;
|
|
else if (GNSS == GAL)sat_num = obs_e->gal_num;
|
|
else if (GNSS == BDS)sat_num = obs_e->bds_num;
|
|
else if (GNSS == GLO)sat_num = obs_e->glo_num;
|
|
|
|
in_lsq.l = mat(sat_num, 1);
|
|
in_lsq.m = mat(sat_num, 1);
|
|
in_lsq.n = mat(sat_num, 1);
|
|
in_lsq.o = mat(sat_num, 1); int k = 0;
|
|
|
|
for (int j = 0; j < sat_num; j++) {
|
|
//Match system and assign the sPRN
|
|
if (GNSS == GPS) {
|
|
sPRN = obs_e->sPRN_GPS[j];
|
|
PERS = obs_b->obs_gps[j][
|
|
code2type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)] / C_V;}
|
|
else if (GNSS == GAL) {
|
|
sPRN = obs_e->sPRN_GAL[j];
|
|
PERS = obs_b->obs_gal[j][
|
|
code2type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)] / C_V;}
|
|
else if (GNSS == BDS) {
|
|
sPRN = obs_e->sPRN_BDS[j];
|
|
PERS = obs_b->obs_bds[j][
|
|
code2type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)] / C_V;}
|
|
else if (GNSS == GLO) {
|
|
sPRN = obs_e->sPRN_GLO[j];
|
|
PERS = obs_b->obs_glo[j][
|
|
code2type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)] / C_V;}
|
|
|
|
//Match the nearest satellite of nav with this epoch
|
|
int best_epoch = select_epoch(Weeksec, sPRN, nav_b, satnum, GNSS);
|
|
|
|
if (best_epoch == -1) continue;//when the sPRN is not exist function will return -1
|
|
if (GNSS == BDS) detat_toc = Weeksec - nav_b[best_epoch].TOE - 14;//BDST = GPST + 14s
|
|
else detat_toc = Weeksec - nav_b[best_epoch].TOE;
|
|
|
|
//Iterative approximate propagation time
|
|
if (GNSS == GLO) {
|
|
//Get the approximately transit of signal
|
|
pos_t.delta_t = PERS - 0 +
|
|
nav_b[best_epoch].sa0 +
|
|
nav_b[best_epoch].sa1 * detat_toc;
|
|
|
|
pos_t.deltat = 0.0;
|
|
//Satellite position calculation
|
|
while (fabs(pos_t.delta_t - pos_t.deltat) > 1.0e-9) {
|
|
pos_t = glo_pos(best_epoch, Weeksec, nav_b, obs_h, pos_t);
|
|
}
|
|
}
|
|
//Exclude GLONASS
|
|
else {
|
|
//Get the approximately transit of signal
|
|
pos_t.delta_t = PERS - 0 +
|
|
nav_b[best_epoch].sa0 +
|
|
nav_b[best_epoch].sa1 * detat_toc +
|
|
nav_b[best_epoch].sa2 * pow(detat_toc, 2);
|
|
|
|
pos_t.deltat = 0.0;
|
|
//Satellite position calculation
|
|
while (fabs(pos_t.delta_t - pos_t.deltat) > 1.0e-9) {
|
|
pos_t = sat_pos(sPRN, best_epoch, Weeksec, GNSS, nav_b, obs_h, pos_t);
|
|
}
|
|
}
|
|
//Satellite blh & rah position calculation
|
|
in_rah = xyz2rah(pos_t, obs_h);
|
|
in_blh = xyz2blh(in_blh, pos_t.X, pos_t.Y, pos_t.Z);
|
|
|
|
if (in_blh.H < 0 || rad2deg(in_rah.H) < Hangle)continue;
|
|
else {
|
|
//Store the accept observation data
|
|
in_lsq.l[k] = pos_t.X, in_lsq.m[k] = pos_t.Y;
|
|
in_lsq.n[k] = pos_t.Z, in_lsq.o[k] = deg2rad(in_rah.H);
|
|
obs.sclk[k] = pos_t.delta_clk;
|
|
|
|
switch (GNSS) {
|
|
case GPS:
|
|
obs.Per1[k] = obs_b->obs_gps[j][code2type(C1, obs_h->obstypenum_gps, obs_h->obscode_gps)];
|
|
obs.Per2[k] = obs_b->obs_gps[j][code2type(C2, obs_h->obstypenum_gps, obs_h->obscode_gps)];
|
|
obs.freq1 = FREQ1; obs.freq2 = FREQ2; break;
|
|
case BDS:
|
|
obs.Per1[k] = obs_b->obs_bds[j][code2type(C2, obs_h->obstypenum_bds, obs_h->obscode_bds)];
|
|
obs.Per2[k] = obs_b->obs_bds[j][code2type(C7, obs_h->obstypenum_bds, obs_h->obscode_bds)];
|
|
obs.freq1 = FREQ1_BDS; obs.freq2 = FREQ2_BDS;
|
|
break;
|
|
case GAL:
|
|
obs.Per1[k] = obs_b->obs_gal[j][code2type(C1, obs_h->obstypenum_gal, obs_h->obscode_gal)];
|
|
obs.Per2[k] = obs_b->obs_gal[j][code2type(C6, obs_h->obstypenum_gal, obs_h->obscode_gal)];
|
|
obs.freq1 = FREQ1; obs.freq2 = FREQ6;
|
|
break;
|
|
case GLO:
|
|
obs.Per1[k] = obs_b->obs_glo[j][code2type(C1, obs_h->obstypenum_glo, obs_h->obscode_glo)];
|
|
obs.Per2[k] = obs_b->obs_glo[j][code2type(C2, obs_h->obstypenum_glo, obs_h->obscode_glo)];
|
|
obs.freq1 = FREQ1_GLO; obs.freq2 = FREQ2_GLO;
|
|
break;}
|
|
|
|
outdata(in_blh, solu, sPRN, GNSS); k++;
|
|
};
|
|
}
|
|
//Solve the position
|
|
if (k >= 4) {
|
|
in_lsq.x = mat(4, 1);
|
|
in_lsq.B = mat(k, 4);
|
|
in_lsq.L = mat(k, 1);
|
|
in_lsq.P = zeros(k, k);
|
|
apX = 0; ap_X = obs_h->apX;
|
|
apY = 0; ap_Y = obs_h->apY;
|
|
apZ = 0; ap_Z = obs_h->apZ;
|
|
int stop = 0;
|
|
while (fabs(ap_X - apX) > 1e-6) {
|
|
|
|
if (stop == 3) break;
|
|
apX = ap_X;
|
|
apY = ap_Y;
|
|
apZ = ap_Z;
|
|
sta = designmat(obs_h, obs,
|
|
in_lsq.l, in_lsq.m, in_lsq.n, in_lsq.o,
|
|
in_lsq.B, in_lsq.P, in_lsq.L,
|
|
ap_X, ap_Y, ap_Z, k,
|
|
ionopt, troopt, GNSS);
|
|
ap_X += sta.X;
|
|
ap_Y += sta.Y;
|
|
ap_Z += sta.Z;
|
|
stop++;
|
|
}solution(solu, obs_h, ap_X, ap_Y, ap_Z);
|
|
}
|
|
else {
|
|
fprintf(solu,"\nRec:insufficient quantity of sat, sat=%d",k);
|
|
}
|
|
free(in_lsq.l); free(in_lsq.m); free(in_lsq.n); free(in_lsq.o);
|
|
free(in_lsq.B); free(in_lsq.P); free(in_lsq.L);
|
|
return 1;
|
|
}
|
|
/**/
|
|
int sat_cal(pnav_body nav_b, pobs_head obs_h, FILE* solu,int satnum, int GNSS) {
|
|
|
|
int ii, i, sPRN=0;
|
|
double Min, min, Weeksec=0, Beginsec, STOP=0;
|
|
|
|
pos_ts pos = { 0 };
|
|
blh in_blh = { 0 };
|
|
|
|
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
|
|
);
|
|
|
|
for (i = 0; i < satnum; i++) {
|
|
|
|
switch (GNSS){
|
|
case GPS:if (nav_b[i].sPRN_GPS == -1) { continue; }
|
|
Weeksec =
|
|
fabs(sPRN - nav_b[i].sPRN_GPS) > 0 ? Beginsec : Weeksec;
|
|
sPRN = nav_b[i].sPRN_GPS;
|
|
STOP = TIMEGAP_GPS/2;
|
|
break;
|
|
case BDS:if (nav_b[i].sPRN_BDS == -1) { continue; }
|
|
Weeksec =
|
|
fabs(sPRN - nav_b[i].sPRN_BDS) > 0 ? Beginsec : Weeksec;
|
|
sPRN = nav_b[i].sPRN_BDS;
|
|
STOP = TIMEGAP_BDS/2;
|
|
break;
|
|
case GAL:if (nav_b[i].sPRN_GAL == -1) { continue; }
|
|
Weeksec =
|
|
fabs(sPRN - nav_b[i].sPRN_GAL) > 0 ? Beginsec : Weeksec;
|
|
sPRN = nav_b[i].sPRN_GAL;
|
|
STOP = TIMEGAP_GAL/2;
|
|
break;
|
|
case GLO:if (nav_b[i].sPRN_GLO == -1) { continue; }
|
|
Weeksec =
|
|
fabs(sPRN - nav_b[i].sPRN_GLO) > 0 ? Beginsec : Weeksec;
|
|
sPRN = nav_b[i].sPRN_GLO;
|
|
STOP = TIMEGAP_GLO/2+18;
|
|
break;
|
|
}
|
|
for (Weeksec; fabs(Weeksec - nav_b[i].TOE) <= STOP; Weeksec += 30.0) {
|
|
if (Weeksec - Beginsec >= DAYSEC) break;
|
|
else if (Weeksec - Beginsec >= DAYSEC - STOP) STOP += STOP;
|
|
if (GNSS != GLO) pos = sat_pos(sPRN, i, Weeksec, GNSS, nav_b, obs_h, pos);
|
|
else if (GNSS == GLO) pos = glo_pos(i, Weeksec, nav_b, obs_h, pos);
|
|
|
|
in_blh = xyz2blh(in_blh, pos.X, pos.Y, pos.Z);
|
|
outdata(in_blh, solu, sPRN, GNSS);
|
|
fprintf(solu, ">>%05d", (int)(Weeksec - Beginsec) / 30 + 1);
|
|
}
|
|
}return 0;
|
|
}
|
|
/**
|
|
* The entrance of function.
|
|
*
|
|
* \param nav_path: The RINEX navigation file's path
|
|
* \param obs_path: The RINEX observation file's path
|
|
* \param res_path: The satellite position solution output path
|
|
* \param GNSS: GNSS code
|
|
* \param Hangle :
|
|
* \param ionopt :
|
|
* \param troopt :
|
|
* \param genmode:
|
|
* \return
|
|
* -1 ,Can not open the nav file
|
|
* -2 ,Can not open the obs file
|
|
* -3 ,Mismatch version or format of nav
|
|
* -4 ,No data of nav
|
|
* -5 ,Mismatch version or format of obs
|
|
* -6 ,No data of obs
|
|
* 0 ,Soulution generated completely
|
|
* >0 ,Some epoch cant be calculate
|
|
*/
|
|
double brdm2pos(
|
|
char nav_path[MAXPATH],
|
|
char obs_path[MAXPATH],
|
|
char res_path[MAXPATH],
|
|
int GNSS,
|
|
int Hangle,
|
|
int ionopt,
|
|
int tropopt,
|
|
int genmode
|
|
)
|
|
{
|
|
int satnum, epochnum, result = 0;
|
|
pos_ts pos_t = { 0 };
|
|
blh apblh = { 0 };
|
|
|
|
pnav_head nav_h = NULL;
|
|
pnav_body nav_b = NULL;
|
|
pobs_head obs_h = NULL;
|
|
pobs_body obs_b = NULL;
|
|
pobs_epoch obs_e = NULL;
|
|
|
|
FILE* fp_nav = fopen(nav_path, "r");
|
|
if (fp_nav == NULL) return -1;//cant read nav file
|
|
|
|
//Get the amount of satellite
|
|
satnum = getsatnum(fp_nav);
|
|
|
|
switch (satnum) {
|
|
case -1: return -3;//mismatch version or format
|
|
case 0: return -4;//no data
|
|
default: break;
|
|
}
|
|
|
|
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;//cant read obs file
|
|
|
|
//Get the amonut of obs epoch
|
|
epochnum = get_epochnum(fp_obs);
|
|
|
|
switch (epochnum) {
|
|
case -1: return -5;//mismatch version or format
|
|
case 0: return -6;//no data
|
|
default: break;
|
|
}
|
|
rewind(fp_obs); obs_h = (pobs_head)malloc(sizeof(obs_head));
|
|
|
|
if (obs_h) {
|
|
|
|
//Read the header of obs
|
|
read_o_h(fp_obs, obs_h);
|
|
|
|
//Convert the approximately position (ECEF -> LLA)
|
|
apblh = xyz2blh(apblh, obs_h->apX, obs_h->apY, obs_h->apZ);
|
|
obs_h->apB = apblh.B;
|
|
obs_h->apL = apblh.L;
|
|
obs_h->apH = apblh.H;
|
|
|
|
FILE* solu = headerout(obs_h, genmode, obs_path, nav_path, res_path);
|
|
if (genmode == 1) {
|
|
sat_cal(nav_b, obs_h, solu, satnum, GNSS);
|
|
fprintf(solu, "\nEND");
|
|
fclose(solu);
|
|
return 0;
|
|
}
|
|
|
|
//Begin to calculate
|
|
for (int i = 0; i < epochnum; i++) {
|
|
|
|
//Allocate memory for obs epoch and body data
|
|
obs_e = (pobs_epoch)malloc(sizeof(obs_epoch));
|
|
obs_b = (pobs_body)malloc(sizeof(obs_body));
|
|
|
|
//Read epoch data
|
|
read_o_eb(fp_obs, obs_h, obs_e, obs_b);
|
|
|
|
//Calculate the position of satellite
|
|
result += sat_pos_cal(obs_h, obs_e, obs_b, nav_b,
|
|
solu, GNSS, satnum, Hangle, ionopt,tropopt);
|
|
|
|
fprintf(solu, "\n");
|
|
|
|
//Free epoch data
|
|
freeobs_e(obs_e, obs_b);
|
|
}
|
|
//Output the signal of ending
|
|
fprintf(solu, "\nEND"); fclose(solu);
|
|
|
|
//free nav & obs memory of data
|
|
free(obs_h); free(nav_h); free(nav_b);
|
|
|
|
if (epochnum == result) return 0;
|
|
else return epochnum - result;
|
|
}
|
|
} |