diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc index 4c7c35716..2fd5667b8 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc @@ -39,11 +39,15 @@ #include #include #include "control_message_factory.h" - +#include "galileo_navigation_message.h" #include "gnss_synchro.h" #include "convolutional.h" +#include +#include + + using google::LogMessage; @@ -286,15 +290,18 @@ int galileo_e1b_telemetry_decoder_cc::general_work (int noutput_items, gr_vector { std::cout<<"Page Odd"< char page_Even[115]="000000010001011111010111101101100111110110101110101111100011001000000000001101110101110010000100101010100000010011"; - //test to detect page 2------> char page_Even[115]="000000100001011111110010111010111100000010111100000010011100000000100001011110100111000111100111000000100000000010"; - //test to detect page 3------>char page_Even[115]="000000110001011111111111111100001001100110001000110001011111110011111100110001011000111110000011011111000011110000"; - //test to detect page 4------>char page_Even[115]="000001000001011111001100000000000000101100000000000111010101111011011000000001000101001111101010011100000000010110:// - /*test to detect page 5------>char page_Even[115]="000001010010000010000000100010000010111010100000011111110001111111000111111001011000110010110011111111110110000000";*/ - //test to detect page 10-----> char page_Even[115]="000010101111101010101010101010101010101010101010101010101010101010101010101010101010111100000000000000000000000000"; - //test to detect page 6------>char page_Even[115]="000001100000000000000000000000011100001100000000000000000000000000100011010111111010001101000011100001000110101101 - /* test to detect page 7------> char page_Even[115]="000001111111101001011111010011000000000110100000001110010111111110010111110111101001000001110000111111110000101101"; */ - /* test to detect page 8------> char page_Even[115]="000010001111000000100001111100000011000001111010100000000000110100000001111010011110111000011110111101001000001110"; */ - /* test to detect page 9------> char page_Even[115]="000010011111101001011111111000101110001100000000011010100000000010111111100000001010101010101010101010101010101010"; */ - /* test to detect page 10------> char page_Even[115]="000010101111101010101010101010101010101010101010101010101010101010101010101010101010111100000000000000000000000000"; */ + static std::string page_Even; //declare in this way it can "remember the previous even page while reading the odd page..ok! - /* test to detect page 0------> char page_Even[115]="000000001001010101010101010101010101010101010101010101010101010101010101010101010101010101010101010010110001100101";*/ - - cout << "Start decoding Galileo I/NAV " << endl; -// cout<<"page input"< TLM_word_for_CRC_bits(TLM_word_for_CRC); std::bitset<24> checksum(CRC_data); + if (Tail_odd.compare(correct_tail) != 0) + std::cout << "Tail odd is not correct!" << std::endl; + else std::cout<<"Tail odd is correct!"<> s_page_number; // from stringstream to string - - std::bitset page_type_bits (s_page_number); // from string to bitset + std::string page_number_bits = Data_k.substr (0,6); + //std::cout << "Page number bits from Data k" << std::endl << page_number_bits << std::endl; + std::bitset page_type_bits (page_number_bits); // from string to bitset Page_type = (int)read_page_type_unsigned(page_type_bits, type); - - std::cout << "Page number (first 6 bits of Data k converted to decimal) = " << Page_type << endl; - - strncpy(Data_jk_ephemeris, &Data_k[0], 112); // Join data_j + data_k = Data_jk_ephemeris; - strncat(Data_jk_ephemeris, Data_j, 16); // Data_jk_ephemeris is the input for the function page decoder - + std::cout << "Page number (first 6 bits of Data k converted to decimal) = " << Page_type << std::endl; + std::string Data_jk_ephemeris = Data_k + Data_j; //std::cout<<"Data j k ephemeris" << endl << Data_jk_ephemeris << endl; - page_jk_decoder(Data_jk_ephemeris); // Corresponding to ephemeris_decode.m in matlab code - - //internal_flag_even_word_arrived=0; + page_jk_decoder(Data_jk_ephemeris.c_str()); // Corresponding to ephemeris_decode.m in matlab code } - } /*if(page[0]=='1') */ + } /*end if (page_string.at(0)=='1') */ else{ - - strncpy(page_Even, &page[0], 114); //ora che ha memorizzato page even dovrebbe mantenerla per la prossima volta che entro nella funzione - std::cout << "Page even in split page" << std::endl << page_Even << std::endl; - strncpy(tail_Even, &page[114], 6); - - if(strcmp (tail_Even,correct_tail) != 0) - std::cout << "Tail even is not correct!" << endl; - else std::cout<<"Tail even is correct!"<> str; - - std::bitset data_jk_bits (str); + std::string data_jk_string = data_jk; + std::bitset data_jk_bits (data_jk_string); //std::cout << "Data_jk_bits (bitset) "<< endl << data_jk_bits << endl; page_number = (int)read_navigation_unsigned(data_jk_bits, PAGE_TYPE_bit); - std::cout << "Page number = " << page_number << endl; + std::cout << "Page number = " << page_number << std::endl; switch (page_number) { @@ -769,9 +814,9 @@ int Galileo_Navigation_Message::page_jk_decoder(char *data_jk) SVID1_7= (double)read_navigation_unsigned(data_jk_bits, SVID1_7_bit); std::cout << "SVID1_7= " << SVID1_7 << std::endl; - Delta_alpha_7= (double)read_navigation_unsigned(data_jk_bits, DELTA_A_7_bit); - Delta_alpha_7= Delta_alpha_7 * DELTA_A_7_LSB; - std::cout << "Delta_alpha_7= " << Delta_alpha_7 << std::endl; + DELTA_A_7= (double)read_navigation_unsigned(data_jk_bits, DELTA_A_7_bit); + DELTA_A_7= DELTA_A_7 * DELTA_A_7_LSB; + std::cout << "DELTA_A_7= " << DELTA_A_7 << std::endl; e_7= (double)read_navigation_unsigned(data_jk_bits, e_7_bit); e_7= e_7 * e_7_LSB; @@ -960,7 +1005,354 @@ int Galileo_Navigation_Message::page_jk_decoder(char *data_jk) break; } - - cout<<"--------------------------------------------------------------------------"<= 0) // is not in the past + { + //Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s + int secondOfLeapSecondEvent = DN_6 * 24 * 60 * 60; + if (weeksToLeapSecondEvent > 0) + { + t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400); + } + else //we are in the same week than the leap second event + { + if (abs(t_e - secondOfLeapSecondEvent) > 21600) + { + /* 5.1.7a + * Whenever the leap second adjusted time indicated by the WN_LSF and the DN values + * is not in the past (relative to the user's present time), and the user's + * present time does not fall in the time span which starts at six hours prior + * to the effective time and ends at six hours after the effective time, + * the GST/Utc relationship is given by + */ + t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400); + } + else + { + /* 5.1.7b + * Whenever the user's current time falls within the time span of six hours + * prior to the leap second adjustment to six hours after the adjustment time, , + * the effective time is computed according to the following equations: + */ + + int W = fmod(t_e - Delta_t_Utc - 43200, 86400) + 43200; + t_Utc_daytime = fmod(W, 86400 + Delta_tLSF_6 - Delta_tLS_6); + //implement something to handle a leap second event! + } + if ( (t_e - secondOfLeapSecondEvent) > 21600) + { + Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800*(double)(WN - WNot_6)); + t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400); + } + } + } + else // the effectivity time is in the past + { + /* 5.1.7c + * Whenever the leap second adjustment time, as indicated by the WN_LSF and DN values, + * is in the past (relative to the user’s current time) and the user’s present time does not + * fall in the time span which starts six hours prior to the leap second adjustment time and + * ends six hours after the adjustment time, the effective time is computed according to + * the following equation: + */ + Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)(WN - WNot_6)); + t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400); + } + + double secondsOfWeekBeforeToday = 43200 * floor(t_e / 43200); + t_Utc = secondsOfWeekBeforeToday + t_Utc_daytime; + return t_Utc; + +} + + + diff --git a/src/core/system_parameters/galileo_navigation_message.h b/src/core/system_parameters/galileo_navigation_message.h index bf8d23494..4992752fc 100644 --- a/src/core/system_parameters/galileo_navigation_message.h +++ b/src/core/system_parameters/galileo_navigation_message.h @@ -50,6 +50,9 @@ #include "galileo_utc_model.h" #include "Galileo_E1.h" +#include "galileo_ephemeris.h" +#include "galileo_iono.h" +#include "galileo_utc_model.h" @@ -134,81 +137,92 @@ public: double spare_5; /*Word type 6: GST-UTC conversion parameters*/ - double A0_6; - double A1_6; - double Delta_tLS_6; - double t0t_6; - double WNot_6; - double WN_LSF_6; - double DN_6; - double Delta_tLSF_6; - double TOW_6; + double A0_6; + double A1_6; + double Delta_tLS_6; + double t0t_6; + double WNot_6; + double WN_LSF_6; + double DN_6; + double Delta_tLSF_6; + double TOW_6; - /*Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number*/ - int IOD_a_7; - double WN_a_7; - double t0a_7; - int SVID1_7; - double Delta_alpha_7; - double e_7; - double omega_7; - double delta_i_7; - double Omega0_7; - double Omega_dot_7; - double M0_7; + /*Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number*/ + int IOD_a_7; + double WN_a_7; + double t0a_7; + int SVID1_7; + double DELTA_A_7; + double e_7; + double omega_7; + double delta_i_7; + double Omega0_7; + double Omega_dot_7; + double M0_7; - /*Word type 8: Almanac for SVID1 (2/2) and SVID2 (1/2)*/ - int IOD_a_8; - double af0_8; - double af1_8; - double E5b_HS_8; - double E1B_HS_8; - int SVID2_8; - double DELTA_A_8; - double e_8; - double omega_8; - double delta_i_8; - double Omega0_8; - double Omega_dot_8; + /*Word type 8: Almanac for SVID1 (2/2) and SVID2 (1/2)*/ + int IOD_a_8; + double af0_8; + double af1_8; + double E5b_HS_8; + double E1B_HS_8; + int SVID2_8; + double DELTA_A_8; + double e_8; + double omega_8; + double delta_i_8; + double Omega0_8; + double Omega_dot_8; - /*Word type 9: Almanac for SVID2 (2/2) and SVID3 (1/2)*/ - - int IOD_a_9; - double WN_a_9; - double t0a_9; - double M0_9; - double af0_9; - double af1_9; - double E5b_HS_9; - double E1B_HS_9; - int SVID3_9; - double DELTA_A_9; - double e_9; - double omega_9; - double delta_i_9; + /*Word type 9: Almanac for SVID2 (2/2) and SVID3 (1/2)*/ + int IOD_a_9; + double WN_a_9; + double t0a_9; + double M0_9; + double af0_9; + double af1_9; + double E5b_HS_9; + double E1B_HS_9; + int SVID3_9; + double DELTA_A_9; + double e_9; + double omega_9; + double delta_i_9; - /*Word type 10: Almanac for SVID3 (2/2) and GST-GPS conversion parameters*/ + /*Word type 10: Almanac for SVID3 (2/2) and GST-GPS conversion parameters*/ + int IOD_a_10; + double Omega0_10; + double Omega_dot_10; + double M0_10; + double af0_10; + double af1_10; + double E5b_HS_10; + double E1B_HS_10; + // GST-GPS conversion + double A_0G_10; //constant term of the offset Δt systems + double A_1G_10; //rate of change of the offset Δt systems + double t_0G_10; //reference time for GGTO data + double WN_0G_10; //Week Number of GGTO reference - int IOD_a_10; - double Omega0_10; - double Omega_dot_10; - double M0_10; - double af0_10; - double af1_10; - double E5b_HS_10; - double E1B_HS_10; - double A_0G_10; - double A_1G_10; - double t_0G_10; - double WN_0G_10; + /*Word type 0: I/NAV Spare Word*/ + double Time_0; + double WN_0; + double TOW_0; + double Galileo_satClkDrift; + double Galileo_dtr; // relativistic clock correction term + + // satellite positions + double galileo_satpos_X; //!< Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis. + double galileo_satpos_Y; //!< Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system. + double galileo_satpos_Z; //!< Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP). + // Satellite velocity + double galileo_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite [m] + double galileo_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m] + double galileo_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite [m] - /*Word type 0: I/NAV Spare Word*/ - double Time_0; - double WN_0; - double TOW_0; /* @@ -218,12 +232,7 @@ public: /* * \brief Takes in input Data_jk (128 bit) and split it in ephemeris parameters according ICD 4.3.5 */ - int page_jk_decoder(char *data_jk); - - - /* - * \brief Write doxigen function description here - */ + int page_jk_decoder(const char *data_jk); /* Takes in input Data_jk (128 bit) and split it in ephemeris parameters according ICD 4.3.5*/ void reset(); @@ -266,6 +275,15 @@ public: */ Galileo_Almanac get_almanac(); + void satellitePosition(double transmitTime); + + double Galileo_System_Time(double WN, double TOW); // Galileo System Time (GST), ICD paragraph 5.1.2 + + double sv_clock_drift(double transmitTime); //Satellite Time Correction Algorithm, ICD 5.1.4 + + double sv_clock_relativistic_term(double transmitTime); //Satellite Time Correction Algorithm, ICD 5.1.4 + + double GST_to_UTC_time(double t_e, int WN); //GST-UTC Conversion Algorithm and Parameters Galileo_Navigation_Message(); }; diff --git a/src/core/system_parameters/gps_utc_model.cc b/src/core/system_parameters/gps_utc_model.cc index 98f071c04..d391ea03c 100644 --- a/src/core/system_parameters/gps_utc_model.cc +++ b/src/core/system_parameters/gps_utc_model.cc @@ -42,6 +42,8 @@ Gps_Utc_Model::Gps_Utc_Model() i_DN = 0; d_DeltaT_LSF = 0; + + } double Gps_Utc_Model::utc_time(double gpstime_corrected, int i_GPS_week)