From 0057e609241bd21740ba3a3de2bb5ef20209843a Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 12 Mar 2018 10:51:06 +0100 Subject: [PATCH] Clean code --- src/algorithms/PVT/libs/rtklib_solver.cc | 12 +- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 47 ++-- .../system_parameters/gps_cnav_ephemeris.h | 43 ++-- .../gps_cnav_navigation_message.cc | 240 +++++++++--------- 4 files changed, 175 insertions(+), 167 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 65a1a0807..eaed5f5ec 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -247,19 +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_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; + eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + /* By the moment, GPS L2 observables are not used in pseudorange computations obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs], gnss_observables_iter->second, eph_data[i].week, - 1); //Band 2 (L2) + 1); //Band 2 (L2) */ break; } } } else - { /* + { + /* By the moment, GPS L2 observables are not used in pseudorange computations // 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); @@ -273,7 +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 b15275214..434f3b03a 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -54,8 +54,6 @@ #include "rtklib_ephemeris.h" #include "rtklib_ionex.h" #include "rtklib_sbas.h" -#include -#include /* pseudorange measurement error variance ------------------------------------*/ double varerr(const prcopt_t *opt, double el, int sys) @@ -132,6 +130,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, double P1_P2 = 0.0; double P1_C1 = 0.0; double P2_C2 = 0.0; + //Intersignal corrections (m). See GPS IS-200 CNAV message double ISCl1 = 0.0; double ISCl2 = 0.0; double ISCl5i = 0.0; @@ -154,8 +153,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { j = 2; } - - if (sys == SYS_GPS) + else if (sys == SYS_GPS or sys == SYS_GLO) { if (obs->code[1] != CODE_NONE) { @@ -203,18 +201,6 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, P1_C1 = nav->cbias[obs->sat - 1][1]; P2_C2 = nav->cbias[obs->sat - 1][2]; - 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.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)); - /* if no P1-P2 DCB, use TGD instead */ if (P1_P2 == 0.0) { @@ -227,12 +213,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, 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) @@ -263,19 +244,26 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } 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; + if (obs->code[j] == CODE_L2S) //L2 single freq. + { + PC = P2 + P1_P2 - ISCl2; + } + else if (obs->code[j] == CODE_L5X) //L5 single freq. + { + PC = P2 + P1_P2 - ISCl5i; + } } - else if (sys == SYS_GAL) + else if (sys == SYS_GAL) // Gal. E5a single freq. { //TODO } @@ -285,9 +273,12 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { if (obs->code[j] == CODE_L2S) /* L1 + L2 */ { - PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2; + //By the moment, GPS L2 pseudoranges are not used + //PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2; + P1 += P1_C1; /* C1->P1 */ + PC = P1 + P1_P2; } - if (obs->code[j] == CODE_L5X) /* L1 + L5 */ + else if (obs->code[j] == CODE_L5X) /* L1 + L5 */ { } } @@ -296,8 +287,6 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, //TODO } } - d_file.write(reinterpret_cast(&PC), sizeof(double)); - d_file.close(); if (opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h index d10c2d8c6..7b63c0857 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.h +++ b/src/core/system_parameters/gps_cnav_ephemeris.h @@ -138,25 +138,30 @@ public: { }; - archive& make_nvp("i_satellite_PRN", i_satellite_PRN); // SV PRN NUMBER - archive& make_nvp("d_TOW", d_TOW); //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] - archive& make_nvp("d_Crs", d_Crs); //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m] - archive& make_nvp("d_M_0", d_M_0); //!< Mean Anomaly at Reference Time [semi-circles] - archive& make_nvp("d_Cuc", d_Cuc); //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] - archive& make_nvp("d_e_eccentricity", d_e_eccentricity); //!< Eccentricity [dimensionless] - archive& make_nvp("d_Cus", d_Cus); //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad] - archive& make_nvp("d_Toe1", d_Toe1); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] - archive& make_nvp("d_Toe2", d_Toe2); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] - archive& make_nvp("d_Toc", d_Toc); //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s] - archive& make_nvp("d_Cic", d_Cic); //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad] - archive& make_nvp("d_OMEGA0", d_OMEGA0); //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles] - archive& make_nvp("d_Cis", d_Cis); //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad] - archive& make_nvp("d_i_0", d_i_0); //!< Inclination Angle at Reference Time [semi-circles] - archive& make_nvp("d_Crc", d_Crc); //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m] - archive& make_nvp("d_OMEGA", d_OMEGA); //!< Argument of Perigee [semi-cicles] - archive& make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s] - archive& make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week] - archive& make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("i_satellite_PRN", i_satellite_PRN); // SV PRN NUMBER + archive& make_nvp("d_TOW", d_TOW); //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] + archive& make_nvp("d_Crs", d_Crs); //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m] + archive& make_nvp("d_M_0", d_M_0); //!< Mean Anomaly at Reference Time [semi-circles] + archive& make_nvp("d_Cuc", d_Cuc); //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] + archive& make_nvp("d_e_eccentricity", d_e_eccentricity); //!< Eccentricity [dimensionless] + archive& make_nvp("d_Cus", d_Cus); //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad] + archive& make_nvp("d_Toe1", d_Toe1); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] + archive& make_nvp("d_Toe2", d_Toe2); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] + archive& make_nvp("d_Toc", d_Toc); //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s] + archive& make_nvp("d_Cic", d_Cic); //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad] + archive& make_nvp("d_OMEGA0", d_OMEGA0); //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles] + archive& make_nvp("d_Cis", d_Cis); //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad] + archive& make_nvp("d_i_0", d_i_0); //!< Inclination Angle at Reference Time [semi-circles] + archive& make_nvp("d_Crc", d_Crc); //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m] + archive& make_nvp("d_OMEGA", d_OMEGA); //!< Argument of Perigee [semi-cicles] + archive& make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s] + archive& make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week] + archive& make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL1", d_ISCL1); //!< Estimated Group Delay Differential: L1P(Y)-L1C/A correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL2", d_ISCL2); //!< Estimated Group Delay Differential: L1P(Y)-L2C correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL5I", d_ISCL5I); //!< Estimated Group Delay Differential: L1P(Y)-L5i correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL5Q", d_ISCL5Q); //!< Estimated Group Delay Differential: L1P(Y)-L5q correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_DELTA_A", d_DELTA_A); //!< Semi-major axis difference at reference time [m] archive& make_nvp("d_A_DOT", d_A_DOT); //!< Change rate in semi-major axis [m/s] archive& make_nvp("d_DELTA_OMEGA_DOT", d_DELTA_OMEGA_DOT); //!< Rate of Right Ascension difference [semi-circles/s] diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index 0f2455e51..d188060bd 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -187,127 +187,141 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_unsigned(data_bits, CNAV_MSG_TYPE)); - switch(page_type) - { - case 10: // Ephemeris 1/2 - ephemeris_record.i_GPS_week = static_cast(read_navigation_unsigned(data_bits, CNAV_WN)); - ephemeris_record.i_signal_health = static_cast(read_navigation_unsigned(data_bits, CNAV_HEALTH)); - ephemeris_record.d_Top = static_cast(read_navigation_unsigned(data_bits, CNAV_TOP1)); - ephemeris_record.d_Top *= CNAV_TOP1_LSB; - ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA)); - ephemeris_record.d_Toe1 = static_cast(read_navigation_unsigned(data_bits, CNAV_TOE1)); - ephemeris_record.d_Toe1 *= CNAV_TOE1_LSB; - ephemeris_record.d_DELTA_A = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_A)); - ephemeris_record.d_DELTA_A *= CNAV_DELTA_A_LSB; - ephemeris_record.d_A_DOT = static_cast(read_navigation_signed(data_bits, CNAV_A_DOT)); - ephemeris_record.d_A_DOT *= CNAV_A_DOT_LSB; - ephemeris_record.d_Delta_n = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0)); - ephemeris_record.d_Delta_n *= CNAV_DELTA_N0_LSB; - ephemeris_record.d_DELTA_DOT_N = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0_DOT)); - ephemeris_record.d_DELTA_DOT_N *= CNAV_DELTA_N0_DOT_LSB; - ephemeris_record.d_M_0 = static_cast(read_navigation_signed(data_bits, CNAV_M0)); - ephemeris_record.d_M_0 *= CNAV_M0_LSB; - ephemeris_record.d_e_eccentricity = static_cast(read_navigation_unsigned(data_bits, CNAV_E_ECCENTRICITY)); - ephemeris_record.d_e_eccentricity *= CNAV_E_ECCENTRICITY_LSB; - ephemeris_record.d_OMEGA = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA)); - ephemeris_record.d_OMEGA *= CNAV_OMEGA_LSB; + switch (page_type) + { + case 10: // Ephemeris 1/2 + ephemeris_record.i_GPS_week = static_cast(read_navigation_unsigned(data_bits, CNAV_WN)); + ephemeris_record.i_signal_health = static_cast(read_navigation_unsigned(data_bits, CNAV_HEALTH)); + ephemeris_record.d_Top = static_cast(read_navigation_unsigned(data_bits, CNAV_TOP1)); + ephemeris_record.d_Top *= CNAV_TOP1_LSB; + ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA)); + ephemeris_record.d_Toe1 = static_cast(read_navigation_unsigned(data_bits, CNAV_TOE1)); + ephemeris_record.d_Toe1 *= CNAV_TOE1_LSB; + ephemeris_record.d_DELTA_A = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_A)); + ephemeris_record.d_DELTA_A *= CNAV_DELTA_A_LSB; + ephemeris_record.d_A_DOT = static_cast(read_navigation_signed(data_bits, CNAV_A_DOT)); + ephemeris_record.d_A_DOT *= CNAV_A_DOT_LSB; + ephemeris_record.d_Delta_n = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0)); + ephemeris_record.d_Delta_n *= CNAV_DELTA_N0_LSB; + ephemeris_record.d_DELTA_DOT_N = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0_DOT)); + ephemeris_record.d_DELTA_DOT_N *= CNAV_DELTA_N0_DOT_LSB; + ephemeris_record.d_M_0 = static_cast(read_navigation_signed(data_bits, CNAV_M0)); + ephemeris_record.d_M_0 *= CNAV_M0_LSB; + ephemeris_record.d_e_eccentricity = static_cast(read_navigation_unsigned(data_bits, CNAV_E_ECCENTRICITY)); + ephemeris_record.d_e_eccentricity *= CNAV_E_ECCENTRICITY_LSB; + ephemeris_record.d_OMEGA = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA)); + ephemeris_record.d_OMEGA *= CNAV_OMEGA_LSB; ephemeris_record.b_integrity_status_flag = static_cast(read_navigation_bool(data_bits, CNAV_INTEGRITY_FLAG)); ephemeris_record.b_l2c_phasing_flag = static_cast(read_navigation_bool(data_bits, CNAV_L2_PHASING_FLAG)); - b_flag_ephemeris_1 = true; - break; - case 11: // Ephemeris 2/2 - ephemeris_record.d_Toe2 = static_cast(read_navigation_unsigned(data_bits, CNAV_TOE2)); - ephemeris_record.d_Toe2 *= CNAV_TOE2_LSB; - ephemeris_record.d_OMEGA0 = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA0)); - ephemeris_record.d_OMEGA0 *= CNAV_OMEGA0_LSB; - ephemeris_record.d_DELTA_OMEGA_DOT = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_OMEGA_DOT)); - ephemeris_record.d_DELTA_OMEGA_DOT *= CNAV_DELTA_OMEGA_DOT_LSB; - ephemeris_record.d_i_0 = static_cast(read_navigation_signed(data_bits, CNAV_I0)); - ephemeris_record.d_i_0 *= CNAV_I0_LSB; - ephemeris_record.d_IDOT = static_cast(read_navigation_signed(data_bits, CNAV_I0_DOT)); - ephemeris_record.d_IDOT *= CNAV_I0_DOT_LSB; - ephemeris_record.d_Cis = static_cast(read_navigation_signed(data_bits, CNAV_CIS)); - ephemeris_record.d_Cis *= CNAV_CIS_LSB; - ephemeris_record.d_Cic = static_cast(read_navigation_signed(data_bits, CNAV_CIC)); - ephemeris_record.d_Cic *= CNAV_CIC_LSB; - ephemeris_record.d_Crs = static_cast(read_navigation_signed(data_bits, CNAV_CRS)); - ephemeris_record.d_Crs *= CNAV_CRS_LSB; - ephemeris_record.d_Crc = static_cast(read_navigation_signed(data_bits, CNAV_CRC)); - ephemeris_record.d_Crc *= CNAV_CRC_LSB; - ephemeris_record.d_Cus = static_cast(read_navigation_signed(data_bits, CNAV_CUS)); - ephemeris_record.d_Cus *= CNAV_CUS_LSB; - ephemeris_record.d_Cuc = static_cast(read_navigation_signed(data_bits, CNAV_CUC)); - ephemeris_record.d_Cuc *= CNAV_CUC_LSB; - b_flag_ephemeris_2 = true; - break; - case 30: // (CLOCK, IONO, GRUP DELAY) - //clock - ephemeris_record.d_Toc = static_cast(read_navigation_unsigned(data_bits, CNAV_TOC)); - ephemeris_record.d_Toc *= CNAV_TOC_LSB; - ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA_NED0)); - ephemeris_record.d_URA1 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED1)); - ephemeris_record.d_URA2 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED2)); - ephemeris_record.d_A_f0 = static_cast(read_navigation_signed(data_bits, CNAV_AF0)); - ephemeris_record.d_A_f0 *= CNAV_AF0_LSB; - ephemeris_record.d_A_f1 = static_cast(read_navigation_signed(data_bits, CNAV_AF1)); - ephemeris_record.d_A_f1 *= CNAV_AF1_LSB; - ephemeris_record.d_A_f2 = static_cast(read_navigation_signed(data_bits, CNAV_AF2)); - ephemeris_record.d_A_f2 *= CNAV_AF2_LSB; - //group delays - //Check if the grup delay values are not available. See IS-GPS-200, Table 30-IV. - //Bit string "1000000000000" is -4096 in 2 complement - ephemeris_record.d_TGD = static_cast(read_navigation_signed(data_bits, CNAV_TGD)); - if(ephemeris_record.d_TGD < -4095.9) { ephemeris_record.d_TGD = 0.0; } - ephemeris_record.d_TGD *= CNAV_TGD_LSB; + b_flag_ephemeris_1 = true; + break; + case 11: // Ephemeris 2/2 + ephemeris_record.d_Toe2 = static_cast(read_navigation_unsigned(data_bits, CNAV_TOE2)); + ephemeris_record.d_Toe2 *= CNAV_TOE2_LSB; + ephemeris_record.d_OMEGA0 = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA0)); + ephemeris_record.d_OMEGA0 *= CNAV_OMEGA0_LSB; + ephemeris_record.d_DELTA_OMEGA_DOT = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_OMEGA_DOT)); + ephemeris_record.d_DELTA_OMEGA_DOT *= CNAV_DELTA_OMEGA_DOT_LSB; + ephemeris_record.d_i_0 = static_cast(read_navigation_signed(data_bits, CNAV_I0)); + ephemeris_record.d_i_0 *= CNAV_I0_LSB; + ephemeris_record.d_IDOT = static_cast(read_navigation_signed(data_bits, CNAV_I0_DOT)); + ephemeris_record.d_IDOT *= CNAV_I0_DOT_LSB; + ephemeris_record.d_Cis = static_cast(read_navigation_signed(data_bits, CNAV_CIS)); + ephemeris_record.d_Cis *= CNAV_CIS_LSB; + ephemeris_record.d_Cic = static_cast(read_navigation_signed(data_bits, CNAV_CIC)); + ephemeris_record.d_Cic *= CNAV_CIC_LSB; + ephemeris_record.d_Crs = static_cast(read_navigation_signed(data_bits, CNAV_CRS)); + ephemeris_record.d_Crs *= CNAV_CRS_LSB; + ephemeris_record.d_Crc = static_cast(read_navigation_signed(data_bits, CNAV_CRC)); + ephemeris_record.d_Crc *= CNAV_CRC_LSB; + ephemeris_record.d_Cus = static_cast(read_navigation_signed(data_bits, CNAV_CUS)); + ephemeris_record.d_Cus *= CNAV_CUS_LSB; + ephemeris_record.d_Cuc = static_cast(read_navigation_signed(data_bits, CNAV_CUC)); + ephemeris_record.d_Cuc *= CNAV_CUC_LSB; + b_flag_ephemeris_2 = true; + break; + case 30: // (CLOCK, IONO, GRUP DELAY) + //clock + ephemeris_record.d_Toc = static_cast(read_navigation_unsigned(data_bits, CNAV_TOC)); + ephemeris_record.d_Toc *= CNAV_TOC_LSB; + ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA_NED0)); + ephemeris_record.d_URA1 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED1)); + ephemeris_record.d_URA2 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED2)); + ephemeris_record.d_A_f0 = static_cast(read_navigation_signed(data_bits, CNAV_AF0)); + ephemeris_record.d_A_f0 *= CNAV_AF0_LSB; + ephemeris_record.d_A_f1 = static_cast(read_navigation_signed(data_bits, CNAV_AF1)); + ephemeris_record.d_A_f1 *= CNAV_AF1_LSB; + ephemeris_record.d_A_f2 = static_cast(read_navigation_signed(data_bits, CNAV_AF2)); + ephemeris_record.d_A_f2 *= CNAV_AF2_LSB; + //group delays + //Check if the grup delay values are not available. See IS-GPS-200, Table 30-IV. + //Bit string "1000000000000" is -4096 in 2 complement + ephemeris_record.d_TGD = static_cast(read_navigation_signed(data_bits, CNAV_TGD)); + if (ephemeris_record.d_TGD < -4095.9) + { + ephemeris_record.d_TGD = 0.0; + } + ephemeris_record.d_TGD *= CNAV_TGD_LSB; - ephemeris_record.d_ISCL1 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL1)); - if(ephemeris_record.d_ISCL1 < -4095.9) { ephemeris_record.d_ISCL1 = 0.0; } - ephemeris_record.d_ISCL1 *= CNAV_ISCL1_LSB; + ephemeris_record.d_ISCL1 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL1)); + if (ephemeris_record.d_ISCL1 < -4095.9) + { + ephemeris_record.d_ISCL1 = 0.0; + } + ephemeris_record.d_ISCL1 *= CNAV_ISCL1_LSB; - ephemeris_record.d_ISCL2 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL2)); - if(ephemeris_record.d_ISCL2 < -4095.9) { ephemeris_record.d_ISCL2 = 0.0; } - ephemeris_record.d_ISCL2 *= CNAV_ISCL2_LSB; + ephemeris_record.d_ISCL2 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL2)); + if (ephemeris_record.d_ISCL2 < -4095.9) + { + ephemeris_record.d_ISCL2 = 0.0; + } + ephemeris_record.d_ISCL2 *= CNAV_ISCL2_LSB; - ephemeris_record.d_ISCL5I = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5I)); - if(ephemeris_record.d_ISCL5I < -4095.9) { ephemeris_record.d_ISCL5I = 0.0; } - ephemeris_record.d_ISCL5I *= CNAV_ISCL5I_LSB; - - ephemeris_record.d_ISCL5Q = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5Q)); - if(ephemeris_record.d_ISCL5Q < -4095.9) { ephemeris_record.d_ISCL5Q = 0.0; } - ephemeris_record.d_ISCL5Q *= CNAV_ISCL5Q_LSB; - //iono - iono_record.d_alpha0 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA0)); - iono_record.d_alpha0 = iono_record.d_alpha0 * CNAV_ALPHA0_LSB; - iono_record.d_alpha1 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA1)); - iono_record.d_alpha1 = iono_record.d_alpha1 * CNAV_ALPHA1_LSB; - iono_record.d_alpha2 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA2)); - iono_record.d_alpha2 = iono_record.d_alpha2 * CNAV_ALPHA2_LSB; - iono_record.d_alpha3 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA3)); - iono_record.d_alpha3 = iono_record.d_alpha3 * CNAV_ALPHA3_LSB; - iono_record.d_beta0 = static_cast(read_navigation_signed(data_bits, CNAV_BETA0)); - iono_record.d_beta0 = iono_record.d_beta0 * CNAV_BETA0_LSB; - iono_record.d_beta1 = static_cast(read_navigation_signed(data_bits, CNAV_BETA1)); - iono_record.d_beta1 = iono_record.d_beta1 * CNAV_BETA1_LSB; - iono_record.d_beta2 = static_cast(read_navigation_signed(data_bits, CNAV_BETA2)); - iono_record.d_beta2 = iono_record.d_beta2 * CNAV_BETA2_LSB; - iono_record.d_beta3 = static_cast(read_navigation_signed(data_bits, CNAV_BETA3)); - iono_record.d_beta3 = iono_record.d_beta3 * CNAV_BETA3_LSB; - b_flag_iono_valid = true; - break; - case 33: // (CLOCK & UTC) - ephemeris_record.d_Top = static_cast(read_navigation_unsigned(data_bits, CNAV_TOP1)); - ephemeris_record.d_Top = ephemeris_record.d_Top * CNAV_TOP1_LSB; - ephemeris_record.d_Toc = static_cast(read_navigation_unsigned(data_bits, CNAV_TOC)); - ephemeris_record.d_Toc = ephemeris_record.d_Toc * CNAV_TOC_LSB; - ephemeris_record.d_A_f0 = static_cast(read_navigation_signed(data_bits, CNAV_AF0)); - ephemeris_record.d_A_f0 = ephemeris_record.d_A_f0 * CNAV_AF0_LSB; - ephemeris_record.d_A_f1 = static_cast(read_navigation_signed(data_bits, CNAV_AF1)); - ephemeris_record.d_A_f1 = ephemeris_record.d_A_f1 * CNAV_AF1_LSB; - ephemeris_record.d_A_f2 = static_cast(read_navigation_signed(data_bits, CNAV_AF2)); - ephemeris_record.d_A_f2 = ephemeris_record.d_A_f2 * CNAV_AF2_LSB; + ephemeris_record.d_ISCL5I = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5I)); + if (ephemeris_record.d_ISCL5I < -4095.9) + { + ephemeris_record.d_ISCL5I = 0.0; + } + ephemeris_record.d_ISCL5I *= CNAV_ISCL5I_LSB; + ephemeris_record.d_ISCL5Q = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5Q)); + if (ephemeris_record.d_ISCL5Q < -4095.9) + { + ephemeris_record.d_ISCL5Q = 0.0; + } + ephemeris_record.d_ISCL5Q *= CNAV_ISCL5Q_LSB; + //iono + iono_record.d_alpha0 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA0)); + iono_record.d_alpha0 = iono_record.d_alpha0 * CNAV_ALPHA0_LSB; + iono_record.d_alpha1 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA1)); + iono_record.d_alpha1 = iono_record.d_alpha1 * CNAV_ALPHA1_LSB; + iono_record.d_alpha2 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA2)); + iono_record.d_alpha2 = iono_record.d_alpha2 * CNAV_ALPHA2_LSB; + iono_record.d_alpha3 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA3)); + iono_record.d_alpha3 = iono_record.d_alpha3 * CNAV_ALPHA3_LSB; + iono_record.d_beta0 = static_cast(read_navigation_signed(data_bits, CNAV_BETA0)); + iono_record.d_beta0 = iono_record.d_beta0 * CNAV_BETA0_LSB; + iono_record.d_beta1 = static_cast(read_navigation_signed(data_bits, CNAV_BETA1)); + iono_record.d_beta1 = iono_record.d_beta1 * CNAV_BETA1_LSB; + iono_record.d_beta2 = static_cast(read_navigation_signed(data_bits, CNAV_BETA2)); + iono_record.d_beta2 = iono_record.d_beta2 * CNAV_BETA2_LSB; + iono_record.d_beta3 = static_cast(read_navigation_signed(data_bits, CNAV_BETA3)); + iono_record.d_beta3 = iono_record.d_beta3 * CNAV_BETA3_LSB; + b_flag_iono_valid = true; + break; + case 33: // (CLOCK & UTC) + ephemeris_record.d_Top = static_cast(read_navigation_unsigned(data_bits, CNAV_TOP1)); + ephemeris_record.d_Top = ephemeris_record.d_Top * CNAV_TOP1_LSB; + ephemeris_record.d_Toc = static_cast(read_navigation_unsigned(data_bits, CNAV_TOC)); + ephemeris_record.d_Toc = ephemeris_record.d_Toc * CNAV_TOC_LSB; + ephemeris_record.d_A_f0 = static_cast(read_navigation_signed(data_bits, CNAV_AF0)); + ephemeris_record.d_A_f0 = ephemeris_record.d_A_f0 * CNAV_AF0_LSB; + ephemeris_record.d_A_f1 = static_cast(read_navigation_signed(data_bits, CNAV_AF1)); + ephemeris_record.d_A_f1 = ephemeris_record.d_A_f1 * CNAV_AF1_LSB; + ephemeris_record.d_A_f2 = static_cast(read_navigation_signed(data_bits, CNAV_AF2)); + ephemeris_record.d_A_f2 = ephemeris_record.d_A_f2 * CNAV_AF2_LSB; utc_model_record.d_A0 = static_cast(read_navigation_signed(data_bits, CNAV_A0)); utc_model_record.d_A0 = utc_model_record.d_A0 * CNAV_A0_LSB;