diff --git a/src/algorithms/libs/gnss_time.h b/src/algorithms/libs/gnss_time.h index 9d56850fe..391f61f4e 100644 --- a/src/algorithms/libs/gnss_time.h +++ b/src/algorithms/libs/gnss_time.h @@ -22,7 +22,8 @@ public: //time_t time; /* time (s) expressed by standard time_t */ int week; /*!< GPS week number (since January 1980) */ //double sec; /*!< second inside the GPS \a week */ - int tow_ms; /* time of week [ms]*/ + int tow_ms; /* time of week [ms]*/ + double tow_ms_fraction; /* tow ms fractional part [ms]*/ template diff --git a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc index 57eb81987..42f0b0070 100644 --- a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc +++ b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc @@ -101,10 +101,17 @@ int Gnss_Sdr_Timestamp::work(int noutput_items, for (size_t ch = 0; ch < output_items.size(); ch++) { std::memcpy(output_items[ch], input_items[ch], noutput_items * input_signature()->sizeof_stream_item(ch)); - int64_t diff_samplecount = uint64diff(nitems_read(ch), next_timetag_samplecount); - if (diff_samplecount > 0 and diff_samplecount < noutput_items) + uint64_t bytes_to_samples = 2; //todo: improve this.. hardcoded 2 bytes -> 1 complex sample! + int64_t diff_samplecount = uint64diff(this->nitems_written(ch), next_timetag_samplecount * bytes_to_samples); + //std::cout << "diff_samplecount: " << diff_samplecount << ", noutput_items: " << noutput_items << "\n"; + if (diff_samplecount <= noutput_items and std::labs(diff_samplecount) <= noutput_items) { - add_item_tag(ch, this->nitems_written(0) + diff_samplecount, pmt::mp("timetag"), pmt::make_any(next_timetag)); + const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); + tmp_obj->tow_ms = next_timetag.tow_ms; + tmp_obj->week = next_timetag.week; + tmp_obj->tow_ms_fraction = 0; + add_item_tag(ch, this->nitems_written(ch) - diff_samplecount, pmt::mp("timetag"), pmt::make_any(tmp_obj)); + std::cout << "[" << this->nitems_written(ch) - diff_samplecount << "] Sent TimeTag SC: " << next_timetag_samplecount * bytes_to_samples << ", Week: " << next_timetag.week << ", TOW: " << next_timetag.tow_ms << " [ms] \n"; get_next_timetag = true; } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt index 8fd6c9189..7620f0524 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt @@ -52,6 +52,7 @@ target_link_libraries(telemetry_decoder_gr_blocks PUBLIC telemetry_decoder_libswiftcnav telemetry_decoder_libs + algorithms_libs core_system_parameters Gnuradio::runtime Boost::headers diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index bea2b3174..d3999ef29 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -118,6 +118,8 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( d_flag_PLL_180_deg_phase_locked = false; d_prev_GPS_frame_4bytes = 0; d_symbol_history.set_capacity(d_required_symbols); + + set_tag_propagation_policy(TPP_DONT); //no tag propagation, the time tag will be adjusted and regenerated in work() } @@ -350,6 +352,7 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ current_symbol = in[0][0]; // add new symbol to the symbol queue d_symbol_history.push_back(current_symbol.Prompt_I); + d_sample_counter++; // count for the processed symbols consume_each(1); d_flag_preamble = false; @@ -518,6 +521,33 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ current_symbol.Carrier_phase_rads += GNSS_PI; } + + //**************** time tags **************** + std::vector tags_vec; + this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + 1); + for (std::vector::iterator it = tags_vec.begin(); it != tags_vec.end(); ++it) + { + try + { + if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + const std::shared_ptr timetag = boost::any_cast>(pmt::any_ref(it->value)); + std::cout << "[" << this->nitems_written(0) + 1 << "] TLM RX TimeTag Week: " << timetag->week << ", TOW: " << timetag->tow_ms << " [ms], TOW fraction: " << timetag->tow_ms_fraction + << " [ms], DELTA TLM TOW: " << static_cast(timetag->tow_ms - current_symbol.TOW_at_current_symbol_ms) + timetag->tow_ms_fraction << " [ms] \n"; + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast &e) + { + std::cout << "msg Bad any_cast: " << e.what(); + } + } + + //************* end time tags ************** + if (d_dump == true) { // MULTIPLEXED FILE RECORDING - Record results to file diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h index aabd96238..3f5edb0e4 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h @@ -16,11 +16,11 @@ #ifndef GNSS_SDR_GPS_L1_CA_TELEMETRY_DECODER_GS_H #define GNSS_SDR_GPS_L1_CA_TELEMETRY_DECODER_GS_H - #include "GPS_L1_CA.h" #include "gnss_block_interface.h" #include "gnss_satellite.h" #include "gnss_synchro.h" +#include "gnss_time.h" //for timetags produced by Tracking #include "gps_navigation_message.h" #include "tlm_conf.h" #include diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 92f11ded9..fcdadf2f2 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -496,7 +496,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl d_rem_carr_phase_rad = 0.0; // sample synchronization - d_sample_counter = 0ULL; + //d_sample_counter = 0ULL; d_acq_sample_stamp = 0ULL; d_current_prn_length_samples = static_cast(d_trk_parameters.vector_length); @@ -588,6 +588,10 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl } d_corrected_doppler = false; d_acc_carrier_phase_initialized = false; + + d_last_timetag_samplecounter = 0; + d_timetag_waiting = false; + set_tag_propagation_policy(TPP_DONT); //no tag propagation, the time tag will be adjusted and regenerated in work() } @@ -1390,7 +1394,7 @@ void dll_pll_veml_tracking::log_data() d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); // PRN start sample stamp - tmp_long_int = d_sample_counter + static_cast(d_current_prn_length_samples); + tmp_long_int = this->nitems_read(0) + static_cast(d_current_prn_length_samples); d_dump_file.write(reinterpret_cast(&tmp_long_int), sizeof(uint64_t)); // accumulated carrier phase tmp_float = static_cast(d_acc_carrier_phase_rad); @@ -1424,7 +1428,7 @@ void dll_pll_veml_tracking::log_data() // AUX vars (for debug purposes) tmp_float = static_cast(d_rem_code_phase_samples); d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + tmp_double = static_cast(this->nitems_read(0) + d_current_prn_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // PRN uint32_t prn_ = d_acquisition_gnss_synchro->PRN; @@ -1682,6 +1686,13 @@ void dll_pll_veml_tracking::stop_tracking() d_state = 0; } +int64_t dll_pll_veml_tracking::uint64diff(uint64_t first, uint64_t second) +{ + uint64_t abs_diff = (first > second) ? (first - second) : (second - first); + assert(abs_diff <= INT64_MAX); + return (first > second) ? (int64_t)abs_diff : -(int64_t)abs_diff; +} + int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) @@ -1695,7 +1706,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) if (d_pull_in_transitory == true) { - if (d_trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) + //if (d_trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) + if (d_trk_parameters.pull_in_time_s < (this->nitems_read(0) - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) { d_pull_in_transitory = false; d_carrier_lock_fail_counter = 0; @@ -1706,7 +1718,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { case 0: // Standby - Consume samples at full throttle, do nothing { - d_sample_counter += static_cast(ninput_items[0]); + //d_sample_counter += static_cast(ninput_items[0]); consume_each(ninput_items[0]); return 0; break; @@ -1714,7 +1726,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) case 1: // Pull-in { // Signal alignment (skip samples until the incoming signal is aligned with local replica) - const int64_t acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); + //const int64_t acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); + const int64_t acq_trk_diff_samples = static_cast(this->nitems_read(0)) - static_cast(d_acq_sample_stamp); const double acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / d_trk_parameters.fs_in; const double delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) - d_acq_code_phase_samples; @@ -1731,7 +1744,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) const int32_t samples_offset = round(d_acq_code_phase_samples); d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); d_state = 2; - d_sample_counter += samples_offset; // count for the processed samples + //d_sample_counter += samples_offset; // count for the processed samples d_cn0_smoother.reset(); d_carrier_lock_test_smoother.reset(); @@ -1762,7 +1775,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) // } // fail-safe: check if the secondary code or bit synchronization has not succeeded in a limited time period - if (d_trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) + //if (d_trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) + if (d_trk_parameters.bit_synchronization_time_limit_s < (this->nitems_read(0) - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) { d_carrier_lock_fail_counter = 300000; // force loss-of-lock condition LOG(INFO) << d_systemName << " " << d_signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel @@ -1981,14 +1995,65 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } } } + + //**************** time tags **************** + std::vector tags_vec; + this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + d_current_prn_length_samples); + for (std::vector::iterator it = tags_vec.begin(); it != tags_vec.end(); ++it) + { + try + { + if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + //std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. counter " << d_sample_counter << " vs. nread " << this->nitems_read(0) << " containing "; + //std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. nread " << this->nitems_read(0) << " containing "; + const std::shared_ptr last_timetag = boost::any_cast>(pmt::any_ref(it->value)); + d_last_timetag = *last_timetag; + d_last_timetag_samplecounter = it->offset; + d_timetag_waiting = true; + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast &e) + { + std::cout << "msg Bad any_cast: " << e.what(); + } + } + + //************* end time tags ************** + consume_each(d_current_prn_length_samples); - d_sample_counter += static_cast(d_current_prn_length_samples); + //d_sample_counter += static_cast(d_current_prn_length_samples); if (current_synchro_data.Flag_valid_symbol_output || loss_of_lock) { current_synchro_data.fs = static_cast(d_trk_parameters.fs_in); - current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.Tracking_sample_counter = this->nitems_read(0); current_synchro_data.Flag_valid_symbol_output = !loss_of_lock; *out[0] = current_synchro_data; + + //generate new tag associated with gnss-synchro object + + if (d_timetag_waiting == true) + { + int64_t diff_samplecount = uint64diff(current_synchro_data.Tracking_sample_counter, d_last_timetag_samplecounter); + + double intpart; + d_last_timetag.tow_ms_fraction = modf(1000.0 * static_cast(diff_samplecount) / d_trk_parameters.fs_in, &intpart); + + const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); + tmp_obj->week = d_last_timetag.week; + tmp_obj->tow_ms = d_last_timetag.tow_ms + static_cast(intpart); + tmp_obj->tow_ms_fraction = d_last_timetag.tow_ms_fraction; + + add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(tmp_obj)); + + std::cout << "[" << this->nitems_written(0) + 1 << "] Sent TimeTag Week: " << d_last_timetag.week << ", TOW: " << d_last_timetag.tow_ms << " [ms], TOW fraction: " << d_last_timetag.tow_ms_fraction << " [ms] \n"; + d_timetag_waiting = false; + } + return 1; } return 0; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 71948e4f0..ac5c0ef8b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -22,6 +22,7 @@ #include "dll_pll_conf.h" #include "exponential_smoother.h" #include "gnss_block_interface.h" +#include "gnss_time.h" //for timetags produced by File_Timestamp_Signal_Source #include "tracking_FLL_PLL_filter.h" // for PLL/FLL filter #include "tracking_loop_filter.h" // for DLL filter #include @@ -83,6 +84,7 @@ private: void log_data(); bool cn0_and_tracking_lock_status(double coh_integration_time_s); bool acquire_secondary(); + int64_t uint64diff(uint64_t first, uint64_t second); int32_t save_matfile() const; Cpu_Multicorrelator_Real_Codes d_multicorrelator_cpu; @@ -163,8 +165,11 @@ private: std::ofstream d_dump_file; - uint64_t d_sample_counter; + //uint64_t d_sample_counter; uint64_t d_acq_sample_stamp; + GnssTime d_last_timetag; + uint64_t d_last_timetag_samplecounter; + bool d_timetag_waiting; float *d_prompt_data_shift; float d_rem_carr_phase_rad;