diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc index 5ba437384..ceb2cc68b 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc @@ -285,7 +285,7 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump d_sample_counter = 0; d_last_sample_nav_output = 0; d_rx_time = 0.0; - d_TOW_at_curr_symbol_constellation = 0.0; + b_rinex_header_written = false; b_rinex_header_updated = false; rp = std::make_shared(); @@ -454,10 +454,9 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (in[i][0].Flag_valid_pseudorange == true) { - gnss_observables_map.insert(std::pair(i, in[i][0])); // store valid observables in a map. - //d_rx_time = in[i][0].d_TOW_at_current_symbol; // all the channels have the same RX timestamp (common RX time pseudoranges) - d_TOW_at_curr_symbol_constellation = in[i][0].d_TOW_at_current_symbol; // d_TOW_at_current_symbol not corrected by delta t (just for debug) - d_rx_time = in[i][0].d_TOW_at_current_symbol; // hybrid rx time, all the channels have the same RX timestamp (common RX time pseudoranges) + // store valid observables in a map. + gnss_observables_map.insert(std::pair(i, in[i][0])); + d_rx_time = in[i][0].RX_time; // hybrid rx time, all the channels have the same RX timestamp (common RX time pseudoranges, not corrected by delta t) if(d_ls_pvt->gps_ephemeris_map.size() > 0) { std::map::iterator tmp_eph_iter = d_ls_pvt->gps_ephemeris_map.find(in[i][0].PRN); @@ -524,7 +523,6 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if ((d_sample_counter % d_output_rate_ms) == 0) { bool pvt_result; - //std::cout<<"obs map size at pvt="<get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging); if (pvt_result == true) diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h index 1865a6e1f..0669d2b80 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h @@ -139,7 +139,7 @@ private: std::shared_ptr d_geojson_printer; std::shared_ptr d_rtcm_printer; double d_rx_time; - double d_TOW_at_curr_symbol_constellation; + std::shared_ptr d_ls_pvt; std::map gnss_observables_map; bool observables_pairCompare_min(const std::pair& a, const std::pair& b); diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index 5ad9db77b..46bdfef12 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -31,6 +31,7 @@ #include "hybrid_ls_pvt.h" #include +#include "GPS_L1_CA.h" #include "Galileo_E1.h" @@ -71,7 +72,7 @@ hybrid_ls_pvt::~hybrid_ls_pvt() } -bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, double hybrid_current_time, bool flag_averaging) +bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, double Rx_time, bool flag_averaging) { std::map::iterator gnss_observables_iter; std::map::iterator galileo_ephemeris_iter; @@ -88,7 +89,6 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou double GST = 0.0; double secondsperweek = 604800.0; - //double utc_tx_corrected = 0.0; //utc computed at tx_time_corrected, added for Galileo constellation (in GPS utc is directly computed at TX_time_corrected_s) double TX_time_corrected_s = 0.0; double SV_clock_bias_s = 0.0; @@ -118,7 +118,6 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou W(valid_obs) = 1; // COMMON RX TIME PVT ALGORITHM - double Rx_time = hybrid_current_time; double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GALILEO_C_m_s; // 2- compute the clock drift using the clock model (broadcast) for this SV @@ -141,7 +140,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz; Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST - GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time); + GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, Rx_time); // SV ECEF DEBUG OUTPUT DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN @@ -175,7 +174,6 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) // first estimate of transmit time - double Rx_time = hybrid_current_time; double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s; // 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect @@ -199,7 +197,8 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou // SV ECEF DEBUG OUTPUT LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN - << " TX Time corrected="< gnss_observables_map, dou // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) // first estimate of transmit time - double Rx_time = hybrid_current_time; double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s; // 2- compute the clock drift using the clock model (broadcast) for this SV @@ -306,7 +304,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds] - DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; + DLOG(INFO) << "Hybrid Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]"; // Compute GST and Gregorian time @@ -342,7 +340,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, dou { double tmp_double; // PVT GPS time - tmp_double = hybrid_current_time; + tmp_double = Rx_time; d_dump_file.write((char*)&tmp_double, sizeof(double)); // ECEF User Position East [m] tmp_double = rx_position_and_time(0); diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.h b/src/algorithms/PVT/libs/hybrid_ls_pvt.h index 6cc9fe76f..75d0f9aae 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.h +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.h @@ -52,7 +52,7 @@ public: hybrid_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file); ~hybrid_ls_pvt(); - bool get_PVT(std::map gnss_observables_map, double hybrid_current_time, bool flag_averaging); + bool get_PVT(std::map gnss_observables_map, double Rx_time, bool flag_averaging); int d_nchannels; //!< Number of available channels for positioning std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 579109dd2..25e04f128 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -200,8 +200,8 @@ int hybrid_observables_cc::general_work (int noutput_items, 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; - //todo: check this - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = d_TOW_reference + GPS_STARTOFFSET_ms / 1000.0; + // Save the estimated RX time (no RX clock offset correction yet!) + current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].RX_time = d_TOW_reference + GPS_STARTOFFSET_ms / 1000.0; if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= history_deep) { 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 0f5bb7413..567cd16ec 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 @@ -169,7 +169,6 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc( flag_even_word_arrived = 0; d_flag_preamble = false; d_channel = 0; - Prn_timestamp_at_preamble_ms = 0.0; flag_TOW_set = false; d_average_count = 0; d_decimation_output_factor = 1; @@ -407,7 +406,6 @@ int galileo_e1b_telemetry_decoder_cc::general_work (int noutput_items __attribut // Since we detected the preamble, then, we are in the last symbol of that preamble, or just at the start of the first page symbol. //flag preamble is true after the all page (even and odd) is received. I/NAV page period is 2 SECONDS { - Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0; 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) { //std::cout<< "Using TOW_5 for timestamping" << std::endl; @@ -461,7 +459,6 @@ int galileo_e1b_telemetry_decoder_cc::general_work (int noutput_items __attribut current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; //todo: move to observables: current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol - delta_t; //delta_t = t_gal - t_gps ----> t_gps = t_gal -delta_t 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; if(d_dump == true) { 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 774ae2f72..00625ef25 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 @@ -122,7 +122,6 @@ private: double d_TOW_at_Preamble; double d_TOW_at_current_symbol; - double Prn_timestamp_at_preamble_ms; bool flag_TOW_set; double delta_t; //GPS-GALILEO time offset diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc index 41da30df6..a2e985fc1 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc @@ -255,7 +255,6 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc( d_flag_preamble = false; d_channel = 0; - Prn_timestamp_at_preamble_ms = 0; flag_TOW_set = false; } @@ -489,8 +488,6 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut //update TOW at the preamble instant //We expect a preamble each 10 seconds (FNAV page period) { - Prn_timestamp_at_preamble_ms = d_preamble_time_seconds * 1000; - //Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0; if (d_nav.flag_TOW_1 == true) { d_TOW_at_Preamble = d_nav.FNAV_TOW_1; @@ -540,7 +537,6 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; 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; if(d_dump == true) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h index 1168df64b..d828919be 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h @@ -120,7 +120,7 @@ private: double d_TOW_at_Preamble; double d_TOW_at_current_symbol; - double Prn_timestamp_at_preamble_ms; + bool flag_TOW_set; std::string d_dump_filename; 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 345d571b3..138d8f315 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 @@ -109,7 +109,6 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( d_word_number = 0; d_decimation_output_factor = 1; d_channel = 0; - Prn_timestamp_at_preamble_ms = 0.0; flag_PLL_180_deg_phase_locked = false; } @@ -341,7 +340,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ { // update TOW at the preamble instant d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + GPS_L1_CA_CODE_PERIOD; - Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0; + d_TOW_at_current_symbol = d_TOW_at_Preamble; flag_TOW_set = true; d_flag_new_tow_available=false; @@ -354,7 +353,6 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; current_synchro_data.Flag_valid_word = flag_TOW_set;//(d_flag_frame_sync == true and d_flag_parity == true and flag_TOW_set == true); 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; if (flag_PLL_180_deg_phase_locked == true) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h index 9251dd0b6..5501c80b7 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h @@ -111,7 +111,6 @@ private: int d_average_count; int d_decimation_output_factor; - //double d_preamble_duration_seconds; // navigation message vars Gps_Navigation_Message d_nav; GpsL1CaSubframeFsm d_GPS_FSM; @@ -125,7 +124,6 @@ private: long double d_TOW_at_Preamble; long double d_TOW_at_current_symbol; - double Prn_timestamp_at_preamble_ms; bool flag_TOW_set; bool flag_PLL_180_deg_phase_locked; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc index 65ca0b3d2..5737fe303 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc @@ -148,13 +148,11 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( } //update TOW at the preamble instant - double Prn_timestamp_at_preamble_ms = (in[0].Tracking_timestamp_secs * 1000.0); d_TOW_at_Preamble=(int)msg.tow; std::cout<<"["<<(int)msg.prn<<"] deco delay: "<