From 8d37013e96672672d56fdb1c31e26e25ccf85285 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 5 Mar 2018 13:07:41 +0100 Subject: [PATCH] debug 5 --- src/algorithms/libs/rtklib/rtklib.h | 1 + .../libs/rtklib/rtklib_conversions.cc | 16 +++-- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 66 ++++++++++++++++++- src/algorithms/libs/rtklib/rtklib_pntpos.h | 6 ++ src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 2 +- 5 files changed, 82 insertions(+), 9 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index 8d172b171..162d445c6 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -464,6 +464,7 @@ typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */ /* GPS/QZS:tgd[0]=TGD */ /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ + double isc[4]; /* GPS :isc[0]=ISCL1, isc[1]=ISCL2, isc[2]=ISCL5I, isc[3]=ISCL5Q */ double Adot,ndot; /* Adot,ndot for CNAV */ } eph_t; diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 7f1d933a6..c0bf8929c 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -117,7 +117,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Glon eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; //Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris rtklib_sat.sat = gal_eph.i_satellite_PRN+NSATGPS+NSATGLO; rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1; @@ -167,7 +167,7 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph) eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; rtklib_sat.sat = gps_eph.i_satellite_PRN; rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A; rtklib_sat.M0 = gps_eph.d_M_0; @@ -216,7 +216,7 @@ eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph) eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris & gps_cnav_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN; const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170 rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A; @@ -245,9 +245,13 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris & gps_cnav_eph) rtklib_sat.f1 = gps_cnav_eph.d_A_f1; rtklib_sat.f2 = gps_cnav_eph.d_A_f2; rtklib_sat.tgd[0] = gps_cnav_eph.d_TGD; - rtklib_sat.tgd[1] = 0; - rtklib_sat.tgd[2] = 0; - rtklib_sat.tgd[3] = 0; + rtklib_sat.tgd[1] = 0.0; + rtklib_sat.tgd[2] = 0.0; + rtklib_sat.tgd[3] = 0.0; + rtklib_sat.isc[0] = gps_cnav_eph.d_ISCL1; + rtklib_sat.isc[1] = gps_cnav_eph.d_ISCL2; + rtklib_sat.isc[2] = gps_cnav_eph.d_ISCL5I; + rtklib_sat.isc[3] = gps_cnav_eph.d_ISCL5Q; rtklib_sat.toes = gps_cnav_eph.d_Toe1; rtklib_sat.toc = gpst2time(rtklib_sat.week,gps_cnav_eph.d_Toc); rtklib_sat.ttr = gpst2time(rtklib_sat.week,gps_cnav_eph.d_TOW); diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 3b951189f..8dccfe679 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -80,6 +80,50 @@ double gettgd(int sat, const nav_t *nav) return 0.0; } +/* get isc parameter (m) -----------------------------------------------------*/ +double getiscl1(int sat, const nav_t *nav) +{ + int i; + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[0]; + } + return 0.0; +} + +double getiscl2(int sat, const nav_t *nav) +{ + int i; + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[1]; + } + return 0.0; +} + +double getiscl5i(int sat, const nav_t *nav) +{ + int i; + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[2]; + } + return 0.0; +} + +double getiscl5q(int sat, const nav_t *nav) +{ + int i; + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[3]; + } + return 0.0; +} /* psendorange with code bias correction -------------------------------------*/ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, @@ -92,6 +136,10 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, double P1_P2; double P1_C1; double P2_C2; + double ISCl1 = 0.0; + double ISCl2 = 0.0; + double ISCl5i = 0.0; + double ISCl5q = 0.0; double gamma_ = 0.0; int i = 0; int j = 1; @@ -150,10 +198,13 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, P1_P2 = nav->cbias[obs->sat-1][0]; P1_C1 = nav->cbias[obs->sat-1][1]; P2_C2 = nav->cbias[obs->sat-1][2]; + std::string d_dump_filename = "/home/antonio/data/dump_prange.dat"; std::ofstream d_file; d_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + d_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary | std::ios::app); + double tmp_double = static_cast(obs->sat); + d_file.write(reinterpret_cast(&tmp_double), sizeof(double)); d_file.write(reinterpret_cast(&P1), sizeof(double)); d_file.write(reinterpret_cast(&P2), sizeof(double)); d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); @@ -168,6 +219,14 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { //TODO } + + if(sys == SYS_GPS) + { + ISCl1 = getiscl1(obs->sat, nav); + ISCl2 = getiscl2(obs->sat, nav); + ISCl5i = getiscl5i(obs->sat, nav); + ISCl5q = getiscl5q(obs->sat, nav); + } d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); if (opt->ionoopt == IONOOPT_IFLC) { /* dual-frequency */ @@ -208,7 +267,10 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { P1 += P1_C1; P2 += P2_C2; - PC = (P2 - gamma_ * P1) / (1.0 - gamma_) - P1_P2 / (gamma_ - 1.0); + if(obs->code[j] == CODE_L2S) + { + PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2 / (gamma_ - 1.0); + } } else if(sys == SYS_GAL) { diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.h b/src/algorithms/libs/rtklib/rtklib_pntpos.h index 5dd969943..ad53558fa 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.h +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.h @@ -69,6 +69,12 @@ double varerr(const prcopt_t *opt, double el, int sys); /* get tgd parameter (m) -----------------------------------------------------*/ double gettgd(int sat, const nav_t *nav); +/* get isc parameter (m) -----------------------------------------------------*/ +double getiscl1(int sat, const nav_t *nav); +double getiscl2(int sat, const nav_t *nav); +double getiscl5i(int sat, const nav_t *nav); +double getiscl5q(int sat, const nav_t *nav); + /* psendorange with code bias correction -------------------------------------*/ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int iter, const prcopt_t *opt, double *var); diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index a9c74ab55..60b95594b 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -2825,7 +2825,7 @@ int readnav(const char *file, nav_t *nav) { FILE *fp; eph_t eph0 = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; geph_t geph0 = {0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {}, {}, {}, 0.0, 0.0, 0.0}; char buff[4096], *p; long toe_time, tof_time, toc_time, ttr_time;