diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc index 4047fad1a..9db529625 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.cc +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -40,7 +40,6 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs) : gr::sync_decimato { message_port_register_out(pmt::mp("sample_counter")); set_max_noutput_items(1); - set_max_output_buffer(1); current_T_rx_ms = 0; report_interval_ms = 1000;//default reporting 1 second flag_enable_send_msg = false; //enable it for reporting time with asynchronous message diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index eea804e17..f69eeee61 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -55,17 +55,22 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)), gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) { - //set_max_noutput_items(1); - //set_max_output_buffer(1); + set_max_noutput_items(1); + set_max_output_buffer(1); d_dump = dump; + set_T_rx_s = false; d_nchannels = nchannels_out; d_dump_filename = dump_filename; T_rx_s = 0.0; T_rx_step_s = 0.001; // 1 ms - max_extrapol_time_s = 0.1; // 100 ms + max_delta = 0.05; // 50 ms valid_channels.resize(d_nchannels, false); d_num_valid_channels = 0; + for(unsigned int i = 0; i < d_nchannels; i++) + { + d_gnss_synchro_history.push_back(std::deque()); + } // ############# ENABLE DATA FILE LOG ################# if (d_dump) @@ -285,7 +290,7 @@ int hybrid_observables_cc::save_matfile() return 0; } -double Hybrid_Interpolate_data(const std::pair& a, const double& ti, int parameter) +double hybrid_observables_cc::interpolate_data(const std::pair& a, const double& ti, int parameter) { // x(ti) = m * ti + c // m = [x(t2) - x(t1)] / [t2 - t1] @@ -321,7 +326,7 @@ double Hybrid_Interpolate_data(const std::pair& a, c return(m * ti + c); } -double Hybrid_Compute_T_rx_s(const Gnss_Synchro& a) +double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro& a) { if(a.Flag_valid_word) { @@ -330,11 +335,6 @@ double Hybrid_Compute_T_rx_s(const Gnss_Synchro& a) else { return 0.0; } } -bool Hybrid_find_trx(const Gnss_Synchro&a) -{ - return(std::fabs(T_rx_s - a.RX_time) > trx_comp_thres); -} - /* bool Hybrid_pairCompare_gnss_synchro_T_rx(const std::pair& a, const std::pair& b) { @@ -393,14 +393,80 @@ bool Hybrid_valueCompare_gnss_synchro_d_TOW(const Gnss_Synchro& a, double b) void hybrid_observables_cc::forecast(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items_required) { - bool available_items = false; +// bool available_items = false; +// for(unsigned int i = 0; i < d_nchannels; i++) +// { +// ninput_items_required[i] = 0; +// if(detail()->input(i)->items_available() > 0) { available_items = true; } +// } +// if(available_items) { ninput_items_required[d_nchannels] = 0; } +// else { ninput_items_required[d_nchannels] = 1; } for(unsigned int i = 0; i < d_nchannels; i++) { ninput_items_required[i] = 0; - if(detail()->input(i)->items_available() > 0) { available_items = true; } } - if(available_items) { ninput_items_required[d_nchannels] = 0; } - else { ninput_items_required[d_nchannels] = 1; } + ninput_items_required[d_nchannels] = 1; +} + +void hybrid_observables_cc::clean_history(std::deque& data) +{ + while(data.size() > 0) + { + if((T_rx_s - data.front().RX_time) > max_delta) { data.pop_front(); } + else { return; } + } +} + +unsigned int hybrid_observables_cc::find_closest(std::deque& data) +{ + unsigned int result = 0; + double delta_t = std::numeric_limits::max(); + std::deque::iterator it; + unsigned int aux = 0; + for(it = data.begin(); it != data.end(); it++) + { + double instant_delta = T_rx_s - it->RX_time; + if((instant_delta > 0) and (instant_delta < delta_t)) + { + delta_t = instant_delta; + result = aux; + } + aux++; + } + return result; +} + +double hybrid_observables_cc::find_min_RX_time() +{ + if(d_num_valid_channels == 0) { return 0.0; } + + std::vector>::iterator it = d_gnss_synchro_history.begin(); + double result = std::numeric_limits::max(); + for(unsigned int i = 0; i < d_nchannels; i++) + { + if(valid_channels[i]) + { + if(it->front().RX_time < result) { result = it->front().RX_time; } + } + it++; + } + return(floor(result * 1000.0) / 1000.0); +} + +void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector& data) +{ + double TOW_ref = std::numeric_limits::lowest(); + std::vector::iterator it; + for(it = data.begin(); it != data.end(); it++) + { + if(it->RX_time > TOW_ref) { TOW_ref = it->RX_time; } + } + for(it = data.begin(); it != data.end(); it++) + { + double traveltime_s = TOW_ref - it->RX_time + GPS_STARTOFFSET_ms / 1000.0; + it->RX_time = TOW_ref + GPS_STARTOFFSET_ms / 1000.0; + it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT; + } } @@ -414,35 +480,14 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) unsigned int i; int total_input_items = 0; for(i = 0; i < d_nchannels; i++) { total_input_items += ninput_items[i]; } - bool compute_output = false; + consume(d_nchannels, 1); ////////////////////////////////////////////////////////////////////////// - if((total_input_items == 0) and (ninput_items[d_nchannels] == 0)) + if((total_input_items == 0) and (d_num_valid_channels == 0)) { return 0; } - else if((total_input_items == 0) and (ninput_items[d_nchannels] > 0) and (d_num_valid_channels == 0)) - { - T_rx_s += T_rx_step_s; - consume(d_nchannels, 1); - return 0; - } - else if((total_input_items == 0) and (ninput_items[d_nchannels] > 0) and (d_num_valid_channels > 0)) - { - T_rx_s += T_rx_step_s; - compute_output = true; - consume(d_nchannels, 1); - } - else if((total_input_items > 0) and (ninput_items[d_nchannels] == 0)) - {} - else if((total_input_items > 0) and (ninput_items[d_nchannels] > 0)) - { - T_rx_s += T_rx_step_s; - compute_output = true; - consume(d_nchannels, 1); - } - else - {} + if(set_T_rx_s) { T_rx_s += T_rx_step_s; } ////////////////////////////////////////////////////////////////////////// @@ -450,45 +495,116 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) if (total_input_items > 0) { i = 0; - for (it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) + for(it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) { - if (ninput_items[i] > 0) + if(ninput_items[i] > 0) { for(int aux = 0; aux < ninput_items[i]; aux++) { - it->push_back(in[i][aux]); - it->at(it->size() - 1).RX_time = Hybrid_Compute_T_rx_s(in[i][aux]); + if(in[i][aux].Flag_valid_word) + { + it->push_back(in[i][aux]); + it->back().RX_time = compute_T_rx_s(in[i][aux]); + } } - consume(ninput_items[i], 1); + consume(i, ninput_items[i]); } - while(std::find(d_gnss_synchro_history.begin(), d_gnss_synchro_history.end(), Hybrid_compare_trx) != d_gnss_synchro_history.end()) - { - - } - if (it->size() > 2) { valid_channels[i] = true; } - else { valid_channels[i] = false; } i++; } } + for(i = 0; i < d_nchannels; i++) + { + if(d_gnss_synchro_history.at(i).size() > 2) { valid_channels[i] = true; } + else { valid_channels[i] = false; } + } d_num_valid_channels = valid_channels.count(); // Check if there is any valid channel after reading the new incoming Gnss_Synchro data - if(d_num_valid_channels == 0) { return 0; } + if(d_num_valid_channels == 0) + { + set_T_rx_s = false; + return 0; + } - for(i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the extrapolation threshold + if(!set_T_rx_s) //Find the lowest RX_time among the valid observables in the history + { + T_rx_s = find_min_RX_time(); + set_T_rx_s = true; + } + + for(i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the threshold { if(valid_channels[i]) { - double delta_t = T_rx_s - d_gnss_synchro_history.at(i).second.RX_time; - //std::cout << "Sat " << d_gnss_synchro_history.at(i).second.PRN << ". Dt = " << delta_t * 1000.0 <<". Rx 2 "<< d_gnss_synchro_history.at(i).second.RX_time<<". Rx 1 "<< d_gnss_synchro_history.at(i).first.RX_time< max_extrapol_time_s) - { valid_channels[i] = false; } + clean_history(d_gnss_synchro_history.at(i)); + if(d_gnss_synchro_history.at(i).size() < 2) { valid_channels[i] = false; } } } - d_num_valid_channels = valid_channels.count(); // Check if there is any valid channel after computing the time distance between the Gnss_Synchro data and the receiver time - if((d_num_valid_channels == 0) or !compute_output) { return 0; } + d_num_valid_channels = valid_channels.count(); + if(d_num_valid_channels == 0) + { + set_T_rx_s = false; + return 0; + } + std::vector epoch_data; + i = 0; + for(it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) + { + if(valid_channels[i]) + { + unsigned int index_closest = find_closest(*it); + unsigned int index1; + unsigned int index2; + if(index_closest == (it->size() - 1)) + { + index1 = index_closest - 1; + index2 = index_closest; + } + else + { + index1 = index_closest; + index2 = index_closest + 1; + } + Gnss_Synchro interpolated_gnss_synchro = it->at(index1); + + interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data( + std::pair(it->at(index2), it->at(index1)), T_rx_s, 0); + + interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data( + std::pair(it->at(index2), it->at(index1)), T_rx_s, 1); + + interpolated_gnss_synchro.RX_time = interpolate_data( + std::pair(it->at(index2), it->at(index1)), T_rx_s, 2); + + //interpolated_gnss_synchro.Code_phase_samples = interpolate_data( + // std::pair(it->at(index2), it->at(index1)), T_rx_s, 3); + + epoch_data.push_back(interpolated_gnss_synchro); + } + i++; + } + + correct_TOW_and_compute_prange(epoch_data); + std::vector::iterator it2 = epoch_data.begin(); + for(i = 0; i < d_nchannels; i++) + { + if(valid_channels[i]) + { + out[i][0] = (*it2); + out[i][0].Flag_valid_pseudorange = true; + it2++; + } + else + { + out[i][0] = Gnss_Synchro(); + out[i][0].Flag_valid_pseudorange = false; + } + } + return 1; + + /* ANTONIO it = d_gnss_synchro_history.begin(); double TOW_ref = std::numeric_limits::max(); for(i = 0; i < d_nchannels; i++) @@ -520,6 +636,8 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) } return 1; + */ + /******************************* OLD ALGORITHM ********************************/ // const Gnss_Synchro** in = reinterpret_cast(&input_items[0]); // Get the input buffer pointer diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index 4d1119a35..ada7b3d47 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -35,7 +35,7 @@ #include #include -//#include //std::pair +#include //std::pair #include //std::vector #include #include @@ -64,14 +64,21 @@ private: friend hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename); hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename); + void clean_history(std::deque& data); + double compute_T_rx_s(const Gnss_Synchro& a); + double interpolate_data(const std::pair& a, const double& ti, int parameter); + double find_min_RX_time(); + unsigned int find_closest(std::deque& data); + void correct_TOW_and_compute_prange(std::vector& data); //Tracking observable history std::vector> d_gnss_synchro_history; boost::dynamic_bitset<> valid_channels; double T_rx_s; double T_rx_step_s; - double max_extrapol_time_s; + double max_delta; bool d_dump; + bool set_T_rx_s; unsigned int d_nchannels; unsigned int d_num_valid_channels; std::string d_dump_filename; 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 5504f84cf..53ad564e8 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 @@ -108,7 +108,6 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { set_max_noutput_items(1); - set_max_output_buffer(1); // Telemetry Bit transition synchronization port out this->message_port_register_out(pmt::mp("preamble_timestamp_s")); // Ephemeris data port out 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 5e94348e6..174d6754d 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 @@ -184,7 +184,6 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { set_max_noutput_items(1); - set_max_output_buffer(1); // Telemetry Bit transition synchronization port out this->message_port_register_out(pmt::mp("preamble_timestamp_s")); // Ephemeris data port out 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 50adeed2d..79810f000 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 @@ -56,7 +56,6 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { set_max_noutput_items(1); - set_max_output_buffer(1); // Telemetry Bit transition synchronization port out this->message_port_register_out(pmt::mp("preamble_timestamp_s")); // Ephemeris data port out 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 1e000a009..e5f37423a 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 @@ -53,7 +53,6 @@ gps_l2c_telemetry_decoder_cc::gps_l2c_telemetry_decoder_cc( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { set_max_noutput_items(1); - set_max_output_buffer(1); // Telemetry Bit transition synchronization port out this->message_port_register_out(pmt::mp("preamble_timestamp_s")); // Ephemeris data port out 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 cd44ffad5..04c0890a6 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 @@ -55,7 +55,6 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { set_max_noutput_items(1); - set_max_output_buffer(1); // Telemetry Bit transition synchronization port out this->message_port_register_out(pmt::mp("preamble_timestamp_s")); // Ephemeris data port out