diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc index 81101c59d..e568da7d6 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.cc +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -33,9 +33,10 @@ #include "gnss_synchro.h" #include -gnss_sdr_sample_counter::gnss_sdr_sample_counter (double _fs) : gr::sync_decimator("sample_counter", +gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs) : gr::sync_decimator("sample_counter", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), floor(_fs*0.001)) + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), + static_cast(floor(_fs * 0.001))) { this->message_port_register_out(pmt::mp("sample_counter")); current_T_rx_ms = 0; @@ -44,14 +45,14 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter (double _fs) : gr::sync_decimat } -gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter (double _fs) +gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs) { gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter(_fs)); return sample_counter_; } -int gnss_sdr_sample_counter::work (int noutput_items __attribute__((unused)), +int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), gr_vector_const_void_star &input_items __attribute__((unused)), gr_vector_void_star &output_items) { @@ -60,7 +61,7 @@ int gnss_sdr_sample_counter::work (int noutput_items __attribute__((unused)), if ((current_T_rx_ms % report_interval_ms) == 0) { std::cout << "Current receiver time: " << static_cast(current_T_rx_ms) / 1000.0 << " [s]" << std::endl; - if(flag_enable_send_msg == true) + if(flag_enable_send_msg) { this->message_port_pub(pmt::mp("receiver_time"), pmt::from_double(static_cast(current_T_rx_ms) / 1000.0)); } diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.h b/src/algorithms/libs/gnss_sdr_sample_counter.h index d5c75920b..b3263e5b7 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.h +++ b/src/algorithms/libs/gnss_sdr_sample_counter.h @@ -28,8 +28,8 @@ * * ------------------------------------------------------------------------- */ -#ifndef GNSS_SDR_sample_counter_H_ -#define GNSS_SDR_sample_counter_H_ +#ifndef GNSS_SDR_SAMPLE_COUNTER_H_ +#define GNSS_SDR_SAMPLE_COUNTER_H_ #include #include @@ -39,20 +39,23 @@ class gnss_sdr_sample_counter; typedef boost::shared_ptr gnss_sdr_sample_counter_sptr; -gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter (double _fs); +gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs); class gnss_sdr_sample_counter : public gr::sync_decimator { - friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs); - gnss_sdr_sample_counter (double _fs); +private: + + gnss_sdr_sample_counter(double _fs); long long int current_T_rx_ms; int report_interval_ms; bool flag_enable_send_msg; public: + + friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs); int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); }; -#endif /*GNSS_SDR_sample_counter_H_*/ +#endif /*GNSS_SDR_SAMPLE_COUNTER_H_*/ diff --git a/src/algorithms/observables/adapters/hybrid_observables.cc b/src/algorithms/observables/adapters/hybrid_observables.cc index 7bf7d53a7..15f7bfc2a 100644 --- a/src/algorithms/observables/adapters/hybrid_observables.cc +++ b/src/algorithms/observables/adapters/hybrid_observables.cc @@ -39,12 +39,8 @@ using google::LogMessage; HybridObservables::HybridObservables(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams) : - role_(role), - in_streams_(in_streams), - out_streams_(out_streams) + std::string role, unsigned int in_streams, unsigned int out_streams) : + role_(role), in_streams_(in_streams), out_streams_(out_streams) { std::string default_dump_filename = "./observables.dat"; DLOG(INFO) << "role " << role; @@ -61,18 +57,14 @@ HybridObservables::HybridObservables(ConfigurationInterface* configuration, } unsigned int history_deep = configuration->property(role + ".history_depth", default_depth); observables_ = hybrid_make_observables_cc(in_streams_, out_streams_, dump_, dump_filename_, history_deep); - DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")"; + DLOG(INFO) << "Observables block ID (" << observables_->unique_id() << ")"; } - - HybridObservables::~HybridObservables() {} - - void HybridObservables::connect(gr::top_block_sptr top_block) { if(top_block) { /* top_block is not null */}; @@ -81,7 +73,6 @@ void HybridObservables::connect(gr::top_block_sptr top_block) } - void HybridObservables::disconnect(gr::top_block_sptr top_block) { if(top_block) { /* top_block is not null */}; @@ -89,16 +80,12 @@ void HybridObservables::disconnect(gr::top_block_sptr top_block) } - - gr::basic_block_sptr HybridObservables::get_left_block() { return observables_; } - - gr::basic_block_sptr HybridObservables::get_right_block() { return observables_; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index fc816d8b0..ce11b6a1d 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -326,7 +326,7 @@ 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) +void hybrid_observables_cc::forecast(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items_required) { for(unsigned int i = 0; i < d_nchannels; i++) { @@ -338,13 +338,11 @@ void hybrid_observables_cc::forecast (int noutput_items __attribute__((unused)), } -int hybrid_observables_cc::general_work (int noutput_items , - gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items) +int hybrid_observables_cc::general_work(int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { - const Gnss_Synchro **in = reinterpret_cast(&input_items[0]); // Get the input buffer pointer - Gnss_Synchro **out = reinterpret_cast(&output_items[0]); // Get the output buffer pointer + const Gnss_Synchro** in = reinterpret_cast(&input_items[0]); // Get the input buffer pointer + Gnss_Synchro** out = reinterpret_cast(&output_items[0]); // Get the output buffer pointer int n_outputs = 0; int n_consume[d_nchannels]; double past_history_s = 100e-3; @@ -363,19 +361,24 @@ int hybrid_observables_cc::general_work (int noutput_items , for (unsigned int i = 0; i < d_nchannels; i++) { n_consume[i] = ninput_items[i]; // full throttle - for (int j = 0; j < n_consume[i]; j++) + for(int j = 0; j < n_consume[i]; j++) { d_gnss_synchro_history_queue[i].push_back(in[i][j]); } } bool channel_history_ok; + do { + + try + { + channel_history_ok = true; - for (unsigned int i = 0; i < d_nchannels; i++) + for(unsigned int i = 0; i < d_nchannels; i++) { - if (d_gnss_synchro_history_queue[i].size() < history_deep && !d_gnss_synchro_history_queue[i].empty()) + if (d_gnss_synchro_history_queue.at(i).size() < history_deep && !d_gnss_synchro_history_queue.at(i).empty()) { channel_history_ok = false; } @@ -392,15 +395,15 @@ int hybrid_observables_cc::general_work (int noutput_items , std::map gnss_synchro_map; for (unsigned int i = 0; i < d_nchannels; i++) { - if (!d_gnss_synchro_history_queue[i].empty()) + if (!d_gnss_synchro_history_queue.at(i).empty()) { - gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].front().Channel_ID, - d_gnss_synchro_history_queue[i].front())); + gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue.at(i).front().Channel_ID, + d_gnss_synchro_history_queue.at(i).front())); } } - if (gnss_synchro_map.empty()) break; + if(gnss_synchro_map.empty()) { break; } // Breaks the do-while loop - gnss_synchro_map_iter = min_element(gnss_synchro_map.cbegin(), + gnss_synchro_map_iter = std::min_element(gnss_synchro_map.cbegin(), gnss_synchro_map.cend(), Hybrid_pairCompare_gnss_synchro_sample_counter); T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / static_cast(gnss_synchro_map_iter->second.fs); @@ -414,13 +417,13 @@ int hybrid_observables_cc::general_work (int noutput_items , // shift channels history to match the reference TOW for (unsigned int i = 0; i < d_nchannels; i++) { - if (!d_gnss_synchro_history_queue[i].empty()) + if (!d_gnss_synchro_history_queue.at(i).empty()) { - gnss_synchro_deque_iter = std::lower_bound(d_gnss_synchro_history_queue[i].cbegin(), - d_gnss_synchro_history_queue[i].cend(), + gnss_synchro_deque_iter = std::lower_bound(d_gnss_synchro_history_queue.at(i).cbegin(), + d_gnss_synchro_history_queue.at(i).cend(), T_rx_s, Hybrid_valueCompare_gnss_synchro_receiver_time); - if (gnss_synchro_deque_iter != d_gnss_synchro_history_queue[i].cend()) + if (gnss_synchro_deque_iter != d_gnss_synchro_history_queue.at(i).cend()) { if (gnss_synchro_deque_iter->Flag_valid_word == true) { @@ -432,24 +435,24 @@ int hybrid_observables_cc::general_work (int noutput_items , { // record the word structure in a map for pseudorange computation // save the previous observable - int distance = std::distance(d_gnss_synchro_history_queue[i].cbegin(), gnss_synchro_deque_iter); + int distance = std::distance(d_gnss_synchro_history_queue.at(i).cbegin(), gnss_synchro_deque_iter); if (distance > 0) { - if (d_gnss_synchro_history_queue[i].at(distance - 1).Flag_valid_word) + if (d_gnss_synchro_history_queue.at(i).at(distance - 1).Flag_valid_word) { - double T_rx_channel_prev = static_cast(d_gnss_synchro_history_queue[i].at(distance - 1).Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); + double T_rx_channel_prev = static_cast(d_gnss_synchro_history_queue.at(i).at(distance - 1).Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); double delta_T_rx_s_prev = T_rx_channel_prev - T_rx_s; if (fabs(delta_T_rx_s_prev) < fabs(delta_T_rx_s)) { - realigned_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, - d_gnss_synchro_history_queue[i].at(distance - 1))); + realigned_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue.at(i).at(distance - 1).Channel_ID, + d_gnss_synchro_history_queue.at(i).at(distance - 1))); adjacent_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); } else { realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - adjacent_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, - d_gnss_synchro_history_queue[i].at(distance - 1))); + adjacent_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue.at(i).at(distance - 1).Channel_ID, + d_gnss_synchro_history_queue.at(i).at(distance - 1))); } } @@ -472,7 +475,7 @@ int hybrid_observables_cc::general_work (int noutput_items , * common RX time algorithm */ // what is the most recent symbol TOW in the current set? -> this will be the reference symbol - gnss_synchro_map_iter = max_element(realigned_gnss_synchro_map.cbegin(), + gnss_synchro_map_iter = std::max_element(realigned_gnss_synchro_map.cbegin(), realigned_gnss_synchro_map.cend(), Hybrid_pairCompare_gnss_synchro_d_TOW); double ref_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); @@ -575,20 +578,33 @@ int hybrid_observables_cc::general_work (int noutput_items , } // Move RX time - T_rx_s = T_rx_s + T_rx_step_s; + T_rx_s += T_rx_step_s; // pop old elements from queue for (unsigned int i = 0; i < d_nchannels; i++) { - if (!d_gnss_synchro_history_queue[i].empty()) + if (!d_gnss_synchro_history_queue.at(i).empty()) { - while (static_cast(d_gnss_synchro_history_queue[i].front().Tracking_sample_counter) / static_cast(d_gnss_synchro_history_queue[i].front().fs) < (T_rx_s - past_history_s)) + while (static_cast(d_gnss_synchro_history_queue.at(i).front().Tracking_sample_counter) / static_cast(d_gnss_synchro_history_queue.at(i).front().fs) < (T_rx_s - past_history_s)) { - d_gnss_synchro_history_queue[i].pop_front(); + d_gnss_synchro_history_queue.at(i).pop_front(); } } } } - } while(channel_history_ok == true && noutput_items > n_outputs); + + }// End of try{...} + catch(std::out_of_range& e) + { + LOG(WARNING) << "Out of range exception thrown by Hybrid Observables block. Exception message: " << e.what(); + std::cout << "Out of range exception thrown by Hybrid Observables block. Exception message: " << e.what() << std::endl; + } + catch(...) + { + LOG(WARNING) << "Undefined exception thrown by Hybrid Observables block."; + std::cout << "Undefined exception thrown by Hybrid Observables block." << std::endl; + } + + }while(channel_history_ok == true && noutput_items > n_outputs); // Multi-rate consume! for (unsigned int i = 0; i < d_nchannels; i++)