diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 7f5d9a43a..6bf7daa1e 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -1638,6 +1638,7 @@ void Rinex_Printer::rinex_nav_header(std::fstream& out, const Beidou_Dnav_Iono& out << line << std::endl; // -------- Line 5 system time correction + line.clear(); line += std::string("BDUT"); line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A0, 16, 2), 18); line += Rinex_Printer::rightJustify(Rinex_Printer::doub2for(utc_model.d_A1, 15, 2), 16); @@ -3770,7 +3771,7 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::mapsecond.i_satellite_PRN < 10) line += std::string("0"); line += boost::lexical_cast(bds_ephemeris_iter->second.i_satellite_PRN); std::string year(timestring, 0, 4); @@ -3813,7 +3814,7 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::mapsecond.d_Cuc, 18, 2); line += std::string(1, ' '); - line += Rinex_Printer::doub2for(bds_ephemeris_iter->second.d_e_eccentricity, 18, 2); + line += Rinex_Printer::doub2for(bds_ephemeris_iter->second.d_eccentricity, 18, 2); line += std::string(1, ' '); line += Rinex_Printer::doub2for(bds_ephemeris_iter->second.d_Cus, 18, 2); line += std::string(1, ' '); @@ -7204,7 +7205,7 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps } -void Rinex_Printer::Rinex_Printer::rinex_obs_header(std::fstream& out, const Beidou_Dnav_Ephemeris& eph, const double d_TOW_first_observation, const std::string bands) +void Rinex_Printer::rinex_obs_header(std::fstream& out, const Beidou_Dnav_Ephemeris& eph, const double d_TOW_first_observation, const std::string bands) { std::string line; version = 3; @@ -7214,7 +7215,7 @@ void Rinex_Printer::Rinex_Printer::rinex_obs_header(std::fstream& out, const Bei line += "3.02"; line += std::string(11, ' '); line += Rinex_Printer::leftJustify("OBSERVATION DATA", 20); - line += satelliteSystem["BeiDou "]; + line += satelliteSystem["Beidou"]; line += std::string(19, ' '); line += std::string("RINEX VERSION / TYPE"); Rinex_Printer::lengthCheck(line); @@ -10414,8 +10415,6 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep void Rinex_Printer::log_rinex_obs(std::fstream& out, const Beidou_Dnav_Ephemeris& eph, double obs_time, const std::map& observables, const std::string bds_bands) { - // RINEX observations timestamps are Galileo timestamps. - // See http://gage14.upc.es/gLAB/HTML/Observation_Rinex_v3.01.html std::string line; boost::posix_time::ptime p_bds_time = Rinex_Printer::compute_BDS_time(eph, obs_time); @@ -10468,11 +10467,11 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Beidou_Dnav_Ephemeris { std::string system_(&observables_iter->second.System, 1); std::string sig_(observables_iter->second.Signal); - if ((system_.compare("C") == 0) && (sig_.compare("B1") == 0)) + if ((system_ == "C") && (sig_ == "B1")) { observablesB1I.insert(std::pair(observables_iter->first, observables_iter->second)); } - if ((system_.compare("C") == 0) && (sig_.compare("B3") == 0)) + if ((system_ == "C") && (sig_ == "B3")) { observablesB3I.insert(std::pair(observables_iter->first, observables_iter->second)); } diff --git a/src/algorithms/PVT/libs/rinex_printer.h b/src/algorithms/PVT/libs/rinex_printer.h index 0485a06d3..cfdcf86a8 100644 --- a/src/algorithms/PVT/libs/rinex_printer.h +++ b/src/algorithms/PVT/libs/rinex_printer.h @@ -406,7 +406,7 @@ public: void update_obs_header(std::fstream& out, const Beidou_Dnav_Utc_Model& utc_model); - std::map satelliteSystem; // satelliteSystem; // observationType; // observationCode; //d_satellite; - // GEO Satellites (PRN 1 to 5) use D2 NAV message - if ( d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6 ) - { - d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D2NAV_SYMBOL_RATE_SPS; - d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; - d_secondary_code_symbols = nullptr; - d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS*d_samples_per_symbol; - d_subframe_length_symbols = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D1NAV_SYMBOL_RATE_SPS; + d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; + d_secondary_code_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_B1I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS*d_samples_per_symbol; + d_subframe_length_symbols = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; - // Setting samples of preamble code - int32_t n = 0; - for (int32_t i = 0; i < d_symbols_per_preamble; i++) - { - int32_t m = 0; - if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') - { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = 1; - n++; - } - } - else - { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -1; - n++; - } - } - } - - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(d_subframe_length_symbols * sizeof(double), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS*d_samples_per_symbol + d_samples_per_preamble; - } - else // MEO/IGSO Satellites (PRN 6 to 37) use D1 NAV message + // Setting samples of secondary code + for (int32_t i = 0; i < BEIDOU_B1I_SECONDARY_CODE_LENGTH; i++) { - d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D1NAV_SYMBOL_RATE_SPS; - d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; - d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; - d_secondary_code_symbols = static_cast(volk_gnsssdr_malloc(BEIDOU_B1I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); - d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS*d_samples_per_symbol; - d_subframe_length_symbols = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; - - // Setting samples of secondary code - for (int32_t i = 0; i < BEIDOU_B1I_SECONDARY_CODE_LENGTH; i++) + if (BEIDOU_B1I_SECONDARY_CODE.at(i) == '1') { - if (BEIDOU_B1I_SECONDARY_CODE.at(i) == '1') - { - d_secondary_code_symbols[i] = 1; - } - else - { - d_secondary_code_symbols[i] = -1; - } + d_secondary_code_symbols[i] = 1; } - - // Setting samples of preamble code - int32_t n = 0; - for (int32_t i = 0; i < d_symbols_per_preamble; i++) + else { - int32_t m = 0; - if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') - { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = d_secondary_code_symbols[m]; - n++; - m++; - m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; - } - } - else - { - for (uint32_t j = 0; j < d_samples_per_symbol; j++) - { - d_preamble_samples[n] = -d_secondary_code_symbols[m]; - n++; - m++; - m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; - } - } + d_secondary_code_symbols[i] = -1; } - - d_subframe_symbols = static_cast(volk_gnsssdr_malloc(d_subframe_length_symbols * sizeof(double), volk_gnsssdr_get_alignment())); - d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS*d_samples_per_symbol + d_samples_per_preamble; } + // Setting samples of preamble code + int32_t n = 0; + for (int32_t i = 0; i < d_symbols_per_preamble; i++) + { + int32_t m = 0; + if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = d_secondary_code_symbols[m]; + n++; + m++; + m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; + } + } + else + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = -d_secondary_code_symbols[m]; + n++; + m++; + m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH; + } + } + } + + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(d_subframe_length_symbols * sizeof(double), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS*d_samples_per_symbol + d_samples_per_preamble; // Generic settings d_sample_counter = 0; @@ -309,8 +268,7 @@ void beidou_b1i_telemetry_decoder_cc::decode_subframe(double *frame_symbols, int if (d_nav.have_new_ephemeris() == true) { // get object for this SV (mandatory) - d_nav.i_satellite_PRN = d_satellite.get_PRN(); - std::shared_ptr tmp_obj = std::make_shared(d_nav.get_ephemeris()); + std::shared_ptr tmp_obj = std::make_shared(d_nav.get_ephemeris()); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite; std::cout << "New BEIDOU B1I DNAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << std::endl; @@ -329,7 +287,7 @@ void beidou_b1i_telemetry_decoder_cc::decode_subframe(double *frame_symbols, int std::shared_ptr tmp_obj = std::make_shared(d_nav.get_iono()); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); LOG(INFO) << "BEIDOU DNAV Iono have been received in channel" << d_channel << " from satellite " << d_satellite; - std::cout << "New BEIDOU B1I DNAV Iono message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << std::endl; + std::cout << "New BEIDOU B1I DNAV Iono message received in channel " << d_channel << ": Iono model parameters from satellite " << d_satellite << std::endl; } if (d_nav.have_new_almanac() == true) { @@ -344,9 +302,57 @@ void beidou_b1i_telemetry_decoder_cc::decode_subframe(double *frame_symbols, int void beidou_b1i_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite) { + uint32_t sat_prn = 0; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite; DLOG(INFO) << "Navigation Satellite set to " << d_satellite; + + // Update satellite information for DNAV decoder + sat_prn = d_satellite.get_PRN(); + d_nav.i_satellite_PRN = sat_prn; + + // Update tel dec parameters for D2 NAV Messages + if ( sat_prn > 0 and sat_prn < 6 ) + { + // Clear values from previous declaration + volk_gnsssdr_free(d_preamble_samples); + volk_gnsssdr_free(d_secondary_code_symbols); + volk_gnsssdr_free(d_subframe_symbols); + + d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D2NAV_SYMBOL_RATE_SPS; + d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; + d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol; + d_secondary_code_symbols = nullptr; + d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS*d_samples_per_symbol; + d_subframe_length_symbols = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; + + // Setting samples of preamble code + int32_t n = 0; + for (int32_t i = 0; i < d_symbols_per_preamble; i++) + { + int32_t m = 0; + if (BEIDOU_DNAV_PREAMBLE.at(i) == '1') + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = 1; + n++; + } + } + else + { + for (uint32_t j = 0; j < d_samples_per_symbol; j++) + { + d_preamble_samples[n] = -1; + n++; + } + } + } + + d_subframe_symbols = static_cast(volk_gnsssdr_malloc(d_subframe_length_symbols * sizeof(double), volk_gnsssdr_get_alignment())); + d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS*d_samples_per_symbol + d_samples_per_preamble; + } } diff --git a/src/core/system_parameters/Beidou_B1I.h b/src/core/system_parameters/Beidou_B1I.h index f7ff788ca..bb3310a40 100644 --- a/src/core/system_parameters/Beidou_B1I.h +++ b/src/core/system_parameters/Beidou_B1I.h @@ -1,8 +1,8 @@ /*! * \file beidou_b1I.h - * \brief Defines system parameters for BeiDou B1I signal and D1 NAV data + * \brief Defines system parameters for BeiDou B1I signal and DNAV data * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com - * + * \author Damian Miralles, 2018. dmiralles2009@gmail.com * ------------------------------------------------------------------------- * * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) @@ -114,8 +114,8 @@ const std::vector > D1_PNUM( { {44,7} } ); // DNAV SCALE FACTORS // EPH const double D1_TOC_LSB = TWO_P3; -const double D1_TGD1_LSB = 0.1; -const double D1_TGD2_LSB = 0.1; +const double D1_TGD1_LSB = 0.1e-9; +const double D1_TGD2_LSB = 0.1e-9; const double D1_ALPHA0_LSB = TWO_N30; const double D1_ALPHA1_LSB = TWO_N27; const double D1_ALPHA2_LSB = TWO_N24; @@ -299,7 +299,7 @@ const std::vector > D2_A0( { {101,12},{121,12} } ); const std::vector > D2_A1_MSB( { {133,4} } ); // D2 NAV, SUBFRAME 1, PAGE 4 -const std::vector > D2_A1_LSB( { {47,6}, {121, 12} } ); +const std::vector > D2_A1_LSB( { {47,6}, {61, 12} } ); const std::vector > D2_A2( { {73,10}, {91,1} } ); const std::vector > D2_AODE( { {92,5} } ); const std::vector > D2_DELTA_N( { {97,16} } ); diff --git a/src/core/system_parameters/beidou_dnav_ephemeris.cc b/src/core/system_parameters/beidou_dnav_ephemeris.cc index b80c0d28c..c2e941c32 100644 --- a/src/core/system_parameters/beidou_dnav_ephemeris.cc +++ b/src/core/system_parameters/beidou_dnav_ephemeris.cc @@ -44,7 +44,7 @@ Beidou_Dnav_Ephemeris::Beidou_Dnav_Ephemeris() d_Delta_n = 0; d_M_0 = 0; d_Cuc = 0; - d_e_eccentricity = 0; + d_eccentricity = 0; d_Cus = 0; d_sqrt_A = 0; d_Toe = 0; @@ -70,6 +70,9 @@ Beidou_Dnav_Ephemeris::Beidou_Dnav_Ephemeris() d_spare1 = 0; d_spare2 = 0; + i_sig_type = 0; + i_nav_type = 0; + d_A_f0 = 0; // Coefficient 0 of code phase offset model [s] d_A_f1 = 0; // Coefficient 1 of code phase offset model [s/s] d_A_f2 = 0; // Coefficient 2 of code phase offset model [s/s^2] @@ -164,7 +167,7 @@ double Beidou_Dnav_Ephemeris::sv_clock_relativistic_term(double transmitTime) for (int ii = 1; ii < 20; ii++) { E_old = E; - E = M + d_e_eccentricity * sin(E); + E = M + d_eccentricity * sin(E); dE = fmod(E - E_old, 2.0 * BEIDOU_PI); if (fabs(dE) < 1e-12) { @@ -174,7 +177,7 @@ double Beidou_Dnav_Ephemeris::sv_clock_relativistic_term(double transmitTime) } // Compute relativistic correction term - d_dtr = BEIDOU_F * d_e_eccentricity * d_sqrt_A * sin(E); + d_dtr = BEIDOU_F * d_eccentricity * d_sqrt_A * sin(E); return d_dtr; } @@ -223,7 +226,7 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime) for (int ii = 1; ii < 20; ii++) { E_old = E; - E = M + d_e_eccentricity * sin(E); + E = M + d_eccentricity * sin(E); dE = fmod(E - E_old, 2.0 * BEIDOU_PI); if (fabs(dE) < 1e-12) { @@ -233,8 +236,8 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime) } // Compute the true anomaly - double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); - double tmp_X = cos(E) - d_e_eccentricity; + double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E); + double tmp_X = cos(E) - d_eccentricity; nu = atan2(tmp_Y, tmp_X); // Compute angle phi (argument of Latitude) @@ -247,7 +250,7 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime) u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi); // Correct radius - r = a * (1.0 - d_e_eccentricity*cos(E)) + d_Crc * cos(2.0 * phi) + d_Crs * sin(2.0 * phi); + r = a * (1.0 - d_eccentricity*cos(E)) + d_Crc * cos(2.0 * phi) + d_Crs * sin(2.0 * phi); // Correct inclination i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi); @@ -275,7 +278,7 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime) double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; /* relativity correction */ - dtr_s -= 2.0 * sqrt(BEIDOU_GM * a) * d_e_eccentricity * sin(E) / (BEIDOU_C_m_s * BEIDOU_C_m_s); + dtr_s -= 2.0 * sqrt(BEIDOU_GM * a) * d_eccentricity * sin(E) / (BEIDOU_C_m_s * BEIDOU_C_m_s); return dtr_s; } diff --git a/src/core/system_parameters/beidou_dnav_ephemeris.h b/src/core/system_parameters/beidou_dnav_ephemeris.h index cc6177b75..aae35a412 100644 --- a/src/core/system_parameters/beidou_dnav_ephemeris.h +++ b/src/core/system_parameters/beidou_dnav_ephemeris.h @@ -63,7 +63,7 @@ public: double d_Delta_n; //!< Mean Motion Difference From Computed Value [semi-circles/s] double d_M_0; //!< Mean Anomaly at Reference Time [semi-circles] double d_Cuc; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] - double d_e_eccentricity; //!< Eccentricity [dimensionless] + double d_eccentricity; //!< Eccentricity [dimensionless] double d_Cus; //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad] double d_sqrt_A; //!< Square Root of the Semi-Major Axis [sqrt(m)] double d_Toe; //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] @@ -85,6 +85,9 @@ public: double d_AODE; //!< Age of Data, Ephemeris int i_AODO; //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s] + int i_sig_type; //!< BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q) */ + int i_nav_type; //!< BDS: nav type (0:unknown,1:IGSO/MEO,2:GEO) */ + bool b_fit_interval_flag;//!< indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours. double d_spare1; double d_spare2; @@ -93,8 +96,6 @@ public: double d_A_f1; //!< Coefficient 1 of code phase offset model [s/s] double d_A_f2; //!< Coefficient 2 of code phase offset model [s/s^2] - // Flags - /*! \brief If true, enhanced level of integrity assurance. * * If false, indicates that the conveying signal is provided with the legacy level of integrity assurance. @@ -142,7 +143,7 @@ public: archive & make_nvp("d_Delta_n", d_Delta_n); //!< Mean Motion Difference From Computed Value [semi-circles/s] 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_e_eccentricity", d_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_sqrt_A", d_sqrt_A); //!< Square Root of the Semi-Major Axis [sqrt(m)] archive & make_nvp("d_Toe", d_Toe); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] diff --git a/src/core/system_parameters/beidou_dnav_navigation_message.cc b/src/core/system_parameters/beidou_dnav_navigation_message.cc index 393939aeb..02af65c07 100644 --- a/src/core/system_parameters/beidou_dnav_navigation_message.cc +++ b/src/core/system_parameters/beidou_dnav_navigation_message.cc @@ -3,7 +3,7 @@ m * \file beidou_navigation_message.cc * \brief Implementation of a BeiDou D1 NAV Data message decoder as described in BeiDou ICD Version 2.1 * * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com - * + * \author Damian Miralles, 2018. dmiralles2009@gmail.com * ------------------------------------------------------------------------- * * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) @@ -59,8 +59,8 @@ void Beidou_Dnav_Navigation_Message::reset() flag_sf1_p10 = false; // D2 NAV Decoding UNique Attributes - d_a1_msb = 0; - d_a1_lsb = 0; + d_A_f1_msb = 0; + d_A_f1_lsb = 0; d_Cuc_msb = 0; d_Cuc_lsb = 0; d_eccentricity_msb = 0; @@ -187,9 +187,9 @@ void Beidou_Dnav_Navigation_Message::reset() d_M0_ALMANAC = 0; almanac_WN = 0; d_toa2 = 0; - d_a0 = 0; - d_a1 = 0; - d_a2 = 0; + d_A_f0 = 0; + d_A_f1 = 0; + d_A_f2 = 0; auto gnss_sat = Gnss_Satellite(); std::string _system ("Beidou"); @@ -489,19 +489,17 @@ int Beidou_Dnav_Navigation_Message::d1_subframe_decoder(std::string const &subfr d_beta3 = static_cast(read_navigation_signed(subframe_bits, D1_BETA3)); d_beta3 = d_beta3 * D1_BETA3_LSB; - d_a2 = static_cast(read_navigation_signed(subframe_bits, D1_A2)); - d_a2 = d_a2 * D1_A2_LSB; - - d_a0 = static_cast(read_navigation_signed(subframe_bits, D1_A0)); - d_a0 = d_a0 * D1_A0_LSB; - - d_a1 = static_cast(read_navigation_signed(subframe_bits, D1_A1)); - d_a1 = d_a1 * D1_A1_LSB; + d_A_f2 = static_cast(read_navigation_signed(subframe_bits, D1_A2)); + d_A_f2 = d_A_f2 * D1_A2_LSB; + d_A_f0 = static_cast(read_navigation_signed(subframe_bits, D1_A0)); + d_A_f0 = d_A_f0 * D1_A0_LSB; + d_A_f1 = static_cast(read_navigation_signed(subframe_bits, D1_A1)); + d_A_f1 = d_A_f1 * D1_A1_LSB; d_AODE = static_cast(read_navigation_unsigned(subframe_bits, D1_AODE)); // Set system flags for message reception - flag_sf1 = true; + flag_d1_sf1 = true; flag_iono_valid = true; flag_utc_model_valid = true; flag_new_SOW_available = true; @@ -539,7 +537,7 @@ int Beidou_Dnav_Navigation_Message::d1_subframe_decoder(std::string const &subfr d_Toe_sf2 = static_cast((static_cast(d_Toe_sf2) << 15)); // Set system flags for message reception - flag_sf2 = true; + flag_d1_sf2 = true; flag_new_SOW_available = true; break; @@ -573,7 +571,7 @@ int Beidou_Dnav_Navigation_Message::d1_subframe_decoder(std::string const &subfr d_OMEGA = d_OMEGA * D1_OMEGA_LSB; // Set system flags for message reception - flag_sf_3 = true; + flag_d1_sf3 = true; flag_new_SOW_available = true; break; @@ -613,7 +611,7 @@ int Beidou_Dnav_Navigation_Message::d1_subframe_decoder(std::string const &subfr d_M0_ALMANAC = d_M0_ALMANAC * D1_M0_ALMANAC_LSB; // Set system flags for message reception - flag_sf_4 = true; + flag_d1_sf4 = true; flag_new_SOW_available = true; break; @@ -781,7 +779,7 @@ int Beidou_Dnav_Navigation_Message::d1_subframe_decoder(std::string const &subfr } // Set system flags for message reception - flag_sf_5 = true; + flag_d1_sf5 = true; flag_new_SOW_available = true; break; @@ -815,6 +813,7 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr switch(page_ID) { case 1: + d_SOW = static_cast(read_navigation_unsigned(subframe_bits, D2_SOW)); i_SV_health = static_cast(read_navigation_unsigned(subframe_bits, D2_SAT_H1)); d_AODC = static_cast(read_navigation_unsigned(subframe_bits, D2_AODC)); i_SV_accuracy = static_cast(read_navigation_unsigned(subframe_bits, D2_URAI)); // (20.3.3.3.1.3) @@ -824,14 +823,15 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr // Set system flags for message reception flag_sf1_p1 = true; + flag_new_SOW_available = true; break; case 2: + d_SOW = static_cast(read_navigation_unsigned(subframe_bits, D2_SOW)); d_alpha0 = static_cast(read_navigation_signed(subframe_bits, D2_ALPHA0))*D1_ALPHA0_LSB; d_alpha1 = static_cast(read_navigation_signed(subframe_bits, D2_ALPHA1))*D1_ALPHA1_LSB; d_alpha2 = static_cast(read_navigation_signed(subframe_bits, D2_ALPHA2))*D1_ALPHA2_LSB; d_alpha3 = static_cast(read_navigation_signed(subframe_bits, D1_ALPHA3))*D1_ALPHA3_LSB; - d_beta0 = static_cast(read_navigation_signed(subframe_bits, D2_BETA0))*D1_BETA0_LSB; d_beta1 = static_cast(read_navigation_signed(subframe_bits, D2_BETA1))*D1_BETA1_LSB; d_beta2 = static_cast(read_navigation_signed(subframe_bits, D2_BETA2))*D1_BETA2_LSB; @@ -839,75 +839,114 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr // Set system flags for message reception flag_sf1_p2 = true; + flag_iono_valid = true; + flag_new_SOW_available = true; break; case 3: - d_a0 = static_cast(read_navigation_signed(subframe_bits, D2_A0))*D1_A0_LSB; - d_a1_msb = static_cast(read_navigation_signed(subframe_bits, D2_A1_MSB)); + d_SOW = static_cast(read_navigation_unsigned(subframe_bits, D2_SOW)); + d_A_f0 = static_cast(read_navigation_signed(subframe_bits, D2_A0))*D1_A0_LSB; + d_A_f1_msb = static_cast(read_navigation_signed(subframe_bits, D2_A1_MSB)); + // Adjust for lsb in next page + d_A_f1_msb = static_cast((static_cast(d_A_f1_msb) << 18)); // Set system flags for message reception flag_sf1_p3 = true; + flag_new_SOW_available = true; break; case 4: - d_a1_lsb = static_cast(read_navigation_signed(subframe_bits, D2_A1_LSB)); - d_a2 = static_cast(read_navigation_signed(subframe_bits, D1_A2))*D1_A2_LSB; + d_SOW = static_cast(read_navigation_unsigned(subframe_bits, D2_SOW)); + d_A_f1_lsb = static_cast(read_navigation_signed(subframe_bits, D2_A1_LSB)); + d_A_f2 = static_cast(read_navigation_signed(subframe_bits, D1_A2))*D1_A2_LSB; d_AODE = static_cast(read_navigation_unsigned(subframe_bits, D2_AODE)); - d_Delta_n = static_cast(read_navigation_unsigned(subframe_bits, D2_DELTA_N))*D1_DELTA_N_LSB; + d_Delta_n = static_cast(read_navigation_signed(subframe_bits, D2_DELTA_N))*D1_DELTA_N_LSB; d_Cuc_msb = static_cast(read_navigation_signed(subframe_bits, D2_CUC_MSB)); + // Adjust for lsb in next page + d_Cuc_msb = static_cast((static_cast(d_Cuc_msb) << 4)); // Set system flags for message reception flag_sf1_p4 = true; + flag_new_SOW_available = true; + break; case 5: + d_SOW = static_cast(read_navigation_unsigned(subframe_bits, D2_SOW)); d_Cuc_lsb = static_cast(read_navigation_signed(subframe_bits, D2_CUC_LSB)); d_M_0 = static_cast(read_navigation_signed(subframe_bits, D2_M0))*D1_M0_LSB; d_Cus = static_cast(read_navigation_signed(subframe_bits, D2_CUS))*D1_CUS_LSB; d_eccentricity_msb = static_cast(read_navigation_unsigned(subframe_bits, D2_E_MSB)); + // Adjust for lsb in next page (shift number of lsb to the left) + d_eccentricity_msb = static_cast((static_cast(d_eccentricity_msb) << 22)); // Set system flags for message reception flag_sf1_p5 = true; + flag_new_SOW_available = true; + break; case 6: + d_SOW = static_cast(read_navigation_unsigned(subframe_bits, D2_SOW)); d_eccentricity_lsb = static_cast(read_navigation_unsigned(subframe_bits, D2_E_LSB)); d_sqrt_A = static_cast(read_navigation_unsigned(subframe_bits, D2_SQRT_A))*D1_SQRT_A_LSB; d_Cic_msb = static_cast(read_navigation_signed(subframe_bits, D2_CIC_MSB)); + // Adjust for lsb in next page (shift number of lsb to the left) + d_Cic_msb = static_cast((static_cast(d_Cic_msb) << 8)); // Set system flags for message reception flag_sf1_p6 = true; + flag_new_SOW_available = true; + break; case 7: + d_SOW = static_cast(read_navigation_unsigned(subframe_bits, D2_SOW)); d_Cic_lsb = static_cast(read_navigation_signed(subframe_bits, D2_CIC_LSB)); d_Cis = static_cast(read_navigation_signed(subframe_bits, D2_CIS))*D1_CIS_LSB; d_Toe = static_cast(read_navigation_unsigned(subframe_bits, D2_TOE))*D1_TOE_LSB; d_i_0_msb = static_cast(read_navigation_signed(subframe_bits, D2_I0_MSB)); + // Adjust for lsb in next page (shift number of lsb to the left) + d_i_0_msb = static_cast((static_cast(d_i_0_msb) << 11)); // Set system flags for message reception flag_sf1_p7 = true; + flag_new_SOW_available = true; + break; case 8: + d_SOW = static_cast(read_navigation_unsigned(subframe_bits, D2_SOW)); d_i_0_lsb = static_cast(read_navigation_signed(subframe_bits, D2_I0_LSB)); d_Crc = static_cast(read_navigation_signed(subframe_bits, D2_CRC))*D1_CRC_LSB; d_Crs = static_cast(read_navigation_signed(subframe_bits, D2_CRS))*D1_CRS_LSB; d_OMEGA_DOT_msb = static_cast(read_navigation_signed(subframe_bits, D2_OMEGA_DOT_MSB)); + // Adjust for lsb in next page (shift number of lsb to the left) + d_OMEGA_DOT_msb = static_cast((static_cast(d_OMEGA_DOT_msb) << 5)); // Set system flags for message reception flag_sf1_p8 = true; + flag_new_SOW_available = true; + break; case 9: + d_SOW = static_cast(read_navigation_unsigned(subframe_bits, D2_SOW)); d_OMEGA_DOT_lsb = static_cast(read_navigation_signed(subframe_bits, D2_OMEGA_DOT_LSB)); d_OMEGA0 = static_cast(read_navigation_signed(subframe_bits, D2_OMEGA0))*D1_OMEGA0_LSB; d_OMEGA_msb = static_cast(read_navigation_signed(subframe_bits, D2_OMEGA_MSB)); + // Adjust for lsb in next page (shift number of lsb to the left) + d_OMEGA_msb = static_cast((static_cast(d_OMEGA_msb) << 5)); // Set system flags for message reception flag_sf1_p9 = true; + flag_new_SOW_available = true; + break; case 10: - d_OMEGA_lsb = static_cast(read_navigation_signed(subframe_bits, D2_OMEGA_LSB)); - d_IDOT = static_cast(read_navigation_signed(subframe_bits, D1_IDOT))*D1_IDOT_LSB; + d_SOW = static_cast(read_navigation_unsigned(subframe_bits, D2_SOW)); + d_OMEGA_lsb = static_cast(read_navigation_unsigned(subframe_bits, D2_OMEGA_LSB)); + d_IDOT = static_cast(read_navigation_signed(subframe_bits, D2_IDOT))*D1_IDOT_LSB; // Set system flags for message reception flag_sf1_p10 = true; + flag_new_SOW_available = true; + break; default: break; @@ -921,166 +960,13 @@ int Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const &subfr case 3: // --- It is subframe 3 ------------------------------------- - break; case 4: // --- It is subframe 4 ---------- Almanac, ionospheric model, UTC parameters, SV health (PRN: 25-32) - d_SOW_SF4 = static_cast(read_navigation_unsigned(subframe_bits, D1_SOW)); - d_SOW = d_SOW_SF4; // Set transmission time - - d_SQRT_A_ALMANAC = static_cast(read_navigation_unsigned(subframe_bits, D1_SQRT_A_ALMANAC)); - d_SQRT_A_ALMANAC = d_SQRT_A_ALMANAC * D1_SQRT_A_ALMANAC_LSB; - - d_A1_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_A1_ALMANAC)); - d_A1_ALMANAC = d_A1_ALMANAC * D1_A1_ALMANAC_LSB; - - d_A0_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_A0_ALMANAC)); - d_A0_ALMANAC = d_A0_ALMANAC * D1_A0_ALMANAC_LSB; - - d_OMEGA0_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_OMEGA0_ALMANAC)); - d_OMEGA0_ALMANAC = d_OMEGA0_ALMANAC * D1_OMEGA0_ALMANAC_LSB; - - d_E_ALMANAC = static_cast(read_navigation_unsigned(subframe_bits, D1_E)); - d_E_ALMANAC = d_E_ALMANAC * D1_E_ALMANAC_LSB; - - d_DELTA_I = static_cast(read_navigation_signed(subframe_bits, D1_DELTA_I)); - d_DELTA_I = D1_DELTA_I_LSB; - - d_TOA = static_cast(read_navigation_unsigned(subframe_bits, D1_TOA)); - d_TOA = d_TOA * D1_TOA_LSB; - - d_OMEGA_DOT_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_OMEGA_DOT_ALMANAC)); - d_OMEGA_DOT_ALMANAC = D1_OMEGA_DOT_ALMANAC_LSB; - - d_OMEGA_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_OMEGA_ALMANAC)); - d_OMEGA_ALMANAC = d_OMEGA_ALMANAC * D1_OMEGA_ALMANAC_LSB; - - d_M0_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_M0)); - d_M0_ALMANAC = d_M0_ALMANAC * D1_M0_ALMANAC_LSB; - - // Set system flags for message reception - flag_sf_4 = true; - flag_new_SOW_available = true; break; case 5://--- It is subframe 5 -----------------almanac health (PRN: 1-24) and Almanac reference week number and time. - int SV_page_5; - d_SOW_SF5 = static_cast(read_navigation_unsigned(subframe_bits, D1_SOW)); - d_SOW = d_SOW_SF5; // Set transmission time - - SV_page_5 = static_cast(read_navigation_unsigned(subframe_bits, D1_PNUM)); - - if (SV_page_5 < 7) - { - d_SOW_SF4 = static_cast(read_navigation_unsigned(subframe_bits, D1_SOW)); - d_SQRT_A_ALMANAC = static_cast(read_navigation_unsigned(subframe_bits, D1_SQRT_A_ALMANAC)); - d_SQRT_A_ALMANAC = d_SQRT_A_ALMANAC * D1_SQRT_A_ALMANAC_LSB; - - d_A1_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_A1_ALMANAC)); - d_A1_ALMANAC = d_A1_ALMANAC * D1_A1_ALMANAC_LSB; - - d_A0_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_A0_ALMANAC)); - d_A0_ALMANAC = d_A0_ALMANAC * D1_A0_ALMANAC_LSB; - - d_OMEGA0_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_OMEGA0_ALMANAC)); - d_OMEGA0_ALMANAC = d_OMEGA0_ALMANAC * D1_OMEGA0_ALMANAC_LSB; - - d_E_ALMANAC = static_cast(read_navigation_unsigned(subframe_bits, D1_E)); - d_E_ALMANAC = d_E_ALMANAC * D1_E_ALMANAC_LSB; - - d_DELTA_I = static_cast(read_navigation_signed(subframe_bits, D1_DELTA_I)); - d_DELTA_I = D1_DELTA_I_LSB; - - d_TOA = static_cast(read_navigation_unsigned(subframe_bits, D1_TOA)); - d_TOA = d_TOA * D1_TOA_LSB; - - d_OMEGA_DOT_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_OMEGA_DOT_ALMANAC)); - d_OMEGA_DOT_ALMANAC = D1_OMEGA_DOT_ALMANAC_LSB; - - d_OMEGA_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_OMEGA_ALMANAC)); - d_OMEGA_ALMANAC = d_OMEGA_ALMANAC * D1_OMEGA_ALMANAC_LSB; - - d_M0_ALMANAC = static_cast(read_navigation_signed(subframe_bits, D1_M0)); - d_M0_ALMANAC = d_M0_ALMANAC * D1_M0_ALMANAC_LSB; - - } - - if (SV_page_5 == 7) - { - //! \TODO read almanac - almanacHealth[1] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA1)); - almanacHealth[2] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA2)); - almanacHealth[3] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA3)); - almanacHealth[4] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA4)); - almanacHealth[5] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA5)); - almanacHealth[6] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA6)); - almanacHealth[7] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA7)); - almanacHealth[8] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA8)); - almanacHealth[9] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA9)); - almanacHealth[10] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA10)); - almanacHealth[11] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA11)); - almanacHealth[12] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA12)); - almanacHealth[13] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA13)); - almanacHealth[14] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA14)); - almanacHealth[15] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA15)); - almanacHealth[16] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA16)); - almanacHealth[17] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA17)); - almanacHealth[18] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA18)); - almanacHealth[19] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA19)); - } - if (SV_page_5 == 8) // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) - { - almanacHealth[20] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA20)); - almanacHealth[21] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA21)); - almanacHealth[22] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA22)); - almanacHealth[23] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA23)); - almanacHealth[24] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA24)); - almanacHealth[25] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA25)); - almanacHealth[26] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA26)); - almanacHealth[27] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA27)); - almanacHealth[28] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA28)); - almanacHealth[29] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA29)); - almanacHealth[30] = static_cast(read_navigation_unsigned(subframe_bits, D1_HEA30)); - almanac_WN = static_cast(read_navigation_unsigned(subframe_bits, D1_WNA)); - d_toa2 = static_cast(read_navigation_unsigned(subframe_bits, D1_TOA2)); - - } - - if (SV_page_5 == 9) // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) - { - d_A0GPS = static_cast(read_navigation_signed(subframe_bits, D1_A0GPS)); - d_A0GPS = d_A0GPS * D1_A0GPS_LSB; - - d_A1GPS = static_cast(read_navigation_signed(subframe_bits, D1_A1GPS)); - d_A1GPS = d_A1GPS * D1_A1GPS_LSB; - - d_A0GAL = static_cast(read_navigation_signed(subframe_bits, D1_A0GAL)); - d_A0GAL = d_A0GAL * D1_A0GAL_LSB; - - d_A1GAL = static_cast(read_navigation_signed(subframe_bits, D1_A1GAL)); - d_A1GAL = d_A1GAL* D1_A1GAL_LSB; - - d_A0GLO = static_cast(read_navigation_signed(subframe_bits, D1_A0GLO)); - d_A0GLO = d_A0GLO * D1_A0GLO_LSB; - - d_A1GLO = static_cast(read_navigation_signed(subframe_bits, D1_A1GLO)); - d_A1GLO = d_A1GLO* D1_A1GLO_LSB; - } - if (SV_page_5 == 10) - { - d_DeltaT_LS = static_cast(read_navigation_signed(subframe_bits, D1_DELTA_T_LS)); - d_DeltaT_LSF = static_cast(read_navigation_signed(subframe_bits, D1_DELTA_T_LSF)); - i_WN_LSF = static_cast(read_navigation_signed(subframe_bits, D1_WN_LSF)); - d_A0UTC = static_cast(read_navigation_signed(subframe_bits, D1_A0UTC)); - d_A0UTC = d_A0GPS * D1_A0GPS_LSB; - d_A1UTC = static_cast(read_navigation_signed(subframe_bits, D1_A1UTC)); - d_A1UTC = d_A1UTC * D1_A1UTC_LSB; - } - - // Set system flags for message reception - flag_sf_5 = true; - flag_new_SOW_available = true; break; @@ -1140,51 +1026,91 @@ double Beidou_Dnav_Navigation_Message::utc_time(const double beidoutime_correcte Beidou_Dnav_Ephemeris Beidou_Dnav_Navigation_Message::get_ephemeris() { - Beidou_Dnav_Ephemeris ephemeris; - ephemeris.i_satellite_PRN = i_satellite_PRN; - ephemeris.d_TOW = d_SOW; - ephemeris.d_Crs = d_Crs; - ephemeris.d_Delta_n = d_Delta_n; - ephemeris.d_M_0 = d_M_0; - ephemeris.d_Cuc = d_Cuc; - ephemeris.d_e_eccentricity = d_eccentricity; - ephemeris.d_Cus = d_Cus; - ephemeris.d_sqrt_A = d_sqrt_A; - ephemeris.d_Toe = ((d_Toe_sf2 + d_Toe_sf3) * D1_TOE_LSB) ; - ephemeris.d_Toc = d_Toc; - ephemeris.d_Cic = d_Cic; - ephemeris.d_OMEGA0 = d_OMEGA0; - ephemeris.d_Cis = d_Cis; - ephemeris.d_i_0 = d_i_0; - ephemeris.d_Crc = d_Crc; - ephemeris.d_OMEGA = d_OMEGA; - ephemeris.d_OMEGA_DOT = d_OMEGA_DOT; - ephemeris.d_IDOT = d_IDOT; - ephemeris.i_BEIDOU_week = i_BEIDOU_week; - ephemeris.i_SV_accuracy = i_SV_accuracy; - ephemeris.i_SV_health = i_SV_health; - ephemeris.d_TGD1 = d_TGD1; - ephemeris.d_AODC = d_AODC; - ephemeris.d_AODE = d_AODE; - ephemeris.b_fit_interval_flag = b_fit_interval_flag; - ephemeris.d_spare1 = d_spare1; - ephemeris.d_spare2 = d_spare2; - ephemeris.d_A_f0 = d_A_f0; - ephemeris.d_A_f1 = d_A_f1; - ephemeris.d_A_f2 = d_A_f2; - ephemeris.b_integrity_status_flag = b_integrity_status_flag; - ephemeris.b_alert_flag = b_alert_flag; - ephemeris.b_antispoofing_flag = b_antispoofing_flag; - ephemeris.d_satClkDrift = d_satClkDrift; - ephemeris.d_dtr = d_dtr; - ephemeris.d_satpos_X = d_satpos_X; - ephemeris.d_satpos_Y = d_satpos_Y; - ephemeris.d_satpos_Z = d_satpos_Z; - ephemeris.d_satvel_X = d_satvel_X; - ephemeris.d_satvel_Y = d_satvel_Y; - ephemeris.d_satvel_Z = d_satvel_Z; + Beidou_Dnav_Ephemeris eph; - return ephemeris; + if(i_satellite_PRN > 0 and i_satellite_PRN < 6) + { + // Order as given by eph_t in rtklib + eph.i_satellite_PRN = i_satellite_PRN; + eph.d_AODC = d_AODC; + eph.d_AODE = d_AODE; + eph.i_SV_accuracy = i_SV_accuracy; + eph.i_SV_health = i_SV_health; + eph.i_BEIDOU_week = i_BEIDOU_week; + eph.i_sig_type = 1; + eph.i_nav_type = 2; + + eph.d_TOW = d_SOW; + eph.d_Toe = d_Toe; + eph.d_Toc = d_Toc; + + eph.d_sqrt_A = d_sqrt_A; + eph.d_eccentricity = (d_eccentricity_msb + d_eccentricity_lsb)*D1_E_LSB; + eph.d_i_0 = (d_i_0_msb + d_i_0_lsb)*D1_I0_LSB; + eph.d_OMEGA0 = d_OMEGA0; + d_OMEGA_lsb = (d_OMEGA_msb < 0)?-d_OMEGA_lsb:d_OMEGA_lsb; + eph.d_OMEGA = (d_OMEGA_msb + d_OMEGA_lsb)*D1_OMEGA_LSB; + eph.d_M_0 = d_M_0; + eph.d_Delta_n = d_Delta_n; + + eph.d_OMEGA_DOT = (d_OMEGA_DOT_msb + d_OMEGA_DOT_lsb)*D1_OMEGA_DOT_LSB; + eph.d_IDOT = d_IDOT; + + eph.d_Crc = d_Crc; + eph.d_Crs = d_Crs; + eph.d_Cuc = (d_Cuc_msb + d_Cuc_lsb)*D1_CUC_LSB; + eph.d_Cus = d_Cus; + eph.d_Cic = (d_Cic_msb + d_Cic_lsb)*D1_CIC_LSB; + eph.d_Cis = d_Cis; + + eph.d_A_f0 = d_A_f0; + eph.d_A_f1 = (d_A_f1_msb + d_A_f1_lsb)*D1_A1_LSB; + eph.d_A_f2 = d_A_f2; + + eph.d_TGD1 = d_TGD1; + eph.d_TGD2 = d_TGD2; + } + else + { + eph.i_satellite_PRN = i_satellite_PRN; + eph.d_AODC = d_AODC; + eph.d_AODE = d_AODE; + eph.i_SV_accuracy = i_SV_accuracy; + eph.i_SV_health = i_SV_health; + eph.i_BEIDOU_week = i_BEIDOU_week; + eph.i_sig_type = 1; + eph.i_nav_type = 1; // MEO/IGSO + + eph.d_TOW = d_SOW; + eph.d_Toe = ((d_Toe_sf2 + d_Toe_sf3) * D1_TOE_LSB) ; + eph.d_Toc = d_Toc; + + eph.d_sqrt_A = d_sqrt_A; + eph.d_eccentricity = d_eccentricity; + eph.d_i_0 = d_i_0; + eph.d_OMEGA0 = d_OMEGA0; + eph.d_OMEGA = d_OMEGA; + eph.d_M_0 = d_M_0; + eph.d_Delta_n = d_Delta_n; + eph.d_OMEGA_DOT = d_OMEGA_DOT; + eph.d_IDOT = d_IDOT; + + eph.d_Crc = d_Crc; + eph.d_Crs = d_Crs; + eph.d_Cuc = d_Cuc; + eph.d_Cus = d_Cus; + eph.d_Cic = d_Cic; + eph.d_Cis = d_Cis; + + eph.d_A_f0 = d_A_f0; + eph.d_A_f1 = d_A_f1; + eph.d_A_f2 = d_A_f2; + + eph.d_TGD1 = d_TGD1; + eph.d_TGD2 = d_TGD2; + } + + return eph; } Beidou_Dnav_Iono Beidou_Dnav_Navigation_Message::get_iono() @@ -1225,22 +1151,50 @@ Beidou_Dnav_Utc_Model Beidou_Dnav_Navigation_Message::get_utc_model() bool Beidou_Dnav_Navigation_Message::have_new_ephemeris() // Check if we have a new ephemeris stored in the galileo navigation class { - if ((flag_sf1 == true) and (flag_sf2 == true) and (flag_sf_3 == true)) - { - // if all ephemeris pages have the same IOD, then they belong to the same block - if (d_previous_aode != d_AODE) + if(i_satellite_PRN > 0 and i_satellite_PRN < 6) + { + if ((flag_sf1_p1 == true) and (flag_sf1_p2 == true) and (flag_sf1_p3 == true) and + (flag_sf1_p4 == true) and (flag_sf1_p5 == true) and (flag_sf1_p6 == true) and + (flag_sf1_p7 == true) and (flag_sf1_p8 == true) and (flag_sf1_p9 == true) and + (flag_sf1_p10 == true)) { - flag_sf1 = false; // clear the flag - flag_sf2 = false; // clear the flag - flag_sf_3 = false; // clear the flag - flag_eph_valid = true; - // Update the time of ephemeris information - d_previous_aode = d_AODE; - DLOG(INFO) << "Beidou Dnav Ephemeris (1, 2) have been received and belong to the same batch" << std::endl; - return true; + // if all ephemeris pages have the same IOD, then they belong to the same block + if (d_previous_aode != d_AODE) + { + // Clear flags for all received pages + flag_sf1_p1 = false; flag_sf1_p2 = false; flag_sf1_p3 = false; + flag_sf1_p4 = false; flag_sf1_p5 = false; flag_sf1_p6 = false; + flag_sf1_p7 = false; flag_sf1_p8 = false; flag_sf1_p9 = false; + flag_sf1_p10 = false; + flag_eph_valid = true; + // Update the time of ephemeris information + d_previous_aode = d_AODE; + DLOG(INFO) << "Beidou D2 NAV Ephemeris have been received and belong to the same batch" << std::endl; + return true; + + } } - } + } + else + { + if ((flag_d1_sf1 == true) and (flag_d1_sf2 == true) and (flag_d1_sf3 == true)) + { + // if all ephemeris pages have the same IOD, then they belong to the same block + if (d_previous_aode != d_AODE) + { + // Clear flags for all received subframes + flag_d1_sf1 = false; flag_d1_sf2 = false; flag_d1_sf3 = false; + + flag_eph_valid = true; + // Update the time of ephemeris information + d_previous_aode = d_AODE; + DLOG(INFO) << "Beidou D1 NAV Ephemeris have been received and belong to the same batch" << std::endl; + return true; + + } + } + } return false; } @@ -1272,11 +1226,11 @@ bool Beidou_Dnav_Navigation_Message::have_new_utc_model() bool Beidou_Dnav_Navigation_Message::have_new_almanac() { - if ((flag_sf_4 == true) and (flag_sf_5 == true)) + if ((flag_d1_sf4 == true) and (flag_d1_sf5 == true)) { // All almanac have been received - flag_sf_4 = false; - flag_sf_5 = false; + flag_d1_sf4 = false; + flag_d1_sf5 = false; return true; } diff --git a/src/core/system_parameters/beidou_dnav_navigation_message.h b/src/core/system_parameters/beidou_dnav_navigation_message.h index accd440a6..750fbc3ff 100644 --- a/src/core/system_parameters/beidou_dnav_navigation_message.h +++ b/src/core/system_parameters/beidou_dnav_navigation_message.h @@ -73,11 +73,11 @@ public: bool flag_eph_valid; bool flag_utc_model_valid; bool flag_iono_valid; - bool flag_sf1; - bool flag_sf2; - bool flag_sf_3; - bool flag_sf_4; - bool flag_sf_5; + bool flag_d1_sf1; + bool flag_d1_sf2; + bool flag_d1_sf3; + bool flag_d1_sf4; + bool flag_d1_sf5; bool flag_new_SOW_available; bool flag_crc_test; double d_previous_aode; @@ -129,8 +129,8 @@ public: //broadcast orbit 6 int i_SV_accuracy; //!< User Range Accuracy (URA) index of the SV int i_SV_health; - double d_TGD1; //!< Estimated Group Delay Differential in B1 - double d_TGD2; //!< Estimated Group Delay Differential in B2 + double d_TGD1; //!< Estimated Group Delay Differential in B1 [s] + double d_TGD2; //!< Estimated Group Delay Differential in B2 [s] double d_AODC; //!< Age of Data, Clock //broadcast orbit 7 // int i_AODO; //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s] @@ -139,17 +139,13 @@ public: double d_spare1; double d_spare2; - double d_A_f0; //!< Coefficient 0 of code phase offset model [s] - double d_A_f1; //!< Coefficient 1 of code phase offset model [s/s] - double d_A_f2; //!< Coefficient 2 of code phase offset model [s/s^2] - - double d_a0; //!< Clock correction parameters - double d_a1; //!< Clock correction parameters - double d_a2; //!< Clock correction parameters + double d_A_f0; //!< Clock correction parameters. Coefficient 0 of code phase offset model [s] + double d_A_f1; //!< Clock correction parameters. Coefficient 1 of code phase offset model [s/s] + double d_A_f2; //!< Clock correction parameters. Coefficient 2 of code phase offset model [s/s^2] // D2 NAV Message Decoding - double d_a1_msb; //!< Clock correction parameters, D2 NAV MSB - double d_a1_lsb; //!< Clock correction parameters, D2 NAV LSB + double d_A_f1_msb; //!< Clock correction parameters, D2 NAV MSB + double d_A_f1_lsb; //!< Clock correction parameters, D2 NAV LSB double d_Cuc_msb; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] double d_Cuc_lsb; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] double d_eccentricity_msb; //!< Eccentricity [dimensionless]