From 85f7e333bb255cb17b87379b90d6cabdd758761b Mon Sep 17 00:00:00 2001 From: Damian Miralles Date: Wed, 23 Aug 2017 15:16:07 -0700 Subject: [PATCH] bug_fix: Fixes bugs in telemetry decoding interface Fixes several bugs with the telemetry decoder interface and clean up the code with unused methods and members of the ephemeris object --- src/algorithms/PVT/libs/rinex_printer.cc | 63 +- .../glonass_l1_ca_telemetry_decoder_cc.cc | 86 +-- .../glonass_l1_ca_telemetry_decoder_cc.h | 3 +- src/core/system_parameters/GLONASS_L1_CA.h | 15 +- .../glonass_gnav_ephemeris.cc | 661 ------------------ .../glonass_gnav_ephemeris.h | 48 -- .../glonass_gnav_utc_model.h | 2 + 7 files changed, 80 insertions(+), 798 deletions(-) diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index facaa6f5e..06508ee75 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -633,6 +633,8 @@ void Rinex_Printer::rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono void Rinex_Printer::rinex_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) { if(glonass_gnav_almanac.i_satellite_freq_channel){} //Avoid compiler warning + //Avoid compiler warning, there is not time system correction between Galileo and GLONASS + if(galileo_almanac.A_0G_10){} std::string line; stringVersion = "3.02"; version = 3; @@ -2047,6 +2049,8 @@ void Rinex_Printer::update_nav_header(std::fstream& out, const Gps_Iono& gps_ion void Rinex_Printer::update_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac) { if(glonass_gnav_almanac.i_satellite_freq_channel){} //Avoid compiler warning + //Avoid compiler warning, there is not time system correction between Galileo and GLONASS + if(galileo_almanac.A_0G_10){} std::vector data; std::string line_aux; @@ -3035,7 +3039,7 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::mapd_preambles_bits, (unsigned short int*)preambles_bits, GLONASS_GNAV_PREAMBLE_LENGTH_BITS*sizeof(unsigned short int)); // preamble bits to sampled symbols d_preambles_symbols = (signed int*)malloc(sizeof(signed int) * d_symbols_per_preamble); int n = 0; - for (int i = 0; i < GLONASS_GNAV_PREAMBLE_LENGTH_BITS; i++) + for (int i = 0; i < GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT; i++) { for (unsigned int j = 0; j < d_samples_per_symbol; j++) { @@ -130,40 +132,26 @@ glonass_l1_ca_telemetry_decoder_cc::~glonass_l1_ca_telemetry_decoder_cc() } -void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *page_part_symbols,int frame_length) +void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *frame_symbols,int frame_length) { - double page_part_symbols_deint[frame_length]; - - // 2. Viterbi decoder - // 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder) - // 2.2 Take into account the possible inversion of the polarity due to PLL lock at 180� - for (int i = 0; i < frame_length; i++) + // 1. Transform from symbols to bits + std::string frame_string; + for(int i = 0; i < (frame_length); i++) { - if ((i + 1) % 2 == 0) + if (frame_symbols[i] > 0) { - page_part_symbols_deint[i] = -page_part_symbols_deint[i]; - } - } - - int page_part_bits[frame_length/2]; - viterbi_decoder(page_part_symbols_deint, page_part_bits); - - // 3. Call the Galileo page decoder - std::string page_String; - for(int i = 0; i < (frame_length/2); i++) - { - if (page_part_bits[i] > 0) - { - page_String.push_back('1'); + frame_string.push_back('1'); } else { - page_String.push_back('0'); + frame_string.push_back('0'); } } + // 2. Call the GLONASS GNAV string decoder d_nav.decode_string(page_String); + // 3. Check operation executed correctly if(d_nav.flag_CRC_test == true) { LOG(INFO) << "GLONASS GNAV CRC correct on channel " << d_channel << " from satellite " << d_satellite; @@ -175,14 +163,11 @@ void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *page_part_symbols LOG(INFO) << "GLONASS GNAV CRC error on channel " << d_channel << " from satellite " << d_satellite; } - - // 4. Push the new navigation data to the queues if (d_nav.have_new_ephemeris() == true) { // get object for this SV (mandatory) std::shared_ptr tmp_obj = std::make_shared(d_nav.get_ephemeris()); - this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } @@ -196,19 +181,10 @@ void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *page_part_symbols { std::shared_ptr tmp_obj= std::make_shared(d_nav.get_almanac()); this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); - //debug - std::cout << "GLONASS GNAV almanac received!" << std::endl; - DLOG(INFO) << "Current parameters:"; - DLOG(INFO) << "d_TOW_at_current_symbol=" << d_TOW_at_current_symbol; - DLOG(INFO) << "d_nav.WN_0=" << d_nav.WN_0; - delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (d_TOW_at_current_symbol - tmp_obj->t_0G_10 + 604800 * (fmod((d_nav.WN_0 - tmp_obj->WN_0G_10), 64))); - DLOG(INFO) << "delta_t=" << delta_t << "[s]"; } } - - int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { @@ -226,7 +202,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib consume_each(1); d_flag_preamble = false; - unsigned int required_symbols=GLONASS_GNAV_PAGE_SYMBOLS+d_symbols_per_preamble; + unsigned int required_symbols=GLONASS_GNAV_FRAME_BITS+d_symbols_per_preamble; if (d_symbol_history.size()>required_symbols) { @@ -283,7 +259,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib // NEW GLONASS string received // 0. fetch the symbols into an array int frame_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble; - double page_part_symbols[frame_length]; + double frame_symbols[frame_length]; //******* SYMBOL TO BIT ******* if (d_symbol_history.at(0).Flag_valid_symbol_output == true) @@ -342,36 +318,20 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true) //update TOW at the preamble instant { - if(d_nav.flag_TOW_5 == true) //page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) - { - //TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay - d_TOW_at_current_symbol = d_nav.TOW_5 + GALILEO_INAV_PAGE_PART_SECONDS+((double)required_symbols)*GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; - d_nav.flag_TOW_5 = false; - } + d_TOW_at_current_symbol = floor((d_nav.d_TOW + 2*GLONASS_L1_CA_CODE_PERIOD + GLONASS_CA_PREAMBLE_DURATION_S)*1000.0)/1000.0; - else if(d_nav.flag_TOW_6 == true) //page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) - { - //TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay - d_TOW_at_current_symbol = d_nav.TOW_6 + GALILEO_INAV_PAGE_PART_SECONDS+((double)required_symbols)*GALILEO_E1_CODE_PERIOD;//-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; - d_nav.flag_TOW_6 = false; - } - else - { - //this page has no timing information - d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD;// + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; - } } else //if there is not a new preamble, we define the TOW of the current symbol { - d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD; + d_TOW_at_current_symbol = d_TOW_at_current_symbol + GLONASS_L1_CA_CODE_PERIOD; } //if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true) - if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) //all GGTO parameters arrived - { - delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64))); - } + // if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) //all GGTO parameters arrived + // { + // delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64))); + // } if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h index eab2136e2..5e4ff6df7 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h @@ -77,12 +77,13 @@ private: glonass_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); glonass_l1_ca_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); - void decode_word(double *symbols); + void decode_string(double *symbols, int frame_length); //!< Preamble decoding unsigned short int d_preambles_bits[GLONASS_GNAV_PREAMBLE_LENGTH_BITS]; int *d_preambles_symbols; unsigned int d_samples_per_symbol; + unsigned int d_samples_per_preamble_symbol; int d_symbols_per_preamble; //!< Storage for incoming data diff --git a/src/core/system_parameters/GLONASS_L1_CA.h b/src/core/system_parameters/GLONASS_L1_CA.h index 3a93d0def..3b03a58e5 100644 --- a/src/core/system_parameters/GLONASS_L1_CA.h +++ b/src/core/system_parameters/GLONASS_L1_CA.h @@ -94,13 +94,14 @@ const double GLONASS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time ( const int GLONASS_L1_CA_HISTORY_DEEP = 100; // NAVIGATION MESSAGE DEMODULATION AND DECODING -#define GLONASS_CA_PREAMBLE {1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 1, 0} -const int GLONASS_CA_PREAMBLE_LENGTH_BITS = 30; -const int GLONASS_CA_PREAMBLE_LENGTH_SYMBOLS = 300; -const double GLONASS_CA_PREAMBLE_DURATION_S = 0.3; -const int GLONASS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] -const int GLONASS_CA_TELEMETRY_SYMBOLS_PER_BIT = 10; -const int GLONASS_CA_TELEMETRY_RATE_SYMBOLS_SECOND = GLONASS_CA_TELEMETRY_RATE_BITS_SECOND*GLONASS_CA_TELEMETRY_SYMBOLS_PER_BIT; //!< NAV message bit rate [symbols/s] +#define GLONASS_GNAV_PREAMBLE {1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 1, 0} +const int GLONASS_GNAV_PREAMBLE_LENGTH_BITS = 30; +const int GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS = 300; +const double GLONASS_GNAV_PREAMBLE_DURATION_S = 0.3; +const int GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] +const int GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT = 10; +const int GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT = 10; +const int GLONASS_GNAV_TELEMETRY_RATE_SYMBOLS_SECOND = GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND*GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT; //!< NAV message bit rate [symbols/s] const int GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 1700; const int GLONASS_GNAV_FRAME_BITS = 1725; //!< Number of chips per frame in the GNAV message 15 strings*(85 data bits + 30 time mark bits)[bits] const int GLONASS_GNAV_FRAME_SECONDS = 30; //!< Subframe duration [seconds] diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.cc b/src/core/system_parameters/glonass_gnav_ephemeris.cc index 3b66f7a12..f5bb76eab 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.cc +++ b/src/core/system_parameters/glonass_gnav_ephemeris.cc @@ -81,18 +81,6 @@ Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris() d_tau_c = 0.0; d_TOW = 0.0; // tow of the start of frame d_WN = 0.0; // week number of the start of frame - // satellite positions - d_satpos_X = 0.0; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. - d_satpos_Y = 0.0; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] - d_satpos_Z = 0.0; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] - // Satellite velocity - d_satvel_X = 0.0; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] - d_satvel_Y = 0.0; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] - d_satvel_Z = 0.0; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] - // Satellite acceleration - d_satacc_X = 0.0; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] - d_satacc_Y = 0.0; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] - d_satacc_Z = 0.0; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] } @@ -107,122 +95,6 @@ boost::posix_time::ptime Glonass_Gnav_Ephemeris::compute_GLONASS_time(const doub } -void Glonass_Gnav_Ephemeris::gravitational_perturbations() -{ - double T = 0.0; - double sigma_days = 0.0; - double tau_prime = 0.0; - double Omega_moon = 0.0; - double q_moon = 0.0; - double q_sun = 0.0; - - double xi_star = 0.0; - double eta_star = 0.0; - double zeta_star = 0.0; - double E_moon = 0.0; - double E_sun = 0.0; - double xi_11 = 0.0; - double xi_12 = 0.0; - double eta_11 = 0.0; - double eta_12 = 0.0; - double zeta_11 = 0.0; - double zeta_12 = 0.0; - - double sin_theta_moon = 0.0; - double cos_theta_moon = 0.0; - double theta_moon = 0.0; - double xi_moon_e = 0.0; - double eta_moon_e = 0.0; - double zeta_moon_e = 0.0; - double r_moon_e = 0.0; - - double sin_theta_sun = 0.0; - double cos_theta_sun = 0.0; - double theta_sun = 0.0; - double xi_sun_e = 0.0; - double eta_sun_e = 0.0; - double zeta_sun_e = 0.0; - double r_sun_e = 0.0; - - double mu_bar_moon = 0.0; - double x_bar_moon = 0.0; - double y_bar_moon = 0.0; - double z_bar_moon = 0.0; - double Delta_o_moon = 0.0; - double mu_bar_sun = 0.0; - double x_bar_sun = 0.0; - double y_bar_sun = 0.0; - double z_bar_sun = 0.0; - double Delta_o_sun = 0.0; - - - double t_e = 0.0; - - // fabs(tk)) - { - // if there is more time left to integrate... - if(fmod(tk,tau) != 0) - { - tau = fmod(tk,tau); - } - else // otherwise make a zero step. - { - tau = 0; - } - } - - // Runge-Kutta Integration Stage K1 - r0 = sqrt(x_0*x_0 + y_0*y_0 + z_0*z_0); - r0_2 = r0*r0; - r0_3 = r0*r0*r0; - r0_5 = r0*r0*r0*r0*r0; - - dVx_1 = - GLONASS_GM / r0_3 * x_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * x_0 * (1 - 5 * (z_0*z_0) / r0_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_0 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_0 + Ax_0; - dVy_1 = - GLONASS_GM / r0_3 * y_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * y_0 * (1 - 5 * (z_0*z_0) / r0_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_0 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_0 + Ay_0; - dVz_1 = - GLONASS_GM / r0_3 * z_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * z_0 * (3 - 5 * (z_0*z_0) / r0_2) + Az_0; - - dx_1 = Vx_0; - dy_1 = Vy_0; - dz_1 = Vz_0; - - // Runge-Kutta Integration Stage K2 - Vx_1 = Vx_0 + 1/2 * tau * dVx_1; - Vy_1 = Vy_0 + 1/2 * tau * dVy_1; - Vz_1 = Vz_0 + 1/2 * tau * dVz_1; - - x_1 = x_0 + 1/2 * tau * dx_1; - y_1 = y_0 + 1/2 * tau * dy_1; - z_1 = z_0 + 1/2 * tau * dz_1; - - r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 ); - r1_2 = r1*r1; - r1_3 = r1*r1*r1; - r1_5 = r1*r1*r1*r1*r1; - - dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax_0; - dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay_0; - dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0; - - dx_2 = Vx_1; - dy_2 = Vy_1; - dz_2 = Vz_1; - - // Runge-Kutta Integration Stage K2 - Vx_2 = Vx_0 + 1/2 * tau * dVx_2; - Vy_2 = Vy_0 + 1/2 * tau * dVy_2; - Vz_2 = Vz_0 + 1/2 * tau * dVz_2; - - x_2 = x_0 + 1/2 * tau * dx_2; - y_2 = y_0 + 1/2 * tau * dy_2; - z_2 = z_0 + 1/2 * tau * dz_2; - - r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 ); - r2_2 = r2*r2; - r2_3 = r2*r2*r2; - r2_5 = r2*r2*r2*r2*r2; - - - dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax_0; - dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay_0; - dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0; - - dx_3 = Vx_2; - dy_3 = Vy_2; - dz_3 = Vz_2; - - // Runge-Kutta Integration Stage K3 - Vx_3 = Vx_0 + tau * dVx_3; - Vy_3 = Vy_0 + tau * dVy_3; - Vz_3 = Vz_0 + tau * dVz_3; - - x_3 = x_0 + tau * dx_3; - y_3 = y_0 + tau * dy_3; - z_3 = z_0 + tau * dz_3; - - r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); - r3_2 = r3*r3; - r3_3 = r3*r3*r3; - r3_5 = r3*r3*r3*r3*r3; - - dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax_0; - dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay_0; - dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0; - - dx_4 = Vx_3; - dy_4 = Vy_3; - dz_4 = Vz_3; - - // Final results showcased here - Vx_0 = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); - Vy_0 = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); - Vz_0 = Vz_0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); - - x_0 = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); - y_0 = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); - z_0 = z_0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); - - } - - // Reasign position, velocities and accelerations for next integration - d_satpos_X = x_0; - d_satpos_Y = y_0; - d_satpos_Z = z_0; - - d_satvel_X = Vx_0; - d_satvel_Y = Vy_0; - d_satvel_Z = Vz_0; - - d_satacc_X = d_AXn; // No change in accelerations reported over interval - d_satacc_Y = d_AYn; // No change in accelerations reported over interval - d_satacc_Z = d_AZn; // No change in accelerations reported over interval - - // Time from ephemeris reference clock - //tk = check_t(transmitTime - d_Toc); - - //double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; - - /* relativity correction */ - //dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); - - return 0; -} - - -double Glonass_Gnav_Ephemeris::satellite_position(double transmitTime) -{ - double dt = 0.0; - double tk = 0.0; - double tau = 0.0; - int numberOfIntegrations = 0; - - // RK 1 - double x_0 = 0.0; - double y_0 = 0.0; - double z_0 = 0.0; - - double Vx_0 = 0.0; - double Vy_0 = 0.0; - double Vz_0 = 0.0; - - double Ax_0 = 0.0; - double Ay_0 = 0.0; - double Az_0 = 0.0; - - double r0 = 0.0; - double r0_2 = 0.0; - double r0_3 = 0.0; - double r0_5 = 0.0; - - double dVx_1 = 0.0; - double dVy_1 = 0.0; - double dVz_1 = 0.0; - - double dx_1 = 0.0; - double dy_1 = 0.0; - double dz_1 = 0.0; - - // Runge-Kutta Integration Stage K2 - double x_1 = 0.0; - double y_1 = 0.0; - double z_1 = 0.0; - - double Vx_1 = 0.0; - double Vy_1 = 0.0; - double Vz_1 = 0.0; - - double r1 = 0.0; - double r1_2 = 0.0; - double r1_3 = 0.0; - double r1_5 = 0.0; - - double dVx_2 = 0.0; - double dVy_2 = 0.0; - double dVz_2 = 0.0; - - double dx_2 = 0.0; - double dy_2 = 0.0; - double dz_2 = 0.0; - - // Runge-Kutta Integration Stage K3 - double x_2 = 0.0; - double y_2 = 0.0; - double z_2 = 0.0; - - double Vx_2 = 0.0; - double Vy_2 = 0.0; - double Vz_2 = 0.0; - - double r2 = 0.0; - double r2_2 = 0.0; - double r2_3 = 0.0; - double r2_5 = 0.0; - - double dVx_3 = 0.0; - double dVy_3 = 0.0; - double dVz_3 = 0.0; - - double dx_3 = 0.0; - double dy_3 = 0.0; - double dz_3 = 0.0; - - // Runge-Kutta Integration Stage K4 - double x_3 = 0.0; - double y_3 = 0.0; - double z_3 = 0.0; - - double Vx_3 = 0.0; - double Vy_3 = 0.0; - double Vz_3 = 0.0; - - double r3 = 0.0; - double r3_2 = 0.0; - double r3_3 = 0.0; - double r3_5 = 0.0; - - double dVx_4 = 0.0; - double dVy_4 = 0.0; - double dVz_4 = 0.0; - - double dx_4 = 0.0; - double dy_4 = 0.0; - double dz_4 = 0.0; - - // Find time difference - dt = check_t(transmitTime - d_t_b); - - // Calculate clock correction - d_satClkDrift = -(d_tau_n + /*d_tau_c*/ - d_gamma_n * dt); - - // Find integration time - tk = dt - d_satClkDrift; - - // Check if to integrate forward or backward - if (tk < 0) - { - tau = -60; - } - else - { - tau = 60; - } - - // Coordinates transformation to an inertial reference frame - // x,y,z coordinates to meters - x_0 = d_Xn * 1e3; - y_0 = d_Yn * 1e3; - z_0 = d_Zn * 1e3; - - // x,y,z velocities to meters/s - Vx_0 = d_VXn * 1e3; - Vy_0 = d_VYn * 1e3; - Vz_0 = d_VZn * 1e3; - - // x,y,z accelerations to meters/sec^2 - Ax_0 = d_AXn * 1e3; - Ay_0 = d_AYn * 1e3; - Az_0 = d_AZn * 1e3; - - for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau) - { - // Check if last integration step. If last integration step, make one more step that has the remaining time length... - if(fabs(numberOfIntegrations) > fabs(tk)) - { - // if there is more time left to integrate... - if (fmod(tk,tau) != 0) - { - tau = fmod(tk,tau); - } - else // otherwise make a zero step. - { - tau = 0; - } - } - - // Runge-Kutta Integration Stage K1 - r0 = sqrt(x_0*x_0 + y_0*y_0 + z_0*z_0); - r0_2 = r0*r0; - r0_3 = r0*r0*r0; - r0_5 = r0*r0*r0*r0*r0; - - dx_1 = Vx_0; - dy_1 = Vy_0; - dz_1 = Vz_0; - - dVx_1 = - GLONASS_GM / r0_3 * x_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * x_0 * (1 - 5 * 1 / (z_0*z_0)) + d_Jx_moon + d_Jx_sun; - dVy_1 = - GLONASS_GM / r0_3 * y_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * y_0 * (1 - 5 * 1 / (z_0*z_0)) + d_Jy_moon + d_Jy_sun; - dVz_1 = - GLONASS_GM / r0_3 * z_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * z_0 * (3 - 5 * 1 / (z_0*z_0)) + d_Jz_moon + d_Jz_sun; - - dx_1 = Vx_0; - dy_1 = Vy_0; - dz_1 = Vz_0; - - // Runge-Kutta Integration Stage K2 - Vx_1 = Vx_0 + 1/2 * tau * dVx_1; - Vy_1 = Vy_0 + 1/2 * tau * dVy_1; - Vz_1 = Vz_0 + 1/2 * tau * dVz_1; - - x_1 = x_0 + 1/2 * tau * dx_1; - y_1 = y_0 + 1/2 * tau * dy_1; - z_1 = z_0 + 1/2 * tau * dz_1; - - r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 ); - r1_2 = r1*r1; - r1_3 = r1*r1*r1; - r1_5 = r1*r1*r1*r1*r1; - - dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax_0; - dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay_0; - dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0; - - dx_2 = Vx_1; - dy_2 = Vy_1; - dz_2 = Vz_1; - - // Runge-Kutta Integration Stage K2 - Vx_2 = Vx_0 + 1/2 * tau * dVx_2; - Vy_2 = Vy_0 + 1/2 * tau * dVy_2; - Vz_2 = Vz_0 + 1/2 * tau * dVz_2; - - x_2 = x_0 + 1/2 * tau * dx_2; - y_2 = y_0 + 1/2 * tau * dy_2; - z_2 = z_0 + 1/2 * tau * dz_2; - - r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 ); - r2_2 = r2*r2; - r2_3 = r2*r2*r2; - r2_5 = r2*r2*r2*r2*r2; - - - dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax_0; - dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay_0; - dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0; - - dx_3 = Vx_2; - dy_3 = Vy_2; - dz_3 = Vz_2; - - // Runge-Kutta Integration Stage K3 - Vx_3 = Vx_0 + tau * dVx_3; - Vy_3 = Vy_0 + tau * dVy_3; - Vz_3 = Vz_0 + tau * dVz_3; - - x_3 = x_0 + tau * dx_3; - y_3 = y_0 + tau * dy_3; - z_3 = z_0 + tau * dz_3; - - r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 ); - r3_2 = r3*r3; - r3_3 = r3*r3*r3; - r3_5 = r3*r3*r3*r3*r3; - - dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax_0; - dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay_0; - dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0; - - dx_4 = Vx_3; - dy_4 = Vy_3; - dz_4 = Vz_3; - - // Final results showcased here - d_satvel_X = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 ); - d_satvel_Y = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 ); - d_satvel_Z = Vz_0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 ); - - d_satpos_X = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 ); - d_satpos_Y = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 ); - d_satpos_Z = z_0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 ); - - } - - // Time from ephemeris reference clock - //tk = check_t(transmitTime - d_Toc); - - //double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; - - /* relativity correction */ - //dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s); - - return d_dtr; -} diff --git a/src/core/system_parameters/glonass_gnav_ephemeris.h b/src/core/system_parameters/glonass_gnav_ephemeris.h index 8a978f816..ef4a0ec12 100644 --- a/src/core/system_parameters/glonass_gnav_ephemeris.h +++ b/src/core/system_parameters/glonass_gnav_ephemeris.h @@ -60,16 +60,6 @@ private: */ double check_t(double time); - void gravitational_perturbations(); - - double d_Jx_moon; //!< Moon gravitational perturbation - double d_Jy_moon; //!< Moon gravitational perturbation - double d_Jz_moon; //!< Moon gravitational perturbation - - double d_Jx_sun; //!< Sun gravitational perturbation - double d_Jy_sun; //!< Sun gravitational perturbation - double d_Jz_sun; //!< Sun gravitational perturbation - public: double d_m; //!< String number within frame [dimensionless] double d_t_k; //!< GLONASS Time (UTC(SU) + 3 h) referenced to the beginning of the frame within the current day [s] @@ -115,19 +105,6 @@ public: double d_TOW; // tow of the start of frame double d_WN; // week number of the start of frame - // satellite positions after RK4 Integration - double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km]. - double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km] - double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km] - // Satellite velocity after RK4 Integration - double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s] - double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s] - double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s] - // Satellite acceleration after RK4 Integration - double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2] - double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2] - double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2] - template /*! @@ -171,37 +148,12 @@ public: archive & make_nvp("d_l5th_n", d_l5th_n); //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless] } - /*! - * \brief Compute the ECEF SV coordinates and ECEF velocity - * Implementation of Algorithm A.3.1.2 in GLONASS ICD v5.1 - * and compute the clock bias term including relativistic effect (return value) - * \param transmitTime Time of ephemeris transmission - * \return clock bias of satellite - */ - double simplified_satellite_position(double transmitTime); - - /*! - * \brief Compute the ECEF SV coordinates and ECEF velocity - * Implementation of Algorithm A.3.1.1 in GLONASS ICD v5.1 - * and compute the clock bias term including relativistic effect (return value) - * \param transmitTime Time of ephemeris transmission - * \return clock bias of satellite - */ - double satellite_position(double transmitTime); - /*! * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction * (IS-GPS-200E, 20.3.3.3.3.1) */ double sv_clock_drift(double transmitTime, double timeCorrUTC); - /*! - * \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction - * (IS-GPS-200E, 20.3.3.3.3.1) - */ - double sv_clock_relativistic_term(double transmitTime); - - /*! * \brief Computes the GLONASS System Time and returns a boost::posix_time::ptime object * \ param offset_time Is the start of day offset to compute the time diff --git a/src/core/system_parameters/glonass_gnav_utc_model.h b/src/core/system_parameters/glonass_gnav_utc_model.h index fa0886418..9e84762d6 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.h +++ b/src/core/system_parameters/glonass_gnav_utc_model.h @@ -1,7 +1,9 @@ /*! * \file glonass_gnav_utc_model.h * \brief Interface of a GLONASS GNAV UTC MODEL storage + * \note Code added as part of GSoC 2017 program * \author Damian Miralles, 2017. dmiralles2009(at)gmail.com + * \see GLONASS ICD * * ------------------------------------------------------------------------- *