diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index f504ebfe0..65a1a0807 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -127,12 +127,12 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ int glo_valid_obs = 0; //GLONASS L1/L2 valid observations counter obsd_t obs_data[MAXOBS]; - eph_t eph_data[MAXOBS]; + eph_t eph_data[MAXOBS]; geph_t geph_data[MAXOBS]; - for(gnss_observables_iter = gnss_observables_map.cbegin(); - gnss_observables_iter != gnss_observables_map.cend(); - gnss_observables_iter++) //CHECK INCONSISTENCY when combining GLONASS + other system + for (gnss_observables_iter = gnss_observables_map.cbegin(); + gnss_observables_iter != gnss_observables_map.cend(); + gnss_observables_iter++) //CHECK INCONSISTENCY when combining GLONASS + other system { switch (gnss_observables_iter->second.System) { @@ -247,17 +247,19 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ { if (eph_data[i].sat == static_cast(gnss_observables_iter->second.PRN)) { - eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + eph_t eph_aux = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + eph_data[i].tgd = eph_aux.tgd; + eph_data[i].isc = eph_aux.isc; obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs], gnss_observables_iter->second, - gps_cnav_ephemeris_iter->second.i_GPS_week, + eph_data[i].week, 1); //Band 2 (L2) break; } } } else - { + { /* // 3. If not found, insert the GPS L2 ephemeris and the observation //convert ephemeris from GNSS-SDR class to RTKLIB structure eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); @@ -271,6 +273,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ gps_cnav_ephemeris_iter->second.i_GPS_week, 1); //Band 2 (L2) valid_obs++; + */ } } else // the ephemeris are not available for this SV diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index aea3dd85f..b15275214 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -125,15 +125,15 @@ double getiscl5q(int sat, const nav_t *nav) double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int iter, const prcopt_t *opt, double *var) { - const double* lam = nav->lam[obs->sat - 1]; + const double *lam = nav->lam[obs->sat - 1]; double PC = 0.0; double P1 = 0.0; double P2 = 0.0; double P1_P2 = 0.0; double P1_C1 = 0.0; double P2_C2 = 0.0; - double ISCl1 = 0.0; - double ISCl2 = 0.0; + double ISCl1 = 0.0; + double ISCl2 = 0.0; double ISCl5i = 0.0; double ISCl5q = 0.0; double gamma_ = 0.0; @@ -142,52 +142,61 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int sys = satsys(obs->sat, NULL); *var = 0.0; - if(sys == SYS_NONE) - { - trace(4, "prange: satsys NULL\n"); - return 0.0; - } + if (sys == SYS_NONE) + { + trace(4, "prange: satsys NULL\n"); + return 0.0; + } /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ - if(sys == SYS_GAL or sys == SYS_SBS) {j = 2;} - - if(sys == SYS_GPS) - { - if(obs->code[1] != CODE_NONE) {j = 1;} - else if(obs->code[2] != CODE_NONE) {j = 2;} - } - - if(lam[i] == 0.0 or lam[j] == 0.0) - { - trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); - printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]); - return 0.0; - } - - /* test snr mask */ - if(iter > 0) - { - if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) + if (sys == SYS_GAL or sys == SYS_SBS) { - trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n", - time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25); + j = 2; + } + + if (sys == SYS_GPS) + { + if (obs->code[1] != CODE_NONE) + { + j = 1; + } + else if (obs->code[2] != CODE_NONE) + { + j = 2; + } + } + + if (lam[i] == 0.0 or lam[j] == 0.0) + { + trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); + printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]); return 0.0; } - if (opt->ionoopt == IONOOPT_IFLC) + + /* test snr mask */ + if (iter > 0) { - if (testsnr(0, j, azel[1], obs->SNR[j] * 0.25, &opt->snrmask)) - { - trace(4, "prange: testsnr error\n"); - return 0.0; - } + if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) + { + trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n", + time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25); + return 0.0; + } + if (opt->ionoopt == IONOOPT_IFLC) + { + if (testsnr(0, j, azel[1], obs->SNR[j] * 0.25, &opt->snrmask)) + { + trace(4, "prange: testsnr error\n"); + return 0.0; + } + } } - } /* fL1^2 / fL2(orL5)^2 . See IS-GPS-200, p. 103 and Galileo ICD p. 48 */ - if(sys == SYS_GPS or sys == SYS_GAL) - { - gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); - } + if (sys == SYS_GPS or sys == SYS_GAL) + { + gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); + } P1 = obs->P[i]; P2 = obs->P[j]; P1_P2 = nav->cbias[obs->sat - 1][0]; @@ -196,86 +205,103 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, std::string d_dump_filename = "/home/aramos/dump_prange.dat"; std::ofstream d_file; - d_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); + d_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); 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)); - d_file.write(reinterpret_cast(&P1_C1), sizeof(double)); - d_file.write(reinterpret_cast(&P2_C2), sizeof(double)); + 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)); + d_file.write(reinterpret_cast(&P1_C1), sizeof(double)); + d_file.write(reinterpret_cast(&P2_C2), sizeof(double)); /* if no P1-P2 DCB, use TGD instead */ - if(P1_P2 == 0.0) { P1_P2 = gettgd(obs->sat, nav); } + if (P1_P2 == 0.0) + { + P1_P2 = gettgd(obs->sat, nav); + } - 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(&ISCl1), sizeof(double)); - d_file.write(reinterpret_cast(&ISCl2), sizeof(double)); - d_file.write(reinterpret_cast(&ISCl5i), sizeof(double)); - d_file.write(reinterpret_cast(&ISCl5q), sizeof(double)); - } - d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); + 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(&ISCl1), sizeof(double)); + d_file.write(reinterpret_cast(&ISCl2), sizeof(double)); + d_file.write(reinterpret_cast(&ISCl5i), sizeof(double)); + d_file.write(reinterpret_cast(&ISCl5q), sizeof(double)); + } + d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); //CHECK IF IT IS STILL NEEDED - if(opt->ionoopt == IONOOPT_IFLC) - { /* dual-frequency */ + if (opt->ionoopt == IONOOPT_IFLC) + { /* dual-frequency */ - if (P1 == 0.0 || P2 == 0.0) { return 0.0; } - if (obs->code[i] == CODE_L1C) { P1 += P1_C1; } /* C1->P1 */ - if (obs->code[j] == CODE_L2C) { P2 += P2_C2; } /* C2->P2 */ + if (P1 == 0.0 || P2 == 0.0) + { + return 0.0; + } + if (obs->code[i] == CODE_L1C) + { + P1 += P1_C1; + } /* C1->P1 */ + if (obs->code[j] == CODE_L2C) + { + P2 += P2_C2; + } /* C2->P2 */ - /* iono-free combination */ - PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); - } + /* iono-free combination */ + PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); + } //////////////////////////////////////////// else - { /* single-frequency */ - if(obs->code[i] == CODE_NONE and obs->code[j] == CODE_NONE) { return 0.0; } + { /* single-frequency */ + if (obs->code[i] == CODE_NONE and obs->code[j] == CODE_NONE) + { + return 0.0; + } - else if(obs->code[i] != CODE_NONE and obs->code[j] == CODE_NONE) - {//CHECK!! - P1 += P1_C1; /* C1->P1 */ - PC = P1 + P1_P2; + else if (obs->code[i] != CODE_NONE and obs->code[j] == CODE_NONE) + { //CHECK!! + P1 += P1_C1; /* C1->P1 */ + PC = P1 + P1_P2; + } + else if (obs->code[i] == CODE_NONE and obs->code[j] != CODE_NONE) + { + if (sys == SYS_GPS) + { //CHECK!! + P2 += P2_C2; /* C2->P2 */ + //PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); + PC = P2 + P1_P2 - ISCl2; + } + else if (sys == SYS_GAL) + { + //TODO + } + } + /* dual-frequency */ + else if (sys == SYS_GPS) + { + if (obs->code[j] == CODE_L2S) /* L1 + L2 */ + { + PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2; + } + if (obs->code[j] == CODE_L5X) /* L1 + L5 */ + { + } + } + else if (sys == SYS_GAL) /* E1 + E5a */ + { + //TODO + } } - else if(obs->code[i] == CODE_NONE and obs->code[j] != CODE_NONE) - { - if(sys == SYS_GPS) - {//CHECK!! - P2 += P2_C2; /* C2->P2 */ - //PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); - PC = P2 + P1_P2 - ISCl2; - } - else if(sys == SYS_GAL) - { - //TODO - } - } - /* dual-frequency */ - else if(sys == SYS_GPS) - { - if(obs->code[j] == CODE_L2S) /* L1 + L2 */ - { - PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2; - } - if(obs->code[j] == CODE_L5X) /* L1 + L5 */ - { - - } - } - else if(sys == SYS_GAL) /* E1 + E5a */ - { - //TODO - } - } - d_file.write(reinterpret_cast(&PC), sizeof(double)); + d_file.write(reinterpret_cast(&PC), sizeof(double)); d_file.close(); - if(opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; } /* sbas clock based C1 */ + if (opt->sateph == EPHOPT_SBAS) + { + PC -= P1_C1; + } /* sbas clock based C1 */ *var = std::pow(ERR_CBIAS, 2.0); return PC; } diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 3456916b5..92c1c51fd 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -253,12 +253,11 @@ const unsigned int tbl_CRC24Q[] = { 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538}; -extern "C" -{ - void dgemm_(char *, char *, int *, int *, int *, double *, double *, int *, double *, int *, double *, double *, int *); - extern void dgetrf_(int *, int *, double *, int *, int *, int *); - extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); - extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, int *, int *); +extern "C" { +void dgemm_(char *, char *, int *, int *, int *, double *, double *, int *, double *, int *, double *, double *, int *); +extern void dgetrf_(int *, int *, double *, int *, int *, int *); +extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); +extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, int *, int *); } @@ -1388,10 +1387,10 @@ double time2gpst(gtime_t t, int *week) { gtime_t t0 = epoch2time(gpst0); time_t sec = t.time - t0.time; - int w = (int)(sec / (86400 * 7)); + int w = static_cast(sec / 604800); if (week) *week = w; - return (double)(sec - (double)w * 86400 * 7) + t.sec; + return (static_cast(sec - static_cast(w * 604800)) + t.sec); } @@ -2993,7 +2992,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; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 20e5c6948..81daa31ca 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -410,7 +410,7 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector::lowest(); for (it = data.begin(); it != data.end(); it++) { - if (it->TOW_at_current_symbol_s > TOW_ref and it->Signal[0] != '2') + if (it->TOW_at_current_symbol_s > TOW_ref) { TOW_ref = it->TOW_at_current_symbol_s; }