diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index f76681cef..8c29fd840 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -99,6 +99,10 @@ bool Hybrid_pairCompare_gnss_synchro_Prn_delay_ms( std::pair a } +bool Hybrid_pairCompare_gnss_synchro_d_TOW_hybrid_at_current_symbol( std::pair a, std::pair b) +{ + return (a.second.d_TOW_hybrid_at_current_symbol) < (b.second.d_TOW_hybrid_at_current_symbol); +} bool Hybrid_pairCompare_gnss_synchro_d_TOW_at_current_symbol( std::pair a, std::pair b) { @@ -106,7 +110,6 @@ bool Hybrid_pairCompare_gnss_synchro_d_TOW_at_current_symbol( std::pair this will be the reference symbol -// gnss_synchro_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), Hybrid_pairCompare_gnss_synchro_d_TOW_at_current_symbol); -// double d_TOW_reference = gnss_synchro_iter->second.d_TOW_at_current_symbol; -// double d_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms; + gnss_synchro_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), Hybrid_pairCompare_gnss_synchro_d_TOW_hybrid_at_current_symbol); + double d_TOW_reference = gnss_synchro_iter->second.d_TOW_hybrid_at_current_symbol; + std::cout<<"d_TOW_hybrid_reference [ms] = "<< d_TOW_reference*1000 <second.Prn_timestamp_ms; + std::cout<<"ref_PRN_rx_time_ms [ms] = "<< d_ref_PRN_rx_time_ms <second.Channel_ID; // Now compute RX time differences due to the PRN alignment in the correlators -// double traveltime_ms; -// double pseudorange_m; -// double delta_rx_time_ms; - for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++) + double traveltime_ms; + double pseudorange_m; + double delta_rx_time_ms; + for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++) { + // compute the required symbol history shift in order to match the reference symbol + delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms-d_ref_PRN_rx_time_ms; + //compute the pseudorange + traveltime_ms = (d_TOW_reference - gnss_synchro_iter->second.d_TOW_hybrid_at_current_symbol)*1000.0 + delta_rx_time_ms + GALILEO_STARTOFFSET_ms; + pseudorange_m = traveltime_ms * GALILEO_C_m_ms; // [m] + std::cout<<"CH "<second.Channel_ID<<" tracking GNSS System "<second.System<<" has PRN start at= "<second.Prn_timestamp_ms<<" [ms], d_TOW_at_current_symbol = "<<(gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000<<" [ms], d_TOW_hybrid_at_current_symbol = "<<(gnss_synchro_iter->second.d_TOW_hybrid_at_current_symbol)*1000<<"[ms], delta_rx_time_ms = "<< delta_rx_time_ms << "[ms], travel_time = " << traveltime_ms << ", pseudorange[m] = "<< pseudorange_m << std::endl; + + // update the pseudorange object + //current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second; + current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; + current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true; + current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_hybrid_at_current_symbol = round(d_TOW_reference*1000)/1000 + GALILEO_STARTOFFSET_ms/1000.0; - std::cout<<"CH "<second.Channel_ID<<" tracking GNSS System "<second.System<<" has PRN start at= "<second.Prn_timestamp_ms<<" [ms]"<second.Prn_timestamp_ms-d_ref_PRN_rx_time_ms; -// //compute the pseudorange -// traveltime_ms = (d_TOW_reference - gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GALILEO_STARTOFFSET_ms; -// pseudorange_m = traveltime_ms * GALILEO_C_m_ms; // [m] -// // update the pseudorange object -// //current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second; -// current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; -// current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true; -// current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000)/1000 + GALILEO_STARTOFFSET_ms/1000.0; -// } std::cout< t_gps = t_gal -delta_t + std::cout<< "delta_t = " << delta_t << std::endl; current_synchro_data.Flag_preamble = d_flag_preamble; current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs * 1000.0; current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h index 899ba68c3..19a7964bb 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h @@ -138,6 +138,8 @@ private: double d_TOW_at_current_symbol; double Prn_timestamp_at_preamble_ms; bool flag_TOW_set; + double delta_t; //GPS-GALILEO time offset + std::string d_dump_filename; std::ofstream d_dump_file; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 34de57449..9c40c2dd7 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -318,6 +318,8 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i current_synchro_data.d_TOW = d_TOW_at_Preamble; current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; + + current_synchro_data.d_TOW_hybrid_at_current_symbol= current_synchro_data.d_TOW_at_current_symbol; // to be used in the hybrid configuration current_synchro_data.Flag_valid_word = (d_flag_frame_sync == true and d_flag_parity == true and flag_TOW_set==true); current_synchro_data.Flag_preamble = d_flag_preamble; current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs * 1000.0; diff --git a/src/core/system_parameters/galileo_navigation_message.cc b/src/core/system_parameters/galileo_navigation_message.cc index 76048f019..b9d014924 100644 --- a/src/core/system_parameters/galileo_navigation_message.cc +++ b/src/core/system_parameters/galileo_navigation_message.cc @@ -66,6 +66,12 @@ void Galileo_Navigation_Message::reset() flag_TOW_5 = 0; flag_TOW_set = false; + flag_GGTO= false; + flag_GGTO_1= false; + flag_GGTO_2= false; + flag_GGTO_3= false; + flag_GGTO_4= false; + IOD_ephemeris = 0; /*Word type 1: Ephemeris (1/4)*/ IOD_nav_1 = 0; @@ -974,14 +980,18 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk) DLOG(INFO) << "E1B_HS_10= " << E1B_HS_10; A_0G_10 = (double)read_navigation_signed(data_jk_bits, A_0G_10_bit); A_0G_10 = A_0G_10 * A_0G_10_LSB; + flag_GGTO_1=true; DLOG(INFO) << "A_0G_10= " << A_0G_10; A_1G_10 = (double)read_navigation_signed(data_jk_bits, A_1G_10_bit); A_1G_10 = A_1G_10 * A_1G_10_LSB; + flag_GGTO_2=true; DLOG(INFO) << "A_1G_10= " << A_1G_10; t_0G_10 = (double)read_navigation_unsigned(data_jk_bits, t_0G_10_bit); t_0G_10 = t_0G_10 * t_0G_10_LSB; + flag_GGTO_3=true; DLOG(INFO) << "t_0G_10= " << t_0G_10; WN_0G_10 = (double)read_navigation_unsigned(data_jk_bits, WN_0G_10_bit); + flag_GGTO_4=true; DLOG(INFO) << "WN_0G_10= " << WN_0G_10; flag_almanac_4 = true; DLOG(INFO) << "flag_tow_set" << flag_TOW_set; diff --git a/src/core/system_parameters/galileo_navigation_message.h b/src/core/system_parameters/galileo_navigation_message.h index e56c0a1dc..f11672541 100644 --- a/src/core/system_parameters/galileo_navigation_message.h +++ b/src/core/system_parameters/galileo_navigation_message.h @@ -87,6 +87,12 @@ public: int IOD_ephemeris; + bool flag_GGTO; + bool flag_GGTO_1; + bool flag_GGTO_2; + bool flag_GGTO_3; + bool flag_GGTO_4; + /*Word type 1: Ephemeris (1/4)*/ int IOD_nav_1; //!< IOD_nav page 1 double t0e_1; //!< Ephemeris reference time [s] diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index 2bb2ffa02..4672afb71 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -70,6 +70,8 @@ public: bool Flag_preamble; //!< Set by Telemetry Decoder processing block double d_TOW; //!< Set by Telemetry Decoder processing block double d_TOW_at_current_symbol; + double d_TOW_hybrid_at_current_symbol; //Galileo TOW is expressed in the GPS time scale (it will be the same for any other constellation) + // Pseudorange double Pseudorange_m; bool Flag_valid_pseudorange;