diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index f40756eca..9d7b77396 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -339,16 +339,6 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump d_ls_pvt->set_averaging_depth(1); d_rx_time = 0.0; - last_pvt_display_T_rx_s = 0.0; - last_RTCM_1019_output_time = 0.0; - last_RTCM_1020_output_time = 0.0; - last_RTCM_1045_output_time = 0.0; - last_RTCM_1077_output_time = 0.0; - last_RTCM_1087_output_time = 0.0; - last_RTCM_1097_output_time = 0.0; - last_RTCM_MSM_output_time = 0.0; - last_RINEX_obs_output_time = 0.0; - last_RINEX_nav_output_time = 0.0; b_rinex_header_written = false; b_rinex_header_updated = false; @@ -600,84 +590,75 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // ############ 2 COMPUTE THE PVT ################################ if (gnss_observables_map.size() > 0) { - // correct the observable to account for the receiver clock offset - //observables fix - //std::cout << TEXT_RED << "(internal) accumulated RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]" << TEXT_RESET << std::endl; - for (std::map::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it) - { - //it->second.RX_time+=d_ls_pvt->get_time_offset_s(); - it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->get_time_offset_s() * GPS_C_m_s; - } - - double current_RX_time = gnss_observables_map.begin()->second.RX_time;//+ d_ls_pvt->get_time_offset_s(); - - if (std::fabs(current_RX_time - d_rx_time) * 1000.0 >= static_cast(d_output_rate_ms)) + double current_RX_time = gnss_observables_map.begin()->second.RX_time; + unsigned int current_RX_time_ms = static_cast(current_RX_time * 1000.0); + //if (std::fabs(current_RX_time - d_rx_time) * 1000.0 >= static_cast(d_output_rate_ms)) + if (current_RX_time_ms % d_output_rate_ms == 0) { flag_compute_pvt_output = true; d_rx_time = current_RX_time; + //std::cout.precision(17); + //std::cout << "current_RX_time: " << current_RX_time << " map time: " << gnss_observables_map.begin()->second.RX_time << std::endl; } // compute on the fly PVT solution if (flag_compute_pvt_output == true) { - if (d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, false)) + // correct the observable to account for the receiver clock offset + //std::cout << TEXT_RED << "(internal) accumulated RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]" << TEXT_RESET << std::endl; + for (std::map::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it) { - if (std::fabs(current_RX_time - last_pvt_display_T_rx_s) * 1000.0 >= static_cast(d_display_rate_ms)) + //todo: check if it has effect to correct the receiver time for the internal pvt solution + // take into account that the RINEX obs with the RX time (integer ms) CAN NOT be corrected to keep the coherence in obs time + it->second.Pseudorange_m = it->second.Pseudorange_m; // - d_ls_pvt->get_time_offset_s() * GPS_C_m_s; + } + + if (d_ls_pvt->get_PVT(gnss_observables_map, false)) + { + if (current_RX_time_ms % d_display_rate_ms == 0) { flag_display_pvt = true; - last_pvt_display_T_rx_s = current_RX_time; } - if ((std::fabs(current_RX_time - last_RTCM_1019_output_time) * 1000.0 >= static_cast(d_rtcm_MT1019_rate_ms)) and (d_rtcm_MT1019_rate_ms != 0)) // allows deactivating messages by setting rate = 0 + if (current_RX_time_ms % d_rtcm_MT1019_rate_ms == 0 and d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 { flag_write_RTCM_1019_output = true; - last_RTCM_1019_output_time = current_RX_time; } - if ((std::fabs(current_RX_time - last_RTCM_1020_output_time) * 1000.0 >= static_cast(d_rtcm_MT1020_rate_ms)) and (d_rtcm_MT1020_rate_ms != 0)) // allows deactivating messages by setting rate = 0 + if (current_RX_time_ms % d_rtcm_MT1020_rate_ms == 0 and d_rtcm_MT1020_rate_ms != 0) // allows deactivating messages by setting rate = 0 { flag_write_RTCM_1020_output = true; - last_RTCM_1020_output_time = current_RX_time; } - if ((std::fabs(current_RX_time - last_RTCM_1045_output_time) * 1000.0 >= static_cast(d_rtcm_MT1045_rate_ms)) and (d_rtcm_MT1045_rate_ms != 0)) + if (current_RX_time_ms % d_rtcm_MT1045_rate_ms == 0 and d_rtcm_MT1045_rate_ms != 0) { flag_write_RTCM_1045_output = true; - last_RTCM_1045_output_time = current_RX_time; } - if ((std::fabs(current_RX_time - last_RTCM_1077_output_time) * 1000.0 >= static_cast(d_rtcm_MT1077_rate_ms)) and (d_rtcm_MT1077_rate_ms != 0)) - { - last_RTCM_1077_output_time = current_RX_time; - } - if ((std::fabs(current_RX_time - last_RTCM_1087_output_time) * 1000.0 >= static_cast(d_rtcm_MT1087_rate_ms)) and (d_rtcm_MT1087_rate_ms != 0)) - { - last_RTCM_1087_output_time = current_RX_time; - } - if ((std::fabs(current_RX_time - last_RTCM_1097_output_time) * 1000.0 >= static_cast(d_rtcm_MT1097_rate_ms)) and (d_rtcm_MT1097_rate_ms != 0)) - { - last_RTCM_1097_output_time = current_RX_time; - } + // if (current_RX_time_ms % d_rtcm_MT1077_rate_ms==0 and d_rtcm_MT1077_rate_ms != 0) + // { + // last_RTCM_1077_output_time = current_RX_time; + // } + // if (current_RX_time_ms % d_rtcm_MT1087_rate_ms==0 and d_rtcm_MT1087_rate_ms != 0) + // { + // last_RTCM_1087_output_time = current_RX_time; + // } + // if (current_RX_time_ms % d_rtcm_MT1097_rate_ms==0 and d_rtcm_MT1097_rate_ms != 0) + // { + // last_RTCM_1097_output_time = current_RX_time; + // } - if ((std::fabs(current_RX_time - last_RTCM_MSM_output_time) * 1000.0 >= static_cast(d_rtcm_MSM_rate_ms)) and (d_rtcm_MSM_rate_ms != 0)) + if (current_RX_time_ms % d_rtcm_MSM_rate_ms == 0 and d_rtcm_MSM_rate_ms != 0) { flag_write_RTCM_MSM_output = true; - last_RTCM_MSM_output_time = current_RX_time; } - if ((std::fabs(current_RX_time - last_RINEX_obs_output_time) >= 1.0)) // TODO: Make it configurable + if (current_RX_time_ms % 1000 == 0) // TODO: Make it configurable { flag_write_RINEX_obs_output = true; - last_RINEX_obs_output_time = current_RX_time; } - if ((std::fabs(current_RX_time - last_RINEX_nav_output_time) >= 6.0)) // TODO: Make it configurable + if (current_RX_time_ms % 6000 == 0) // TODO: Make it configurable { flag_write_RINEX_nav_output = true; - last_RINEX_nav_output_time = current_RX_time; } - // correct the observable to account for the receiver clock offset - //for (std::map::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it) - // { - // it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->get_time_offset_s() * GPS_C_m_s; - // } if (first_fix == true) { std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) @@ -2090,7 +2071,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item gtime_t rtklib_utc_time = gpst2time(adjgpsweek(d_ls_pvt->gps_ephemeris_map.cbegin()->second.i_GPS_week), d_rx_time); p_time = boost::posix_time::from_time_t(rtklib_utc_time.time); p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6)); - std::cout << TEXT_MAGENTA << "Observable RX time (GPST) " << boost::posix_time::to_simple_string(p_time)<< TEXT_RESET << std::endl; + std::cout << TEXT_MAGENTA << "Observable RX time (GPST) " << boost::posix_time::to_simple_string(p_time) << TEXT_RESET << std::endl; LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index b208f4c0c..37b87a0b4 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -126,16 +126,7 @@ private: std::shared_ptr d_geojson_printer; std::shared_ptr d_rtcm_printer; double d_rx_time; - double last_pvt_display_T_rx_s; - double last_RTCM_1019_output_time; - double last_RTCM_1020_output_time; - double last_RTCM_1045_output_time; - double last_RTCM_1077_output_time; - double last_RTCM_1087_output_time; - double last_RTCM_1097_output_time; - double last_RTCM_MSM_output_time; - double last_RINEX_obs_output_time; - double last_RINEX_nav_output_time; + std::shared_ptr d_ls_pvt; std::map gnss_observables_map; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index f7fb73dc9..1a251c314 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -133,7 +133,7 @@ double rtklib_solver::get_vdop() const } -bool rtklib_solver::get_PVT(const std::map& gnss_observables_map, double Rx_time, bool flag_averaging) +bool rtklib_solver::get_PVT(const std::map& gnss_observables_map, bool flag_averaging) { std::map::const_iterator gnss_observables_iter; std::map::const_iterator galileo_ephemeris_iter; @@ -501,10 +501,13 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ //this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s)); // accumulate the rx time error for the next iteration [meters]->[seconds] this->set_time_offset_s(rx_position_and_time(3)); - DLOG(INFO) << "RTKLIB Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; + DLOG(INFO) << "RTKLIB Position at RX TOW = " << gnss_observables_map.begin()->second.RX_time + << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; boost::posix_time::ptime p_time; - gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time); + //gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time); + + gtime_t rtklib_utc_time = gpst2time(adjgpsweek(nav_data.eph[0].week), gnss_observables_map.begin()->second.RX_time); p_time = boost::posix_time::from_time_t(rtklib_utc_time.time); p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6)); this->set_position_UTC_time(p_time); @@ -523,7 +526,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ { double tmp_double; // PVT GPS time - tmp_double = Rx_time; + tmp_double = gnss_observables_map.begin()->second.RX_time; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // ECEF User Position East [m] tmp_double = rx_position_and_time(0); diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index ac180f617..aa5557606 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -85,12 +85,12 @@ public: rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t& rtk); ~rtklib_solver(); - bool get_PVT(const std::map& gnss_observables_map, double Rx_time, bool flag_averaging); + bool get_PVT(const std::map& gnss_observables_map, bool flag_averaging); double get_hdop() const; double get_vdop() const; double get_pdop() const; double get_gdop() const; - + std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris std::map gps_cnav_ephemeris_map; //!< Map storing new GPS_CNAV_Ephemeris diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index b210cb4ee..84cc936b3 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -62,9 +62,9 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, d_nchannels = nchannels_out; d_dump_filename = dump_filename; T_rx_s = 0.0; - T_rx_step_s = 0.001; // 1 ms - max_delta = 1.5; // 1.5 s - d_latency = 0.5; // 300 ms + T_rx_step_ms = 1; // 1 ms + max_delta = 1.5; // 1.5 s + d_latency = 0.5; // 300 ms valid_channels.resize(d_nchannels, false); d_num_valid_channels = 0; d_gnss_synchro_history = new Gnss_circular_deque(static_cast(max_delta * 1000.0), d_nchannels); @@ -87,8 +87,8 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, } } } - T_rx_TOW_s=0.0; - T_rx_TOW_set=false; + T_rx_TOW_ms = 0; + T_rx_TOW_set = false; } @@ -310,7 +310,10 @@ bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, const unsigned i } find_interp_elements(ch, ti); - //Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1) + //1st: copy the nearest gnss_synchro data for that channel + out = d_gnss_synchro_history->at(ch, 0); + + //2nd: Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1) // CARRIER PHASE INTERPOLATION out.Carrier_phase_rads = d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads + (d_gnss_synchro_history->at(ch, 1).Carrier_phase_rads - d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time); @@ -319,7 +322,12 @@ bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, const unsigned i out.Carrier_Doppler_hz = d_gnss_synchro_history->at(ch, 0).Carrier_Doppler_hz + (d_gnss_synchro_history->at(ch, 1).Carrier_Doppler_hz - d_gnss_synchro_history->at(ch, 0).Carrier_Doppler_hz) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time); // TOW INTERPOLATION - out.TOW_at_current_symbol_s = d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_s + (d_gnss_synchro_history->at(ch, 1).TOW_at_current_symbol_s - d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_s) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time); + out.interp_TOW_ms = static_cast(d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_ms) + (static_cast(d_gnss_synchro_history->at(ch, 1).TOW_at_current_symbol_ms) - static_cast(d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_ms)) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time); + + + //std::cout.precision(17); + //std::cout << "Diff TOW_at_current_symbol_ms(1) - out.interp_TOW_ms: " << static_cast(d_gnss_synchro_history->at(ch, 1).TOW_at_current_symbol_ms) - out.interp_TOW_ms << std::endl; + return true; } @@ -417,8 +425,8 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vectorPRN == it2->PRN and it->System == it2->System) { - double tow_dif_ = std::fabs(it->TOW_at_current_symbol_s - it2->TOW_at_current_symbol_s); - if (tow_dif_ > thr_) + double tow_dif_ = std::fabs(it->TOW_at_current_symbol_ms - it2->TOW_at_current_symbol_ms); + if (tow_dif_ > thr_ * 1000.0) { DLOG(INFO) << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal << ". TOW difference in PRN " << it->PRN @@ -433,27 +441,39 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector::lowest(); + if (!T_rx_TOW_set) - { - for (it = data.begin(); it != data.end(); it++) - { - if (it->TOW_at_current_symbol_s > TOW_ref) - { - TOW_ref = it->TOW_at_current_symbol_s; - } - } - T_rx_TOW_s=round(TOW_ref*1000.0)/1000.0; - T_rx_TOW_set=true; - }else{ - T_rx_TOW_s+=T_rx_step_s; - TOW_ref=T_rx_TOW_s; - } + { + unsigned int TOW_ref = std::numeric_limits::lowest(); + for (it = data.begin(); it != data.end(); it++) + { + if (it->TOW_at_current_symbol_ms > TOW_ref) + { + TOW_ref = it->TOW_at_current_symbol_ms; + } + } + T_rx_TOW_ms = TOW_ref; + T_rx_TOW_set = true; + } + else + { + T_rx_TOW_ms += T_rx_step_ms; + //todo: check what happen during the week rollover + if (T_rx_TOW_ms >= 604800000) + { + T_rx_TOW_ms = T_rx_TOW_ms % 604800000; + } + } for (it = data.begin(); it != data.end(); it++) { - double traveltime_s = TOW_ref - it->TOW_at_current_symbol_s + GPS_STARTOFFSET_ms / 1000.0; - it->RX_time = TOW_ref + GPS_STARTOFFSET_ms / 1000.0; + double traveltime_s = (static_cast(T_rx_TOW_ms) - it->interp_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0; + + //std::cout.precision(17); + //std::cout << "Diff T_rx_TOW_ms - interp_TOW_ms: " << static_cast(T_rx_TOW_ms) - it->interp_TOW_ms << std::endl; + + it->RX_time = (T_rx_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0; it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT; } } @@ -475,7 +495,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) } for (int epoch = 0; epoch < ninput_items[d_nchannels]; epoch++) { - T_rx_s += T_rx_step_s; + T_rx_s += (static_cast(T_rx_step_ms) / 1000.0); ////////////////////////////////////////////////////////////////////////// if ((total_input_items == 0) and (d_num_valid_channels == 0)) @@ -557,7 +577,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) { if (valid_channels[i]) { - Gnss_Synchro interpolated_gnss_synchro = d_gnss_synchro_history->back(i); + Gnss_Synchro interpolated_gnss_synchro; // empty set, it is required to COPY the nearest in the interpolation history = d_gnss_synchro_history->back(i); if (interpolate_data(interpolated_gnss_synchro, i, T_rx_s_out)) { epoch_data.push_back(interpolated_gnss_synchro); @@ -600,7 +620,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) { tmp_double = out[i][epoch].RX_time; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = out[i][epoch].TOW_at_current_symbol_s; + tmp_double = out[i][epoch].interp_TOW_ms / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_double = out[i][epoch].Carrier_Doppler_hz; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index e9ba00a2a..c2008ceaf 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -76,10 +76,10 @@ private: Gnss_circular_deque* d_gnss_synchro_history; boost::dynamic_bitset<> valid_channels; double T_rx_s; - double T_rx_step_s; + unsigned int T_rx_step_ms; //rx time follow GPST bool T_rx_TOW_set; - double T_rx_TOW_s; + unsigned int T_rx_TOW_ms; double max_delta; double d_latency; bool d_dump; 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 264646524..274eb112f 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 @@ -456,8 +456,9 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute current_symbol.Flag_valid_word = false; } - current_symbol.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; - current_symbol.TOW_at_current_symbol_s -= delta_t; //Galileo to GPS TOW + current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0); + //todo: Galileo to GPS time conversion should be moved to observable block. + //current_symbol.TOW_at_current_symbol_ms -= delta_t; //Galileo to GPS TOW if (d_dump == true) { 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 3b3739248..96431880b 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 @@ -470,7 +470,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute current_sample.Flag_valid_word = false; } - current_sample.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; + current_sample.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0); if (d_dump) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc index baacf8fce..795f460f3 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc @@ -413,8 +413,9 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu } current_symbol.PRN = this->d_satellite.get_PRN(); - current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol; - current_symbol.TOW_at_current_symbol_s -= delta_t; // Galileo to GPS TOW + current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0); + //todo: glonass time to gps time should be done in observables block + //current_symbol.TOW_at_current_symbol_s -= delta_t; // Galileo to GPS TOW if (d_dump == true) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_cc.cc index 0c8813694..91338516b 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_cc.cc @@ -413,8 +413,9 @@ int glonass_l2_ca_telemetry_decoder_cc::general_work(int noutput_items __attribu } current_symbol.PRN = this->d_satellite.get_PRN(); - current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol; - current_symbol.TOW_at_current_symbol_s -= delta_t; // Galileo to GPS TOW + current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0); + //todo: glonass time to gps time should be done in observables block + //current_symbol.TOW_at_current_symbol_s -= delta_t; if (d_dump == true) { 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 1621eef58..a338c02eb 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 @@ -91,7 +91,6 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( d_prev_GPS_frame_4bytes = 0; d_flag_parity = false; d_TOW_at_Preamble = 0.0; - d_TOW_at_current_symbol = 0.0; flag_TOW_set = false; d_average_count = 0; d_flag_preamble = false; @@ -396,25 +395,17 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ //2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_flag_new_tow_available == true) { - //double decoder_latency_ms=(double)(current_symbol.Tracking_sample_counter-d_symbol_history.at(0).Tracking_sample_counter) - // /(double)current_symbol.fs; - // update TOW at the preamble instant (account with decoder latency) - d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 2.0 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; d_TOW_at_current_symbol_ms = static_cast(d_GPS_FSM.d_nav.d_TOW) * 1000 + 161; - //d_TOW_at_current_symbol = floor(d_TOW_at_Preamble * 1000.0) / 1000.0; - d_TOW_at_current_symbol = d_TOW_at_Preamble; flag_TOW_set = true; d_flag_new_tow_available = false; } else { - d_TOW_at_current_symbol += GPS_L1_CA_CODE_PERIOD; d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS; } - current_symbol.TOW_at_current_symbol_s = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; - //current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol; + current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; current_symbol.Flag_valid_word = flag_TOW_set; if (flag_PLL_180_deg_phase_locked == true) @@ -430,7 +421,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ { double tmp_double; unsigned long int tmp_ulong_int; - tmp_double = d_TOW_at_current_symbol; + tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_ulong_int = current_symbol.Tracking_sample_counter; d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(unsigned long int)); 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 5a6c4c7db..9d8c0e631 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 @@ -112,7 +112,6 @@ private: unsigned long int d_preamble_time_samples; double d_TOW_at_Preamble; - double d_TOW_at_current_symbol; unsigned int d_TOW_at_current_symbol_ms; bool flag_TOW_set; 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 5c4e34184..5cc385207 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 @@ -201,7 +201,7 @@ int gps_l2c_telemetry_decoder_cc::general_work(int noutput_items __attribute__(( d_flag_valid_word = false; } } - current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol; + current_synchro_data.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0); current_synchro_data.Flag_valid_word = d_flag_valid_word; if (d_dump == true) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc index 56ba9267f..87412096e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc @@ -253,7 +253,7 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u d_flag_valid_word = false; } } - current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol; + current_synchro_data.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0); current_synchro_data.Flag_valid_word = d_flag_valid_word; if (d_dump == true) diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index f26e92f59..b278f3c36 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -67,8 +67,8 @@ const double GPS_L1_CA_CHIP_PERIOD = 9.7752e-07; //!< GPS L1 C/A chip period const double MAX_TOA_DELAY_MS = 20; //#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here -const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) - +//const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) +const double GPS_STARTOFFSET_ms = 69.0; // OBSERVABLE HISTORY DEEP FOR INTERPOLATION const int GPS_L1_CA_HISTORY_DEEP = 100; diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index 7d572dffa..922420bc4 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -65,13 +65,14 @@ public: int correlation_length_ms; //!< Set by Tracking processing block //Telemetry Decoder - bool Flag_valid_word; //!< Set by Telemetry Decoder processing block - double TOW_at_current_symbol_s; //!< Set by Telemetry Decoder processing block + bool Flag_valid_word; //!< Set by Telemetry Decoder processing block + unsigned int TOW_at_current_symbol_ms; //!< Set by Telemetry Decoder processing block // Observables double Pseudorange_m; //!< Set by Observables processing block double RX_time; //!< Set by Observables processing block bool Flag_valid_pseudorange; //!< Set by Observables processing block + double interp_TOW_ms; //!< Set by Observables processing block }; #endif