From 8885333aa78ba98f64bea8083dab0b0f66d0d51a Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 13 Feb 2018 18:16:03 +0100 Subject: [PATCH 01/63] Modify sample counter --- .../libs/gnss_sdr_sample_counter.cc | 41 +- src/algorithms/libs/gnss_sdr_sample_counter.h | 14 +- .../adapters/hybrid_observables.cc | 4 +- .../gnuradio_blocks/hybrid_observables_cc.cc | 449 +++++++++--------- .../gnuradio_blocks/hybrid_observables_cc.h | 6 +- src/core/receiver/gnss_block_factory.cc | 10 +- src/core/receiver/gnss_flowgraph.cc | 25 +- src/core/system_parameters/GPS_L1_CA.h | 2 +- 8 files changed, 287 insertions(+), 264 deletions(-) diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc index 026e08548..81101c59d 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.cc +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -33,39 +33,38 @@ #include "gnss_synchro.h" #include -gnss_sdr_sample_counter::gnss_sdr_sample_counter () : gr::sync_block("sample_counter", - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), - gr::io_signature::make(0,0,0)) +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)) { this->message_port_register_out(pmt::mp("sample_counter")); - last_T_rx_s = 0; - report_interval_s = 1;//default reporting 1 second + 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 } -gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter () +gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter (double _fs) { - gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter()); + 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, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_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) { - const Gnss_Synchro *in = reinterpret_cast(input_items[0]); // input - - double current_T_rx_s = in[noutput_items - 1].Tracking_sample_counter / static_cast(in[noutput_items - 1].fs); - if ((current_T_rx_s - last_T_rx_s) > report_interval_s) + Gnss_Synchro* out = reinterpret_cast(output_items[0]); + out[0] = Gnss_Synchro(); + 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) { - std::cout << "Current receiver time: " << floor(current_T_rx_s) << " [s]" << std::endl; - if(flag_enable_send_msg == true) - { - this->message_port_pub(pmt::mp("receiver_time"), pmt::from_double(current_T_rx_s)); - } - last_T_rx_s = current_T_rx_s; + this->message_port_pub(pmt::mp("receiver_time"), pmt::from_double(static_cast(current_T_rx_ms) / 1000.0)); } - return noutput_items; + } + current_T_rx_ms++; + return 1; } diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.h b/src/algorithms/libs/gnss_sdr_sample_counter.h index ec1560ee8..d5c75920b 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.h +++ b/src/algorithms/libs/gnss_sdr_sample_counter.h @@ -31,7 +31,7 @@ #ifndef GNSS_SDR_sample_counter_H_ #define GNSS_SDR_sample_counter_H_ -#include +#include #include @@ -39,14 +39,14 @@ class gnss_sdr_sample_counter; typedef boost::shared_ptr gnss_sdr_sample_counter_sptr; -gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter (); +gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter (double _fs); -class gnss_sdr_sample_counter : public gr::sync_block +class gnss_sdr_sample_counter : public gr::sync_decimator { - friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(); - gnss_sdr_sample_counter (); - double last_T_rx_s; - double report_interval_s; + friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs); + gnss_sdr_sample_counter (double _fs); + long long int current_T_rx_ms; + int report_interval_ms; bool flag_enable_send_msg; public: diff --git a/src/algorithms/observables/adapters/hybrid_observables.cc b/src/algorithms/observables/adapters/hybrid_observables.cc index a3a33d38a..7bf7d53a7 100644 --- a/src/algorithms/observables/adapters/hybrid_observables.cc +++ b/src/algorithms/observables/adapters/hybrid_observables.cc @@ -57,10 +57,10 @@ HybridObservables::HybridObservables(ConfigurationInterface* configuration, } else { - default_depth = 500; + default_depth = 100; } unsigned int history_deep = configuration->property(role + ".history_depth", default_depth); - observables_ = hybrid_make_observables_cc(in_streams_, dump_, dump_filename_, history_deep); + observables_ = hybrid_make_observables_cc(in_streams_, out_streams_, dump_, dump_filename_, history_deep); DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")"; } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 66cd0fce0..fc816d8b0 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -47,19 +47,19 @@ using google::LogMessage; -hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) +hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename, unsigned int deep_history) { - return hybrid_observables_cc_sptr(new hybrid_observables_cc(nchannels, dump, dump_filename, deep_history)); + return hybrid_observables_cc_sptr(new hybrid_observables_cc(nchannels_in, nchannels_out, dump, dump_filename, deep_history)); } -hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) : - gr::block("hybrid_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), - gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro))) +hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename, unsigned int deep_history) : + gr::block("hybrid_observables_cc", gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)), + gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) { // initialize internal vars d_dump = dump; - d_nchannels = nchannels; + d_nchannels = nchannels_out; d_dump_filename = dump_filename; history_deep = deep_history; T_rx_s = 0.0; @@ -328,21 +328,13 @@ 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 zero_samples = true; for(unsigned int i = 0; i < d_nchannels; i++) - { - int items = detail()->input(i)->items_available(); - if (items > 0) zero_samples = false; - ninput_items_required[i] = items; // set the required available samples in each call - } - - if (zero_samples == true) - { - for(unsigned int i = 0; i < d_nchannels; i++) - { - ninput_items_required[i] = 1; // set the required available samples in each call - } - } + { + ninput_items_required[i] = 0; + //std::cout << "IN buffer "<< i << ". Number of items " << detail()->input(i)->items_available() << std::endl; + } + //std::cout << "SC buffer. Number of items " << detail()->input(d_nchannels)->items_available() << std::endl; + ninput_items_required[d_nchannels] = 1; // set the required available samples in each call } @@ -360,239 +352,252 @@ int hybrid_observables_cc::general_work (int noutput_items , Gnss_Synchro current_gnss_synchro[d_nchannels]; Gnss_Synchro aux = Gnss_Synchro(); for(unsigned int i = 0; i < d_nchannels; i++) - { - current_gnss_synchro[i] = aux; - } + { + current_gnss_synchro[i] = aux; + } /* * 1. Read the GNSS SYNCHRO objects from available channels. * Multi-rate GNURADIO Block. Read how many input items are avaliable in each channel * Record all synchronization data into queues */ 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++) { - n_consume[i] = ninput_items[i]; // full throttle - for (int j = 0; j < n_consume[i]; j++) - { - d_gnss_synchro_history_queue[i].push_back(in[i][j]); - } + d_gnss_synchro_history_queue[i].push_back(in[i][j]); } + } bool channel_history_ok; do + { + channel_history_ok = true; + for (unsigned int i = 0; i < d_nchannels; i++) { - channel_history_ok = true; + if (d_gnss_synchro_history_queue[i].size() < history_deep && !d_gnss_synchro_history_queue[i].empty()) + { + channel_history_ok = false; + } + } + if (channel_history_ok == true) + { + std::map::const_iterator gnss_synchro_map_iter; + std::deque::const_iterator gnss_synchro_deque_iter; + + // 1. If the RX time is not set, set the Rx time + if (T_rx_s == 0) + { + // 0. Read a gnss_synchro snapshot from the queue and store it in a map + std::map gnss_synchro_map; + for (unsigned int i = 0; i < d_nchannels; i++) + { + if (!d_gnss_synchro_history_queue[i].empty()) + { + gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].front().Channel_ID, + d_gnss_synchro_history_queue[i].front())); + } + } + if (gnss_synchro_map.empty()) break; + + gnss_synchro_map_iter = 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); + T_rx_s = floor(T_rx_s * 1000.0) / 1000.0; // truncate to ms + T_rx_s += past_history_s; // increase T_rx to have a minimum past history to interpolate + } + + // 2. Realign RX time in all valid channels + std::map realigned_gnss_synchro_map; // container for the aligned set of observables for the selected T_rx + std::map adjacent_gnss_synchro_map; // container for the previous observable values to interpolate + // 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[i].size() < history_deep) + gnss_synchro_deque_iter = std::lower_bound(d_gnss_synchro_history_queue[i].cbegin(), + d_gnss_synchro_history_queue[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->Flag_valid_word == true) { - channel_history_ok = false; - } - } - if (channel_history_ok == true) - { - std::map::const_iterator gnss_synchro_map_iter; - std::deque::const_iterator gnss_synchro_deque_iter; + double T_rx_channel = static_cast(gnss_synchro_deque_iter->Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); + double delta_T_rx_s = T_rx_channel - T_rx_s; - // 1. If the RX time is not set, set the Rx time - if (T_rx_s == 0) - { - // 0. Read a gnss_synchro snapshot from the queue and store it in a map - std::map gnss_synchro_map; - for (unsigned int i = 0; i < d_nchannels; i++) + // check that T_rx difference is less than a threshold (the correlation interval) + if (delta_T_rx_s * 1000.0 < static_cast(gnss_synchro_deque_iter->correlation_length_ms)) + { + // 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); + if (distance > 0) { - 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_iter = 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); - T_rx_s = floor(T_rx_s * 1000.0) / 1000.0; // truncate to ms - T_rx_s += past_history_s; // increase T_rx to have a minimum past history to interpolate - } - - // 2. Realign RX time in all valid channels - std::map realigned_gnss_synchro_map; // container for the aligned set of observables for the selected T_rx - std::map adjacent_gnss_synchro_map; // container for the previous observable values to interpolate - // shift channels history to match the reference TOW - for (unsigned int i = 0; i < d_nchannels; i++) - { - gnss_synchro_deque_iter = std::lower_bound(d_gnss_synchro_history_queue[i].cbegin(), - d_gnss_synchro_history_queue[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->Flag_valid_word == true) + if (d_gnss_synchro_history_queue[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 delta_T_rx_s_prev = T_rx_channel_prev - T_rx_s; + if (fabs(delta_T_rx_s_prev) < fabs(delta_T_rx_s)) { - double T_rx_channel = static_cast(gnss_synchro_deque_iter->Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); - double delta_T_rx_s = T_rx_channel - T_rx_s; - - // check that T_rx difference is less than a threshold (the correlation interval) - if (delta_T_rx_s * 1000.0 < static_cast(gnss_synchro_deque_iter->correlation_length_ms)) - { - // 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); - if (distance > 0) - { - if (d_gnss_synchro_history_queue[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 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))); - 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))); - } - } - - } - else - { - realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - } - - } + 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))); + 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))); } - } - } - - if(!realigned_gnss_synchro_map.empty()) - { - /* - * 2.1 Use CURRENT set of measurements and find the nearest satellite - * 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(), - realigned_gnss_synchro_map.cend(), - Hybrid_pairCompare_gnss_synchro_d_TOW); - double ref_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); - - // compute interpolated TOW value at T_rx_s - int ref_channel_key = gnss_synchro_map_iter->second.Channel_ID; - Gnss_Synchro adj_obs; - adj_obs = adjacent_gnss_synchro_map.at(ref_channel_key); - double ref_adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / ref_fs_hz + adj_obs.Code_phase_samples / ref_fs_hz; - - double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; - double d_ref_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / ref_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / ref_fs_hz; - - double selected_T_rx_s = T_rx_s; - // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) - double ref_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + - (selected_T_rx_s - ref_adj_T_rx_s) * (d_TOW_reference - adj_obs.TOW_at_current_symbol_s) / (d_ref_T_rx_s - ref_adj_T_rx_s); - - // Now compute RX time differences due to the PRN alignment in the correlators - double traveltime_ms; - double pseudorange_m; - double channel_T_rx_s; - double channel_fs_hz; - double channel_TOW_s; - for(gnss_synchro_map_iter = realigned_gnss_synchro_map.cbegin(); gnss_synchro_map_iter != realigned_gnss_synchro_map.cend(); gnss_synchro_map_iter++) - { - channel_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); - channel_TOW_s = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; - channel_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / channel_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / channel_fs_hz; - // compute interpolated observation values - // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) - // TOW at the selected receiver time T_rx_s - int element_key = gnss_synchro_map_iter->second.Channel_ID; - try{ - adj_obs = adjacent_gnss_synchro_map.at(element_key); - }catch(const std::exception & ex) - { - continue; } - double adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / channel_fs_hz + adj_obs.Code_phase_samples / channel_fs_hz; - - double channel_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + (selected_T_rx_s - adj_T_rx_s) * (channel_TOW_s - adj_obs.TOW_at_current_symbol_s) / (channel_T_rx_s - adj_T_rx_s); - - // Doppler and Accumulated carrier phase - double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_phase_rads - adj_obs.Carrier_phase_rads) / (channel_T_rx_s - adj_T_rx_s); - double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_Doppler_hz - adj_obs.Carrier_Doppler_hz) / (channel_T_rx_s - adj_T_rx_s); - - // compute the pseudorange (no rx time offset correction) - traveltime_ms = (ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s) * 1000.0 + GPS_STARTOFFSET_ms; - // convert to meters - pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] - // update the pseudorange object - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID] = gnss_synchro_map_iter->second; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Flag_valid_pseudorange = true; - // Save the estimated RX time (no RX clock offset correction yet!) - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = ref_TOW_at_T_rx_s + GPS_STARTOFFSET_ms / 1000.0; - - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_phase_rads = Carrier_phase_lin_rads; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_Doppler_hz = Carrier_Doppler_lin_hz; } - - if(d_dump == true) + else { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - for (unsigned int i = 0; i < d_nchannels; i++) - { - tmp_double = current_gnss_synchro[i].RX_time; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_s; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_phase_rads / GPS_TWO_PI; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Pseudorange_m; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].PRN; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(current_gnss_synchro[i].Flag_valid_pseudorange); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - } + realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); } - for (unsigned int i = 0; i < d_nchannels; i++) - { - out[i][n_outputs] = current_gnss_synchro[i]; - } - - n_outputs++; - } - - // Move RX time - T_rx_s = T_rx_s + T_rx_step_s; - // pop old elements from queue - for (unsigned int i = 0; i < d_nchannels; i++) - { - 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)) - { - d_gnss_synchro_history_queue[i].pop_front(); - } + } } + } } - } while(channel_history_ok == true && noutput_items > n_outputs); + } + + if(!realigned_gnss_synchro_map.empty()) + { + /* + * 2.1 Use CURRENT set of measurements and find the nearest satellite + * 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(), + realigned_gnss_synchro_map.cend(), + Hybrid_pairCompare_gnss_synchro_d_TOW); + double ref_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); + + // compute interpolated TOW value at T_rx_s + int ref_channel_key = gnss_synchro_map_iter->second.Channel_ID; + Gnss_Synchro adj_obs; + adj_obs = adjacent_gnss_synchro_map.at(ref_channel_key); + double ref_adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / ref_fs_hz + adj_obs.Code_phase_samples / ref_fs_hz; + + double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; + double d_ref_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / ref_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / ref_fs_hz; + + double selected_T_rx_s = T_rx_s; + // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) + double ref_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + + (selected_T_rx_s - ref_adj_T_rx_s) * (d_TOW_reference - adj_obs.TOW_at_current_symbol_s) / (d_ref_T_rx_s - ref_adj_T_rx_s); + + // Now compute RX time differences due to the PRN alignment in the correlators + double traveltime_ms; + double pseudorange_m; + double channel_T_rx_s; + double channel_fs_hz; + double channel_TOW_s; + for(gnss_synchro_map_iter = realigned_gnss_synchro_map.cbegin(); gnss_synchro_map_iter != realigned_gnss_synchro_map.cend(); gnss_synchro_map_iter++) + { + channel_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); + channel_TOW_s = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; + channel_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / channel_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / channel_fs_hz; + // compute interpolated observation values + // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) + // TOW at the selected receiver time T_rx_s + int element_key = gnss_synchro_map_iter->second.Channel_ID; + try{ + adj_obs = adjacent_gnss_synchro_map.at(element_key); + }catch(const std::exception & ex) + { + continue; + } + + double adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / channel_fs_hz + adj_obs.Code_phase_samples / channel_fs_hz; + + double channel_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + (selected_T_rx_s - adj_T_rx_s) * (channel_TOW_s - adj_obs.TOW_at_current_symbol_s) / (channel_T_rx_s - adj_T_rx_s); + + // Doppler and Accumulated carrier phase + double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_phase_rads - adj_obs.Carrier_phase_rads) / (channel_T_rx_s - adj_T_rx_s); + double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_Doppler_hz - adj_obs.Carrier_Doppler_hz) / (channel_T_rx_s - adj_T_rx_s); + + // compute the pseudorange (no rx time offset correction) + traveltime_ms = (ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s) * 1000.0 + GPS_STARTOFFSET_ms; + // convert to meters + pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] + // update the pseudorange object + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID] = gnss_synchro_map_iter->second; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Flag_valid_pseudorange = true; + // Save the estimated RX time (no RX clock offset correction yet!) + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = ref_TOW_at_T_rx_s + GPS_STARTOFFSET_ms / 1000.0; + + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_phase_rads = Carrier_phase_lin_rads; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_Doppler_hz = Carrier_Doppler_lin_hz; + } + + if(d_dump == true) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + for (unsigned int i = 0; i < d_nchannels; i++) + { + tmp_double = current_gnss_synchro[i].RX_time; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_s; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].Carrier_phase_rads / GPS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].Pseudorange_m; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].PRN; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(current_gnss_synchro[i].Flag_valid_pseudorange); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + } + catch (const std::ifstream::failure& e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + } + } + + for (unsigned int i = 0; i < d_nchannels; i++) + { + out[i][n_outputs] = current_gnss_synchro[i]; + } + + n_outputs++; + } + + // Move RX time + T_rx_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()) + { + 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)) + { + d_gnss_synchro_history_queue[i].pop_front(); + } + } + } + } + } while(channel_history_ok == true && noutput_items > n_outputs); // Multi-rate consume! for (unsigned int i = 0; i < d_nchannels; i++) - { - consume(i, n_consume[i]); // which input, how many items - } + { + consume(i, n_consume[i]); // which input, how many items + } + //consume monitor channel always + consume(d_nchannels, 1); return n_outputs; } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index 494f84e75..f1a1c3da2 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -44,7 +44,7 @@ class hybrid_observables_cc; typedef boost::shared_ptr hybrid_observables_cc_sptr; hybrid_observables_cc_sptr -hybrid_make_observables_cc(unsigned int n_channels, bool dump, std::string dump_filename, unsigned int deep_history); +hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename, unsigned int deep_history); /*! * \brief This class implements a block that computes Galileo observables @@ -58,8 +58,8 @@ public: void forecast (int noutput_items, gr_vector_int &ninput_items_required); private: friend hybrid_observables_cc_sptr - hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); - hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); + hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename, unsigned int deep_history); + hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename, unsigned int deep_history); //Tracking observable history std::vector> d_gnss_synchro_history_queue; diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 02aeffeb0..cf1ba3995 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -248,7 +248,15 @@ std::unique_ptr GNSSBlockFactory::GetObservables(std::shared GPS_channels += configuration->property("Channels_2S.count", 0); GPS_channels += configuration->property("Channels_L5.count", 0); unsigned int Glonass_channels = configuration->property("Channels_1G.count", 0); - return GetBlock(configuration, "Observables", implementation, Galileo_channels + GPS_channels + Glonass_channels, Galileo_channels + GPS_channels + Glonass_channels); + unsigned int extra_channels = 1; // For monitor channel sample counter + return GetBlock(configuration, "Observables", implementation, + Galileo_channels + + GPS_channels + + Glonass_channels + + extra_channels, + Galileo_channels + + GPS_channels + + Glonass_channels); } diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 1b4438cea..e8c9798bf 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -248,6 +248,24 @@ void GNSSFlowgraph::connect() } DLOG(INFO) << "Signal source connected to signal conditioner"; + //connect the signal source to sample counter + //connect the sample counter to Observables + try + { + double fs = static_cast(configuration_->property("GNSS-SDR.internal_fs_sps", 0)); + ch_out_sample_counter = gnss_sdr_make_sample_counter(fs); + top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter, 0); + top_block_->connect(ch_out_sample_counter, 0, observables_->get_left_block(), channels_count_); //extra port for the sample counter pulse + } + catch (const std::exception & e) + { + LOG(WARNING) << "Can't connect sample counter"; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; + } + + // Signal conditioner (selected_signal_source) >> channels (i) (dependent of their associated SignalSource_ID) int selected_signal_conditioner_ID; for (unsigned int i = 0; i < channels_count_; i++) @@ -296,13 +314,6 @@ void GNSSFlowgraph::connect() { LOG(INFO) << "Channel " << i << " connected to observables in standby mode"; } - //connect the sample counter to the channel 0 - if (i == 0) - { - ch_out_sample_counter = gnss_sdr_make_sample_counter(); - top_block_->connect(channels_.at(i)->get_right_block(), 0, ch_out_sample_counter, 0); - - } } /* diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index a2976fe46..3944e3bc8 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -74,7 +74,7 @@ const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this // OBSERVABLE HISTORY DEEP FOR INTERPOLATION -const int GPS_L1_CA_HISTORY_DEEP = 500; +const int GPS_L1_CA_HISTORY_DEEP = 100; // NAVIGATION MESSAGE DEMODULATION AND DECODING From 28cc4a1a050872d0e799130a66baf35a5d56ed47 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 14 Feb 2018 11:56:22 +0100 Subject: [PATCH 02/63] Add try-catch block to hybrid observables --- .../libs/gnss_sdr_sample_counter.cc | 11 +-- src/algorithms/libs/gnss_sdr_sample_counter.h | 15 ++-- .../adapters/hybrid_observables.cc | 19 +---- .../gnuradio_blocks/hybrid_observables_cc.cc | 80 +++++++++++-------- 4 files changed, 66 insertions(+), 59 deletions(-) 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++) From ab6e62af72f5ef239cad01d5a31ef30bf39c99fe Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 14 Feb 2018 16:24:29 +0100 Subject: [PATCH 03/63] Return WORK_DONE when throwing an exception --- .../gnuradio_blocks/hybrid_observables_cc.cc | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index ce11b6a1d..8fbf02e21 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -83,6 +83,7 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned catch (const std::ifstream::failure & e) { LOG(WARNING) << "Exception opening observables dump file " << e.what(); + d_dump = false; } } } @@ -105,7 +106,7 @@ hybrid_observables_cc::~hybrid_observables_cc() if(d_dump == true) { std::cout << "Writing observables .mat files ..."; - hybrid_observables_cc::save_matfile(); + save_matfile(); std::cout << " done." << std::endl; } } @@ -509,12 +510,7 @@ int hybrid_observables_cc::general_work(int noutput_items, gr_vector_int &ninput // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) // TOW at the selected receiver time T_rx_s int element_key = gnss_synchro_map_iter->second.Channel_ID; - try{ - adj_obs = adjacent_gnss_synchro_map.at(element_key); - }catch(const std::exception & ex) - { - continue; - } + adj_obs = adjacent_gnss_synchro_map.at(element_key); double adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / channel_fs_hz + adj_obs.Code_phase_samples / channel_fs_hz; @@ -566,6 +562,7 @@ int hybrid_observables_cc::general_work(int noutput_items, gr_vector_int &ninput catch (const std::ifstream::failure& e) { LOG(WARNING) << "Exception writing observables dump file " << e.what(); + d_dump = false; } } @@ -593,15 +590,17 @@ int hybrid_observables_cc::general_work(int noutput_items, gr_vector_int &ninput } }// End of try{...} - catch(std::out_of_range& e) + catch(const 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; + return gr::block::WORK_DONE; } - catch(...) + catch(const std::exception& e) { - LOG(WARNING) << "Undefined exception thrown by Hybrid Observables block."; - std::cout << "Undefined exception thrown by Hybrid Observables block." << std::endl; + LOG(WARNING) << "Exception thrown by Hybrid Observables block. Exception message: " << e.what(); + std::cout << "Exception thrown by Hybrid Observables block. Exception message: " << e.what() << std::endl; + return gr::block::WORK_DONE; } }while(channel_history_ok == true && noutput_items > n_outputs); From 756fd1904eec11beedb2c8e5ce0d2b43a0d86c95 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 16 Feb 2018 18:10:48 +0100 Subject: [PATCH 04/63] Modify observables algorithm --- .../libs/gnss_sdr_sample_counter.cc | 6 +- .../adapters/hybrid_observables.cc | 13 +- .../gnuradio_blocks/hybrid_observables_cc.cc | 806 +++++++++++------- .../gnuradio_blocks/hybrid_observables_cc.h | 16 +- .../galileo_e1b_telemetry_decoder_cc.cc | 2 + .../galileo_e5a_telemetry_decoder_cc.cc | 2 + .../gps_l1_ca_telemetry_decoder_cc.cc | 2 + .../gps_l2c_telemetry_decoder_cc.cc | 2 + .../gps_l5_telemetry_decoder_cc.cc | 2 + .../galileo_e1_dll_pll_veml_tracking_cc.cc | 1 + .../galileo_e5a_dll_pll_tracking_cc.cc | 1 + .../gps_l1_ca_dll_pll_tracking_cc.cc | 1 + .../gps_l2_m_dll_pll_tracking_cc.cc | 1 + .../gps_l5i_dll_pll_tracking_cc.cc | 1 + 14 files changed, 522 insertions(+), 334 deletions(-) diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc index e568da7d6..4047fad1a 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.cc +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -38,7 +38,9 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs) : gr::sync_decimato gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), static_cast(floor(_fs * 0.001))) { - this->message_port_register_out(pmt::mp("sample_counter")); + 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 @@ -63,7 +65,7 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), std::cout << "Current receiver time: " << static_cast(current_T_rx_ms) / 1000.0 << " [s]" << std::endl; if(flag_enable_send_msg) { - this->message_port_pub(pmt::mp("receiver_time"), pmt::from_double(static_cast(current_T_rx_ms) / 1000.0)); + message_port_pub(pmt::mp("receiver_time"), pmt::from_double(static_cast(current_T_rx_ms) / 1000.0)); } } current_T_rx_ms++; diff --git a/src/algorithms/observables/adapters/hybrid_observables.cc b/src/algorithms/observables/adapters/hybrid_observables.cc index 15f7bfc2a..d93b82598 100644 --- a/src/algorithms/observables/adapters/hybrid_observables.cc +++ b/src/algorithms/observables/adapters/hybrid_observables.cc @@ -46,17 +46,8 @@ HybridObservables::HybridObservables(ConfigurationInterface* configuration, DLOG(INFO) << "role " << role; dump_ = configuration->property(role + ".dump", false); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); - unsigned int default_depth = 0; - if (GPS_L1_CA_HISTORY_DEEP == GALILEO_E1_HISTORY_DEEP) - { - default_depth = GPS_L1_CA_HISTORY_DEEP; - } - else - { - default_depth = 100; - } - 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); + + observables_ = hybrid_make_observables_cc(in_streams_, out_streams_, dump_, dump_filename_); DLOG(INFO) << "Observables block ID (" << observables_->unique_id() << ")"; } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 8fbf02e21..eadbc64be 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -32,10 +32,7 @@ #include #include #include -#include -#include -#include -#include +#include #include #include #include @@ -47,63 +44,66 @@ using google::LogMessage; -hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename, unsigned int deep_history) +hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename) { - return hybrid_observables_cc_sptr(new hybrid_observables_cc(nchannels_in, nchannels_out, dump, dump_filename, deep_history)); + return hybrid_observables_cc_sptr(new hybrid_observables_cc(nchannels_in, nchannels_out, dump, dump_filename)); } -hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename, unsigned int deep_history) : - gr::block("hybrid_observables_cc", gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)), - gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) +hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename) : + gr::block("hybrid_observables_cc", + gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)), + gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) { - // initialize internal vars + set_max_noutput_items(1); + set_max_output_buffer(1); d_dump = dump; d_nchannels = nchannels_out; d_dump_filename = dump_filename; - history_deep = deep_history; T_rx_s = 0.0; - T_rx_step_s = 1e-3; // todo: move to gnss-sdr config + T_rx_step_s = 0.001; // 1 ms + max_extrapol_time_s = 0.1; // 100 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_queue.push_back(std::deque()); - } + { + d_gnss_synchro_history.push_back(std::pair()); + d_gnss_synchro_history.at(i).first.Flag_valid_word = false; + d_gnss_synchro_history.at(i).second.Flag_valid_word = false; + } // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) + if (d_dump) + { + if (!d_dump_file.is_open()) { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure & e) - { - LOG(WARNING) << "Exception opening observables dump file " << e.what(); - d_dump = false; - } - } + try + { + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure & e) + { + LOG(WARNING) << "Exception opening observables dump file " << e.what(); + d_dump = false; + } } + } } hybrid_observables_cc::~hybrid_observables_cc() { - if (d_dump_file.is_open() == true) + if (d_dump_file.is_open()) { - try - { - d_dump_file.close(); - } + try { d_dump_file.close(); } catch(const std::exception & ex) { LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } - if(d_dump == true) + if(d_dump) { std::cout << "Writing observables .mat files ..."; save_matfile(); @@ -120,14 +120,11 @@ int hybrid_observables_cc::save_matfile() int epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels; std::ifstream dump_file; dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); - try - { - dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); - } + try { dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); } catch(const std::ifstream::failure &e) { - std::cerr << "Problem opening dump file:" << e.what() << std::endl; - return 1; + std::cerr << "Problem opening dump file:" << e.what() << std::endl; + return 1; } // count number of epochs and rewind long int num_epoch = 0; @@ -137,10 +134,7 @@ int hybrid_observables_cc::save_matfile() num_epoch = static_cast(size) / static_cast(epoch_size_bytes); dump_file.seekg(0, std::ios::beg); } - else - { - return 1; - } + else { return 1; } double ** RX_time = new double * [d_nchannels]; double ** TOW_at_current_symbol_s = new double * [d_nchannels]; double ** Carrier_Doppler_hz = new double * [d_nchannels]; @@ -296,323 +290,505 @@ int hybrid_observables_cc::save_matfile() return 0; } - -bool Hybrid_pairCompare_gnss_synchro_sample_counter(const std::pair& a, const std::pair& b) +double Hybrid_Interpolate_data(const std::pair& a, const double& ti, int parameter) { - return (a.second.Tracking_sample_counter) < (b.second.Tracking_sample_counter); + // x(ti) = m * ti + c + // m = [x(t2) - x(t1)] / [t2 - t1] + // c = x(t1) - m * t1 + + double m = 0.0; + double c = 0.0; + + if(!a.first.Flag_valid_word or !a.second.Flag_valid_word) { return 0.0; } + + switch(parameter) + { + case 0:// Doppler + m = (a.first.Carrier_Doppler_hz - a.second.Carrier_Doppler_hz) / (a.first.RX_time - a.second.RX_time); + c = a.second.Carrier_Doppler_hz - m * a.second.RX_time; + break; + + case 1:// Carrier phase + m = (a.first.Carrier_phase_rads - a.second.Carrier_phase_rads) / (a.first.RX_time - a.second.RX_time); + c = a.second.Carrier_phase_rads - m * a.second.RX_time; + break; + + case 2:// TOW + m = (a.first.TOW_at_current_symbol_s - a.second.TOW_at_current_symbol_s) / (a.first.RX_time - a.second.RX_time); + c = a.second.TOW_at_current_symbol_s - m * a.second.RX_time; + break; + + case 3:// Code phase samples + m = (a.first.Code_phase_samples - a.second.Code_phase_samples) / (a.first.RX_time - a.second.RX_time); + c = a.second.Code_phase_samples - m * a.second.RX_time; + break; + } + return(m * ti + c); +} + +double Hybrid_Compute_T_rx_s(const Gnss_Synchro& a) +{ + if(a.Flag_valid_word) + { + return((static_cast(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast(a.fs)); + } + else { return 0.0; } +} + +/* +bool Hybrid_pairCompare_gnss_synchro_T_rx(const std::pair& a, const std::pair& b) +{ + if(a.second.Flag_valid_word and !b.second.Flag_valid_word) { return true; } + else if(!a.second.Flag_valid_word and b.second.Flag_valid_word) { return false; } + else if(!a.second.Flag_valid_word and !b.second.Flag_valid_word) {return false; } + else + { + return(Hybrid_Compute_T_rx_s(a.second) < Hybrid_Compute_T_rx_s(b.second)); + } +} + + +bool Hybrid_pairCompare_gnss_synchro_sample_counter(const std::pair& a, const std::pair& b) +{ + if(a.second.Flag_valid_word and !b.second.Flag_valid_word) { return true; } + else if(!a.second.Flag_valid_word and b.second.Flag_valid_word) { return false; } + else if(!a.second.Flag_valid_word and !b.second.Flag_valid_word) {return false; } + else + { + return(a.second.Tracking_sample_counter < b.second.Tracking_sample_counter); + } } bool Hybrid_valueCompare_gnss_synchro_sample_counter(const Gnss_Synchro& a, unsigned long int b) { - return (a.Tracking_sample_counter) < (b); + return(a.Tracking_sample_counter < b); } bool Hybrid_valueCompare_gnss_synchro_receiver_time(const Gnss_Synchro& a, double b) { - return ((static_cast(a.Tracking_sample_counter) + static_cast(a.Code_phase_samples)) / static_cast(a.fs) ) < (b); + return((static_cast(a.Tracking_sample_counter) + static_cast(a.Code_phase_samples)) / static_cast(a.fs) ) < (b); } -bool Hybrid_pairCompare_gnss_synchro_d_TOW(const std::pair& a, const std::pair& b) +bool Hybrid_pairCompare_gnss_synchro_TOW(const std::pair& a, const std::pair& b) { - return (a.second.TOW_at_current_symbol_s) < (b.second.TOW_at_current_symbol_s); + if(a.first.Flag_valid_word and !b.first.Flag_valid_word) { return true; } + else if(!a.first.Flag_valid_word and b.first.Flag_valid_word) { return false; } + else if(!a.first.Flag_valid_word and !b.first.Flag_valid_word) {return false; } + else + { + return(a.first.TOW_at_current_symbol_s < b.second.TOW_at_current_symbol_s); + } } bool Hybrid_valueCompare_gnss_synchro_d_TOW(const Gnss_Synchro& a, double b) { - return (a.TOW_at_current_symbol_s) < (b); + return(a.TOW_at_current_symbol_s < 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) { + bool available_items = false; for(unsigned int i = 0; i < d_nchannels; i++) { ninput_items_required[i] = 0; - //std::cout << "IN buffer "<< i << ". Number of items " << detail()->input(i)->items_available() << std::endl; + if(detail()->input(i)->items_available() > 0) { available_items = true; } } - //std::cout << "SC buffer. Number of items " << detail()->input(d_nchannels)->items_available() << std::endl; - ninput_items_required[d_nchannels] = 1; // set the required available samples in each call + if(available_items) { ninput_items_required[d_nchannels] = 0; } + else { ninput_items_required[d_nchannels] = 1; } } -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 __attribute__((unused)), + 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 - int n_outputs = 0; - int n_consume[d_nchannels]; - double past_history_s = 100e-3; + const Gnss_Synchro** in = reinterpret_cast(&input_items[0]); + Gnss_Synchro** out = reinterpret_cast(&output_items[0]); - Gnss_Synchro current_gnss_synchro[d_nchannels]; - Gnss_Synchro aux = Gnss_Synchro(); - for(unsigned int i = 0; i < d_nchannels; i++) + 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; + + ////////////////////////////////////////////////////////////////////////// + if((total_input_items == 0) and (ninput_items[d_nchannels] == 0)) { - current_gnss_synchro[i] = aux; + return 0; } - /* - * 1. Read the GNSS SYNCHRO objects from available channels. - * Multi-rate GNURADIO Block. Read how many input items are avaliable in each channel - * Record all synchronization data into queues - */ - for (unsigned int i = 0; i < d_nchannels; i++) + else if((total_input_items == 0) and (ninput_items[d_nchannels] > 0) and (d_num_valid_channels == 0)) { - n_consume[i] = ninput_items[i]; // full throttle - for(int j = 0; j < n_consume[i]; j++) + 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 + {} + ////////////////////////////////////////////////////////////////////////// + + + std::vector>::iterator it; + if (total_input_items > 0) + { + i = 0; + for (it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) { - d_gnss_synchro_history_queue[i].push_back(in[i][j]); + if (ninput_items[i] > 0 and (Hybrid_Compute_T_rx_s(in[i][0]) < T_rx_s)) + { + it->second = it->first; // second is the older Gnss_Synchro + it->first = in[i][0]; // first is the newest Gnss_Synchro + it->first.RX_time = Hybrid_Compute_T_rx_s(it->first); + consume(i, 1); + } + if (it->first.Flag_valid_word and it->second.Flag_valid_word) { valid_channels[i] = true; } + else { valid_channels[i] = false; } + i++; } } + 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; } - bool channel_history_ok; - - do + for(i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the extrapolation threshold { - - try - { - - channel_history_ok = true; - for(unsigned int i = 0; i < d_nchannels; i++) + if(valid_channels[i]) { - if (d_gnss_synchro_history_queue.at(i).size() < history_deep && !d_gnss_synchro_history_queue.at(i).empty()) - { - channel_history_ok = false; - } + 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; } } - if (channel_history_ok == true) + } + 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; } + + it = d_gnss_synchro_history.begin(); + double TOW_ref = std::numeric_limits::max(); + for(i = 0; i < d_nchannels; i++) + { + if(!valid_channels[i]) { out[i][0] = Gnss_Synchro(); } + else { - std::map::const_iterator gnss_synchro_map_iter; - std::deque::const_iterator gnss_synchro_deque_iter; - - // 1. If the RX time is not set, set the Rx time - if (T_rx_s == 0) - { - // 0. Read a gnss_synchro snapshot from the queue and store it in a map - std::map gnss_synchro_map; - for (unsigned int i = 0; i < d_nchannels; i++) - { - if (!d_gnss_synchro_history_queue.at(i).empty()) - { - 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; } // Breaks the do-while loop - - 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); - T_rx_s = floor(T_rx_s * 1000.0) / 1000.0; // truncate to ms - T_rx_s += past_history_s; // increase T_rx to have a minimum past history to interpolate - } - - // 2. Realign RX time in all valid channels - std::map realigned_gnss_synchro_map; // container for the aligned set of observables for the selected T_rx - std::map adjacent_gnss_synchro_map; // container for the previous observable values to interpolate - // shift channels history to match the reference TOW - for (unsigned int i = 0; i < d_nchannels; i++) - { - if (!d_gnss_synchro_history_queue.at(i).empty()) - { - 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.at(i).cend()) - { - if (gnss_synchro_deque_iter->Flag_valid_word == true) - { - double T_rx_channel = static_cast(gnss_synchro_deque_iter->Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); - double delta_T_rx_s = T_rx_channel - T_rx_s; - - // check that T_rx difference is less than a threshold (the correlation interval) - if (delta_T_rx_s * 1000.0 < static_cast(gnss_synchro_deque_iter->correlation_length_ms)) - { - // record the word structure in a map for pseudorange computation - // save the previous observable - 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.at(i).at(distance - 1).Flag_valid_word) - { - 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.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.at(i).at(distance - 1).Channel_ID, - d_gnss_synchro_history_queue.at(i).at(distance - 1))); - } - } - - } - else - { - realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - } - - } - } - } - } - } - - if(!realigned_gnss_synchro_map.empty()) - { - /* - * 2.1 Use CURRENT set of measurements and find the nearest satellite - * 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 = 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); - - // compute interpolated TOW value at T_rx_s - int ref_channel_key = gnss_synchro_map_iter->second.Channel_ID; - Gnss_Synchro adj_obs; - adj_obs = adjacent_gnss_synchro_map.at(ref_channel_key); - double ref_adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / ref_fs_hz + adj_obs.Code_phase_samples / ref_fs_hz; - - double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; - double d_ref_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / ref_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / ref_fs_hz; - - double selected_T_rx_s = T_rx_s; - // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) - double ref_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + - (selected_T_rx_s - ref_adj_T_rx_s) * (d_TOW_reference - adj_obs.TOW_at_current_symbol_s) / (d_ref_T_rx_s - ref_adj_T_rx_s); - - // Now compute RX time differences due to the PRN alignment in the correlators - double traveltime_ms; - double pseudorange_m; - double channel_T_rx_s; - double channel_fs_hz; - double channel_TOW_s; - for(gnss_synchro_map_iter = realigned_gnss_synchro_map.cbegin(); gnss_synchro_map_iter != realigned_gnss_synchro_map.cend(); gnss_synchro_map_iter++) - { - channel_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); - channel_TOW_s = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; - channel_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / channel_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / channel_fs_hz; - // compute interpolated observation values - // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) - // TOW at the selected receiver time T_rx_s - int element_key = gnss_synchro_map_iter->second.Channel_ID; - adj_obs = adjacent_gnss_synchro_map.at(element_key); - - double adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / channel_fs_hz + adj_obs.Code_phase_samples / channel_fs_hz; - - double channel_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + (selected_T_rx_s - adj_T_rx_s) * (channel_TOW_s - adj_obs.TOW_at_current_symbol_s) / (channel_T_rx_s - adj_T_rx_s); - - // Doppler and Accumulated carrier phase - double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_phase_rads - adj_obs.Carrier_phase_rads) / (channel_T_rx_s - adj_T_rx_s); - double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_Doppler_hz - adj_obs.Carrier_Doppler_hz) / (channel_T_rx_s - adj_T_rx_s); - - // compute the pseudorange (no rx time offset correction) - traveltime_ms = (ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s) * 1000.0 + GPS_STARTOFFSET_ms; - // convert to meters - pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] - // update the pseudorange object - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID] = gnss_synchro_map_iter->second; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Flag_valid_pseudorange = true; - // Save the estimated RX time (no RX clock offset correction yet!) - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = ref_TOW_at_T_rx_s + GPS_STARTOFFSET_ms / 1000.0; - - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_phase_rads = Carrier_phase_lin_rads; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_Doppler_hz = Carrier_Doppler_lin_hz; - } - - if(d_dump == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - for (unsigned int i = 0; i < d_nchannels; i++) - { - tmp_double = current_gnss_synchro[i].RX_time; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_s; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_phase_rads / GPS_TWO_PI; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Pseudorange_m; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].PRN; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(current_gnss_synchro[i].Flag_valid_pseudorange); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - d_dump = false; - } - } - - for (unsigned int i = 0; i < d_nchannels; i++) - { - out[i][n_outputs] = current_gnss_synchro[i]; - } - - n_outputs++; - } - - // Move RX time - 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.at(i).empty()) - { - 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.at(i).pop_front(); - } - } - } + out[i][0] = it->first; + out[i][0].Flag_valid_pseudorange = true; + out[i][0].Carrier_Doppler_hz = Hybrid_Interpolate_data(*it, T_rx_s, 0); + out[i][0].Carrier_phase_rads = Hybrid_Interpolate_data(*it, T_rx_s, 1); + out[i][0].RX_time = Hybrid_Interpolate_data(*it, T_rx_s, 2); + out[i][0].Code_phase_samples = Hybrid_Interpolate_data(*it, T_rx_s, 3); + //std::cout<<"T2: "<< it->first.RX_time<<". T1: "<< it->second.RX_time <<" T i: " << T_rx_s <first.Carrier_Doppler_hz<<","<< it->second.Carrier_Doppler_hz<<" Doppler interp: " << out[i][0].Carrier_Doppler_hz <(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.at(i).pop_front(); +// } +// } +// } +// } +// +// }// End of try{...} +// catch(const 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; +// return gr::block::WORK_DONE; +// } +// catch(const std::exception& e) +// { +// LOG(WARNING) << "Exception thrown by Hybrid Observables block. Exception message: " << e.what(); +// std::cout << "Exception thrown by Hybrid Observables block. Exception message: " << e.what() << std::endl; +// return gr::block::WORK_DONE; +// } +// +// }while(channel_history_ok == true && noutput_items > n_outputs); +// +// // Multi-rate consume! +// for (unsigned int i = 0; i < d_nchannels; i++) +// { +// consume(i, n_consume[i]); // which input, how many items +// } +// +// //consume monitor channel always +// consume(d_nchannels, 1); +// return n_outputs; +// +// } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index f1a1c3da2..139dd8644 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -35,6 +35,9 @@ #include #include +#include //std::pair +#include //std::vector +#include #include #include "gnss_synchro.h" @@ -44,7 +47,7 @@ class hybrid_observables_cc; typedef boost::shared_ptr hybrid_observables_cc_sptr; hybrid_observables_cc_sptr -hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename, unsigned int deep_history); +hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename); /*! * \brief This class implements a block that computes Galileo observables @@ -58,17 +61,18 @@ public: void forecast (int noutput_items, gr_vector_int &ninput_items_required); private: friend hybrid_observables_cc_sptr - hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename, unsigned int deep_history); - hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename, unsigned int deep_history); + 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); //Tracking observable history - std::vector> d_gnss_synchro_history_queue; - + 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; bool d_dump; unsigned int d_nchannels; - unsigned int history_deep; + unsigned int d_num_valid_channels; std::string d_dump_filename; std::ofstream d_dump_file; 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 97c4a9927..5504f84cf 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 @@ -107,6 +107,8 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc( gr::block("galileo_e1b_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), 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 4f76a6d99..5e94348e6 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 @@ -183,6 +183,8 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), 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 c47d448e7..50adeed2d 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 @@ -55,6 +55,8 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( gr::block("gps_navigation_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), 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 7b1334836..1e000a009 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 @@ -52,6 +52,8 @@ gps_l2c_telemetry_decoder_cc::gps_l2c_telemetry_decoder_cc( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), 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 2d604b221..cd44ffad5 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 @@ -54,6 +54,8 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), 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/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index 9ed1259dc..bb59c62bc 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -117,6 +117,7 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( gr::block("galileo_e1_dll_pll_veml_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + set_max_noutput_items(1); // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); this->set_relative_rate(1.0 / vector_length); diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc index 139097148..49db923e4 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc @@ -97,6 +97,7 @@ Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc( gr::block("Galileo_E5a_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + set_max_noutput_items(1); // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); this->message_port_register_out(pmt::mp("events")); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index 3bc3d0f9f..fe7201bc9 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -93,6 +93,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( gr::block("Gps_L1_Ca_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + set_max_noutput_items(1); // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); this->message_port_register_out(pmt::mp("events")); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc index 2a28283f4..25f8191f1 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc @@ -90,6 +90,7 @@ gps_l2_m_dll_pll_tracking_cc::gps_l2_m_dll_pll_tracking_cc( gr::block("gps_l2_m_dll_pll_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + set_max_noutput_items(1); // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); this->message_port_register_out(pmt::mp("events")); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc index c7205f6bf..cae86d1b7 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc @@ -90,6 +90,7 @@ gps_l5i_dll_pll_tracking_cc::gps_l5i_dll_pll_tracking_cc( gr::block("gps_l5i_dll_pll_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { + set_max_noutput_items(1); // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); this->message_port_register_out(pmt::mp("events")); From c3657f32515443a5c7920a54ce0f9d3fc52992c9 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 19 Feb 2018 10:29:12 +0100 Subject: [PATCH 05/63] Modify hybrid observables --- .../gnuradio_blocks/hybrid_observables_cc.cc | 38 +++++++++++-------- .../gnuradio_blocks/hybrid_observables_cc.h | 5 ++- 2 files changed, 25 insertions(+), 18 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index eadbc64be..eea804e17 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -55,8 +55,8 @@ 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; d_nchannels = nchannels_out; d_dump_filename = dump_filename; @@ -65,12 +65,7 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned max_extrapol_time_s = 0.1; // 100 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::pair()); - d_gnss_synchro_history.at(i).first.Flag_valid_word = false; - d_gnss_synchro_history.at(i).second.Flag_valid_word = false; - } + // ############# ENABLE DATA FILE LOG ################# if (d_dump) @@ -335,6 +330,11 @@ 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) { @@ -446,20 +446,26 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) ////////////////////////////////////////////////////////////////////////// - std::vector>::iterator it; + std::vector>::iterator it; if (total_input_items > 0) { i = 0; for (it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) { - if (ninput_items[i] > 0 and (Hybrid_Compute_T_rx_s(in[i][0]) < T_rx_s)) + if (ninput_items[i] > 0) { - it->second = it->first; // second is the older Gnss_Synchro - it->first = in[i][0]; // first is the newest Gnss_Synchro - it->first.RX_time = Hybrid_Compute_T_rx_s(it->first); - consume(i, 1); + 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]); + } + consume(ninput_items[i], 1); } - if (it->first.Flag_valid_word and it->second.Flag_valid_word) { valid_channels[i] = true; } + 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++; } @@ -506,7 +512,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) { if(valid_channels[i]) { - double traveltime_ms = (out[i][0].RX_time - TOW_ref)*1000.0 + GPS_STARTOFFSET_ms; + double traveltime_ms = (out[i][0].RX_time - TOW_ref) * 1000.0 + GPS_STARTOFFSET_ms; out[i][0].Pseudorange_m = traveltime_ms * GPS_C_m_ms; out[i][0].RX_time = TOW_ref + GPS_STARTOFFSET_ms / 1000.0; //std::cout << "Sat " << out[i][0].PRN << ". Prang = " << out[i][0].Pseudorange_m << ". TOW = " << out[i][0].RX_time << std::endl; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index 139dd8644..4d1119a35 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -35,8 +35,9 @@ #include #include -#include //std::pair +//#include //std::pair #include //std::vector +#include #include #include #include "gnss_synchro.h" @@ -65,7 +66,7 @@ private: hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename); //Tracking observable history - std::vector> d_gnss_synchro_history; + std::vector> d_gnss_synchro_history; boost::dynamic_bitset<> valid_channels; double T_rx_s; double T_rx_step_s; From 82084dd8672371332479711a9a14f4dda35dc4d0 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 19 Feb 2018 17:20:34 +0100 Subject: [PATCH 06/63] Minor changes --- .../libs/gnss_sdr_sample_counter.cc | 1 - .../gnuradio_blocks/hybrid_observables_cc.cc | 232 +++++++++++++----- .../gnuradio_blocks/hybrid_observables_cc.h | 11 +- .../galileo_e1b_telemetry_decoder_cc.cc | 1 - .../galileo_e5a_telemetry_decoder_cc.cc | 1 - .../gps_l1_ca_telemetry_decoder_cc.cc | 1 - .../gps_l2c_telemetry_decoder_cc.cc | 1 - .../gps_l5_telemetry_decoder_cc.cc | 1 - 8 files changed, 184 insertions(+), 65 deletions(-) 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 From 350821830706d210eb490aef863f1879f86bcf01 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 20 Feb 2018 10:58:56 +0100 Subject: [PATCH 07/63] Check PRN coherency in observables --- .../gnuradio_blocks/hybrid_observables_cc.cc | 89 +++---------------- 1 file changed, 11 insertions(+), 78 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index f69eeee61..f8a8ae768 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -56,7 +56,6 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) { set_max_noutput_items(1); - set_max_output_buffer(1); d_dump = dump; set_T_rx_s = false; d_nchannels = nchannels_out; @@ -335,76 +334,11 @@ double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro& a) else { return 0.0; } } -/* -bool Hybrid_pairCompare_gnss_synchro_T_rx(const std::pair& a, const std::pair& b) -{ - if(a.second.Flag_valid_word and !b.second.Flag_valid_word) { return true; } - else if(!a.second.Flag_valid_word and b.second.Flag_valid_word) { return false; } - else if(!a.second.Flag_valid_word and !b.second.Flag_valid_word) {return false; } - else - { - return(Hybrid_Compute_T_rx_s(a.second) < Hybrid_Compute_T_rx_s(b.second)); - } -} - - -bool Hybrid_pairCompare_gnss_synchro_sample_counter(const std::pair& a, const std::pair& b) -{ - if(a.second.Flag_valid_word and !b.second.Flag_valid_word) { return true; } - else if(!a.second.Flag_valid_word and b.second.Flag_valid_word) { return false; } - else if(!a.second.Flag_valid_word and !b.second.Flag_valid_word) {return false; } - else - { - return(a.second.Tracking_sample_counter < b.second.Tracking_sample_counter); - } -} - - -bool Hybrid_valueCompare_gnss_synchro_sample_counter(const Gnss_Synchro& a, unsigned long int b) -{ - return(a.Tracking_sample_counter < b); -} - - -bool Hybrid_valueCompare_gnss_synchro_receiver_time(const Gnss_Synchro& a, double b) -{ - return((static_cast(a.Tracking_sample_counter) + static_cast(a.Code_phase_samples)) / static_cast(a.fs) ) < (b); -} - - -bool Hybrid_pairCompare_gnss_synchro_TOW(const std::pair& a, const std::pair& b) -{ - if(a.first.Flag_valid_word and !b.first.Flag_valid_word) { return true; } - else if(!a.first.Flag_valid_word and b.first.Flag_valid_word) { return false; } - else if(!a.first.Flag_valid_word and !b.first.Flag_valid_word) {return false; } - else - { - return(a.first.TOW_at_current_symbol_s < b.second.TOW_at_current_symbol_s); - } -} - - -bool Hybrid_valueCompare_gnss_synchro_d_TOW(const Gnss_Synchro& a, double b) -{ - return(a.TOW_at_current_symbol_s < b); -} -*/ void hybrid_observables_cc::forecast(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items_required) { -// 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; - } + for(unsigned int i = 0; i < d_nchannels; i++) { ninput_items_required[i] = 0; } ninput_items_required[d_nchannels] = 1; } @@ -499,12 +433,18 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) { if(ninput_items[i] > 0) { + // Add the new Gnss_Synchros to their corresponding deque for(int aux = 0; aux < ninput_items[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]); + // Check if the last Gnss_Synchro comes from the same satellite as the previous ones + if(it->size() > 1) + { + if(it->front().PRN != it->back().PRN) { it->clear(); } + } } } consume(i, ninput_items[i]); @@ -568,18 +508,11 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) index2 = index_closest + 1; } Gnss_Synchro interpolated_gnss_synchro = it->at(index1); + std::pair gnss_pair(it->at(index2), 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); + interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(gnss_pair, T_rx_s, 0); + interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(gnss_pair, T_rx_s, 1); + interpolated_gnss_synchro.RX_time = interpolate_data(gnss_pair, T_rx_s, 2); epoch_data.push_back(interpolated_gnss_synchro); } From 832f828d52754d8baf688c7a933986941720f0c8 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 20 Feb 2018 11:17:41 +0100 Subject: [PATCH 08/63] Clean code --- .../gnuradio_blocks/hybrid_observables_cc.cc | 350 ++---------------- 1 file changed, 34 insertions(+), 316 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index f8a8ae768..3984bb2f8 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -95,19 +95,19 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned hybrid_observables_cc::~hybrid_observables_cc() { if (d_dump_file.is_open()) + { + try { d_dump_file.close(); } + catch(const std::exception & ex) { - try { d_dump_file.close(); } - catch(const std::exception & ex) - { - LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); - } + LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } + } if(d_dump) - { - std::cout << "Writing observables .mat files ..."; - save_matfile(); - std::cout << " done." << std::endl; - } + { + std::cout << "Writing observables .mat files ..."; + save_matfile(); + std::cout << " done." << std::endl; + } } @@ -535,317 +535,35 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) 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++) + if(d_dump) { - if(!valid_channels[i]) { out[i][0] = Gnss_Synchro(); } - else + // MULTIPLEXED FILE RECORDING - Record results to file + try { - out[i][0] = it->first; - out[i][0].Flag_valid_pseudorange = true; - out[i][0].Carrier_Doppler_hz = Hybrid_Interpolate_data(*it, T_rx_s, 0); - out[i][0].Carrier_phase_rads = Hybrid_Interpolate_data(*it, T_rx_s, 1); - out[i][0].RX_time = Hybrid_Interpolate_data(*it, T_rx_s, 2); - out[i][0].Code_phase_samples = Hybrid_Interpolate_data(*it, T_rx_s, 3); - //std::cout<<"T2: "<< it->first.RX_time<<". T1: "<< it->second.RX_time <<" T i: " << T_rx_s <first.Carrier_Doppler_hz<<","<< it->second.Carrier_Doppler_hz<<" Doppler interp: " << out[i][0].Carrier_Doppler_hz <(&tmp_double), sizeof(double)); + tmp_double = out[i][0].TOW_at_current_symbol_s; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = out[i][0].Carrier_Doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = out[i][0].Carrier_phase_rads / GPS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = out[i][0].Pseudorange_m; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(out[i][0].PRN); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(out[i][0].Flag_valid_pseudorange); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } } - it++; - } - for(i = 0; i < d_nchannels; i++) - { - if(valid_channels[i]) + catch (const std::ifstream::failure& e) { - double traveltime_ms = (out[i][0].RX_time - TOW_ref) * 1000.0 + GPS_STARTOFFSET_ms; - out[i][0].Pseudorange_m = traveltime_ms * GPS_C_m_ms; - out[i][0].RX_time = TOW_ref + GPS_STARTOFFSET_ms / 1000.0; - //std::cout << "Sat " << out[i][0].PRN << ". Prang = " << out[i][0].Pseudorange_m << ". TOW = " << out[i][0].RX_time << std::endl; + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + d_dump = false; } } return 1; - - */ - - /******************************* OLD ALGORITHM ********************************/ - -// 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; -// -// Gnss_Synchro current_gnss_synchro[d_nchannels]; -// Gnss_Synchro aux = Gnss_Synchro(); -// for(unsigned int i = 0; i < d_nchannels; i++) -// { -// current_gnss_synchro[i] = aux; -// } -// /* -// * 1. Read the GNSS SYNCHRO objects from available channels. -// * Multi-rate GNURADIO Block. Read how many input items are avaliable in each channel -// * Record all synchronization data into queues -// */ -// 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++) -// { -// 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++) -// { -// if (d_gnss_synchro_history_queue.at(i).size() < history_deep && !d_gnss_synchro_history_queue.at(i).empty()) -// { -// channel_history_ok = false; -// } -// } -// if (channel_history_ok == true) -// { -// std::map::const_iterator gnss_synchro_map_iter; -// std::deque::const_iterator gnss_synchro_deque_iter; -// -// // 1. If the RX time is not set, set the Rx time -// if (T_rx_s == 0) -// { -// // 0. Read a gnss_synchro snapshot from the queue and store it in a map -// std::map gnss_synchro_map; -// for (unsigned int i = 0; i < d_nchannels; i++) -// { -// if (!d_gnss_synchro_history_queue.at(i).empty()) -// { -// 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; } // Breaks the do-while loop -// -// 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); -// T_rx_s = floor(T_rx_s * 1000.0) / 1000.0; // truncate to ms -// T_rx_s += past_history_s; // increase T_rx to have a minimum past history to interpolate -// } -// -// // 2. Realign RX time in all valid channels -// std::map realigned_gnss_synchro_map; // container for the aligned set of observables for the selected T_rx -// std::map adjacent_gnss_synchro_map; // container for the previous observable values to interpolate -// // shift channels history to match the reference TOW -// for (unsigned int i = 0; i < d_nchannels; i++) -// { -// if (!d_gnss_synchro_history_queue.at(i).empty()) -// { -// 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.at(i).cend()) -// { -// if (gnss_synchro_deque_iter->Flag_valid_word == true) -// { -// double T_rx_channel = static_cast(gnss_synchro_deque_iter->Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); -// double delta_T_rx_s = T_rx_channel - T_rx_s; -// -// // check that T_rx difference is less than a threshold (the correlation interval) -// if (delta_T_rx_s * 1000.0 < static_cast(gnss_synchro_deque_iter->correlation_length_ms)) -// { -// // record the word structure in a map for pseudorange computation -// // save the previous observable -// 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.at(i).at(distance - 1).Flag_valid_word) -// { -// 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.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.at(i).at(distance - 1).Channel_ID, -// d_gnss_synchro_history_queue.at(i).at(distance - 1))); -// } -// } -// -// } -// else -// { -// realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); -// } -// -// } -// } -// } -// } -// } -// -// if(!realigned_gnss_synchro_map.empty()) -// { -// /* -// * 2.1 Use CURRENT set of measurements and find the nearest satellite -// * 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 = 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); -// -// // compute interpolated TOW value at T_rx_s -// int ref_channel_key = gnss_synchro_map_iter->second.Channel_ID; -// Gnss_Synchro adj_obs; -// adj_obs = adjacent_gnss_synchro_map.at(ref_channel_key); -// double ref_adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / ref_fs_hz + adj_obs.Code_phase_samples / ref_fs_hz; -// -// double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; -// double d_ref_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / ref_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / ref_fs_hz; -// -// double selected_T_rx_s = T_rx_s; -// // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) -// double ref_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + -// (selected_T_rx_s - ref_adj_T_rx_s) * (d_TOW_reference - adj_obs.TOW_at_current_symbol_s) / (d_ref_T_rx_s - ref_adj_T_rx_s); -// -// // Now compute RX time differences due to the PRN alignment in the correlators -// double traveltime_ms; -// double pseudorange_m; -// double channel_T_rx_s; -// double channel_fs_hz; -// double channel_TOW_s; -// for(gnss_synchro_map_iter = realigned_gnss_synchro_map.cbegin(); gnss_synchro_map_iter != realigned_gnss_synchro_map.cend(); gnss_synchro_map_iter++) -// { -// channel_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); -// channel_TOW_s = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; -// channel_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / channel_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / channel_fs_hz; -// // compute interpolated observation values -// // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) -// // TOW at the selected receiver time T_rx_s -// int element_key = gnss_synchro_map_iter->second.Channel_ID; -// adj_obs = adjacent_gnss_synchro_map.at(element_key); -// -// double adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / channel_fs_hz + adj_obs.Code_phase_samples / channel_fs_hz; -// -// double channel_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + (selected_T_rx_s - adj_T_rx_s) * (channel_TOW_s - adj_obs.TOW_at_current_symbol_s) / (channel_T_rx_s - adj_T_rx_s); -// -// // Doppler and Accumulated carrier phase -// double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_phase_rads - adj_obs.Carrier_phase_rads) / (channel_T_rx_s - adj_T_rx_s); -// double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_Doppler_hz - adj_obs.Carrier_Doppler_hz) / (channel_T_rx_s - adj_T_rx_s); -// -// // compute the pseudorange (no rx time offset correction) -// traveltime_ms = (ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s) * 1000.0 + GPS_STARTOFFSET_ms; -// // convert to meters -// pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] -// // update the pseudorange object -// current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID] = gnss_synchro_map_iter->second; -// current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; -// current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Flag_valid_pseudorange = true; -// // Save the estimated RX time (no RX clock offset correction yet!) -// current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = ref_TOW_at_T_rx_s + GPS_STARTOFFSET_ms / 1000.0; -// -// current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_phase_rads = Carrier_phase_lin_rads; -// current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_Doppler_hz = Carrier_Doppler_lin_hz; -// } -// -// if(d_dump == true) -// { -// // MULTIPLEXED FILE RECORDING - Record results to file -// try -// { -// double tmp_double; -// for (unsigned int i = 0; i < d_nchannels; i++) -// { -// tmp_double = current_gnss_synchro[i].RX_time; -// d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); -// tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_s; -// d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); -// tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; -// d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); -// tmp_double = current_gnss_synchro[i].Carrier_phase_rads / GPS_TWO_PI; -// d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); -// tmp_double = current_gnss_synchro[i].Pseudorange_m; -// d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); -// tmp_double = current_gnss_synchro[i].PRN; -// d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); -// tmp_double = static_cast(current_gnss_synchro[i].Flag_valid_pseudorange); -// d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); -// } -// } -// catch (const std::ifstream::failure& e) -// { -// LOG(WARNING) << "Exception writing observables dump file " << e.what(); -// d_dump = false; -// } -// } -// -// for (unsigned int i = 0; i < d_nchannels; i++) -// { -// out[i][n_outputs] = current_gnss_synchro[i]; -// } -// -// n_outputs++; -// } -// -// // Move RX time -// 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.at(i).empty()) -// { -// 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.at(i).pop_front(); -// } -// } -// } -// } -// -// }// End of try{...} -// catch(const 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; -// return gr::block::WORK_DONE; -// } -// catch(const std::exception& e) -// { -// LOG(WARNING) << "Exception thrown by Hybrid Observables block. Exception message: " << e.what(); -// std::cout << "Exception thrown by Hybrid Observables block. Exception message: " << e.what() << std::endl; -// return gr::block::WORK_DONE; -// } -// -// }while(channel_history_ok == true && noutput_items > n_outputs); -// -// // Multi-rate consume! -// for (unsigned int i = 0; i < d_nchannels; i++) -// { -// consume(i, n_consume[i]); // which input, how many items -// } -// -// //consume monitor channel always -// consume(d_nchannels, 1); -// return n_outputs; -// -// } - From cc178495c99e2ff281f47fd54c88e44a948daab4 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 20 Feb 2018 11:28:34 +0100 Subject: [PATCH 09/63] Modify authors --- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 5 +++-- .../observables/gnuradio_blocks/hybrid_observables_cc.h | 7 ++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 3984bb2f8..7654a019b 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -1,11 +1,12 @@ /*! * \file hybrid_observables_cc.cc - * \brief Implementation of the pseudorange computation block for Galileo E1 + * \brief Implementation of the pseudorange computation block * \author Javier Arribas 2017. jarribas(at)cttc.es + * \author Antonio Ramos 2018. antonio.ramos(at)cttc.es * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index ada7b3d47..c3692b24b 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -1,12 +1,13 @@ /*! * \file hybrid_observables_cc.h - * \brief Interface of the observables computation block for Galileo E1 + * \brief Interface of the observables computation block * \author Mara Branzanti 2013. mara.branzanti(at)gmail.com * \author Javier Arribas 2013. jarribas(at)cttc.es + * \author Antonio Ramos 2018. antonio.ramos(at)cttc.es * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -51,7 +52,7 @@ hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename); /*! - * \brief This class implements a block that computes Galileo observables + * \brief This class implements a block that computes observables */ class hybrid_observables_cc : public gr::block { From dfb5f1118cc379eb2eb1f8edc19f86fb666f8bdf Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 20 Feb 2018 15:44:45 +0100 Subject: [PATCH 10/63] Fix unit tests --- src/core/receiver/gnss_flowgraph.cc | 6 ++++++ src/tests/unit-tests/control-plane/control_thread_test.cc | 3 +++ src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc | 7 ++++--- 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index e8c9798bf..a8be2a89f 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -253,6 +253,12 @@ void GNSSFlowgraph::connect() try { double fs = static_cast(configuration_->property("GNSS-SDR.internal_fs_sps", 0)); + if(fs == 0.0) + { + LOG(WARNING) << "Set GNSS-SDR.internal_fs_sps in configuration file"; + std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file" << std::endl; + throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration")); + } ch_out_sample_counter = gnss_sdr_make_sample_counter(fs); top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter, 0); top_block_->connect(ch_out_sample_counter, 0, observables_->get_left_block(), channels_count_); //extra port for the sample counter pulse diff --git a/src/tests/unit-tests/control-plane/control_thread_test.cc b/src/tests/unit-tests/control-plane/control_thread_test.cc index ac3f00bff..a8fdd0d9d 100644 --- a/src/tests/unit-tests/control-plane/control_thread_test.cc +++ b/src/tests/unit-tests/control-plane/control_thread_test.cc @@ -114,6 +114,7 @@ TEST_F(ControlThreadTest, InstantiateRunControlMessages) config->set_property("Observables.item_type", "gr_complex"); config->set_property("PVT.implementation", "RTKLIB_PVT"); config->set_property("PVT.item_type", "gr_complex"); + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); std::shared_ptr control_thread = std::make_shared(config); @@ -174,6 +175,7 @@ TEST_F(ControlThreadTest, InstantiateRunControlMessages2) config->set_property("Observables.item_type", "gr_complex"); config->set_property("PVT.implementation", "RTKLIB_PVT"); config->set_property("PVT.item_type", "gr_complex"); + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); std::unique_ptr control_thread2(new ControlThread(config)); @@ -238,6 +240,7 @@ TEST_F(ControlThreadTest, StopReceiverProgrammatically) config->set_property("Observables.item_type", "gr_complex"); config->set_property("PVT.implementation", "RTKLIB_PVT"); config->set_property("PVT.item_type", "gr_complex"); + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); std::shared_ptr control_thread = std::make_shared(config); gr::msg_queue::sptr control_queue = gr::msg_queue::make(0); diff --git a/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc b/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc index 8c31aaca2..63e184766 100644 --- a/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc +++ b/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc @@ -49,6 +49,7 @@ TEST(GNSSFlowgraph, InstantiateConnectStartStopOldNotation) std::shared_ptr config = std::make_shared(); config->set_property("GNSS-SDR.SUPL_gps_enabled", "false"); + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); config->set_property("SignalSource.sampling_frequency", "4000000"); config->set_property("SignalSource.implementation", "File_Signal_Source"); config->set_property("SignalSource.item_type", "gr_complex"); @@ -82,7 +83,7 @@ TEST(GNSSFlowgraph, InstantiateConnectStartStopOldNotation) TEST(GNSSFlowgraph, InstantiateConnectStartStop) { std::shared_ptr config = std::make_shared(); - + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); config->set_property("SignalSource.sampling_frequency", "4000000"); config->set_property("SignalSource.implementation", "File_Signal_Source"); config->set_property("SignalSource.item_type", "gr_complex"); @@ -116,7 +117,7 @@ TEST(GNSSFlowgraph, InstantiateConnectStartStop) TEST(GNSSFlowgraph, InstantiateConnectStartStopGalileoE1B) { std::shared_ptr config = std::make_shared(); - + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); config->set_property("SignalSource.sampling_frequency", "4000000"); config->set_property("SignalSource.implementation", "File_Signal_Source"); config->set_property("SignalSource.item_type", "gr_complex"); @@ -151,7 +152,7 @@ TEST(GNSSFlowgraph, InstantiateConnectStartStopGalileoE1B) TEST(GNSSFlowgraph, InstantiateConnectStartStopHybrid) { std::shared_ptr config = std::make_shared(); - + config->set_property("GNSS-SDR.internal_fs_sps", "4000000"); config->set_property("SignalSource.sampling_frequency", "4000000"); config->set_property("SignalSource.implementation", "File_Signal_Source"); config->set_property("SignalSource.item_type", "gr_complex"); From c793b47181c6b7fa43d37a505f94447c779148fe Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 21 Feb 2018 10:41:33 +0100 Subject: [PATCH 11/63] Interpolate TOW_at_current_symbol in observables block --- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 7654a019b..be350f859 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -514,6 +514,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(gnss_pair, T_rx_s, 0); interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(gnss_pair, T_rx_s, 1); interpolated_gnss_synchro.RX_time = interpolate_data(gnss_pair, T_rx_s, 2); + interpolated_gnss_synchro.TOW_at_current_symbol_s = interpolated_gnss_synchro.RX_time; epoch_data.push_back(interpolated_gnss_synchro); } From 281687615ad11c926d0dfbd7409530b05c221df6 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 21 Feb 2018 10:59:53 +0100 Subject: [PATCH 12/63] Set observables history to 100 ms --- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index be350f859..db02f4856 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -63,7 +63,7 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned d_dump_filename = dump_filename; T_rx_s = 0.0; T_rx_step_s = 0.001; // 1 ms - max_delta = 0.05; // 50 ms + max_delta = 0.1; // 100 ms valid_channels.resize(d_nchannels, false); d_num_valid_channels = 0; From 5da7a83eeb292ed934c0e7346af09dcd6580876e Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 21 Feb 2018 11:28:35 +0100 Subject: [PATCH 13/63] Correct interpolation indexes --- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index db02f4856..8deb9746b 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -498,15 +498,15 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) unsigned int index_closest = find_closest(*it); unsigned int index1; unsigned int index2; - if(index_closest == (it->size() - 1)) + if(index_closest > 0) { index1 = index_closest - 1; index2 = index_closest; } else { - index1 = index_closest; - index2 = index_closest + 1; + index1 = 0; + index2 = 1; } Gnss_Synchro interpolated_gnss_synchro = it->at(index1); std::pair gnss_pair(it->at(index2), it->at(index1)); From 98cde6583238f0c8c23ff8595c45d8c31e5024bd Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 21 Feb 2018 12:56:44 +0100 Subject: [PATCH 14/63] Improved find_closest algorithm in observables --- .../gnuradio_blocks/hybrid_observables_cc.cc | 44 ++++++++++--------- .../gnuradio_blocks/hybrid_observables_cc.h | 2 +- 2 files changed, 25 insertions(+), 21 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 8deb9746b..92a308356 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -352,22 +352,39 @@ void hybrid_observables_cc::clean_history(std::deque& data) } } -unsigned int hybrid_observables_cc::find_closest(std::deque& data) +std::pair hybrid_observables_cc::find_closest(std::deque& data) { - unsigned int result = 0; + std::pair result; + unsigned int index = 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)) + double instant_delta = std::fabs(T_rx_s - it->RX_time); + if(instant_delta < delta_t) { delta_t = instant_delta; - result = aux; + index = aux; } aux++; } + delta_t = T_rx_s - data.at(index).RX_time; + if((index == (data.size() - 1)) or (delta_t < 0.0)) + { + result.first = data.at(index); + result.second = data.at(index - 1); + } + else if(index == 0) + { + result.first = data.at(1); + result.second = data.at(0); + } + else + { + result.first = data.at(index + 1); + result.second = data.at(index); + } return result; } @@ -495,21 +512,8 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) { if(valid_channels[i]) { - unsigned int index_closest = find_closest(*it); - unsigned int index1; - unsigned int index2; - if(index_closest > 0) - { - index1 = index_closest - 1; - index2 = index_closest; - } - else - { - index1 = 0; - index2 = 1; - } - Gnss_Synchro interpolated_gnss_synchro = it->at(index1); - std::pair gnss_pair(it->at(index2), it->at(index1)); + std::pair gnss_pair = find_closest(*it); + Gnss_Synchro interpolated_gnss_synchro = gnss_pair.second; interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(gnss_pair, T_rx_s, 0); interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(gnss_pair, T_rx_s, 1); diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index c3692b24b..13a8e70cd 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -69,7 +69,7 @@ private: 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); + std::pair find_closest(std::deque& data); void correct_TOW_and_compute_prange(std::vector& data); //Tracking observable history From a2eaf403fffea1150d5a486929023a7d4752166b Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 21 Feb 2018 13:12:45 +0100 Subject: [PATCH 15/63] Avoid acces to a out of range value --- .../gnuradio_blocks/hybrid_observables_cc.cc | 33 ++++++++++++------- 1 file changed, 21 insertions(+), 12 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 92a308356..19b0e89ba 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -369,21 +369,30 @@ std::pair hybrid_observables_cc::find_closest(std::d } aux++; } - delta_t = T_rx_s - data.at(index).RX_time; - if((index == (data.size() - 1)) or (delta_t < 0.0)) + try { - result.first = data.at(index); - result.second = data.at(index - 1); + delta_t = T_rx_s - data.at(index).RX_time; + if(index == 0) + { + result.first = data.at(1); + result.second = data.at(0); + } + else if((index == (data.size() - 1)) or (delta_t < 0.0)) + { + result.first = data.at(index); + result.second = data.at(index - 1); + } + else + { + result.first = data.at(index + 1); + result.second = data.at(index); + } } - else if(index == 0) + catch(const std::exception& e) { - result.first = data.at(1); - result.second = data.at(0); - } - else - { - result.first = data.at(index + 1); - result.second = data.at(index); + result.first = Gnss_Synchro(); + result.second = Gnss_Synchro(); + LOG(WARNING) << e.what(); } return result; } From df5f51ba91b9b8825cff16d0d94f7dc33e79d84b Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 21 Feb 2018 16:10:03 +0100 Subject: [PATCH 16/63] Dump incoming Gnss Synchros in observables --- .../gnuradio_blocks/hybrid_observables_cc.cc | 60 ++++++++++++++++--- .../gnuradio_blocks/hybrid_observables_cc.h | 2 + 2 files changed, 53 insertions(+), 9 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 19b0e89ba..3653b16dc 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -61,6 +61,7 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned set_T_rx_s = false; d_nchannels = nchannels_out; d_dump_filename = dump_filename; + d_dump_filename_in = d_dump_filename; T_rx_s = 0.0; T_rx_step_s = 0.001; // 1 ms max_delta = 0.1; // 100 ms @@ -80,6 +81,7 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned try { d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); + d_dump_filename.append(".bin"); d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); } @@ -89,6 +91,21 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned d_dump = false; } } + if (!d_dump_in.is_open()) + { + try + { + d_dump_in.exceptions (std::ifstream::failbit | std::ifstream::badbit ); + d_dump_filename_in.append("_in.bin"); + d_dump_in.open(d_dump_filename_in.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure & e) + { + LOG(WARNING) << "Exception opening observables dump file " << e.what(); + d_dump = false; + } + } } } @@ -103,6 +120,14 @@ hybrid_observables_cc::~hybrid_observables_cc() LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } + if (d_dump_in.is_open()) + { + try { d_dump_in.close(); } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + } + } if(d_dump) { std::cout << "Writing observables .mat files ..."; @@ -225,7 +250,6 @@ int hybrid_observables_cc::save_matfile() mat_t *matfp; matvar_t *matvar; std::string filename = d_dump_filename; - filename.erase(filename.length() - 4, 4); filename.append(".mat"); matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); if(reinterpret_cast(matfp) != NULL) @@ -392,7 +416,7 @@ std::pair hybrid_observables_cc::find_closest(std::d { result.first = Gnss_Synchro(); result.second = Gnss_Synchro(); - LOG(WARNING) << e.what(); + LOG(WARNING) << "Exception computing observables " << e.what(); } return result; } @@ -420,11 +444,11 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector::iterator it; for(it = data.begin(); it != data.end(); it++) { - if(it->RX_time > TOW_ref) { TOW_ref = it->RX_time; } + if(it->TOW_at_current_symbol_s > TOW_ref) { TOW_ref = it->TOW_at_current_symbol_s; } } for(it = data.begin(); it != data.end(); it++) { - double traveltime_s = TOW_ref - it->RX_time + GPS_STARTOFFSET_ms / 1000.0; + 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; it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT; } @@ -465,12 +489,31 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) { if(in[i][aux].Flag_valid_word) { + bool __dump = false; it->push_back(in[i][aux]); it->back().RX_time = compute_T_rx_s(in[i][aux]); + __dump = true; // Check if the last Gnss_Synchro comes from the same satellite as the previous ones if(it->size() > 1) { - if(it->front().PRN != it->back().PRN) { it->clear(); } + if(it->front().PRN != it->back().PRN) { it->clear(); __dump = false; } + } + if(d_dump && __dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + int tmp_int = static_cast(it->back().PRN); + d_dump_in.write(reinterpret_cast(&tmp_int), sizeof(int)); + d_dump_in.write(reinterpret_cast(&it->back().RX_time), sizeof(double)); + d_dump_in.write(reinterpret_cast(&it->back().TOW_at_current_symbol_s), sizeof(double)); + d_dump_in.write(it->back().Signal, 3 * sizeof(char)); + } + catch (const std::ifstream::failure& e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + d_dump = false; + } } } } @@ -524,10 +567,9 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) std::pair gnss_pair = find_closest(*it); Gnss_Synchro interpolated_gnss_synchro = gnss_pair.second; - interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(gnss_pair, T_rx_s, 0); - interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(gnss_pair, T_rx_s, 1); - interpolated_gnss_synchro.RX_time = interpolate_data(gnss_pair, T_rx_s, 2); - interpolated_gnss_synchro.TOW_at_current_symbol_s = interpolated_gnss_synchro.RX_time; + interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(gnss_pair, T_rx_s, 0); + interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(gnss_pair, T_rx_s, 1); + interpolated_gnss_synchro.TOW_at_current_symbol_s = interpolate_data(gnss_pair, T_rx_s, 2); epoch_data.push_back(interpolated_gnss_synchro); } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index 13a8e70cd..d4b50a36b 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -83,7 +83,9 @@ private: unsigned int d_nchannels; unsigned int d_num_valid_channels; std::string d_dump_filename; + std::string d_dump_filename_in; std::ofstream d_dump_file; + std::ofstream d_dump_in; int save_matfile(); }; From ca9a551a862cc2f1090ebe6c0168773e0e329518 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 22 Feb 2018 11:14:57 +0100 Subject: [PATCH 17/63] Avoid extrapolation --- .../gnuradio_blocks/hybrid_observables_cc.cc | 99 +++++++------------ .../gnuradio_blocks/hybrid_observables_cc.h | 9 +- 2 files changed, 40 insertions(+), 68 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 3653b16dc..27568c0c6 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -58,7 +58,6 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned { set_max_noutput_items(1); d_dump = dump; - set_T_rx_s = false; d_nchannels = nchannels_out; d_dump_filename = dump_filename; d_dump_filename_in = d_dump_filename; @@ -376,7 +375,7 @@ void hybrid_observables_cc::clean_history(std::deque& data) } } -std::pair hybrid_observables_cc::find_closest(std::deque& data) +std::pair hybrid_observables_cc::find_closest(std::deque& data, const double& ti) { std::pair result; unsigned int index = 0; @@ -385,7 +384,7 @@ std::pair hybrid_observables_cc::find_closest(std::d unsigned int aux = 0; for(it = data.begin(); it != data.end(); it++) { - double instant_delta = std::fabs(T_rx_s - it->RX_time); + double instant_delta = std::fabs(ti - it->RX_time); if(instant_delta < delta_t) { delta_t = instant_delta; @@ -393,50 +392,31 @@ std::pair hybrid_observables_cc::find_closest(std::d } aux++; } - try + delta_t = ti - data.at(index).RX_time; + if( (index == 0) or (index == (data.size() - 1)) ) { - delta_t = T_rx_s - data.at(index).RX_time; - if(index == 0) - { - result.first = data.at(1); - result.second = data.at(0); - } - else if((index == (data.size() - 1)) or (delta_t < 0.0)) - { - result.first = data.at(index); - result.second = data.at(index - 1); - } - else - { - result.first = data.at(index + 1); - result.second = data.at(index); - } + Gnss_Synchro invalid_data; + invalid_data.Flag_valid_pseudorange = false; + result.first = invalid_data; + result.second = invalid_data; } - catch(const std::exception& e) + else if(delta_t < 0.0) { - result.first = Gnss_Synchro(); - result.second = Gnss_Synchro(); - LOG(WARNING) << "Exception computing observables " << e.what(); + result.first = data.at(index); + result.first.Flag_valid_pseudorange = true; + result.second = data.at(index - 1); + result.second.Flag_valid_pseudorange = true; + } + else + { + result.first = data.at(index + 1); + result.first.Flag_valid_pseudorange = true; + result.second = data.at(index); + result.second.Flag_valid_pseudorange = true; } 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) { @@ -466,16 +446,15 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) int total_input_items = 0; for(i = 0; i < d_nchannels; i++) { total_input_items += ninput_items[i]; } consume(d_nchannels, 1); + T_rx_s += T_rx_step_s; ////////////////////////////////////////////////////////////////////////// if((total_input_items == 0) and (d_num_valid_channels == 0)) { return 0; } - if(set_T_rx_s) { T_rx_s += T_rx_step_s; } ////////////////////////////////////////////////////////////////////////// - std::vector>::iterator it; if (total_input_items > 0) { @@ -498,7 +477,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) { if(it->front().PRN != it->back().PRN) { it->clear(); __dump = false; } } - if(d_dump && __dump) + if(d_dump and __dump) { // MULTIPLEXED FILE RECORDING - Record results to file try @@ -529,17 +508,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) } 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) - { - set_T_rx_s = false; - return 0; - } - - 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; - } + if(d_num_valid_channels == 0) { return 0; } for(i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the threshold { @@ -552,11 +521,8 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) // Check if there is any valid channel after computing the time distance between the Gnss_Synchro data and the receiver time d_num_valid_channels = valid_channels.count(); - if(d_num_valid_channels == 0) - { - set_T_rx_s = false; - return 0; - } + double T_rx_s_out = T_rx_s - (max_delta / 2.0); + if((d_num_valid_channels == 0) or (T_rx_s_out < 0.0)) { return 0; } std::vector epoch_data; i = 0; @@ -564,14 +530,17 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) { if(valid_channels[i]) { - std::pair gnss_pair = find_closest(*it); + std::pair gnss_pair = find_closest(*it, T_rx_s_out); Gnss_Synchro interpolated_gnss_synchro = gnss_pair.second; + if(interpolated_gnss_synchro.Flag_valid_pseudorange) + { + interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(gnss_pair, T_rx_s_out, 0); + interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(gnss_pair, T_rx_s_out, 1); + interpolated_gnss_synchro.TOW_at_current_symbol_s = interpolate_data(gnss_pair, T_rx_s_out, 2); - interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(gnss_pair, T_rx_s, 0); - interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(gnss_pair, T_rx_s, 1); - interpolated_gnss_synchro.TOW_at_current_symbol_s = interpolate_data(gnss_pair, T_rx_s, 2); - - epoch_data.push_back(interpolated_gnss_synchro); + epoch_data.push_back(interpolated_gnss_synchro); + } + else { valid_channels[i] = false; } } i++; } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index d4b50a36b..b562a7232 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -68,8 +68,7 @@ private: 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(); - std::pair find_closest(std::deque& data); + std::pair find_closest(std::deque& data, const double& ti); void correct_TOW_and_compute_prange(std::vector& data); //Tracking observable history @@ -79,7 +78,6 @@ private: double T_rx_step_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; @@ -87,6 +85,11 @@ private: std::ofstream d_dump_file; std::ofstream d_dump_in; + std::string text_red = "\033[31m"; + std::string text_green = "\033[32m"; + std::string text_reset = "\033[0m"; + + int save_matfile(); }; From 9533662a0e4eaf889981fd092bf40751b9ec1c0f Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 22 Feb 2018 11:18:37 +0100 Subject: [PATCH 18/63] Minor change --- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 27568c0c6..28221f150 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -544,6 +544,8 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) } i++; } + d_num_valid_channels = valid_channels.count(); + if(d_num_valid_channels == 0) { return 0; } correct_TOW_and_compute_prange(epoch_data); std::vector::iterator it2 = epoch_data.begin(); From 58dd5428a8f3297235b372514e9ac675b396f086 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 22 Feb 2018 11:39:38 +0100 Subject: [PATCH 19/63] debug --- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 28221f150..ecee876d9 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -399,6 +399,7 @@ std::pair hybrid_observables_cc::find_closest(std::d invalid_data.Flag_valid_pseudorange = false; result.first = invalid_data; result.second = invalid_data; + std::cout << text_red << data.at(index).Signal << text_reset << std::endl; } else if(delta_t < 0.0) { @@ -406,6 +407,7 @@ std::pair hybrid_observables_cc::find_closest(std::d result.first.Flag_valid_pseudorange = true; result.second = data.at(index - 1); result.second.Flag_valid_pseudorange = true; + std::cout << text_green << data.at(index).Signal << text_reset << std::endl; } else { @@ -413,6 +415,7 @@ std::pair hybrid_observables_cc::find_closest(std::d result.first.Flag_valid_pseudorange = true; result.second = data.at(index); result.second.Flag_valid_pseudorange = true; + std::cout << text_green << data.at(index).Signal << text_reset << std::endl; } return result; } From 592d50af79b4379b89bc06526ff298b8d66b0610 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 22 Feb 2018 12:42:09 +0100 Subject: [PATCH 20/63] debug 2 --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 6 ++++-- .../gnuradio_blocks/hybrid_observables_cc.cc | 5 +---- .../gnuradio_blocks/hybrid_observables_cc.h | 5 ----- .../gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc | 10 ++++++++++ 4 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 85290da73..1554884b0 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -1713,9 +1713,11 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite // DEBUG MESSAGE: Display position in console output if( (d_ls_pvt->is_valid_position() == true) && (flag_display_pvt == true) ) { - std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) + std::string text_green = "\033[32m"; + std::string text_reset = "\033[0m"; + std::cout << text_green << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() - << " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << std::endl; + << " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << text_reset << std::endl; LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using "<< d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index ecee876d9..105e1fc38 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -63,7 +63,7 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned d_dump_filename_in = d_dump_filename; T_rx_s = 0.0; T_rx_step_s = 0.001; // 1 ms - max_delta = 0.1; // 100 ms + max_delta = 0.15; // 150 ms valid_channels.resize(d_nchannels, false); d_num_valid_channels = 0; @@ -399,7 +399,6 @@ std::pair hybrid_observables_cc::find_closest(std::d invalid_data.Flag_valid_pseudorange = false; result.first = invalid_data; result.second = invalid_data; - std::cout << text_red << data.at(index).Signal << text_reset << std::endl; } else if(delta_t < 0.0) { @@ -407,7 +406,6 @@ std::pair hybrid_observables_cc::find_closest(std::d result.first.Flag_valid_pseudorange = true; result.second = data.at(index - 1); result.second.Flag_valid_pseudorange = true; - std::cout << text_green << data.at(index).Signal << text_reset << std::endl; } else { @@ -415,7 +413,6 @@ std::pair hybrid_observables_cc::find_closest(std::d result.first.Flag_valid_pseudorange = true; result.second = data.at(index); result.second.Flag_valid_pseudorange = true; - std::cout << text_green << data.at(index).Signal << text_reset << std::endl; } return result; } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index b562a7232..5f9d0722f 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -85,11 +85,6 @@ private: std::ofstream d_dump_file; std::ofstream d_dump_in; - std::string text_red = "\033[31m"; - std::string text_green = "\033[32m"; - std::string text_reset = "\033[0m"; - - int save_matfile(); }; 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 e5f37423a..20be2dcf9 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 @@ -156,8 +156,18 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( //* delay by the formulae: //* \code //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + (12 * 20); 12 symbols of the encoder's transitory + double tmp_tow= d_TOW_at_current_symbol += GPS_L2_M_PERIOD; d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L2_M_PERIOD + 12 * GPS_L2_M_PERIOD; d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; + double tmp_diff = std::fabs(tmp_tow-d_TOW_at_current_symbol); + if ((tmp_tow != 0) or (tmp_diff > 0.0)) + { + std::string text_red = "\033[31m"; + std::string text_reset = "\033[0m"; + std::cout << text_red << + "GPS L2C. TOW incoherence on channel: "<< d_channel << ". TOW difference = " << + tmp_diff * 1000.0 << " [ms]" << text_reset << std::endl; + } d_flag_valid_word = true; } else From e2c2da67c4b7163bd539cb33f2570b8dc597f8cf Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 22 Feb 2018 16:15:07 +0100 Subject: [PATCH 21/63] debug3 --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 6 ++---- .../gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc | 10 +++++++++- .../gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc | 10 ++++------ src/core/system_parameters/MATH_CONSTANTS.h | 7 +++++++ 4 files changed, 22 insertions(+), 11 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 1554884b0..4914bf020 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -1713,11 +1713,9 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite // DEBUG MESSAGE: Display position in console output if( (d_ls_pvt->is_valid_position() == true) && (flag_display_pvt == true) ) { - std::string text_green = "\033[32m"; - std::string text_reset = "\033[0m"; - std::cout << text_green << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) + std::cout << TEXT_GREEN << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() - << " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << text_reset << std::endl; + << " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << TEXT_RESET << std::endl; LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using "<< d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() 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 79810f000..f0766f528 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 @@ -350,11 +350,19 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ //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 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; + double tmp_tow = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD; + d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 2 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; d_TOW_at_current_symbol = floor(d_TOW_at_Preamble * 1000.0) / 1000.0; flag_TOW_set = true; d_flag_new_tow_available = false; + double tmp_diff = std::fabs(tmp_tow - d_TOW_at_current_symbol); + if ((tmp_tow != 0) or (tmp_diff > 0.0)) + { + std::cout << TEXT_RED << + "GPS L1 C/A. TOW incoherence on channel: "<< d_channel << ". TOW difference = " << + tmp_diff * 1000.0 << " [ms]" << TEXT_RESET << std::endl; + } } else { 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 20be2dcf9..be426b70a 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 @@ -156,17 +156,15 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( //* delay by the formulae: //* \code //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + (12 * 20); 12 symbols of the encoder's transitory - double tmp_tow= d_TOW_at_current_symbol += GPS_L2_M_PERIOD; + double tmp_tow = d_TOW_at_current_symbol + GPS_L2_M_PERIOD; d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L2_M_PERIOD + 12 * GPS_L2_M_PERIOD; d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; - double tmp_diff = std::fabs(tmp_tow-d_TOW_at_current_symbol); + double tmp_diff = std::fabs(tmp_tow - d_TOW_at_current_symbol); if ((tmp_tow != 0) or (tmp_diff > 0.0)) { - std::string text_red = "\033[31m"; - std::string text_reset = "\033[0m"; - std::cout << text_red << + std::cout << TEXT_RED << "GPS L2C. TOW incoherence on channel: "<< d_channel << ". TOW difference = " << - tmp_diff * 1000.0 << " [ms]" << text_reset << std::endl; + tmp_diff * 1000.0 << " [ms]" << TEXT_RESET << std::endl; } d_flag_valid_word = true; } diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 8c8dfbbe5..2a5f376e6 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -31,6 +31,8 @@ #ifndef GNSS_SDR_MATH_CONSTANTS_H_ #define GNSS_SDR_MATH_CONSTANTS_H_ +#include + /* Constants for scaling the ephemeris found in the data message the format is the following: TWO_N5 -> 2^-5, TWO_P4 -> 2^4, PI_TWO_N43 -> Pi*2^-43, etc etc Additionally some of the PI*2^N terms are used in the tracking stuff @@ -41,6 +43,11 @@ ONE_PI_TWO_PX = (1/Pi)*2^X */ +const std::string TEXT_RED = "\033[31m"; +const std::string TEXT_GREEN = "\033[32m"; +const std::string TEXT_RESET = "\033[0m"; + + const double PI = 3.1415926535897932; //!< pi const double PI_2 = 2.0 * PI; //!< 2 * pi From 44f05263f00541a9e654808878f53301004a0123 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 22 Feb 2018 18:03:24 +0100 Subject: [PATCH 22/63] debug4 --- .../gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 f0766f528..4084db964 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 @@ -357,7 +357,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ flag_TOW_set = true; d_flag_new_tow_available = false; double tmp_diff = std::fabs(tmp_tow - d_TOW_at_current_symbol); - if ((tmp_tow != 0) or (tmp_diff > 0.0)) + if (tmp_diff > 0.0) { std::cout << TEXT_RED << "GPS L1 C/A. TOW incoherence on channel: "<< d_channel << ". TOW difference = " << From 3921000ed2f45df3402528cfcbab399829c1f546 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 22 Feb 2018 19:16:06 +0100 Subject: [PATCH 23/63] debug5 --- .../gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc | 4 ++-- .../gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc | 10 ++++++---- 2 files changed, 8 insertions(+), 6 deletions(-) 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 4084db964..880b9b7fb 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 @@ -357,10 +357,10 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ flag_TOW_set = true; d_flag_new_tow_available = false; double tmp_diff = std::fabs(tmp_tow - d_TOW_at_current_symbol); - if (tmp_diff > 0.0) + if (tmp_diff > 0.000001) { std::cout << TEXT_RED << - "GPS L1 C/A. TOW incoherence on channel: "<< d_channel << ". TOW difference = " << + "GPS L1 C/A. TOW incoherence on PRN: "<< current_symbol.PRN << ". TOW difference = " << tmp_diff * 1000.0 << " [ms]" << TEXT_RESET << std::endl; } } 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 be426b70a..95d34aa4f 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 @@ -150,7 +150,8 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( } //update TOW at the preamble instant - d_TOW_at_Preamble = static_cast(msg.tow); + std::cout<<"delta tow at preamble: "<(msg.tow) * 6.0-d_TOW_at_Preamble*6.0<(msg.tow); //std::cout<<"["<<(int)msg.prn<<"] deco delay: "<tow * 6000 + *pdelay * 20 + (12 * 20); 12 symbols of the encoder's transitory double tmp_tow = d_TOW_at_current_symbol + GPS_L2_M_PERIOD; d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L2_M_PERIOD + 12 * GPS_L2_M_PERIOD; - d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; + + //d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; double tmp_diff = std::fabs(tmp_tow - d_TOW_at_current_symbol); - if ((tmp_tow != 0) or (tmp_diff > 0.0)) + if (tmp_diff > 0.000001) { std::cout << TEXT_RED << - "GPS L2C. TOW incoherence on channel: "<< d_channel << ". TOW difference = " << + "GPS L2C. TOW incoherence on PRN: "<< current_synchro_data.PRN << ". TOW difference = " << tmp_diff * 1000.0 << " [ms]" << TEXT_RESET << std::endl; } d_flag_valid_word = true; From c0f1d95b32df023a46bc2805a2c11a2f4aa1de87 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 23 Feb 2018 10:50:22 +0100 Subject: [PATCH 24/63] debug6 --- .../gnuradio_blocks/hybrid_observables_cc.cc | 22 +++++++++++++++++++ .../gps_l2c_telemetry_decoder_cc.cc | 2 -- 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 105e1fc38..4ecb8d236 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -422,6 +422,28 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector::lowest(); std::vector::iterator it; + + +/////////////////////// DEBUG ////////////////////////// + std::vector::iterator it2; + double thr_ = 250.0 / 3e8; + for(it = data.begin(); it != (data.end() - 1); it++) + { + for(it2 = it + 1; it2 != data.end(); it2++) + { + if(it->PRN == it2->PRN) + { + double tow_dif_ = std::fabs(it->TOW_at_current_symbol_s - it2->TOW_at_current_symbol_s); + if(tow_dif_ > thr_) + { + std::cout << TEXT_RED << "L1 - L2 TOW difference in PRN " << it->PRN << + " = " << tow_dif_ << "[ms]. Equivalent to " << tow_dif_ * 3e8 << " meters in pseudorange" + << TEXT_RESET << std::endl; + } + } + } + } +/////////////////////////////////////////////////////////// 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; } 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 95d34aa4f..281698845 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 @@ -150,9 +150,7 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( } //update TOW at the preamble instant - std::cout<<"delta tow at preamble: "<(msg.tow) * 6.0-d_TOW_at_Preamble*6.0<(msg.tow); - //std::cout<<"["<<(int)msg.prn<<"] deco delay: "< Date: Fri, 23 Feb 2018 12:00:20 +0100 Subject: [PATCH 25/63] debug7 --- .../gnuradio_blocks/hybrid_observables_cc.cc | 60 ++++--------------- .../gnuradio_blocks/hybrid_observables_cc.h | 4 +- .../gps_l1_ca_telemetry_decoder_cc.cc | 8 --- .../gps_l2c_telemetry_decoder_cc.cc | 9 --- 4 files changed, 12 insertions(+), 69 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 4ecb8d236..beda350d3 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -60,7 +60,6 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned d_dump = dump; d_nchannels = nchannels_out; d_dump_filename = dump_filename; - d_dump_filename_in = d_dump_filename; T_rx_s = 0.0; T_rx_step_s = 0.001; // 1 ms max_delta = 0.15; // 150 ms @@ -90,21 +89,6 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned d_dump = false; } } - if (!d_dump_in.is_open()) - { - try - { - d_dump_in.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_dump_filename_in.append("_in.bin"); - d_dump_in.open(d_dump_filename_in.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure & e) - { - LOG(WARNING) << "Exception opening observables dump file " << e.what(); - d_dump = false; - } - } } } @@ -119,14 +103,6 @@ hybrid_observables_cc::~hybrid_observables_cc() LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } - if (d_dump_in.is_open()) - { - try { d_dump_in.close(); } - catch(const std::exception & ex) - { - LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); - } - } if(d_dump) { std::cout << "Writing observables .mat files ..."; @@ -425,25 +401,30 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector::iterator it2; - double thr_ = 250.0 / 3e8; + double thr_ = 250.0 / 3e8; // Maximum pseudorange difference = 250 meters for(it = data.begin(); it != (data.end() - 1); it++) { for(it2 = it + 1; it2 != data.end(); it2++) { - if(it->PRN == it2->PRN) + if(it->PRN == 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_) { - std::cout << TEXT_RED << "L1 - L2 TOW difference in PRN " << it->PRN << - " = " << tow_dif_ << "[ms]. Equivalent to " << tow_dif_ * 3e8 << " meters in pseudorange" - << TEXT_RESET << std::endl; + DLOG(INFO) << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal + << ". TOW difference in PRN " << it->PRN + << " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * 3e8 + << " meters in pseudorange"; } } } } +#endif /////////////////////////////////////////////////////////// + + 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; } @@ -490,31 +471,12 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) { if(in[i][aux].Flag_valid_word) { - bool __dump = false; it->push_back(in[i][aux]); it->back().RX_time = compute_T_rx_s(in[i][aux]); - __dump = true; // Check if the last Gnss_Synchro comes from the same satellite as the previous ones if(it->size() > 1) { - if(it->front().PRN != it->back().PRN) { it->clear(); __dump = false; } - } - if(d_dump and __dump) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - int tmp_int = static_cast(it->back().PRN); - d_dump_in.write(reinterpret_cast(&tmp_int), sizeof(int)); - d_dump_in.write(reinterpret_cast(&it->back().RX_time), sizeof(double)); - d_dump_in.write(reinterpret_cast(&it->back().TOW_at_current_symbol_s), sizeof(double)); - d_dump_in.write(it->back().Signal, 3 * sizeof(char)); - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - d_dump = false; - } + if(it->front().PRN != it->back().PRN) { it->clear(); } } } } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index 5f9d0722f..024e1b4c0 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -70,6 +70,7 @@ private: double interpolate_data(const std::pair& a, const double& ti, int parameter); std::pair find_closest(std::deque& data, const double& ti); void correct_TOW_and_compute_prange(std::vector& data); + int save_matfile(); //Tracking observable history std::vector> d_gnss_synchro_history; @@ -81,11 +82,8 @@ private: unsigned int d_nchannels; unsigned int d_num_valid_channels; std::string d_dump_filename; - std::string d_dump_filename_in; std::ofstream d_dump_file; - std::ofstream d_dump_in; - int save_matfile(); }; #endif 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 880b9b7fb..ae303a20f 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 @@ -350,19 +350,11 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ //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) - double tmp_tow = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD; d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 2 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; d_TOW_at_current_symbol = floor(d_TOW_at_Preamble * 1000.0) / 1000.0; flag_TOW_set = true; d_flag_new_tow_available = false; - double tmp_diff = std::fabs(tmp_tow - d_TOW_at_current_symbol); - if (tmp_diff > 0.000001) - { - std::cout << TEXT_RED << - "GPS L1 C/A. TOW incoherence on PRN: "<< current_symbol.PRN << ". TOW difference = " << - tmp_diff * 1000.0 << " [ms]" << TEXT_RESET << std::endl; - } } else { 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 281698845..4e046f1c4 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 @@ -155,17 +155,8 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( //* delay by the formulae: //* \code //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + (12 * 20); 12 symbols of the encoder's transitory - double tmp_tow = d_TOW_at_current_symbol + GPS_L2_M_PERIOD; d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L2_M_PERIOD + 12 * GPS_L2_M_PERIOD; - //d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; - double tmp_diff = std::fabs(tmp_tow - d_TOW_at_current_symbol); - if (tmp_diff > 0.000001) - { - std::cout << TEXT_RED << - "GPS L2C. TOW incoherence on PRN: "<< current_synchro_data.PRN << ". TOW difference = " << - tmp_diff * 1000.0 << " [ms]" << TEXT_RESET << std::endl; - } d_flag_valid_word = true; } else From 5cba843eaab16b87bfc079c617063d4e80c9e4af Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 23 Feb 2018 16:17:51 +0100 Subject: [PATCH 26/63] Add colors to display --- CMakeLists.txt | 1 + .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 3 +- .../gnuradio_blocks/hybrid_observables_cc.cc | 11 +-- src/core/system_parameters/MATH_CONSTANTS.h | 5 -- src/core/system_parameters/display.h | 77 +++++++++++++++++++ src/main/main.cc | 1 + 6 files changed, 87 insertions(+), 11 deletions(-) create mode 100644 src/core/system_parameters/display.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 28435b368..18032be21 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,6 +137,7 @@ set(OS_IS_LINUX "") if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") set(OperatingSystem "Linux") set(OS_IS_LINUX TRUE) + add_definitions(-DDISPLAY_COLORS) if(ARCH_64BITS) set(ARCH_ "(64 bits)") else(ARCH_64BITS) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 4914bf020..c6af2e47a 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -40,6 +40,7 @@ #include #include #include +#include "display.h" using google::LogMessage; @@ -1713,7 +1714,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite // DEBUG MESSAGE: Display position in console output if( (d_ls_pvt->is_valid_position() == true) && (flag_display_pvt == true) ) { - std::cout << TEXT_GREEN << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) + std::cout << TEXT_BOLD_GREEN << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() << " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << TEXT_RESET << std::endl; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index beda350d3..2f3d7f5c6 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -396,14 +396,15 @@ std::pair hybrid_observables_cc::find_closest(std::d void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector& data) { - double TOW_ref = std::numeric_limits::lowest(); std::vector::iterator it; - /////////////////////// DEBUG ////////////////////////// +// Logs if there is a pseudorange difference between +// signals of the same satellite higher than a threshold +//////////////////////////////////////////////////////// #ifndef NDEBUG std::vector::iterator it2; - double thr_ = 250.0 / 3e8; // Maximum pseudorange difference = 250 meters + double thr_ = 250.0 / SPEED_OF_LIGHT; // Maximum pseudorange difference = 250 meters for(it = data.begin(); it != (data.end() - 1); it++) { for(it2 = it + 1; it2 != data.end(); it2++) @@ -415,7 +416,7 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vectorSystem << ". Signals " << it->Signal << " and " << it2->Signal << ". TOW difference in PRN " << it->PRN - << " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * 3e8 + << " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT << " meters in pseudorange"; } } @@ -424,7 +425,7 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector::lowest(); 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; } diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 2a5f376e6..d7da95651 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -43,11 +43,6 @@ ONE_PI_TWO_PX = (1/Pi)*2^X */ -const std::string TEXT_RED = "\033[31m"; -const std::string TEXT_GREEN = "\033[32m"; -const std::string TEXT_RESET = "\033[0m"; - - const double PI = 3.1415926535897932; //!< pi const double PI_2 = 2.0 * PI; //!< 2 * pi diff --git a/src/core/system_parameters/display.h b/src/core/system_parameters/display.h new file mode 100644 index 000000000..b6c3c05b8 --- /dev/null +++ b/src/core/system_parameters/display.h @@ -0,0 +1,77 @@ +/*! + * \file display.h + * \brief Defines useful display constants + * \author Antonio Ramos, 2018. antonio.ramos(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_DISPLAY_H_ +#define GNSS_SDR_DISPLAY_H_ + +#include + +#ifdef DISPLAY_COLORS + +const std::string TEXT_RESET = "\033[0m"; +const std::string TEXT_BLACK = "\033[30m"; +const std::string TEXT_RED = "\033[31m"; +const std::string TEXT_GREEN = "\033[32m"; +const std::string TEXT_YELLOW = "\033[33m"; +const std::string TEXT_BLUE = "\033[34m"; +const std::string TEXT_MAGENTA = "\033[35m"; +const std::string TEXT_CYAN = "\033[36m"; +const std::string TEXT_WHITE = "\033[37m"; +const std::string TEXT_BOLD_BLACK = "\033[1m\033[30m"; +const std::string TEXT_BOLD_RED = "\033[1m\033[31m"; +const std::string TEXT_BOLD_GREEN = "\033[1m\033[32m"; +const std::string TEXT_BOLD_YELLOW = "\033[1m\033[33m"; +const std::string TEXT_BOLD_BLUE = "\033[1m\033[34m"; +const std::string TEXT_BOLD_MAGENTA = "\033[1m\033[35m"; +const std::string TEXT_BOLD_CYAN = "\033[1m\033[36m"; +const std::string TEXT_BOLD_WHITE = "\033[1m\033[37m"; + +#else + +const std::string TEXT_RESET = ""; +const std::string TEXT_BLACK = ""; +const std::string TEXT_RED = ""; +const std::string TEXT_GREEN = ""; +const std::string TEXT_YELLOW = ""; +const std::string TEXT_BLUE = ""; +const std::string TEXT_MAGENTA = ""; +const std::string TEXT_CYAN = ""; +const std::string TEXT_WHITE = ""; +const std::string TEXT_BOLD_BLACK = ""; +const std::string TEXT_BOLD_RED = ""; +const std::string TEXT_BOLD_GREEN = ""; +const std::string TEXT_BOLD_YELLOW = ""; +const std::string TEXT_BOLD_BLUE = ""; +const std::string TEXT_BOLD_MAGENTA = ""; +const std::string TEXT_BOLD_CYAN = ""; +const std::string TEXT_BOLD_WHITE = ""; + +#endif /* DISPLAY_COLORS */ +#endif /* GNSS_SDR_DISPLAY_H_ */ diff --git a/src/main/main.cc b/src/main/main.cc index af45ab3a7..ef2a0effb 100644 --- a/src/main/main.cc +++ b/src/main/main.cc @@ -50,6 +50,7 @@ #include "concurrent_queue.h" #include "concurrent_map.h" #include "gnss_sdr_flags.h" +#include "display.h" #if CUDA_GPU_ACCEL // For the CUDA runtime routines (prefixed with "cuda_") From e964bf060f731ef5d097fa2d8c996f7977795a8f Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 26 Feb 2018 10:40:08 +0100 Subject: [PATCH 27/63] Clean code --- CMakeLists.txt | 2 +- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 3 --- .../observables/gnuradio_blocks/hybrid_observables_cc.h | 2 -- 3 files changed, 1 insertion(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9867f24e7..540930e3f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -137,7 +137,7 @@ set(OS_IS_LINUX "") if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") set(OperatingSystem "Linux") set(OS_IS_LINUX TRUE) - add_definitions(-DDISPLAY_COLORS) + add_definitions( -DDISPLAY_COLORS=1 ) if(ARCH_64BITS) set(ARCH_ "(64 bits)") else(ARCH_64BITS) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index f26b6d294..27be559d7 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -30,13 +30,10 @@ */ #include "hybrid_observables_cc.h" -#include "Galileo_E1.h" #include "GPS_L1_CA.h" #include #include #include -#include -#include #include #include #include diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index dc5085cd3..f48f5ce22 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -42,8 +42,6 @@ #include //std::vector #include #include -#include -#include "gnss_synchro.h" class hybrid_observables_cc; From eed6ed1f5e74b7ba3ced917c08a8b567873e9032 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 26 Feb 2018 12:33:40 +0100 Subject: [PATCH 28/63] Remove set_max_noutput_items --- .../gnuradio_blocks/hybrid_observables_cc.cc | 117 ++----- .../gnuradio_blocks/hybrid_observables_cc.h | 4 +- .../galileo_e1b_telemetry_decoder_cc.cc | 1 - .../galileo_e5a_telemetry_decoder_cc.cc | 1 - .../gps_l1_ca_telemetry_decoder_cc.cc | 20 +- .../gps_l1_ca_telemetry_decoder_cc.h | 5 +- .../gps_l2c_telemetry_decoder_cc.cc | 8 +- .../gps_l5_telemetry_decoder_cc.cc | 1 - .../galileo_e1_dll_pll_veml_tracking_cc.cc | 1 - .../galileo_e5a_dll_pll_tracking_cc.cc | 1 - .../gps_l1_ca_dll_pll_tracking_cc.cc | 1 - .../gps_l2_m_dll_pll_tracking_cc.cc | 1 - .../gps_l5i_dll_pll_tracking_cc.cc | 1 - .../gps_navigation_message.cc | 18 +- .../observables/hybrid_observables_test.cc | 328 +++++++++--------- 15 files changed, 233 insertions(+), 275 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 27be559d7..559caafdc 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -39,6 +39,7 @@ #include #include #include +#include "display.h" using google::LogMessage; @@ -55,7 +56,6 @@ 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); d_dump = dump; d_nchannels = nchannels_out; d_dump_filename = dump_filename; @@ -78,7 +78,6 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned try { d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_dump_filename.append(".bin"); d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); } @@ -224,6 +223,7 @@ int hybrid_observables_cc::save_matfile() mat_t *matfp; matvar_t *matvar; std::string filename = d_dump_filename; + if(filename.size() > 4) { filename.erase(filename.end() - 4, filename.end()); } filename.append(".mat"); matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); if(reinterpret_cast(matfp) != NULL) @@ -288,40 +288,36 @@ int hybrid_observables_cc::save_matfile() return 0; } -double hybrid_observables_cc::interpolate_data(const std::pair& a, const double& ti, int parameter) +bool hybrid_observables_cc::interpolate_data(Gnss_Synchro& out, std::deque& data, const double& ti) { - // x(ti) = m * ti + c - // m = [x(t2) - x(t1)] / [t2 - t1] - // c = x(t1) - m * t1 + std::deque::iterator it; - double m = 0.0; - double c = 0.0; + arma::vec t = arma::vec(data.size()); + arma::vec dop = t; + arma::vec cph = t; + arma::vec tow = t; + arma::vec tiv = arma::vec(1); + arma::vec result; + tiv(0) = ti; - if(!a.first.Flag_valid_word or !a.second.Flag_valid_word) { return 0.0; } - - switch(parameter) + unsigned int aux = 0; + for(it = data.begin(); it != data.end(); it++) { - case 0:// Doppler - m = (a.first.Carrier_Doppler_hz - a.second.Carrier_Doppler_hz) / (a.first.RX_time - a.second.RX_time); - c = a.second.Carrier_Doppler_hz - m * a.second.RX_time; - break; + t(aux) = it->RX_time; + dop(aux) = it->Carrier_Doppler_hz; + cph(aux) = it->Carrier_phase_rads; + tow(aux) = it->TOW_at_current_symbol_s; - case 1:// Carrier phase - m = (a.first.Carrier_phase_rads - a.second.Carrier_phase_rads) / (a.first.RX_time - a.second.RX_time); - c = a.second.Carrier_phase_rads - m * a.second.RX_time; - break; - - case 2:// TOW - m = (a.first.TOW_at_current_symbol_s - a.second.TOW_at_current_symbol_s) / (a.first.RX_time - a.second.RX_time); - c = a.second.TOW_at_current_symbol_s - m * a.second.RX_time; - break; - - case 3:// Code phase samples - m = (a.first.Code_phase_samples - a.second.Code_phase_samples) / (a.first.RX_time - a.second.RX_time); - c = a.second.Code_phase_samples - m * a.second.RX_time; - break; + aux++; } - return(m * ti + c); + arma::interp1(t, dop, tiv, result); + out.Carrier_Doppler_hz = result(0); + arma::interp1(t, cph, tiv, result); + out.Carrier_phase_rads = result(0); + arma::interp1(t, tow, tiv, result); + out.TOW_at_current_symbol_s = result(0); + + return result.is_finite(); } double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro& a) @@ -350,49 +346,6 @@ void hybrid_observables_cc::clean_history(std::deque& data) } } -std::pair hybrid_observables_cc::find_closest(std::deque& data, const double& ti) -{ - std::pair result; - unsigned int index = 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 = std::fabs(ti - it->RX_time); - if(instant_delta < delta_t) - { - delta_t = instant_delta; - index = aux; - } - aux++; - } - delta_t = ti - data.at(index).RX_time; - if( (index == 0) or (index == (data.size() - 1)) ) - { - Gnss_Synchro invalid_data; - invalid_data.Flag_valid_pseudorange = false; - result.first = invalid_data; - result.second = invalid_data; - } - else if(delta_t < 0.0) - { - result.first = data.at(index); - result.first.Flag_valid_pseudorange = true; - result.second = data.at(index - 1); - result.second.Flag_valid_pseudorange = true; - } - else - { - result.first = data.at(index + 1); - result.first.Flag_valid_pseudorange = true; - result.second = data.at(index); - result.second.Flag_valid_pseudorange = true; - } - return result; -} - - void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector& data) { std::vector::iterator it; @@ -417,6 +370,10 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vectorPRN << " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT << " meters in pseudorange"; + std::cout << TEXT_RED << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal + << ". TOW difference in PRN " << it->PRN + << " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT + << " meters in pseudorange" << TEXT_RESET << std::endl; } } } @@ -514,17 +471,15 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) { if(valid_channels[i]) { - std::pair gnss_pair = find_closest(*it, T_rx_s_out); - Gnss_Synchro interpolated_gnss_synchro = gnss_pair.second; - if(interpolated_gnss_synchro.Flag_valid_pseudorange) + Gnss_Synchro interpolated_gnss_synchro; + if(interpolate_data(interpolated_gnss_synchro, *it, T_rx_s_out)) { - interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(gnss_pair, T_rx_s_out, 0); - interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(gnss_pair, T_rx_s_out, 1); - interpolated_gnss_synchro.TOW_at_current_symbol_s = interpolate_data(gnss_pair, T_rx_s_out, 2); - epoch_data.push_back(interpolated_gnss_synchro); } - else { valid_channels[i] = false; } + else + { + valid_channels[i] = false; + } } i++; } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index f48f5ce22..127bba60a 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -38,7 +38,6 @@ #include #include #include -#include //std::pair #include //std::vector #include #include @@ -67,8 +66,7 @@ private: 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); - std::pair find_closest(std::deque& data, const double& ti); + bool interpolate_data(Gnss_Synchro& out, std::deque& data, const double& ti); void correct_TOW_and_compute_prange(std::vector& data); int save_matfile(); 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 40d737246..34b829252 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 @@ -107,7 +107,6 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc( gr::block("galileo_e1b_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - set_max_noutput_items(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 928f14ac6..6ae735b57 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 @@ -183,7 +183,6 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - set_max_noutput_items(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 ccbd17d9e..df6cbb6d8 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 @@ -55,7 +55,6 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( gr::block("gps_navigation_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - set_max_noutput_items(1); // Telemetry Bit transition synchronization port out this->message_port_register_out(pmt::mp("preamble_timestamp_s")); // Ephemeris data port out @@ -95,8 +94,8 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( d_GPS_frame_4bytes = 0; d_prev_GPS_frame_4bytes = 0; d_flag_parity = false; - d_TOW_at_Preamble = 0; - d_TOW_at_current_symbol = 0; + 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; @@ -106,6 +105,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( d_channel = 0; flag_PLL_180_deg_phase_locked = false; d_preamble_time_samples = 0; + d_TOW_at_current_symbol_ms = 0; } @@ -206,7 +206,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ else if (d_stat == 1) //check 6 seconds of preamble separation { preamble_diff_ms = round(((static_cast(d_symbol_history.at(0).Tracking_sample_counter) - d_preamble_time_samples) / static_cast(d_symbol_history.at(0).fs)) * 1000.0); - if (abs(preamble_diff_ms - GPS_SUBFRAME_MS) < 1) + if (abs(preamble_diff_ms - GPS_SUBFRAME_MS) == 0) { DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite; d_GPS_FSM.Event_gps_word_preamble(); @@ -351,17 +351,21 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ // /(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 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; - d_TOW_at_current_symbol = floor(d_TOW_at_Preamble * 1000.0) / 1000.0; + d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 1.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 = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD; + d_TOW_at_current_symbol += GPS_L1_CA_CODE_PERIOD; + d_TOW_at_current_symbol_ms++; } - current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol; + 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.Flag_valid_word = flag_TOW_set; 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 28c006adf..b43893f09 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 @@ -107,8 +107,9 @@ private: unsigned long int d_preamble_time_samples; - long double d_TOW_at_Preamble; - long double d_TOW_at_current_symbol; + double d_TOW_at_Preamble; + double d_TOW_at_current_symbol; + unsigned int d_TOW_at_current_symbol_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 f4a64d06f..5a2bbf8e4 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 @@ -32,6 +32,7 @@ #include "gps_l2c_telemetry_decoder_cc.h" #include "gnss_synchro.h" +#include "display.h" #include #include #include @@ -54,7 +55,6 @@ gps_l2c_telemetry_decoder_cc::gps_l2c_telemetry_decoder_cc( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - set_max_noutput_items(1); // Telemetry Bit transition synchronization port out this->message_port_register_out(pmt::mp("preamble_timestamp_s")); // Ephemeris data port out @@ -133,21 +133,21 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( { // get ephemeris object for this SV std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_ephemeris()); - std::cout << "New GPS CNAV message received: ephemeris from satellite " << d_satellite << std::endl; + std::cout << TEXT_BLUE << "New GPS CNAV message received: ephemeris from satellite " << d_satellite << TEXT_RESET << std::endl; this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } if (d_CNAV_Message.have_new_iono() == true) { std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_iono()); - std::cout << "New GPS CNAV message received: iono model parameters from satellite " << d_satellite << std::endl; + std::cout << TEXT_BLUE << "New GPS CNAV message received: iono model parameters from satellite " << d_satellite << TEXT_RESET << std::endl; this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } if (d_CNAV_Message.have_new_utc_model() == true) { std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_utc_model()); - std::cout << "New GPS CNAV message received: UTC model parameters from satellite " << d_satellite << std::endl; + std::cout << TEXT_BLUE << "New GPS CNAV message received: UTC model parameters from satellite " << d_satellite << TEXT_RESET << std::endl; this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } 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 eb8e4a976..44ebc407b 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 @@ -56,7 +56,6 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc( gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - set_max_noutput_items(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/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index 8359f0e77..925ac457d 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -118,7 +118,6 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( gr::block("galileo_e1_dll_pll_veml_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - set_max_noutput_items(1); // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); this->set_relative_rate(1.0 / vector_length); diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc index c7f30d867..c91d59a68 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc @@ -98,7 +98,6 @@ Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc( gr::block("Galileo_E5a_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - set_max_noutput_items(1); // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); this->message_port_register_out(pmt::mp("events")); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index 89f3ba0ec..cfd4c69bd 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -94,7 +94,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( gr::block("Gps_L1_Ca_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - set_max_noutput_items(1); // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); this->message_port_register_out(pmt::mp("events")); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc index 9bd8d7967..bf1f1513b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc @@ -91,7 +91,6 @@ gps_l2_m_dll_pll_tracking_cc::gps_l2_m_dll_pll_tracking_cc( gr::block("gps_l2_m_dll_pll_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - set_max_noutput_items(1); // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); this->message_port_register_out(pmt::mp("events")); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc index b97cc804a..f0b56afa6 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc @@ -92,7 +92,6 @@ gps_l5i_dll_pll_tracking_cc::gps_l5i_dll_pll_tracking_cc( gr::block("gps_l5i_dll_pll_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { - set_max_noutput_items(1); // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); this->message_port_register_out(pmt::mp("events")); diff --git a/src/core/system_parameters/gps_navigation_message.cc b/src/core/system_parameters/gps_navigation_message.cc index 27a4e26c7..cf0d7aa0d 100644 --- a/src/core/system_parameters/gps_navigation_message.cc +++ b/src/core/system_parameters/gps_navigation_message.cc @@ -39,7 +39,7 @@ m * \file gps_navigation_message.cc void Gps_Navigation_Message::reset() { b_valid_ephemeris_set_flag = false; - d_TOW = 0; + d_TOW = 0.0; d_TOW_SF1 = 0; d_TOW_SF2 = 0; d_TOW_SF3 = 0; @@ -70,7 +70,7 @@ void Gps_Navigation_Message::reset() i_SV_accuracy = 0; i_SV_health = 0; d_TGD = 0; - d_IODC = -1; + d_IODC = -1.0; i_AODO = 0; b_fit_interval_flag = false; @@ -298,7 +298,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe) //TOW = bin2dec(subframe(31:47)) * 6; d_TOW_SF1 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); //we are in the first subframe (the transmitted TOW is the start time of the next subframe) ! - d_TOW_SF1 = d_TOW_SF1 * 6; + d_TOW_SF1 = d_TOW_SF1 * 6.0; d_TOW = d_TOW_SF1; // Set transmission time b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); @@ -324,7 +324,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe) case 2: //--- It is subframe 2 ------------------- d_TOW_SF2 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); - d_TOW_SF2 = d_TOW_SF2 * 6; + d_TOW_SF2 = d_TOW_SF2 * 6.0; d_TOW = d_TOW_SF2; // Set transmission time b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); @@ -354,7 +354,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe) case 3: // --- It is subframe 3 ------------------------------------- d_TOW_SF3 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); - d_TOW_SF3 = d_TOW_SF3 * 6; + d_TOW_SF3 = d_TOW_SF3 * 6.0; d_TOW = d_TOW_SF3; // Set transmission time b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); @@ -383,7 +383,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe) int SV_data_ID; int SV_page; d_TOW_SF4 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); - d_TOW_SF4 = d_TOW_SF4 * 6; + d_TOW_SF4 = d_TOW_SF4 * 6.0; d_TOW = d_TOW_SF4; // Set transmission time b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); @@ -459,7 +459,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe) int SV_data_ID_5; int SV_page_5; d_TOW_SF5 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); - d_TOW_SF5 = d_TOW_SF5 * 6; + d_TOW_SF5 = d_TOW_SF5 * 6.0; d_TOW = d_TOW_SF5; // Set transmission time b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); @@ -679,9 +679,9 @@ bool Gps_Navigation_Message::satellite_validation() // First Step: // check Issue Of Ephemeris Data (IODE IODC..) to find a possible interrupted reception // and check if the data have been filled (!=0) - if (d_TOW_SF1 != 0 and d_TOW_SF2 != 0 and d_TOW_SF3 != 0) + if (d_TOW_SF1 != 0.0 and d_TOW_SF2 != 0.0 and d_TOW_SF3 != 0.0) { - if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!= -1) + if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC != -1.0) { flag_data_valid = true; b_valid_ephemeris_set_flag = true; diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc index 3f5923460..32113d21a 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -36,11 +36,8 @@ #include #include #include -#include -#include #include #include -#include #include #include "GPS_L1_CA.h" #include "gnss_satellite.h" @@ -57,9 +54,10 @@ #include "observables_dump_reader.h" #include "tlm_dump_reader.h" #include "gps_l1_ca_dll_pll_tracking.h" -#include "gps_l1_ca_dll_pll_c_aid_tracking.h" #include "hybrid_observables.h" #include "signal_generator_flags.h" +#include "gnss_sdr_sample_counter.h" +#include // ######## GNURADIO BLOCK MESSAGE RECEVER FOR TRACKING MESSAGES ######### @@ -189,18 +187,17 @@ public: int configure_generator(); int generate_signal(); void check_results_carrier_phase( - arma::vec & true_ch0_phase_cycles, - arma::vec & true_ch1_phase_cycles, - arma::vec & true_ch0_tow_s, - arma::vec & measuded_ch0_phase_cycles, - arma::vec & measuded_ch1_phase_cycles, - arma::vec & measuded_ch0_RX_time_s); + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1); void check_results_code_psudorange( - arma::vec & true_ch0_dist_m, arma::vec & true_ch1_dist_m, - arma::vec & true_ch0_tow_s, - arma::vec & measuded_ch0_Pseudorange_m, - arma::vec & measuded_ch1_Pseudorange_m, - arma::vec & measuded_ch0_RX_time_s); + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1); HybridObservablesTest() { @@ -289,7 +286,7 @@ void HybridObservablesTest::configure_receiver() config->set_property("Tracking_1C.if", "0"); config->set_property("Tracking_1C.dump", "true"); config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); - config->set_property("Tracking_1C.pll_bw_hz", "15.0"); + config->set_property("Tracking_1C.pll_bw_hz", "35.0"); config->set_property("Tracking_1C.dll_bw_hz", "0.5"); config->set_property("Tracking_1C.early_late_space_chips", "0.5"); @@ -300,27 +297,37 @@ void HybridObservablesTest::configure_receiver() } void HybridObservablesTest::check_results_carrier_phase( - arma::vec & true_ch0_phase_cycles, - arma::vec & true_ch1_phase_cycles, - arma::vec & true_ch0_tow_s, - arma::vec & measuded_ch0_phase_cycles, - arma::vec & measuded_ch1_phase_cycles, - arma::vec & measuded_ch0_RX_time_s) + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1) { //1. True value interpolation to match the measurement times + double t0 = std::max(measured_ch0(0, 0), measured_ch1(0, 0)); + int size1 = measured_ch0.col(0).n_rows; + int size2 = measured_ch1.col(0).n_rows; + double t1 = std::min(measured_ch0(size1 - 1, 0), measured_ch1(size2 - 1, 0)); + arma::vec t = arma::linspace(t0, t1, floor((t1 - t0) * 1e3)); + arma::vec true_ch0_phase_interp; arma::vec true_ch1_phase_interp; - arma::interp1(true_ch0_tow_s, true_ch0_phase_cycles, measuded_ch0_RX_time_s, true_ch0_phase_interp); - arma::interp1(true_ch0_tow_s, true_ch1_phase_cycles, measuded_ch0_RX_time_s, true_ch1_phase_interp); + arma::interp1(true_tow_s, true_ch0.col(3), t, true_ch0_phase_interp); + arma::interp1(true_tow_s, true_ch1.col(3), t, true_ch1_phase_interp); + + arma::vec meas_ch0_phase_interp; + arma::vec meas_ch1_phase_interp; + arma::interp1(measured_ch0.col(0), measured_ch0.col(3), t, meas_ch0_phase_interp); + arma::interp1(measured_ch1.col(0), measured_ch1.col(3), t, meas_ch1_phase_interp); //2. RMSE arma::vec err_ch0_cycles; arma::vec err_ch1_cycles; //compute error without the accumulated carrier phase offsets (which depends on the receiver starting time) - err_ch0_cycles = measuded_ch0_phase_cycles - true_ch0_phase_interp - measuded_ch0_phase_cycles(0) + true_ch0_phase_interp(0); - err_ch1_cycles = measuded_ch1_phase_cycles - true_ch1_phase_interp - measuded_ch1_phase_cycles(0) + true_ch1_phase_interp(0); + err_ch0_cycles = meas_ch0_phase_interp - true_ch0_phase_interp - meas_ch0_phase_interp(0) + true_ch0_phase_interp(0); + err_ch1_cycles = meas_ch1_phase_interp - true_ch1_phase_interp - meas_ch1_phase_interp(0) + true_ch1_phase_interp(0); arma::vec err2_ch0 = arma::square(err_ch0_cycles); double rmse_ch0 = sqrt(arma::mean(err2_ch0)); @@ -347,58 +354,68 @@ void HybridObservablesTest::check_results_carrier_phase( //5. report std::streamsize ss = std::cout.precision(); - std::cout << std::setprecision(10) << "Channel 0 Carrier phase RMSE=" - << rmse_ch0 << ", mean=" << error_mean_ch0 - << ", stdev=" << sqrt(error_var_ch0) - << " (max,min)=" << max_error_ch0 + std::cout << std::setprecision(10) << "Channel 0 Carrier phase RMSE = " + << rmse_ch0 << ", mean = " << error_mean_ch0 + << ", stdev = " << sqrt(error_var_ch0) + << " (max,min) = " << max_error_ch0 << "," << min_error_ch0 << " [cycles]" << std::endl; std::cout.precision (ss); - ASSERT_LT(rmse_ch0, 1e-2); - ASSERT_LT(error_mean_ch0, 1e-2); - ASSERT_GT(error_mean_ch0, -1e-2); - ASSERT_LT(error_var_ch0, 1e-2); - ASSERT_LT(max_error_ch0, 5e-2); - ASSERT_GT(min_error_ch0, -5e-2); + ASSERT_LT(rmse_ch0, 5e-2); + ASSERT_LT(error_mean_ch0, 5e-2); + ASSERT_GT(error_mean_ch0, -5e-2); + ASSERT_LT(error_var_ch0, 5e-2); + ASSERT_LT(max_error_ch0, 5e-2); + ASSERT_GT(min_error_ch0, -5e-2); //5. report ss = std::cout.precision(); - std::cout << std::setprecision(10) << "Channel 1 Carrier phase RMSE=" - << rmse_ch1 << ", mean=" << error_mean_ch1 - << ", stdev=" << sqrt(error_var_ch1) - << " (max,min)=" << max_error_ch1 + std::cout << std::setprecision(10) << "Channel 1 Carrier phase RMSE = " + << rmse_ch1 << ", mean = " << error_mean_ch1 + << ", stdev = " << sqrt(error_var_ch1) + << " (max,min) = " << max_error_ch1 << "," << min_error_ch1 << " [cycles]" << std::endl; std::cout.precision (ss); - ASSERT_LT(rmse_ch1, 1e-2); - ASSERT_LT(error_mean_ch1, 1e-2); - ASSERT_GT(error_mean_ch1, -1e-2); - ASSERT_LT(error_var_ch1, 1e-2); - ASSERT_LT(max_error_ch1, 5e-2); - ASSERT_GT(min_error_ch1, -5e-2); + ASSERT_LT(rmse_ch1, 5e-2); + ASSERT_LT(error_mean_ch1, 5e-2); + ASSERT_GT(error_mean_ch1, -5e-2); + ASSERT_LT(error_var_ch1, 5e-2); + ASSERT_LT(max_error_ch1, 5e-2); + ASSERT_GT(min_error_ch1, -5e-2); } void HybridObservablesTest::check_results_code_psudorange( - arma::vec & true_ch0_dist_m, - arma::vec & true_ch1_dist_m, - arma::vec & true_ch0_tow_s, - arma::vec & measuded_ch0_Pseudorange_m, - arma::vec & measuded_ch1_Pseudorange_m, - arma::vec & measuded_ch0_RX_time_s) + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1) { //1. True value interpolation to match the measurement times + double t0 = std::max(measured_ch0(0, 0), measured_ch1(0, 0)); + int size1 = measured_ch0.col(0).n_rows; + int size2 = measured_ch1.col(0).n_rows; + double t1 = std::min(measured_ch0(size1 - 1, 0), measured_ch1(size2 - 1, 0)); + arma::vec t = arma::linspace(t0, t1, floor((t1 - t0) * 1e3)); + arma::vec true_ch0_dist_interp; arma::vec true_ch1_dist_interp; - arma::interp1(true_ch0_tow_s, true_ch0_dist_m, measuded_ch0_RX_time_s, true_ch0_dist_interp); - arma::interp1(true_ch0_tow_s, true_ch1_dist_m, measuded_ch0_RX_time_s, true_ch1_dist_interp); + arma::interp1(true_tow_s, true_ch0.col(1), t, true_ch0_dist_interp); + arma::interp1(true_tow_s, true_ch1.col(1), t, true_ch1_dist_interp); + + arma::vec meas_ch0_dist_interp; + arma::vec meas_ch1_dist_interp; + arma::interp1(measured_ch0.col(0), measured_ch0.col(4), t, meas_ch0_dist_interp); + arma::interp1(measured_ch1.col(0), measured_ch1.col(4), t, meas_ch1_dist_interp); // generate delta pseudoranges - arma::vec delta_true_dist_m = true_ch0_dist_interp-true_ch1_dist_interp; - arma::vec delta_measured_dist_m = measuded_ch0_Pseudorange_m-measuded_ch1_Pseudorange_m; + arma::vec delta_true_dist_m = true_ch0_dist_interp - true_ch1_dist_interp; + arma::vec delta_measured_dist_m = meas_ch0_dist_interp - meas_ch1_dist_interp; //2. RMSE arma::vec err; @@ -417,20 +434,20 @@ void HybridObservablesTest::check_results_code_psudorange( //5. report std::streamsize ss = std::cout.precision(); - std::cout << std::setprecision(10) << "Delta Observables RMSE=" - << rmse << ", mean=" << error_mean - << ", stdev=" << sqrt(error_var) - << " (max,min)=" << max_error + std::cout << std::setprecision(10) << "Delta Observables RMSE = " + << rmse << ", mean = " << error_mean + << ", stdev = " << sqrt(error_var) + << " (max,min) = " << max_error << "," << min_error << " [meters]" << std::endl; std::cout.precision (ss); - ASSERT_LT(rmse, 0.5); - ASSERT_LT(error_mean, 0.5); + ASSERT_LT(rmse, 0.5); + ASSERT_LT(error_mean, 0.5); ASSERT_GT(error_mean, -0.5); - ASSERT_LT(error_var, 0.5); - ASSERT_LT(max_error, 2); - ASSERT_GT(min_error, -2); + ASSERT_LT(error_var, 0.5); + ASSERT_LT(max_error, 2.0); + ASSERT_GT(min_error, -2.0); } @@ -440,7 +457,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults) configure_generator(); // Generate signal raw signal samples and observations RINEX file - if (FLAGS_disable_generator==false) + if (FLAGS_disable_generator == false) { generate_signal(); } @@ -478,9 +495,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults) top_block = gr::make_top_block("Telemetry_Decoder test"); std::shared_ptr tracking_ch0 = std::make_shared(config.get(), "Tracking_1C", 1, 1); - //std::shared_ptr tracking_ch1 = std::make_shared(config.get(), "Tracking_1C", 1, 1); std::shared_ptr tracking_ch1 = std::make_shared(config.get(), "Tracking_1C", 1, 1); - //std::shared_ptr tracking_ch1 = std::make_shared(config.get(), "Tracking_1C", 1, 1); boost::shared_ptr msg_rx_ch0 = HybridObservablesTest_msg_rx_make(); boost::shared_ptr msg_rx_ch1 = HybridObservablesTest_msg_rx_make(); @@ -524,15 +539,15 @@ TEST_F(HybridObservablesTest, ValidationOfResults) tlm_ch0->set_channel(0); tlm_ch1->set_channel(1); - tlm_ch0->set_satellite(Gnss_Satellite(std::string("GPS"),gnss_synchro_ch0.PRN)); - tlm_ch1->set_satellite(Gnss_Satellite(std::string("GPS"),gnss_synchro_ch1.PRN)); + tlm_ch0->set_satellite(Gnss_Satellite(std::string("GPS"), gnss_synchro_ch0.PRN)); + tlm_ch1->set_satellite(Gnss_Satellite(std::string("GPS"), gnss_synchro_ch1.PRN)); }) << "Failure setting gnss_synchro."; boost::shared_ptr tlm_msg_rx_ch1 = HybridObservablesTest_tlm_msg_rx_make(); boost::shared_ptr tlm_msg_rx_ch2 = HybridObservablesTest_tlm_msg_rx_make(); //Observables - std::shared_ptr observables(new HybridObservables(config.get(), "Observables",2, 2)); + std::shared_ptr observables(new HybridObservables(config.get(), "Observables",3, 2)); ASSERT_NO_THROW( { tracking_ch0->set_channel(gnss_synchro_ch0.Channel_ID); @@ -556,7 +571,10 @@ TEST_F(HybridObservablesTest, ValidationOfResults) gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make(); gr::blocks::null_sink::sptr sink_ch0 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); gr::blocks::null_sink::sptr sink_ch1 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); + gnss_sdr_sample_counter_sptr samp_counter = gnss_sdr_make_sample_counter(static_cast(baseband_sampling_freq)); top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0); + top_block->connect(gr_interleaved_char_to_complex, 0, samp_counter, 0); + //ch0 top_block->connect(gr_interleaved_char_to_complex, 0, tracking_ch0->get_left_block(), 0); top_block->connect(tracking_ch0->get_right_block(), 0, tlm_ch0->get_left_block(), 0); @@ -570,6 +588,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults) top_block->connect(observables->get_right_block(), 0, sink_ch0, 0); top_block->connect(observables->get_right_block(), 1, sink_ch1, 0); + top_block->connect(samp_counter, 0, observables->get_left_block(), 2); }) << "Failure connecting the blocks."; @@ -589,48 +608,43 @@ TEST_F(HybridObservablesTest, ValidationOfResults) true_observables_reader true_observables; ASSERT_NO_THROW({ - if ( true_observables.open_obs_file(std::string("./obs_out.bin")) == false) - { - throw std::exception(); - }; + if(true_observables.open_obs_file(std::string("./obs_out.bin")) == false) + { + throw std::exception(); + } }) << "Failure opening true observables file"; - long int nepoch = true_observables.num_epochs(); + unsigned int nepoch = static_cast(true_observables.num_epochs()); - std::cout << "True observation epochs=" << nepoch << std::endl; - arma::vec true_ch0_dist_m = arma::zeros(nepoch, 1); - arma::vec true_ch0_acc_carrier_phase_cycles = arma::zeros(nepoch, 1); - arma::vec true_ch0_Doppler_Hz = arma::zeros(nepoch, 1); - arma::vec true_ch0_tow_s = arma::zeros(nepoch, 1); - arma::vec true_ch1_dist_m = arma::zeros(nepoch, 1); - arma::vec true_ch1_acc_carrier_phase_cycles = arma::zeros(nepoch, 1); - arma::vec true_ch1_Doppler_Hz = arma::zeros(nepoch, 1); - arma::vec true_ch1_tow_s = arma::zeros(nepoch, 1); + std::cout << "True observation epochs = " << nepoch << std::endl; + // Matrices for storing columnwise true GPS time, Range, Doppler and Carrier phase + arma::mat true_ch0 = arma::zeros(nepoch, 4); + arma::mat true_ch1 = arma::zeros(nepoch, 4); true_observables.restart(); long int epoch_counter = 0; ASSERT_NO_THROW({ while(true_observables.read_binary_obs()) { - if (round(true_observables.prn[0])!=gnss_synchro_ch0.PRN) + if(round(true_observables.prn[0]) != gnss_synchro_ch0.PRN) { - std::cout<<"True observables SV PRN do not match"< 0) and index(0) < (nepoch - 1)) + { measured_ch0.shed_rows(index(0) + 1, nepoch - 1); } + index = arma::find(measured_ch1.col(0) > 0.0, 1, "last"); + if((index.size() > 0) and index(0) < (nepoch - 1)) + { measured_ch1.shed_rows(index(0) + 1, nepoch - 1); } + //Cut measurement initial transitory of the measurements - arma::uvec initial_meas_point = arma::find(measuded_ch0_RX_time_s >= true_ch0_tow_s(0), 1, "first"); + index = arma::find(measured_ch0.col(0) >= true_ch0(0, 0), 1, "first"); + if((index.size() > 0) and (index(0) > 0)) + { measured_ch0.shed_rows(0, index(0)); } + index = arma::find(measured_ch1.col(0) >= true_ch1(0, 0), 1, "first"); + if((index.size() > 0) and (index(0) > 0)) + { measured_ch1.shed_rows(0, index(0)); } - measuded_ch0_RX_time_s = measuded_ch0_RX_time_s.subvec(initial_meas_point(0), measuded_ch0_RX_time_s.size() - 1); - measuded_ch0_Pseudorange_m = measuded_ch0_Pseudorange_m.subvec(initial_meas_point(0), measuded_ch0_Pseudorange_m.size() - 1); - measuded_ch0_Acc_carrier_phase_hz = measuded_ch0_Acc_carrier_phase_hz.subvec(initial_meas_point(0), measuded_ch0_Acc_carrier_phase_hz.size() - 1); - - measuded_ch1_RX_time_s = measuded_ch1_RX_time_s.subvec(initial_meas_point(0), measuded_ch1_RX_time_s.size() - 1); - measuded_ch1_Pseudorange_m = measuded_ch1_Pseudorange_m.subvec(initial_meas_point(0), measuded_ch1_Pseudorange_m.size() - 1); - measuded_ch1_Acc_carrier_phase_hz = measuded_ch1_Acc_carrier_phase_hz.subvec(initial_meas_point(0), measuded_ch1_Acc_carrier_phase_hz.size() - 1); - - //correct the clock error using true values (it is not possible for a receiver to correct + //Correct the clock error using true values (it is not possible for a receiver to correct //the receiver clock offset error at the observables level because it is required the //decoding of the ephemeris data and solve the PVT equations) - //find the reference satellite and compute the receiver time offset at obsevable level + //Find the reference satellite (the nearest) and compute the receiver time offset at observable level arma::vec receiver_time_offset_s; - if (measuded_ch0_Pseudorange_m(0) Date: Thu, 1 Mar 2018 15:54:51 +0100 Subject: [PATCH 29/63] Check data validity --- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 559caafdc..d8b3a5e34 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -290,6 +290,7 @@ int hybrid_observables_cc::save_matfile() bool hybrid_observables_cc::interpolate_data(Gnss_Synchro& out, std::deque& data, const double& ti) { + if((ti < data.front().RX_time) or (ti > data.back().RX_time)) { return false; } std::deque::iterator it; arma::vec t = arma::vec(data.size()); From e58f96a196c6ed19e3443c8602c05ef73e4b7021 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 1 Mar 2018 17:23:48 +0100 Subject: [PATCH 30/63] Minor change --- src/algorithms/observables/adapters/hybrid_observables.cc | 2 -- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 3 +-- .../gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc | 4 ++-- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/algorithms/observables/adapters/hybrid_observables.cc b/src/algorithms/observables/adapters/hybrid_observables.cc index c95866a09..64c61522b 100644 --- a/src/algorithms/observables/adapters/hybrid_observables.cc +++ b/src/algorithms/observables/adapters/hybrid_observables.cc @@ -32,8 +32,6 @@ #include "hybrid_observables.h" #include "configuration_interface.h" -#include "Galileo_E1.h" -#include "GPS_L1_CA.h" #include diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index d8b3a5e34..fa4ba397c 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -472,7 +472,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) { if(valid_channels[i]) { - Gnss_Synchro interpolated_gnss_synchro; + Gnss_Synchro interpolated_gnss_synchro = it->back(); if(interpolate_data(interpolated_gnss_synchro, *it, T_rx_s_out)) { epoch_data.push_back(interpolated_gnss_synchro); @@ -486,7 +486,6 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) } d_num_valid_channels = valid_channels.count(); if(d_num_valid_channels == 0) { return 0; } - correct_TOW_and_compute_prange(epoch_data); std::vector::iterator it2 = epoch_data.begin(); for(i = 0; i < d_nchannels; i++) 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 df6cbb6d8..b28064ee3 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 @@ -206,7 +206,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ else if (d_stat == 1) //check 6 seconds of preamble separation { preamble_diff_ms = round(((static_cast(d_symbol_history.at(0).Tracking_sample_counter) - d_preamble_time_samples) / static_cast(d_symbol_history.at(0).fs)) * 1000.0); - if (abs(preamble_diff_ms - GPS_SUBFRAME_MS) == 0) + if (abs(preamble_diff_ms - GPS_SUBFRAME_MS) < 1) { DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite; d_GPS_FSM.Event_gps_word_preamble(); @@ -351,7 +351,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ // /(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 + 1.0 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; + 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; From b3c6d94cb6d4820da3cf88692b04920da17dcb0c Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 2 Mar 2018 10:29:43 +0100 Subject: [PATCH 31/63] debug1 --- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 69f093703..13ec3434b 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -54,6 +54,7 @@ #include "rtklib_ephemeris.h" #include "rtklib_ionex.h" #include "rtklib_sbas.h" +#include /* pseudorange measurement error variance ------------------------------------*/ double varerr(const prcopt_t *opt, double el, int sys) @@ -163,7 +164,9 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } else if((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE)) { - P2 += P2_C2; /* C2->P2 */ + std::cout << "P1_P2 = " << P1_P2 << std::endl; + std::cout << "P2_C2 = " << P2_C2 << std::endl; + P2 += P2_C2; /* C2->P2 */ PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); } /* dual-frequency */ From a216d2250970c1684821034ac60b21a72a67b5f1 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 2 Mar 2018 11:22:46 +0100 Subject: [PATCH 32/63] debug2 --- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 2 -- src/core/system_parameters/gps_cnav_navigation_message.cc | 3 +++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 13ec3434b..07267c427 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -164,8 +164,6 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } else if((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE)) { - std::cout << "P1_P2 = " << P1_P2 << std::endl; - std::cout << "P2_C2 = " << P2_C2 << std::endl; P2 += P2_C2; /* C2->P2 */ PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); } diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index 1bdc48360..c4f49f849 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -258,6 +258,9 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_signed(data_bits, CNAV_TGD)); + //Check if the grup delay value is not available. See IS-GPS-200, Table 30-IV. + //Bit string "1000000000000" is -4096 in 2 complement + if(ephemeris_record.d_TGD < -4095.9) { ephemeris_record.d_TGD = 0.0; } ephemeris_record.d_TGD = ephemeris_record.d_TGD * CNAV_TGD_LSB; ephemeris_record.d_ISCL1 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL1)); ephemeris_record.d_ISCL1 = ephemeris_record.d_ISCL1 * CNAV_ISCL1_LSB; From 3ce49ada96a293cd59b4b0557a55b767a5005402 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 2 Mar 2018 13:30:21 +0100 Subject: [PATCH 33/63] debug 3 --- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 143 ++++++++++++-------- 1 file changed, 83 insertions(+), 60 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 07267c427..01f6f7b4b 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -54,7 +54,6 @@ #include "rtklib_ephemeris.h" #include "rtklib_ionex.h" #include "rtklib_sbas.h" -#include /* pseudorange measurement error variance ------------------------------------*/ double varerr(const prcopt_t *opt, double el, int sys) @@ -91,47 +90,52 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, *var = 0.0; if (!(sys = satsys(obs->sat, NULL))) - { - trace(4, "prange: satsys NULL\n"); - return 0.0; - } + { + trace(4, "prange: satsys NULL\n"); + return 0.0; + } /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ if (sys & (SYS_GAL | SYS_SBS)) {j = 2;} if (sys == SYS_GPS) - { - if(obs->code[1] != CODE_NONE) {j = 1;} - else if(obs->code[2] != CODE_NONE) {j = 2;} - } + { + if(obs->code[1] != CODE_NONE) {j = 1;} + else if(obs->code[2] != CODE_NONE) {j = 2;} + } if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0) - { - trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); - printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]); - return 0.0; - } + { + trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); + printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]); + return 0.0; + } /* test snr mask */ if (iter > 0) + { + if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) { - if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) - { - trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n", - time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25); - return 0.0; - } - if (opt->ionoopt == IONOOPT_IFLC) - { - if (testsnr(0, j, azel[1], obs->SNR[j] * 0.25, &opt->snrmask)) - { - trace(4, "prange: testsnr error\n"); - return 0.0; - } - } + trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n", + time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25); + return 0.0; } - gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); /* f1^2/f2^2 */ + if (opt->ionoopt == IONOOPT_IFLC) + { + if (testsnr(0, j, azel[1], obs->SNR[j] * 0.25, &opt->snrmask)) + { + trace(4, "prange: testsnr error\n"); + return 0.0; + } + } + } + /* fL1^2 / fL2(orL5)^2 . See IS-GPS-200, p. 103 and Galileo ICD p. 48 */ + if(sys == SYS_GPS or sys == SYS_GAL) + { + gamma_ = std::pow(lam[i], 2.0) / std::pow(lam[j], 2.0); + } + P1 = obs->P[i]; P2 = obs->P[j]; P1_P2 = nav->cbias[obs->sat-1][0]; @@ -139,42 +143,61 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, P2_C2 = nav->cbias[obs->sat-1][2]; /* if no P1-P2 DCB, use TGD instead */ - if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) //CHECK! - { - P1_P2 = (1.0 - gamma_) * gettgd(obs->sat, nav); - } + if(P1_P2 == 0.0 and sys == SYS_GPS) + { + P1_P2 = (gamma_ - 1.0) * gettgd(obs->sat, nav); + } + else if(P1_P2 == 0.0 and sys == SYS_GAL) + { + //TODO + } + if (opt->ionoopt == IONOOPT_IFLC) - { /* dual-frequency */ + { /* dual-frequency */ - if (P1 == 0.0 || P2 == 0.0) { return 0.0; } - if (obs->code[i] == CODE_L1C) { P1 += P1_C1; } /* C1->P1 */ - if (obs->code[j] == CODE_L2C) { P2 += P2_C2; } /* C2->P2 */ + if (P1 == 0.0 || P2 == 0.0) { return 0.0; } + if (obs->code[i] == CODE_L1C) { P1 += P1_C1; } /* C1->P1 */ + if (obs->code[j] == CODE_L2C) { P2 += P2_C2; } /* C2->P2 */ - /* iono-free combination */ - PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); - } + /* iono-free combination */ + PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); + } else - { /* single-frequency */ - if((obs->code[i] == CODE_NONE) && (obs->code[j] == CODE_NONE)) { return 0.0; } + { /* single-frequency */ + if((obs->code[i] == CODE_NONE) && (obs->code[j] == CODE_NONE)) { return 0.0; } - else if((obs->code[i] != CODE_NONE) && (obs->code[j] == CODE_NONE)) - { - P1 += P1_C1; /* C1->P1 */ - PC = P1 - P1_P2 / (1.0 - gamma_); - } - else if((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE)) - { - P2 += P2_C2; /* C2->P2 */ - PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); - } - /* dual-frequency */ - else - { - P1 += P1_C1; - P2 += P2_C2; - PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); - } - } + else if((obs->code[i] != CODE_NONE) && (obs->code[j] == CODE_NONE)) + {//CHECK!! + P1 += P1_C1; /* C1->P1 */ + PC = P1 - P1_P2 / (1.0 - gamma_); + } + else if((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE)) + { + if(sys == SYS_GPS) + {//CHECK!! + P2 += P2_C2; /* C2->P2 */ + PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); + } + else if(sys == SYS_GAL) + { + //TODO + } + } + /* dual-frequency */ + else + { + if(sys == SYS_GPS) /* See IS-GPS-200 p. 179 */ + { + P1 += P1_C1; + P2 += P2_C2; + PC = (P2 - gamma_ * P1) / (1 - gamma_) - P1_P2 / (gamma_ - 1); + } + else if(sys == SYS_GAL) + { + //TODO + } + } + } if (opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; } /* sbas clock based C1 */ *var = std::pow(ERR_CBIAS, 2.0); From 764656831453e5633654ac6e024070be0e2692f1 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 5 Mar 2018 11:05:58 +0100 Subject: [PATCH 34/63] debug4 --- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 43 +++++++++++++++------ 1 file changed, 31 insertions(+), 12 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 01f6f7b4b..3b951189f 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -54,6 +54,8 @@ #include "rtklib_ephemeris.h" #include "rtklib_ionex.h" #include "rtklib_sbas.h" +#include +#include /* pseudorange measurement error variance ------------------------------------*/ double varerr(const prcopt_t *opt, double el, int sys) @@ -84,8 +86,16 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int iter, const prcopt_t *opt, double *var) { const double *lam = nav->lam[obs->sat - 1]; - double PC, P1, P2, P1_P2, P1_C1, P2_C2, gamma_; - int i = 0, j = 1, sys; + double PC; + double P1; + double P2; + double P1_P2; + double P1_C1; + double P2_C2; + double gamma_ = 0.0; + int i = 0; + int j = 1; + int sys; *var = 0.0; @@ -133,15 +143,22 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, /* fL1^2 / fL2(orL5)^2 . See IS-GPS-200, p. 103 and Galileo ICD p. 48 */ if(sys == SYS_GPS or sys == SYS_GAL) { - gamma_ = std::pow(lam[i], 2.0) / std::pow(lam[j], 2.0); + gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); } - P1 = obs->P[i]; P2 = obs->P[j]; P1_P2 = nav->cbias[obs->sat-1][0]; P1_C1 = nav->cbias[obs->sat-1][1]; P2_C2 = nav->cbias[obs->sat-1][2]; - + std::string d_dump_filename = "/home/antonio/data/dump_prange.dat"; + std::ofstream d_file; + d_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); + d_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + d_file.write(reinterpret_cast(&P1), sizeof(double)); + d_file.write(reinterpret_cast(&P2), sizeof(double)); + d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); + d_file.write(reinterpret_cast(&P1_C1), sizeof(double)); + d_file.write(reinterpret_cast(&P2_C2), sizeof(double)); /* if no P1-P2 DCB, use TGD instead */ if(P1_P2 == 0.0 and sys == SYS_GPS) { @@ -151,7 +168,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { //TODO } - + d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); if (opt->ionoopt == IONOOPT_IFLC) { /* dual-frequency */ @@ -166,17 +183,18 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { /* single-frequency */ if((obs->code[i] == CODE_NONE) && (obs->code[j] == CODE_NONE)) { return 0.0; } - else if((obs->code[i] != CODE_NONE) && (obs->code[j] == CODE_NONE)) + else if((obs->code[i] != CODE_NONE) and (obs->code[j] == CODE_NONE)) {//CHECK!! P1 += P1_C1; /* C1->P1 */ - PC = P1 - P1_P2 / (1.0 - gamma_); + PC = P1 + P1_P2 / (gamma_ - 1.0); } - else if((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE)) + else if((obs->code[i] == CODE_NONE) and (obs->code[j] != CODE_NONE)) { if(sys == SYS_GPS) {//CHECK!! P2 += P2_C2; /* C2->P2 */ - PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); + //PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); + PC = P2 + gamma_ * P1_P2 / (gamma_ - 1.0); } else if(sys == SYS_GAL) { @@ -190,7 +208,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { P1 += P1_C1; P2 += P2_C2; - PC = (P2 - gamma_ * P1) / (1 - gamma_) - P1_P2 / (gamma_ - 1); + PC = (P2 - gamma_ * P1) / (1.0 - gamma_) - P1_P2 / (gamma_ - 1.0); } else if(sys == SYS_GAL) { @@ -198,10 +216,11 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } } } + d_file.write(reinterpret_cast(&PC), sizeof(double)); if (opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; } /* sbas clock based C1 */ *var = std::pow(ERR_CBIAS, 2.0); - + d_file.close(); return PC; } From 8d37013e96672672d56fdb1c31e26e25ccf85285 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 5 Mar 2018 13:07:41 +0100 Subject: [PATCH 35/63] debug 5 --- src/algorithms/libs/rtklib/rtklib.h | 1 + .../libs/rtklib/rtklib_conversions.cc | 16 +++-- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 66 ++++++++++++++++++- src/algorithms/libs/rtklib/rtklib_pntpos.h | 6 ++ src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 2 +- 5 files changed, 82 insertions(+), 9 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index 8d172b171..162d445c6 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -464,6 +464,7 @@ typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */ /* GPS/QZS:tgd[0]=TGD */ /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ + double isc[4]; /* GPS :isc[0]=ISCL1, isc[1]=ISCL2, isc[2]=ISCL5I, isc[3]=ISCL5Q */ double Adot,ndot; /* Adot,ndot for CNAV */ } eph_t; diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 7f1d933a6..c0bf8929c 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -117,7 +117,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris & glonass_gnav_eph, const Glon eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; //Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris rtklib_sat.sat = gal_eph.i_satellite_PRN+NSATGPS+NSATGLO; rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1; @@ -167,7 +167,7 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph) eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; rtklib_sat.sat = gps_eph.i_satellite_PRN; rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A; rtklib_sat.M0 = gps_eph.d_M_0; @@ -216,7 +216,7 @@ eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph) eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris & gps_cnav_eph) { eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN; const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170 rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A; @@ -245,9 +245,13 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris & gps_cnav_eph) rtklib_sat.f1 = gps_cnav_eph.d_A_f1; rtklib_sat.f2 = gps_cnav_eph.d_A_f2; rtklib_sat.tgd[0] = gps_cnav_eph.d_TGD; - rtklib_sat.tgd[1] = 0; - rtklib_sat.tgd[2] = 0; - rtklib_sat.tgd[3] = 0; + rtklib_sat.tgd[1] = 0.0; + rtklib_sat.tgd[2] = 0.0; + rtklib_sat.tgd[3] = 0.0; + rtklib_sat.isc[0] = gps_cnav_eph.d_ISCL1; + rtklib_sat.isc[1] = gps_cnav_eph.d_ISCL2; + rtklib_sat.isc[2] = gps_cnav_eph.d_ISCL5I; + rtklib_sat.isc[3] = gps_cnav_eph.d_ISCL5Q; rtklib_sat.toes = gps_cnav_eph.d_Toe1; rtklib_sat.toc = gpst2time(rtklib_sat.week,gps_cnav_eph.d_Toc); rtklib_sat.ttr = gpst2time(rtklib_sat.week,gps_cnav_eph.d_TOW); diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 3b951189f..8dccfe679 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -80,6 +80,50 @@ double gettgd(int sat, const nav_t *nav) return 0.0; } +/* get isc parameter (m) -----------------------------------------------------*/ +double getiscl1(int sat, const nav_t *nav) +{ + int i; + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[0]; + } + return 0.0; +} + +double getiscl2(int sat, const nav_t *nav) +{ + int i; + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[1]; + } + return 0.0; +} + +double getiscl5i(int sat, const nav_t *nav) +{ + int i; + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[2]; + } + return 0.0; +} + +double getiscl5q(int sat, const nav_t *nav) +{ + int i; + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].isc[3]; + } + return 0.0; +} /* psendorange with code bias correction -------------------------------------*/ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, @@ -92,6 +136,10 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, double P1_P2; double P1_C1; double P2_C2; + double ISCl1 = 0.0; + double ISCl2 = 0.0; + double ISCl5i = 0.0; + double ISCl5q = 0.0; double gamma_ = 0.0; int i = 0; int j = 1; @@ -150,10 +198,13 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, P1_P2 = nav->cbias[obs->sat-1][0]; P1_C1 = nav->cbias[obs->sat-1][1]; P2_C2 = nav->cbias[obs->sat-1][2]; + std::string d_dump_filename = "/home/antonio/data/dump_prange.dat"; std::ofstream d_file; d_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + d_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary | std::ios::app); + double tmp_double = static_cast(obs->sat); + d_file.write(reinterpret_cast(&tmp_double), sizeof(double)); d_file.write(reinterpret_cast(&P1), sizeof(double)); d_file.write(reinterpret_cast(&P2), sizeof(double)); d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); @@ -168,6 +219,14 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { //TODO } + + if(sys == SYS_GPS) + { + ISCl1 = getiscl1(obs->sat, nav); + ISCl2 = getiscl2(obs->sat, nav); + ISCl5i = getiscl5i(obs->sat, nav); + ISCl5q = getiscl5q(obs->sat, nav); + } d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); if (opt->ionoopt == IONOOPT_IFLC) { /* dual-frequency */ @@ -208,7 +267,10 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { P1 += P1_C1; P2 += P2_C2; - PC = (P2 - gamma_ * P1) / (1.0 - gamma_) - P1_P2 / (gamma_ - 1.0); + if(obs->code[j] == CODE_L2S) + { + PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2 / (gamma_ - 1.0); + } } else if(sys == SYS_GAL) { diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.h b/src/algorithms/libs/rtklib/rtklib_pntpos.h index 5dd969943..ad53558fa 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.h +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.h @@ -69,6 +69,12 @@ double varerr(const prcopt_t *opt, double el, int sys); /* get tgd parameter (m) -----------------------------------------------------*/ double gettgd(int sat, const nav_t *nav); +/* get isc parameter (m) -----------------------------------------------------*/ +double getiscl1(int sat, const nav_t *nav); +double getiscl2(int sat, const nav_t *nav); +double getiscl5i(int sat, const nav_t *nav); +double getiscl5q(int sat, const nav_t *nav); + /* psendorange with code bias correction -------------------------------------*/ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int iter, const prcopt_t *opt, double *var); diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index a9c74ab55..60b95594b 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -2825,7 +2825,7 @@ int readnav(const char *file, nav_t *nav) { FILE *fp; eph_t eph0 = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; geph_t geph0 = {0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {}, {}, {}, 0.0, 0.0, 0.0}; char buff[4096], *p; long toe_time, tof_time, toc_time, ttr_time; From a25a6fb556bdd5d7d93100648ec8b2ca863bc6a0 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 5 Mar 2018 18:34:20 +0100 Subject: [PATCH 36/63] debug 6 --- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 81 ++++++++++--------- .../gps_cnav_navigation_message.cc | 2 + 2 files changed, 43 insertions(+), 40 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 8dccfe679..5f1e17156 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -83,8 +83,7 @@ double gettgd(int sat, const nav_t *nav) /* get isc parameter (m) -----------------------------------------------------*/ double getiscl1(int sat, const nav_t *nav) { - int i; - for (i = 0; i < nav->n; i++) + for (int i = 0; i < nav->n; i++) { if (nav->eph[i].sat != sat) continue; return SPEED_OF_LIGHT * nav->eph[i].isc[0]; @@ -94,8 +93,7 @@ double getiscl1(int sat, const nav_t *nav) double getiscl2(int sat, const nav_t *nav) { - int i; - for (i = 0; i < nav->n; i++) + for (int i = 0; i < nav->n; i++) { if (nav->eph[i].sat != sat) continue; return SPEED_OF_LIGHT * nav->eph[i].isc[1]; @@ -105,8 +103,7 @@ double getiscl2(int sat, const nav_t *nav) double getiscl5i(int sat, const nav_t *nav) { - int i; - for (i = 0; i < nav->n; i++) + for (int i = 0; i < nav->n; i++) { if (nav->eph[i].sat != sat) continue; return SPEED_OF_LIGHT * nav->eph[i].isc[2]; @@ -116,8 +113,7 @@ double getiscl5i(int sat, const nav_t *nav) double getiscl5q(int sat, const nav_t *nav) { - int i; - for (i = 0; i < nav->n; i++) + for (int i = 0; i < nav->n; i++) { if (nav->eph[i].sat != sat) continue; return SPEED_OF_LIGHT * nav->eph[i].isc[3]; @@ -129,13 +125,13 @@ double getiscl5q(int sat, const nav_t *nav) double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int iter, const prcopt_t *opt, double *var) { - const double *lam = nav->lam[obs->sat - 1]; - double PC; - double P1; - double P2; - double P1_P2; - double P1_C1; - double P2_C2; + const double* lam = nav->lam[obs->sat - 1]; + double PC = 0.0; + double P1 = 0.0; + double P2 = 0.0; + double P1_P2 = 0.0; + double P1_C1 = 0.0; + double P2_C2 = 0.0; double ISCl1 = 0.0; double ISCl2 = 0.0; double ISCl5i = 0.0; @@ -143,11 +139,10 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, double gamma_ = 0.0; int i = 0; int j = 1; - int sys; - + int sys = satsys(obs->sat, NULL); *var = 0.0; - if (!(sys = satsys(obs->sat, NULL))) + if(sys == SYS_NONE) { trace(4, "prange: satsys NULL\n"); return 0.0; @@ -155,15 +150,15 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ - if (sys & (SYS_GAL | SYS_SBS)) {j = 2;} + if(sys == SYS_GAL or sys == SYS_SBS) {j = 2;} - if (sys == SYS_GPS) + if(sys == SYS_GPS) { if(obs->code[1] != CODE_NONE) {j = 1;} else if(obs->code[2] != CODE_NONE) {j = 2;} } - if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0) + if(lam[i] == 0.0 or lam[j] == 0.0) { trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]); @@ -171,7 +166,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } /* test snr mask */ - if (iter > 0) + if(iter > 0) { if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) { @@ -226,9 +221,15 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, ISCl2 = getiscl2(obs->sat, nav); ISCl5i = getiscl5i(obs->sat, nav); ISCl5q = getiscl5q(obs->sat, nav); + d_file.write(reinterpret_cast(&ISCl1), sizeof(double)); + d_file.write(reinterpret_cast(&ISCl2), sizeof(double)); + d_file.write(reinterpret_cast(&ISCl5i), sizeof(double)); + d_file.write(reinterpret_cast(&ISCl5q), sizeof(double)); } d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); - if (opt->ionoopt == IONOOPT_IFLC) + + //CHECK IF IT IS STILL NEEDED + if(opt->ionoopt == IONOOPT_IFLC) { /* dual-frequency */ if (P1 == 0.0 || P2 == 0.0) { return 0.0; } @@ -238,22 +239,23 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, /* iono-free combination */ PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); } + //////////////////////////////////////////// else { /* single-frequency */ - if((obs->code[i] == CODE_NONE) && (obs->code[j] == CODE_NONE)) { return 0.0; } + if(obs->code[i] == CODE_NONE and obs->code[j] == CODE_NONE) { return 0.0; } - else if((obs->code[i] != CODE_NONE) and (obs->code[j] == CODE_NONE)) + else if(obs->code[i] != CODE_NONE and obs->code[j] == CODE_NONE) {//CHECK!! P1 += P1_C1; /* C1->P1 */ PC = P1 + P1_P2 / (gamma_ - 1.0); } - else if((obs->code[i] == CODE_NONE) and (obs->code[j] != CODE_NONE)) + else if(obs->code[i] == CODE_NONE and obs->code[j] != CODE_NONE) { if(sys == SYS_GPS) {//CHECK!! P2 += P2_C2; /* C2->P2 */ //PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); - PC = P2 + gamma_ * P1_P2 / (gamma_ - 1.0); + PC = P2 + P1_P2 / (gamma_ - 1.0) - ISCl2; } else if(sys == SYS_GAL) { @@ -261,28 +263,27 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } } /* dual-frequency */ - else + else if(sys == SYS_GPS) { - if(sys == SYS_GPS) /* See IS-GPS-200 p. 179 */ + if(obs->code[j] == CODE_L2S) /* L1 + L2 */ { - P1 += P1_C1; - P2 += P2_C2; - if(obs->code[j] == CODE_L2S) - { - PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2 / (gamma_ - 1.0); - } + //PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2 / (gamma_ - 1.0); + PC = (P1 + P2) / 2.0; } - else if(sys == SYS_GAL) + if(obs->code[j] == CODE_L5X) /* L1 + L5 */ { - //TODO + } } + else if(sys == SYS_GAL) /* E1 + E5a */ + { + + } } d_file.write(reinterpret_cast(&PC), sizeof(double)); - if (opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; } /* sbas clock based C1 */ - - *var = std::pow(ERR_CBIAS, 2.0); d_file.close(); + if(opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; } /* sbas clock based C1 */ + *var = std::pow(ERR_CBIAS, 2.0); return PC; } diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index c4f49f849..bc8507c89 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -32,6 +32,7 @@ #include "gps_cnav_navigation_message.h" #include "gnss_satellite.h" +#include void Gps_CNAV_Navigation_Message::reset() @@ -266,6 +267,7 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_signed(data_bits, CNAV_ISCL2)); ephemeris_record.d_ISCL2 = ephemeris_record.d_ISCL2 * CNAV_ISCL2_LSB; + std::cout << "ISCL2 * c = " << ephemeris_record.d_ISCL2 * 3e8 << " [m]" << std::endl; ephemeris_record.d_ISCL5I = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5I)); ephemeris_record.d_ISCL5I = ephemeris_record.d_ISCL5I * CNAV_ISCL5I_LSB; ephemeris_record.d_ISCL5Q = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5Q)); From 40be1e3ad4903d4d8e268d0b1e3c2a67ea495690 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 5 Mar 2018 18:45:18 +0100 Subject: [PATCH 37/63] debug7 --- src/core/system_parameters/GPS_CNAV.h | 4 ++-- .../gps_cnav_navigation_message.cc | 24 ++++++++++++------- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/core/system_parameters/GPS_CNAV.h b/src/core/system_parameters/GPS_CNAV.h index fc2239ab6..e703ea4cd 100644 --- a/src/core/system_parameters/GPS_CNAV.h +++ b/src/core/system_parameters/GPS_CNAV.h @@ -120,11 +120,11 @@ const std::vector > CNAV_URA_NED2({{58,3}}); const std::vector > CNAV_TOC({{61,11}}); const double CNAV_TOC_LSB = 300.0; const std::vector > CNAV_AF0({{72,26}}); -const double CNAV_AF0_LSB = TWO_N60; +const double CNAV_AF0_LSB = TWO_N35; const std::vector > CNAV_AF1({{98,20}}); const double CNAV_AF1_LSB = TWO_N48; const std::vector > CNAV_AF2({{118,10}}); -const double CNAV_AF2_LSB = TWO_N35; +const double CNAV_AF2_LSB = TWO_N60; const std::vector > CNAV_TGD({{128,13}}); const double CNAV_TGD_LSB = TWO_N35; const std::vector > CNAV_ISCL1({{141,13}}); diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index bc8507c89..7a1479ed0 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -32,7 +32,6 @@ #include "gps_cnav_navigation_message.h" #include "gnss_satellite.h" -#include void Gps_CNAV_Navigation_Message::reset() @@ -258,20 +257,27 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_signed(data_bits, CNAV_AF2)); ephemeris_record.d_A_f2 = ephemeris_record.d_A_f2 * CNAV_AF2_LSB; //group delays - ephemeris_record.d_TGD = static_cast(read_navigation_signed(data_bits, CNAV_TGD)); - //Check if the grup delay value is not available. See IS-GPS-200, Table 30-IV. + //Check if the grup delay values are not available. See IS-GPS-200, Table 30-IV. //Bit string "1000000000000" is -4096 in 2 complement + ephemeris_record.d_TGD = static_cast(read_navigation_signed(data_bits, CNAV_TGD)); if(ephemeris_record.d_TGD < -4095.9) { ephemeris_record.d_TGD = 0.0; } - ephemeris_record.d_TGD = ephemeris_record.d_TGD * CNAV_TGD_LSB; + ephemeris_record.d_TGD *= CNAV_TGD_LSB; + ephemeris_record.d_ISCL1 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL1)); - ephemeris_record.d_ISCL1 = ephemeris_record.d_ISCL1 * CNAV_ISCL1_LSB; + if(ephemeris_record.d_ISCL1 < -4095.9) { ephemeris_record.d_ISCL1 = 0.0; } + ephemeris_record.d_ISCL1 *= CNAV_ISCL1_LSB; + ephemeris_record.d_ISCL2 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL2)); - ephemeris_record.d_ISCL2 = ephemeris_record.d_ISCL2 * CNAV_ISCL2_LSB; - std::cout << "ISCL2 * c = " << ephemeris_record.d_ISCL2 * 3e8 << " [m]" << std::endl; + if(ephemeris_record.d_ISCL2 < -4095.9) { ephemeris_record.d_ISCL2 = 0.0; } + ephemeris_record.d_ISCL2 *= CNAV_ISCL2_LSB; + ephemeris_record.d_ISCL5I = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5I)); - ephemeris_record.d_ISCL5I = ephemeris_record.d_ISCL5I * CNAV_ISCL5I_LSB; + if(ephemeris_record.d_ISCL5I < -4095.9) { ephemeris_record.d_ISCL5I = 0.0; } + ephemeris_record.d_ISCL5I *= CNAV_ISCL5I_LSB; + ephemeris_record.d_ISCL5Q = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5Q)); - ephemeris_record.d_ISCL5Q = ephemeris_record.d_ISCL5Q * CNAV_ISCL5Q_LSB; + if(ephemeris_record.d_ISCL5Q < -4095.9) { ephemeris_record.d_ISCL5Q = 0.0; } + ephemeris_record.d_ISCL5Q *= CNAV_ISCL5Q_LSB; //iono iono_record.d_alpha0 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA0)); iono_record.d_alpha0 = iono_record.d_alpha0 * CNAV_ALPHA0_LSB; From 21fff4e696bbef5a7ac08a0908911a5b6c86b0e2 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 6 Mar 2018 13:23:22 +0100 Subject: [PATCH 38/63] Fix CNAV telemetry reader --- src/algorithms/PVT/libs/rtklib_solver.cc | 2 +- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 11 ++-- .../gps_cnav_navigation_message.cc | 50 +++++++++---------- 3 files changed, 31 insertions(+), 32 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 2067e42f8..84f636820 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -132,7 +132,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ for(gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); - gnss_observables_iter++) + gnss_observables_iter++) //CHECK INCONSISTENCY when combining GLONASS + other system { switch(gnss_observables_iter->second.System) { diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 5f1e17156..6ef2629b8 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -194,7 +194,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, P1_C1 = nav->cbias[obs->sat-1][1]; P2_C2 = nav->cbias[obs->sat-1][2]; - std::string d_dump_filename = "/home/antonio/data/dump_prange.dat"; + std::string d_dump_filename = "/home/aramos/dump_prange.dat"; std::ofstream d_file; d_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); d_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary | std::ios::app); @@ -208,7 +208,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, /* if no P1-P2 DCB, use TGD instead */ if(P1_P2 == 0.0 and sys == SYS_GPS) { - P1_P2 = (gamma_ - 1.0) * gettgd(obs->sat, nav); + P1_P2 = gettgd(obs->sat, nav); } else if(P1_P2 == 0.0 and sys == SYS_GAL) { @@ -247,7 +247,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, else if(obs->code[i] != CODE_NONE and obs->code[j] == CODE_NONE) {//CHECK!! P1 += P1_C1; /* C1->P1 */ - PC = P1 + P1_P2 / (gamma_ - 1.0); + PC = P1 + P1_P2; } else if(obs->code[i] == CODE_NONE and obs->code[j] != CODE_NONE) { @@ -255,7 +255,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, {//CHECK!! P2 += P2_C2; /* C2->P2 */ //PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); - PC = P2 + P1_P2 / (gamma_ - 1.0) - ISCl2; + PC = P2 + P1_P2 - ISCl2; } else if(sys == SYS_GAL) { @@ -267,8 +267,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { if(obs->code[j] == CODE_L2S) /* L1 + L2 */ { - //PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2 / (gamma_ - 1.0); - PC = (P1 + P2) / 2.0; + PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2; } if(obs->code[j] == CODE_L5X) /* L1 + L5 */ { diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index 7a1479ed0..c2cd29ddd 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -180,7 +180,7 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_unsigned(data_bits, CNAV_TOW)); - d_TOW = d_TOW * CNAV_TOW_LSB; + d_TOW *= CNAV_TOW_LSB; ephemeris_record.d_TOW = d_TOW; alert_flag = static_cast(read_navigation_bool(data_bits, CNAV_ALERT_FLAG)); @@ -194,24 +194,24 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_unsigned(data_bits, CNAV_WN)); ephemeris_record.i_signal_health = static_cast(read_navigation_unsigned(data_bits, CNAV_HEALTH)); ephemeris_record.d_Top = static_cast(read_navigation_unsigned(data_bits, CNAV_TOP1)); - ephemeris_record.d_Top = ephemeris_record.d_Top * CNAV_TOP1_LSB; + ephemeris_record.d_Top *= CNAV_TOP1_LSB; ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA)); ephemeris_record.d_Toe1 = static_cast(read_navigation_unsigned(data_bits, CNAV_TOE1)); - ephemeris_record.d_Toe1 = ephemeris_record.d_Toe1 * CNAV_TOE1_LSB; + ephemeris_record.d_Toe1 *= CNAV_TOE1_LSB; ephemeris_record.d_DELTA_A = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_A)); - ephemeris_record.d_DELTA_A = ephemeris_record.d_DELTA_A * CNAV_DELTA_A_LSB; + ephemeris_record.d_DELTA_A *= CNAV_DELTA_A_LSB; ephemeris_record.d_A_DOT = static_cast(read_navigation_signed(data_bits, CNAV_A_DOT)); - ephemeris_record.d_A_DOT = ephemeris_record.d_A_DOT * CNAV_A_DOT_LSB; + ephemeris_record.d_A_DOT *= CNAV_A_DOT_LSB; ephemeris_record.d_Delta_n = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0)); - ephemeris_record.d_Delta_n = ephemeris_record.d_Delta_n * CNAV_DELTA_N0_LSB; + ephemeris_record.d_Delta_n *= CNAV_DELTA_N0_LSB; ephemeris_record.d_DELTA_DOT_N = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0_DOT)); - ephemeris_record.d_DELTA_DOT_N = ephemeris_record.d_DELTA_DOT_N * CNAV_DELTA_N0_DOT_LSB; + ephemeris_record.d_DELTA_DOT_N *= CNAV_DELTA_N0_DOT_LSB; ephemeris_record.d_M_0 = static_cast(read_navigation_signed(data_bits, CNAV_M0)); - ephemeris_record.d_M_0 = ephemeris_record.d_M_0 * CNAV_M0_LSB; + ephemeris_record.d_M_0 *= CNAV_M0_LSB; ephemeris_record.d_e_eccentricity = static_cast(read_navigation_unsigned(data_bits, CNAV_E_ECCENTRICITY)); - ephemeris_record.d_e_eccentricity = ephemeris_record.d_e_eccentricity * CNAV_E_ECCENTRICITY_LSB; + ephemeris_record.d_e_eccentricity *= CNAV_E_ECCENTRICITY_LSB; ephemeris_record.d_OMEGA = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA)); - ephemeris_record.d_OMEGA = ephemeris_record.d_OMEGA * CNAV_OMEGA_LSB; + ephemeris_record.d_OMEGA *= CNAV_OMEGA_LSB; ephemeris_record.b_integrity_status_flag = static_cast(read_navigation_bool(data_bits, CNAV_INTEGRITY_FLAG)); ephemeris_record.b_l2c_phasing_flag = static_cast(read_navigation_bool(data_bits, CNAV_L2_PHASING_FLAG)); @@ -220,42 +220,42 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_unsigned(data_bits, CNAV_TOE2)); - ephemeris_record.d_Toe2 = ephemeris_record.d_Toe2 * CNAV_TOE2_LSB; + ephemeris_record.d_Toe2 *= CNAV_TOE2_LSB; ephemeris_record.d_OMEGA0 = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA0)); - ephemeris_record.d_OMEGA0 = ephemeris_record.d_OMEGA0 * CNAV_OMEGA0_LSB; + ephemeris_record.d_OMEGA0 *= CNAV_OMEGA0_LSB; ephemeris_record.d_DELTA_OMEGA_DOT = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_OMEGA_DOT)); - ephemeris_record.d_DELTA_OMEGA_DOT = ephemeris_record.d_DELTA_OMEGA_DOT * CNAV_DELTA_OMEGA_DOT_LSB; + ephemeris_record.d_DELTA_OMEGA_DOT *= CNAV_DELTA_OMEGA_DOT_LSB; ephemeris_record.d_i_0 = static_cast(read_navigation_signed(data_bits, CNAV_I0)); - ephemeris_record.d_i_0 = ephemeris_record.d_i_0 * CNAV_I0_LSB; + ephemeris_record.d_i_0 *= CNAV_I0_LSB; ephemeris_record.d_IDOT = static_cast(read_navigation_signed(data_bits, CNAV_I0_DOT)); - ephemeris_record.d_IDOT = ephemeris_record.d_IDOT * CNAV_I0_DOT_LSB; + ephemeris_record.d_IDOT *= CNAV_I0_DOT_LSB; ephemeris_record.d_Cis = static_cast(read_navigation_signed(data_bits, CNAV_CIS)); - ephemeris_record.d_Cis = ephemeris_record.d_Cis * CNAV_CIS_LSB; + ephemeris_record.d_Cis *= CNAV_CIS_LSB; ephemeris_record.d_Cic = static_cast(read_navigation_signed(data_bits, CNAV_CIC)); - ephemeris_record.d_Cic = ephemeris_record.d_Cic * CNAV_CIC_LSB; + ephemeris_record.d_Cic *= CNAV_CIC_LSB; ephemeris_record.d_Crs = static_cast(read_navigation_signed(data_bits, CNAV_CRS)); - ephemeris_record.d_Crs = ephemeris_record.d_Crs * CNAV_CRS_LSB; + ephemeris_record.d_Crs *= CNAV_CRS_LSB; ephemeris_record.d_Crc = static_cast(read_navigation_signed(data_bits, CNAV_CRC)); - ephemeris_record.d_Cic = ephemeris_record.d_Cic * CNAV_CRC_LSB; + ephemeris_record.d_Crc *= CNAV_CRC_LSB; ephemeris_record.d_Cus = static_cast(read_navigation_signed(data_bits, CNAV_CUS)); - ephemeris_record.d_Cus = ephemeris_record.d_Cus * CNAV_CUS_LSB; + ephemeris_record.d_Cus *= CNAV_CUS_LSB; ephemeris_record.d_Cuc = static_cast(read_navigation_signed(data_bits, CNAV_CUC)); - ephemeris_record.d_Cuc = ephemeris_record.d_Cuc * CNAV_CUS_LSB; + ephemeris_record.d_Cuc *= CNAV_CUC_LSB; b_flag_ephemeris_2 = true; break; case 30: // (CLOCK, IONO, GRUP DELAY) //clock ephemeris_record.d_Toc = static_cast(read_navigation_unsigned(data_bits, CNAV_TOC)); - ephemeris_record.d_Toc = ephemeris_record.d_Toc * CNAV_TOC_LSB; + ephemeris_record.d_Toc *= CNAV_TOC_LSB; ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA_NED0)); ephemeris_record.d_URA1 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED1)); ephemeris_record.d_URA2 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED2)); ephemeris_record.d_A_f0 = static_cast(read_navigation_signed(data_bits, CNAV_AF0)); - ephemeris_record.d_A_f0 = ephemeris_record.d_A_f0 * CNAV_AF0_LSB; + ephemeris_record.d_A_f0 *= CNAV_AF0_LSB; ephemeris_record.d_A_f1 = static_cast(read_navigation_signed(data_bits, CNAV_AF1)); - ephemeris_record.d_A_f1 = ephemeris_record.d_A_f1 * CNAV_AF1_LSB; + ephemeris_record.d_A_f1 *= CNAV_AF1_LSB; ephemeris_record.d_A_f2 = static_cast(read_navigation_signed(data_bits, CNAV_AF2)); - ephemeris_record.d_A_f2 = ephemeris_record.d_A_f2 * CNAV_AF2_LSB; + ephemeris_record.d_A_f2 *= CNAV_AF2_LSB; //group delays //Check if the grup delay values are not available. See IS-GPS-200, Table 30-IV. //Bit string "1000000000000" is -4096 in 2 complement From de3bf9d480a4707de5a2c6535bd3ee213aef9e53 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 6 Mar 2018 15:58:48 +0100 Subject: [PATCH 39/63] debug8 --- src/algorithms/libs/rtklib/rtklib_conversions.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index c0bf8929c..91c50c151 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -192,9 +192,9 @@ eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph) rtklib_sat.f1 = gps_eph.d_A_f1; rtklib_sat.f2 = gps_eph.d_A_f2; rtklib_sat.tgd[0] = gps_eph.d_TGD; - rtklib_sat.tgd[1] = 0; - rtklib_sat.tgd[2] = 0; - rtklib_sat.tgd[3] = 0; + rtklib_sat.tgd[1] = 0.0; + rtklib_sat.tgd[2] = 0.0; + rtklib_sat.tgd[3] = 0.0; rtklib_sat.toes = gps_eph.d_Toe; rtklib_sat.toc = gpst2time(rtklib_sat.week, gps_eph.d_Toc); rtklib_sat.ttr = gpst2time(rtklib_sat.week, gps_eph.d_TOW); From 93d2fdaf1436d6adef162d1e0fe6031582f2d1bc Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 8 Mar 2018 18:05:22 +0100 Subject: [PATCH 40/63] debug 9 --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 2 +- src/algorithms/PVT/libs/rtklib_solver.cc | 6 +++--- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 12 +++--------- src/algorithms/libs/rtklib/rtklib_rtcm.cc | 2 +- src/algorithms/libs/rtklib/rtklib_rtcm2.cc | 2 +- src/algorithms/libs/rtklib/rtklib_rtcm3.cc | 12 ++++++------ src/algorithms/libs/rtklib/rtklib_rtksvr.cc | 2 +- 7 files changed, 16 insertions(+), 22 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index e688ada7d..43da2f97f 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -385,7 +385,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc() msgctl(sysv_msqid, IPC_RMID, NULL); //save GPS L2CM ephemeris to XML file - std::string file_name = "eph_GPS_L2CM_L5.xml"; + std::string file_name = "eph_GPS_CNAV.xml"; if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) { diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 081f38bfd..f504ebfe0 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -127,7 +127,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ int glo_valid_obs = 0; //GLONASS L1/L2 valid observations counter obsd_t obs_data[MAXOBS]; - eph_t eph_data[MAXOBS]; + eph_t eph_data[MAXOBS]; geph_t geph_data[MAXOBS]; for(gnss_observables_iter = gnss_observables_map.cbegin(); @@ -404,7 +404,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ // ********************************************************************** this->set_valid_position(false); - if (valid_obs > 0 || glo_valid_obs > 0) + if ((valid_obs + glo_valid_obs) > 3) { int result = 0; nav_t nav_data; @@ -495,5 +495,5 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ } } } - return this->is_valid_position(); + return is_valid_position(); } diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index b8795531c..aea3dd85f 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -205,15 +205,9 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); d_file.write(reinterpret_cast(&P1_C1), sizeof(double)); d_file.write(reinterpret_cast(&P2_C2), sizeof(double)); + /* if no P1-P2 DCB, use TGD instead */ - if(P1_P2 == 0.0 and sys == SYS_GPS) - { - P1_P2 = gettgd(obs->sat, nav); - } - else if(P1_P2 == 0.0 and sys == SYS_GAL) - { - //TODO - } + if(P1_P2 == 0.0) { P1_P2 = gettgd(obs->sat, nav); } if(sys == SYS_GPS) { @@ -276,7 +270,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } else if(sys == SYS_GAL) /* E1 + E5a */ { - + //TODO } } d_file.write(reinterpret_cast(&PC), sizeof(double)); diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm.cc b/src/algorithms/libs/rtklib/rtklib_rtcm.cc index e3c0d15b9..3ffb4502d 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtcm.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtcm.cc @@ -69,7 +69,7 @@ int init_rtcm(rtcm_t *rtcm) obsd_t data0 = {{0, 0.0}, 0, 0, {0}, {0}, {0}, {0.0}, {0.0}, {0.0}}; eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; geph_t geph0 = {0, -1, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0.0}, {0.0}, {0.0}, 0.0, 0.0, 0.0}; ssr_t ssr0 = {{{0, 0.0}}, {0.0}, {0}, 0, 0, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, {0.0}, {0.0}, {0.0}, 0.0, 0.0, '0'}; diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm2.cc b/src/algorithms/libs/rtklib/rtklib_rtcm2.cc index a754db2e0..c7af88ccd 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtcm2.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtcm2.cc @@ -219,7 +219,7 @@ int decode_type17(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; int i = 48, week, prn, sat; diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm3.cc b/src/algorithms/libs/rtklib/rtklib_rtcm3.cc index 53e88739d..249953f64 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtcm3.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtcm3.cc @@ -846,7 +846,7 @@ int decode_type1019(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; char *msg; int i = 24 + 12, prn, sat, week, sys = SYS_GPS; @@ -1293,7 +1293,7 @@ int decode_type1044(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; char *msg; int i = 24 + 12, prn, sat, week, sys = SYS_QZS; @@ -1398,7 +1398,7 @@ int decode_type1045(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; char *msg; int i = 24 + 12, prn, sat, week, e5a_hs, e5a_dvs, sys = SYS_GAL; @@ -1502,7 +1502,7 @@ int decode_type1046(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; char *msg; int i = 24 + 12, prn, sat, week, e5a_hs, e5a_dvs, sys = SYS_GAL; @@ -1606,7 +1606,7 @@ int decode_type1047(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; ; double toc, sqrtA; char *msg; @@ -1716,7 +1716,7 @@ int decode_type63(rtcm_t *rtcm) { eph_t eph = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; double toc, sqrtA; char *msg; int i = 24 + 12, prn, sat, week, sys = SYS_BDS; diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index 732d5cc28..0a2875f97 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -592,7 +592,7 @@ int rtksvrinit(rtksvr_t *svr) '0', '0', '0', 0, 0, 0}; eph_t eph0 = {0, -1, -1, 0, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, 0.0, 0.0}; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {0.0}, {0.0}, 0.0, 0.0}; geph_t geph0 = {0, -1, 0, 0, 0, 0, {0, 0.0}, {0, 0.0}, {0.0}, {0.0}, {0.0}, 0.0, 0.0, 0.0}; seph_t seph0 = {0, {0, 0.0}, {0, 0.0}, 0, 0, {0.0}, {0.0}, {0.0}, 0.0, 0.0}; From ab7cabc8e87603bbe2b052fb58248fd51b905817 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 8 Mar 2018 18:32:55 +0100 Subject: [PATCH 41/63] code style --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 100 ++-- .../gnuradio_blocks/hybrid_observables_cc.cc | 469 ++++++++++-------- 2 files changed, 311 insertions(+), 258 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 43da2f97f..75438755c 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -534,13 +534,13 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // ############ 1. READ PSEUDORANGES #### for (unsigned int i = 0; i < d_nchannels; i++) { - if (in[i][epoch].Flag_valid_pseudorange == true) + if (in[i][epoch].Flag_valid_pseudorange) { std::map::const_iterator tmp_eph_iter_gps = d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_gal = d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_cnav = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_glo_gnav = d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN); - if (((tmp_eph_iter_gps->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1C") == 0)) || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2S") == 0)) || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1B") == 0)) || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("5X") == 0)) || ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1G") == 0)) || ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2G") == 0)) || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("L5") == 0))) + if (((tmp_eph_iter_gps->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("1C") == 0)) or ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("2S") == 0)) or ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("1B") == 0)) or ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("5X") == 0)) or ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("1G") == 0)) or ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("2G") == 0)) or ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) and (std::string(in[i][epoch].Signal).compare("L5") == 0))) { // store valid observables in a map. gnss_observables_map.insert(std::pair(i, in[i][epoch])); @@ -601,36 +601,36 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item 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)) && (d_rtcm_MT1019_rate_ms != 0)) // allows deactivating messages by setting rate = 0 + 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 { 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)) && (d_rtcm_MT1020_rate_ms != 0)) // allows deactivating messages by setting rate = 0 + 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 { 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)) && (d_rtcm_MT1045_rate_ms != 0)) + 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)) { 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)) && (d_rtcm_MT1077_rate_ms != 0)) + 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)) && (d_rtcm_MT1087_rate_ms != 0)) + 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)) && (d_rtcm_MT1097_rate_ms != 0)) + 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 ((std::fabs(current_RX_time - last_RTCM_MSM_output_time) * 1000.0 >= static_cast(d_rtcm_MSM_rate_ms)) && (d_rtcm_MSM_rate_ms != 0)) + 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)) { flag_write_RTCM_MSM_output = true; last_RTCM_MSM_output_time = current_RX_time; @@ -776,7 +776,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 7) // GPS L1 C/A + GPS L2C { - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time); rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); @@ -786,7 +786,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { std::string gal_signal("1B"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); @@ -796,7 +796,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 10) // GPS L1 C/A + Galileo E5a { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { std::string gal_signal("5X"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); @@ -806,7 +806,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 11) // GPS L1 C/A + Galileo E5b { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { std::string gal_signal("7X"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); @@ -867,7 +867,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend())) { std::string glo_signal("1G"); rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); @@ -883,7 +883,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 27) // Galileo E1B + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend())) { std::string glo_signal("1G"); std::string gal_signal("1B"); @@ -894,7 +894,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 28) // GPS L2C + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { std::string glo_signal("1G"); rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, glo_signal); @@ -919,7 +919,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); } - if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6)) // Galileo + if ((type_of_rx == 4) or (type_of_rx == 5) or (type_of_rx == 6)) // Galileo { rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); } @@ -927,15 +927,15 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); } - if ((type_of_rx == 9) || (type_of_rx == 10) || (type_of_rx == 11)) // GPS L1 C/A + Galileo + if ((type_of_rx == 9) or (type_of_rx == 10) or (type_of_rx == 11)) // GPS L1 C/A + Galileo { rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); } - if ((type_of_rx == 14) || (type_of_rx == 15)) // Galileo E1B + Galileo E5a + if ((type_of_rx == 14) or (type_of_rx == 15)) // Galileo E1B + Galileo E5a { rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); } - if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS L1 C/A, GLONASS L2 C/A + if ((type_of_rx == 23) or (type_of_rx == 24) or (type_of_rx == 25)) // GLONASS L1 C/A, GLONASS L2 C/A { rp->log_rinex_nav(rp->navGloFile, d_ls_pvt->glonass_gnav_ephemeris_map); } @@ -972,7 +972,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); @@ -985,7 +985,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); rp->update_nav_header(rp->navFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); @@ -998,7 +998,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); rp->update_nav_header(rp->navFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); @@ -1011,7 +1011,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B"); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); @@ -1024,7 +1024,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "5X"); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); @@ -1037,7 +1037,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "7X"); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); @@ -1046,11 +1046,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 7) // GPS L1 C/A + GPS L2C { - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); @@ -1059,11 +1059,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 9) // GPS L1 C/A + Galileo E1B { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); @@ -1076,7 +1076,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 5X"); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); @@ -1089,7 +1089,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 7X"); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); @@ -1102,7 +1102,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1C"); } - if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) { rp->update_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); @@ -1115,7 +1115,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "2C"); } - if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) { rp->update_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); @@ -1128,7 +1128,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item { rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1C 2C"); } - if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) { rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); @@ -1137,11 +1137,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 26) // GPS L1 C/A + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); @@ -1150,11 +1150,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 27) // Galileo E1B + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->galileo_utc_model.A0_6 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); @@ -1163,11 +1163,11 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } if (type_of_rx == 28) // GPS L2C + GLONASS L1 C/A { - if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) { rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); } - if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) + if (!b_rinex_header_updated and (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) { rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); @@ -1200,7 +1200,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } } } - if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo + if ((type_of_rx == 4) or (type_of_rx == 5) or (type_of_rx == 6) or (type_of_rx == 14) or (type_of_rx == 15)) // Galileo { if (flag_write_RTCM_1045_output == true) { @@ -1234,7 +1234,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); std::map::const_iterator gps_cnav_ephemeris_iter; gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -1305,7 +1305,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } } } - if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS + if ((type_of_rx == 23) or (type_of_rx == 24) or (type_of_rx == 25)) // GLONASS { if (flag_write_RTCM_1020_output == true) { @@ -1472,7 +1472,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } b_rtcm_writing_started = true; } - if ((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo + if ((type_of_rx == 4) or (type_of_rx == 5) or (type_of_rx == 6) or (type_of_rx == 14) or (type_of_rx == 15)) // Galileo { for (std::map::const_iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend(); gal_ephemeris_iter++) { @@ -1497,7 +1497,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item std::map::const_iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); std::map::const_iterator gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) and (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend())) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } @@ -1550,18 +1550,18 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item i++; } - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() && (d_rtcm_MT1077_rate_ms != 0)) + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() and (d_rtcm_MT1077_rate_ms != 0)) { d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0)) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() and (d_rtcm_MT1097_rate_ms != 0)) { d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } - if ((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS + if ((type_of_rx == 23) or (type_of_rx == 24) or (type_of_rx == 25)) // GLONASS { for (std::map::const_iterator glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend(); glonass_gnav_ephemeris_iter++) { @@ -1696,7 +1696,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item } // DEBUG MESSAGE: Display position in console output - if ((d_ls_pvt->is_valid_position() == true) && (flag_display_pvt == true)) + if ((d_ls_pvt->is_valid_position() == true) and (flag_display_pvt == true)) { std::cout << TEXT_BOLD_GREEN << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index c5e2c983e..20e5c6948 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -51,62 +51,64 @@ hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels_in, } -hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename) : - gr::block("hybrid_observables_cc", - gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)), - gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) +hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename) : gr::block("hybrid_observables_cc", + gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)), + gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) { d_dump = dump; d_nchannels = nchannels_out; d_dump_filename = dump_filename; T_rx_s = 0.0; - T_rx_step_s = 0.001; // 1 ms - max_delta = 0.15; // 150 ms + T_rx_step_s = 0.001; // 1 ms + max_delta = 0.15; // 150 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()); - } + for (unsigned int i = 0; i < d_nchannels; i++) + { + d_gnss_synchro_history.push_back(std::deque()); + } // ############# ENABLE DATA FILE LOG ################# if (d_dump) - { - if (!d_dump_file.is_open()) { - try - { - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure & e) - { - LOG(WARNING) << "Exception opening observables dump file " << e.what(); - d_dump = false; - } + if (!d_dump_file.is_open()) + { + try + { + d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception opening observables dump file " << e.what(); + d_dump = false; + } + } } - } } hybrid_observables_cc::~hybrid_observables_cc() { if (d_dump_file.is_open()) - { - try { d_dump_file.close(); } - catch(const std::exception & ex) { - LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + try + { + d_dump_file.close(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + } + } + if (d_dump) + { + std::cout << "Writing observables .mat files ..."; + save_matfile(); + std::cout << " done." << std::endl; } - } - if(d_dump) - { - std::cout << "Writing observables .mat files ..."; - save_matfile(); - std::cout << " done." << std::endl; - } } @@ -118,12 +120,15 @@ int hybrid_observables_cc::save_matfile() int epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels; std::ifstream dump_file; dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); - try { dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); } - catch(const std::ifstream::failure &e) - { - std::cerr << "Problem opening dump file:" << e.what() << std::endl; - return 1; - } + try + { + dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << std::endl; + return 1; + } // count number of epochs and rewind long int num_epoch = 0; if (dump_file.is_open()) @@ -132,14 +137,17 @@ int hybrid_observables_cc::save_matfile() num_epoch = static_cast(size) / static_cast(epoch_size_bytes); dump_file.seekg(0, std::ios::beg); } - else { return 1; } - double ** RX_time = new double * [d_nchannels]; - double ** TOW_at_current_symbol_s = new double * [d_nchannels]; - double ** Carrier_Doppler_hz = new double * [d_nchannels]; - double ** Carrier_phase_cycles = new double * [d_nchannels]; - double ** Pseudorange_m = new double * [d_nchannels]; - double ** PRN = new double * [d_nchannels]; - double ** Flag_valid_pseudorange = new double * [d_nchannels]; + else + { + return 1; + } + double **RX_time = new double *[d_nchannels]; + double **TOW_at_current_symbol_s = new double *[d_nchannels]; + double **Carrier_Doppler_hz = new double *[d_nchannels]; + double **Carrier_phase_cycles = new double *[d_nchannels]; + double **Pseudorange_m = new double *[d_nchannels]; + double **PRN = new double *[d_nchannels]; + double **Flag_valid_pseudorange = new double *[d_nchannels]; for (unsigned int i = 0; i < d_nchannels; i++) { @@ -223,7 +231,10 @@ int hybrid_observables_cc::save_matfile() mat_t *matfp; matvar_t *matvar; std::string filename = d_dump_filename; - if(filename.size() > 4) { filename.erase(filename.end() - 4, filename.end()); } + if (filename.size() > 4) + { + filename.erase(filename.end() - 4, filename.end()); + } filename.append(".mat"); matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); if (reinterpret_cast(matfp) != NULL) @@ -287,66 +298,81 @@ int hybrid_observables_cc::save_matfile() return 0; } -bool hybrid_observables_cc::interpolate_data(Gnss_Synchro& out, std::deque& data, const double& ti) +bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, std::deque &data, const double &ti) { - if((ti < data.front().RX_time) or (ti > data.back().RX_time)) { return false; } + if ((ti < data.front().RX_time) or (ti > data.back().RX_time)) + { + return false; + } std::deque::iterator it; - arma::vec t = arma::vec(data.size()); - arma::vec dop = t; - arma::vec cph = t; - arma::vec tow = t; - arma::vec tiv = arma::vec(1); + arma::vec t = arma::vec(data.size()); + arma::vec dop = t; + arma::vec cph = t; + arma::vec tow = t; + arma::vec tiv = arma::vec(1); arma::vec result; tiv(0) = ti; unsigned int aux = 0; - for(it = data.begin(); it != data.end(); it++) - { - t(aux) = it->RX_time; - dop(aux) = it->Carrier_Doppler_hz; - cph(aux) = it->Carrier_phase_rads; - tow(aux) = it->TOW_at_current_symbol_s; + for (it = data.begin(); it != data.end(); it++) + { + t(aux) = it->RX_time; + dop(aux) = it->Carrier_Doppler_hz; + cph(aux) = it->Carrier_phase_rads; + tow(aux) = it->TOW_at_current_symbol_s; - aux++; - } + aux++; + } arma::interp1(t, dop, tiv, result); - out.Carrier_Doppler_hz = result(0); + out.Carrier_Doppler_hz = result(0); arma::interp1(t, cph, tiv, result); - out.Carrier_phase_rads = result(0); + out.Carrier_phase_rads = result(0); arma::interp1(t, tow, tiv, result); out.TOW_at_current_symbol_s = result(0); return result.is_finite(); } -double hybrid_observables_cc::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) - { - return((static_cast(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast(a.fs)); - } - else { return 0.0; } + if (a.Flag_valid_word) + { + return ((static_cast(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast(a.fs)); + } + else + { + return 0.0; + } } void hybrid_observables_cc::forecast(int noutput_items __attribute__((unused)), - gr_vector_int &ninput_items_required) + gr_vector_int &ninput_items_required) { - for(unsigned int i = 0; i < d_nchannels; i++) { ninput_items_required[i] = 0; } + for (unsigned int i = 0; i < d_nchannels; i++) + { + ninput_items_required[i] = 0; + } ninput_items_required[d_nchannels] = 1; } -void hybrid_observables_cc::clean_history(std::deque& data) +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; } - } + while (data.size() > 0) + { + if ((T_rx_s - data.front().RX_time) > max_delta) + { + data.pop_front(); + } + else + { + return; + } + } } -void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector& data) +void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector &data) { std::vector::iterator it; @@ -356,180 +382,207 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector::iterator it2; - double thr_ = 250.0 / SPEED_OF_LIGHT; // Maximum pseudorange difference = 250 meters - for(it = data.begin(); it != (data.end() - 1); it++) - { - for(it2 = it + 1; it2 != data.end(); it2++) + double thr_ = 250.0 / SPEED_OF_LIGHT; // Maximum pseudorange difference = 250 meters + for (it = data.begin(); it != (data.end() - 1); it++) { - if(it->PRN == 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_) + for (it2 = it + 1; it2 != data.end(); it2++) { - DLOG(INFO) << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal - << ". TOW difference in PRN " << it->PRN - << " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT - << " meters in pseudorange"; - std::cout << TEXT_RED << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal - << ". TOW difference in PRN " << it->PRN - << " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT - << " meters in pseudorange" << TEXT_RESET << std::endl; + if (it->PRN == 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_) + { + DLOG(INFO) << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal + << ". TOW difference in PRN " << it->PRN + << " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT + << " meters in pseudorange"; + std::cout << TEXT_RED << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal + << ". TOW difference in PRN " << it->PRN + << " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT + << " meters in pseudorange" << TEXT_RESET << std::endl; + } + } } - } } - } #endif -/////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////// double TOW_ref = std::numeric_limits::lowest(); - 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; } - } - 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; - it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT; - } + for (it = data.begin(); it != data.end(); it++) + { + if (it->TOW_at_current_symbol_s > TOW_ref and it->Signal[0] != '2') + { + TOW_ref = it->TOW_at_current_symbol_s; + } + } + 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; + it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT; + } } int hybrid_observables_cc::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) + 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]); - Gnss_Synchro** out = reinterpret_cast(&output_items[0]); + const Gnss_Synchro **in = reinterpret_cast(&input_items[0]); + Gnss_Synchro **out = reinterpret_cast(&output_items[0]); unsigned int i; int total_input_items = 0; - for(i = 0; i < d_nchannels; i++) { total_input_items += ninput_items[i]; } + for (i = 0; i < d_nchannels; i++) + { + total_input_items += ninput_items[i]; + } consume(d_nchannels, 1); T_rx_s += T_rx_step_s; ////////////////////////////////////////////////////////////////////////// - if((total_input_items == 0) and (d_num_valid_channels == 0)) - { - return 0; - } + if ((total_input_items == 0) and (d_num_valid_channels == 0)) + { + return 0; + } ////////////////////////////////////////////////////////////////////////// std::vector>::iterator it; if (total_input_items > 0) - { - i = 0; - for(it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) { - if(ninput_items[i] > 0) - { - // Add the new Gnss_Synchros to their corresponding deque - for(int aux = 0; aux < ninput_items[i]; aux++) + i = 0; + for (it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) { - if(in[i][aux].Flag_valid_word) - { - it->push_back(in[i][aux]); - it->back().RX_time = compute_T_rx_s(in[i][aux]); - // Check if the last Gnss_Synchro comes from the same satellite as the previous ones - if(it->size() > 1) + if (ninput_items[i] > 0) { - if(it->front().PRN != it->back().PRN) { it->clear(); } + // Add the new Gnss_Synchros to their corresponding deque + for (int aux = 0; aux < ninput_items[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]); + // Check if the last Gnss_Synchro comes from the same satellite as the previous ones + if (it->size() > 1) + { + if (it->front().PRN != it->back().PRN) + { + it->clear(); + } + } + } + } + consume(i, ninput_items[i]); } - } + 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; } - consume(i, ninput_items[i]); - } - 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; } - - for(i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the threshold - { - if(valid_channels[i]) + if (d_num_valid_channels == 0) { - clean_history(d_gnss_synchro_history.at(i)); - if(d_gnss_synchro_history.at(i).size() < 2) { valid_channels[i] = false; } + return 0; + } + + for (i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the threshold + { + if (valid_channels[i]) + { + clean_history(d_gnss_synchro_history.at(i)); + if (d_gnss_synchro_history.at(i).size() < 2) + { + valid_channels[i] = false; + } + } } - } // Check if there is any valid channel after computing the time distance between the Gnss_Synchro data and the receiver time d_num_valid_channels = valid_channels.count(); double T_rx_s_out = T_rx_s - (max_delta / 2.0); - if((d_num_valid_channels == 0) or (T_rx_s_out < 0.0)) { return 0; } + if ((d_num_valid_channels == 0) or (T_rx_s_out < 0.0)) + { + 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]) + for (it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++) { - Gnss_Synchro interpolated_gnss_synchro = it->back(); - if(interpolate_data(interpolated_gnss_synchro, *it, T_rx_s_out)) - { - epoch_data.push_back(interpolated_gnss_synchro); - } - else - { - valid_channels[i] = false; - } + if (valid_channels[i]) + { + Gnss_Synchro interpolated_gnss_synchro = it->back(); + if (interpolate_data(interpolated_gnss_synchro, *it, T_rx_s_out)) + { + epoch_data.push_back(interpolated_gnss_synchro); + } + else + { + valid_channels[i] = false; + } + } + i++; } - i++; - } d_num_valid_channels = valid_channels.count(); - if(d_num_valid_channels == 0) { return 0; } + if (d_num_valid_channels == 0) + { + return 0; + } 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]) + for (i = 0; i < d_nchannels; i++) { - out[i][0] = (*it2); - out[i][0].Flag_valid_pseudorange = true; - it2++; + 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; + } } - else + if (d_dump) { - out[i][0] = Gnss_Synchro(); - out[i][0].Flag_valid_pseudorange = false; + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + for (i = 0; i < d_nchannels; i++) + { + tmp_double = out[i][0].RX_time; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = out[i][0].TOW_at_current_symbol_s; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = out[i][0].Carrier_Doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = out[i][0].Carrier_phase_rads / GPS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = out[i][0].Pseudorange_m; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(out[i][0].PRN); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(out[i][0].Flag_valid_pseudorange); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + d_dump = false; + } } - } - if(d_dump) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - for (i = 0; i < d_nchannels; i++) - { - tmp_double = out[i][0].RX_time; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = out[i][0].TOW_at_current_symbol_s; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = out[i][0].Carrier_Doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = out[i][0].Carrier_phase_rads / GPS_TWO_PI; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = out[i][0].Pseudorange_m; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(out[i][0].PRN); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(out[i][0].Flag_valid_pseudorange); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - d_dump = false; - } - } return 1; } From ecb4a4970a9a788186fb9a1457bc0c7bdbc266da Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 9 Mar 2018 13:05:25 +0100 Subject: [PATCH 42/63] Disable eph update when L1 + L2 --- src/algorithms/PVT/libs/rtklib_solver.cc | 17 +- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 238 ++++++++++-------- src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 17 +- .../gnuradio_blocks/hybrid_observables_cc.cc | 2 +- 4 files changed, 151 insertions(+), 123 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index f504ebfe0..65a1a0807 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -127,12 +127,12 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ int glo_valid_obs = 0; //GLONASS L1/L2 valid observations counter obsd_t obs_data[MAXOBS]; - eph_t eph_data[MAXOBS]; + eph_t eph_data[MAXOBS]; geph_t geph_data[MAXOBS]; - for(gnss_observables_iter = gnss_observables_map.cbegin(); - gnss_observables_iter != gnss_observables_map.cend(); - gnss_observables_iter++) //CHECK INCONSISTENCY when combining GLONASS + other system + for (gnss_observables_iter = gnss_observables_map.cbegin(); + gnss_observables_iter != gnss_observables_map.cend(); + gnss_observables_iter++) //CHECK INCONSISTENCY when combining GLONASS + other system { switch (gnss_observables_iter->second.System) { @@ -247,17 +247,19 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ { if (eph_data[i].sat == static_cast(gnss_observables_iter->second.PRN)) { - eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + eph_t eph_aux = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + eph_data[i].tgd = eph_aux.tgd; + eph_data[i].isc = eph_aux.isc; obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs], gnss_observables_iter->second, - gps_cnav_ephemeris_iter->second.i_GPS_week, + eph_data[i].week, 1); //Band 2 (L2) break; } } } else - { + { /* // 3. If not found, insert the GPS L2 ephemeris and the observation //convert ephemeris from GNSS-SDR class to RTKLIB structure eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); @@ -271,6 +273,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ gps_cnav_ephemeris_iter->second.i_GPS_week, 1); //Band 2 (L2) valid_obs++; + */ } } else // the ephemeris are not available for this SV diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index aea3dd85f..b15275214 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -125,15 +125,15 @@ double getiscl5q(int sat, const nav_t *nav) double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int iter, const prcopt_t *opt, double *var) { - const double* lam = nav->lam[obs->sat - 1]; + const double *lam = nav->lam[obs->sat - 1]; double PC = 0.0; double P1 = 0.0; double P2 = 0.0; double P1_P2 = 0.0; double P1_C1 = 0.0; double P2_C2 = 0.0; - double ISCl1 = 0.0; - double ISCl2 = 0.0; + double ISCl1 = 0.0; + double ISCl2 = 0.0; double ISCl5i = 0.0; double ISCl5q = 0.0; double gamma_ = 0.0; @@ -142,52 +142,61 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int sys = satsys(obs->sat, NULL); *var = 0.0; - if(sys == SYS_NONE) - { - trace(4, "prange: satsys NULL\n"); - return 0.0; - } + if (sys == SYS_NONE) + { + trace(4, "prange: satsys NULL\n"); + return 0.0; + } /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ - if(sys == SYS_GAL or sys == SYS_SBS) {j = 2;} - - if(sys == SYS_GPS) - { - if(obs->code[1] != CODE_NONE) {j = 1;} - else if(obs->code[2] != CODE_NONE) {j = 2;} - } - - if(lam[i] == 0.0 or lam[j] == 0.0) - { - trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); - printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]); - return 0.0; - } - - /* test snr mask */ - if(iter > 0) - { - if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) + if (sys == SYS_GAL or sys == SYS_SBS) { - trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n", - time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25); + j = 2; + } + + if (sys == SYS_GPS) + { + if (obs->code[1] != CODE_NONE) + { + j = 1; + } + else if (obs->code[2] != CODE_NONE) + { + j = 2; + } + } + + if (lam[i] == 0.0 or lam[j] == 0.0) + { + trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); + printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]); return 0.0; } - if (opt->ionoopt == IONOOPT_IFLC) + + /* test snr mask */ + if (iter > 0) { - if (testsnr(0, j, azel[1], obs->SNR[j] * 0.25, &opt->snrmask)) - { - trace(4, "prange: testsnr error\n"); - return 0.0; - } + if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) + { + trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n", + time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25); + return 0.0; + } + if (opt->ionoopt == IONOOPT_IFLC) + { + if (testsnr(0, j, azel[1], obs->SNR[j] * 0.25, &opt->snrmask)) + { + trace(4, "prange: testsnr error\n"); + return 0.0; + } + } } - } /* fL1^2 / fL2(orL5)^2 . See IS-GPS-200, p. 103 and Galileo ICD p. 48 */ - if(sys == SYS_GPS or sys == SYS_GAL) - { - gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); - } + if (sys == SYS_GPS or sys == SYS_GAL) + { + gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); + } P1 = obs->P[i]; P2 = obs->P[j]; P1_P2 = nav->cbias[obs->sat - 1][0]; @@ -196,86 +205,103 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, std::string d_dump_filename = "/home/aramos/dump_prange.dat"; std::ofstream d_file; - d_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); + d_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); d_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary | std::ios::app); double tmp_double = static_cast(obs->sat); - d_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - d_file.write(reinterpret_cast(&P1), sizeof(double)); - d_file.write(reinterpret_cast(&P2), sizeof(double)); - d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); - d_file.write(reinterpret_cast(&P1_C1), sizeof(double)); - d_file.write(reinterpret_cast(&P2_C2), sizeof(double)); + d_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + d_file.write(reinterpret_cast(&P1), sizeof(double)); + d_file.write(reinterpret_cast(&P2), sizeof(double)); + d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); + d_file.write(reinterpret_cast(&P1_C1), sizeof(double)); + d_file.write(reinterpret_cast(&P2_C2), sizeof(double)); /* if no P1-P2 DCB, use TGD instead */ - if(P1_P2 == 0.0) { P1_P2 = gettgd(obs->sat, nav); } + if (P1_P2 == 0.0) + { + P1_P2 = gettgd(obs->sat, nav); + } - if(sys == SYS_GPS) - { - ISCl1 = getiscl1(obs->sat, nav); - ISCl2 = getiscl2(obs->sat, nav); - ISCl5i = getiscl5i(obs->sat, nav); - ISCl5q = getiscl5q(obs->sat, nav); - d_file.write(reinterpret_cast(&ISCl1), sizeof(double)); - d_file.write(reinterpret_cast(&ISCl2), sizeof(double)); - d_file.write(reinterpret_cast(&ISCl5i), sizeof(double)); - d_file.write(reinterpret_cast(&ISCl5q), sizeof(double)); - } - d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); + if (sys == SYS_GPS) + { + ISCl1 = getiscl1(obs->sat, nav); + ISCl2 = getiscl2(obs->sat, nav); + ISCl5i = getiscl5i(obs->sat, nav); + ISCl5q = getiscl5q(obs->sat, nav); + d_file.write(reinterpret_cast(&ISCl1), sizeof(double)); + d_file.write(reinterpret_cast(&ISCl2), sizeof(double)); + d_file.write(reinterpret_cast(&ISCl5i), sizeof(double)); + d_file.write(reinterpret_cast(&ISCl5q), sizeof(double)); + } + d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); //CHECK IF IT IS STILL NEEDED - if(opt->ionoopt == IONOOPT_IFLC) - { /* dual-frequency */ + if (opt->ionoopt == IONOOPT_IFLC) + { /* dual-frequency */ - if (P1 == 0.0 || P2 == 0.0) { return 0.0; } - if (obs->code[i] == CODE_L1C) { P1 += P1_C1; } /* C1->P1 */ - if (obs->code[j] == CODE_L2C) { P2 += P2_C2; } /* C2->P2 */ + if (P1 == 0.0 || P2 == 0.0) + { + return 0.0; + } + if (obs->code[i] == CODE_L1C) + { + P1 += P1_C1; + } /* C1->P1 */ + if (obs->code[j] == CODE_L2C) + { + P2 += P2_C2; + } /* C2->P2 */ - /* iono-free combination */ - PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); - } + /* iono-free combination */ + PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); + } //////////////////////////////////////////// else - { /* single-frequency */ - if(obs->code[i] == CODE_NONE and obs->code[j] == CODE_NONE) { return 0.0; } + { /* single-frequency */ + if (obs->code[i] == CODE_NONE and obs->code[j] == CODE_NONE) + { + return 0.0; + } - else if(obs->code[i] != CODE_NONE and obs->code[j] == CODE_NONE) - {//CHECK!! - P1 += P1_C1; /* C1->P1 */ - PC = P1 + P1_P2; + else if (obs->code[i] != CODE_NONE and obs->code[j] == CODE_NONE) + { //CHECK!! + P1 += P1_C1; /* C1->P1 */ + PC = P1 + P1_P2; + } + else if (obs->code[i] == CODE_NONE and obs->code[j] != CODE_NONE) + { + if (sys == SYS_GPS) + { //CHECK!! + P2 += P2_C2; /* C2->P2 */ + //PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); + PC = P2 + P1_P2 - ISCl2; + } + else if (sys == SYS_GAL) + { + //TODO + } + } + /* dual-frequency */ + else if (sys == SYS_GPS) + { + if (obs->code[j] == CODE_L2S) /* L1 + L2 */ + { + PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2; + } + if (obs->code[j] == CODE_L5X) /* L1 + L5 */ + { + } + } + else if (sys == SYS_GAL) /* E1 + E5a */ + { + //TODO + } } - else if(obs->code[i] == CODE_NONE and obs->code[j] != CODE_NONE) - { - if(sys == SYS_GPS) - {//CHECK!! - P2 += P2_C2; /* C2->P2 */ - //PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); - PC = P2 + P1_P2 - ISCl2; - } - else if(sys == SYS_GAL) - { - //TODO - } - } - /* dual-frequency */ - else if(sys == SYS_GPS) - { - if(obs->code[j] == CODE_L2S) /* L1 + L2 */ - { - PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2; - } - if(obs->code[j] == CODE_L5X) /* L1 + L5 */ - { - - } - } - else if(sys == SYS_GAL) /* E1 + E5a */ - { - //TODO - } - } - d_file.write(reinterpret_cast(&PC), sizeof(double)); + d_file.write(reinterpret_cast(&PC), sizeof(double)); d_file.close(); - if(opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; } /* sbas clock based C1 */ + if (opt->sateph == EPHOPT_SBAS) + { + PC -= P1_C1; + } /* sbas clock based C1 */ *var = std::pow(ERR_CBIAS, 2.0); return PC; } diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 3456916b5..92c1c51fd 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -253,12 +253,11 @@ const unsigned int tbl_CRC24Q[] = { 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538}; -extern "C" -{ - void dgemm_(char *, char *, int *, int *, int *, double *, double *, int *, double *, int *, double *, double *, int *); - extern void dgetrf_(int *, int *, double *, int *, int *, int *); - extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); - extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, int *, int *); +extern "C" { +void dgemm_(char *, char *, int *, int *, int *, double *, double *, int *, double *, int *, double *, double *, int *); +extern void dgetrf_(int *, int *, double *, int *, int *, int *); +extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); +extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, int *, int *); } @@ -1388,10 +1387,10 @@ double time2gpst(gtime_t t, int *week) { gtime_t t0 = epoch2time(gpst0); time_t sec = t.time - t0.time; - int w = (int)(sec / (86400 * 7)); + int w = static_cast(sec / 604800); if (week) *week = w; - return (double)(sec - (double)w * 86400 * 7) + t.sec; + return (static_cast(sec - static_cast(w * 604800)) + t.sec); } @@ -2993,7 +2992,7 @@ int readnav(const char *file, nav_t *nav) { FILE *fp; eph_t eph0 = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0 }; + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0}; geph_t geph0 = {0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {}, {}, {}, 0.0, 0.0, 0.0}; char buff[4096], *p; long toe_time, tof_time, toc_time, ttr_time; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 20e5c6948..81daa31ca 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -410,7 +410,7 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector::lowest(); for (it = data.begin(); it != data.end(); it++) { - if (it->TOW_at_current_symbol_s > TOW_ref and it->Signal[0] != '2') + if (it->TOW_at_current_symbol_s > TOW_ref) { TOW_ref = it->TOW_at_current_symbol_s; } From 0057e609241bd21740ba3a3de2bb5ef20209843a Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 12 Mar 2018 10:51:06 +0100 Subject: [PATCH 43/63] Clean code --- src/algorithms/PVT/libs/rtklib_solver.cc | 12 +- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 47 ++-- .../system_parameters/gps_cnav_ephemeris.h | 43 ++-- .../gps_cnav_navigation_message.cc | 240 +++++++++--------- 4 files changed, 175 insertions(+), 167 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 65a1a0807..eaed5f5ec 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -247,19 +247,19 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ { if (eph_data[i].sat == static_cast(gnss_observables_iter->second.PRN)) { - eph_t eph_aux = eph_to_rtklib(gps_cnav_ephemeris_iter->second); - eph_data[i].tgd = eph_aux.tgd; - eph_data[i].isc = eph_aux.isc; + eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + /* By the moment, GPS L2 observables are not used in pseudorange computations obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs], gnss_observables_iter->second, eph_data[i].week, - 1); //Band 2 (L2) + 1); //Band 2 (L2) */ break; } } } else - { /* + { + /* By the moment, GPS L2 observables are not used in pseudorange computations // 3. If not found, insert the GPS L2 ephemeris and the observation //convert ephemeris from GNSS-SDR class to RTKLIB structure eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); @@ -273,7 +273,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ gps_cnav_ephemeris_iter->second.i_GPS_week, 1); //Band 2 (L2) valid_obs++; - */ + */ } } else // the ephemeris are not available for this SV diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index b15275214..434f3b03a 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -54,8 +54,6 @@ #include "rtklib_ephemeris.h" #include "rtklib_ionex.h" #include "rtklib_sbas.h" -#include -#include /* pseudorange measurement error variance ------------------------------------*/ double varerr(const prcopt_t *opt, double el, int sys) @@ -132,6 +130,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, double P1_P2 = 0.0; double P1_C1 = 0.0; double P2_C2 = 0.0; + //Intersignal corrections (m). See GPS IS-200 CNAV message double ISCl1 = 0.0; double ISCl2 = 0.0; double ISCl5i = 0.0; @@ -154,8 +153,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { j = 2; } - - if (sys == SYS_GPS) + else if (sys == SYS_GPS or sys == SYS_GLO) { if (obs->code[1] != CODE_NONE) { @@ -203,18 +201,6 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, P1_C1 = nav->cbias[obs->sat - 1][1]; P2_C2 = nav->cbias[obs->sat - 1][2]; - std::string d_dump_filename = "/home/aramos/dump_prange.dat"; - std::ofstream d_file; - d_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); - d_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary | std::ios::app); - double tmp_double = static_cast(obs->sat); - d_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - d_file.write(reinterpret_cast(&P1), sizeof(double)); - d_file.write(reinterpret_cast(&P2), sizeof(double)); - d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); - d_file.write(reinterpret_cast(&P1_C1), sizeof(double)); - d_file.write(reinterpret_cast(&P2_C2), sizeof(double)); - /* if no P1-P2 DCB, use TGD instead */ if (P1_P2 == 0.0) { @@ -227,12 +213,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, ISCl2 = getiscl2(obs->sat, nav); ISCl5i = getiscl5i(obs->sat, nav); ISCl5q = getiscl5q(obs->sat, nav); - d_file.write(reinterpret_cast(&ISCl1), sizeof(double)); - d_file.write(reinterpret_cast(&ISCl2), sizeof(double)); - d_file.write(reinterpret_cast(&ISCl5i), sizeof(double)); - d_file.write(reinterpret_cast(&ISCl5q), sizeof(double)); } - d_file.write(reinterpret_cast(&P1_P2), sizeof(double)); //CHECK IF IT IS STILL NEEDED if (opt->ionoopt == IONOOPT_IFLC) @@ -263,19 +244,26 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } else if (obs->code[i] != CODE_NONE and obs->code[j] == CODE_NONE) - { //CHECK!! + { P1 += P1_C1; /* C1->P1 */ PC = P1 + P1_P2; } else if (obs->code[i] == CODE_NONE and obs->code[j] != CODE_NONE) { if (sys == SYS_GPS) - { //CHECK!! + { P2 += P2_C2; /* C2->P2 */ //PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); - PC = P2 + P1_P2 - ISCl2; + if (obs->code[j] == CODE_L2S) //L2 single freq. + { + PC = P2 + P1_P2 - ISCl2; + } + else if (obs->code[j] == CODE_L5X) //L5 single freq. + { + PC = P2 + P1_P2 - ISCl5i; + } } - else if (sys == SYS_GAL) + else if (sys == SYS_GAL) // Gal. E5a single freq. { //TODO } @@ -285,9 +273,12 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, { if (obs->code[j] == CODE_L2S) /* L1 + L2 */ { - PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2; + //By the moment, GPS L2 pseudoranges are not used + //PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2; + P1 += P1_C1; /* C1->P1 */ + PC = P1 + P1_P2; } - if (obs->code[j] == CODE_L5X) /* L1 + L5 */ + else if (obs->code[j] == CODE_L5X) /* L1 + L5 */ { } } @@ -296,8 +287,6 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, //TODO } } - d_file.write(reinterpret_cast(&PC), sizeof(double)); - d_file.close(); if (opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h index d10c2d8c6..7b63c0857 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.h +++ b/src/core/system_parameters/gps_cnav_ephemeris.h @@ -138,25 +138,30 @@ public: { }; - archive& make_nvp("i_satellite_PRN", i_satellite_PRN); // SV PRN NUMBER - archive& make_nvp("d_TOW", d_TOW); //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] - archive& make_nvp("d_Crs", d_Crs); //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m] - archive& make_nvp("d_M_0", d_M_0); //!< Mean Anomaly at Reference Time [semi-circles] - archive& make_nvp("d_Cuc", d_Cuc); //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] - archive& make_nvp("d_e_eccentricity", d_e_eccentricity); //!< Eccentricity [dimensionless] - archive& make_nvp("d_Cus", d_Cus); //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad] - archive& make_nvp("d_Toe1", d_Toe1); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] - archive& make_nvp("d_Toe2", d_Toe2); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] - archive& make_nvp("d_Toc", d_Toc); //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s] - archive& make_nvp("d_Cic", d_Cic); //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad] - archive& make_nvp("d_OMEGA0", d_OMEGA0); //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles] - archive& make_nvp("d_Cis", d_Cis); //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad] - archive& make_nvp("d_i_0", d_i_0); //!< Inclination Angle at Reference Time [semi-circles] - archive& make_nvp("d_Crc", d_Crc); //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m] - archive& make_nvp("d_OMEGA", d_OMEGA); //!< Argument of Perigee [semi-cicles] - archive& make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s] - archive& make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week] - archive& make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("i_satellite_PRN", i_satellite_PRN); // SV PRN NUMBER + archive& make_nvp("d_TOW", d_TOW); //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] + archive& make_nvp("d_Crs", d_Crs); //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m] + archive& make_nvp("d_M_0", d_M_0); //!< Mean Anomaly at Reference Time [semi-circles] + archive& make_nvp("d_Cuc", d_Cuc); //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] + archive& make_nvp("d_e_eccentricity", d_e_eccentricity); //!< Eccentricity [dimensionless] + archive& make_nvp("d_Cus", d_Cus); //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad] + archive& make_nvp("d_Toe1", d_Toe1); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] + archive& make_nvp("d_Toe2", d_Toe2); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] + archive& make_nvp("d_Toc", d_Toc); //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s] + archive& make_nvp("d_Cic", d_Cic); //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad] + archive& make_nvp("d_OMEGA0", d_OMEGA0); //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles] + archive& make_nvp("d_Cis", d_Cis); //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad] + archive& make_nvp("d_i_0", d_i_0); //!< Inclination Angle at Reference Time [semi-circles] + archive& make_nvp("d_Crc", d_Crc); //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m] + archive& make_nvp("d_OMEGA", d_OMEGA); //!< Argument of Perigee [semi-cicles] + archive& make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s] + archive& make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week] + archive& make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL1", d_ISCL1); //!< Estimated Group Delay Differential: L1P(Y)-L1C/A correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL2", d_ISCL2); //!< Estimated Group Delay Differential: L1P(Y)-L2C correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL5I", d_ISCL5I); //!< Estimated Group Delay Differential: L1P(Y)-L5i correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_ISCL5Q", d_ISCL5Q); //!< Estimated Group Delay Differential: L1P(Y)-L5q correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive& make_nvp("d_DELTA_A", d_DELTA_A); //!< Semi-major axis difference at reference time [m] archive& make_nvp("d_A_DOT", d_A_DOT); //!< Change rate in semi-major axis [m/s] archive& make_nvp("d_DELTA_OMEGA_DOT", d_DELTA_OMEGA_DOT); //!< Rate of Right Ascension difference [semi-circles/s] diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index 0f2455e51..d188060bd 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -187,127 +187,141 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset(read_navigation_unsigned(data_bits, CNAV_MSG_TYPE)); - switch(page_type) - { - case 10: // Ephemeris 1/2 - ephemeris_record.i_GPS_week = static_cast(read_navigation_unsigned(data_bits, CNAV_WN)); - ephemeris_record.i_signal_health = static_cast(read_navigation_unsigned(data_bits, CNAV_HEALTH)); - ephemeris_record.d_Top = static_cast(read_navigation_unsigned(data_bits, CNAV_TOP1)); - ephemeris_record.d_Top *= CNAV_TOP1_LSB; - ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA)); - ephemeris_record.d_Toe1 = static_cast(read_navigation_unsigned(data_bits, CNAV_TOE1)); - ephemeris_record.d_Toe1 *= CNAV_TOE1_LSB; - ephemeris_record.d_DELTA_A = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_A)); - ephemeris_record.d_DELTA_A *= CNAV_DELTA_A_LSB; - ephemeris_record.d_A_DOT = static_cast(read_navigation_signed(data_bits, CNAV_A_DOT)); - ephemeris_record.d_A_DOT *= CNAV_A_DOT_LSB; - ephemeris_record.d_Delta_n = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0)); - ephemeris_record.d_Delta_n *= CNAV_DELTA_N0_LSB; - ephemeris_record.d_DELTA_DOT_N = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0_DOT)); - ephemeris_record.d_DELTA_DOT_N *= CNAV_DELTA_N0_DOT_LSB; - ephemeris_record.d_M_0 = static_cast(read_navigation_signed(data_bits, CNAV_M0)); - ephemeris_record.d_M_0 *= CNAV_M0_LSB; - ephemeris_record.d_e_eccentricity = static_cast(read_navigation_unsigned(data_bits, CNAV_E_ECCENTRICITY)); - ephemeris_record.d_e_eccentricity *= CNAV_E_ECCENTRICITY_LSB; - ephemeris_record.d_OMEGA = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA)); - ephemeris_record.d_OMEGA *= CNAV_OMEGA_LSB; + switch (page_type) + { + case 10: // Ephemeris 1/2 + ephemeris_record.i_GPS_week = static_cast(read_navigation_unsigned(data_bits, CNAV_WN)); + ephemeris_record.i_signal_health = static_cast(read_navigation_unsigned(data_bits, CNAV_HEALTH)); + ephemeris_record.d_Top = static_cast(read_navigation_unsigned(data_bits, CNAV_TOP1)); + ephemeris_record.d_Top *= CNAV_TOP1_LSB; + ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA)); + ephemeris_record.d_Toe1 = static_cast(read_navigation_unsigned(data_bits, CNAV_TOE1)); + ephemeris_record.d_Toe1 *= CNAV_TOE1_LSB; + ephemeris_record.d_DELTA_A = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_A)); + ephemeris_record.d_DELTA_A *= CNAV_DELTA_A_LSB; + ephemeris_record.d_A_DOT = static_cast(read_navigation_signed(data_bits, CNAV_A_DOT)); + ephemeris_record.d_A_DOT *= CNAV_A_DOT_LSB; + ephemeris_record.d_Delta_n = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0)); + ephemeris_record.d_Delta_n *= CNAV_DELTA_N0_LSB; + ephemeris_record.d_DELTA_DOT_N = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_N0_DOT)); + ephemeris_record.d_DELTA_DOT_N *= CNAV_DELTA_N0_DOT_LSB; + ephemeris_record.d_M_0 = static_cast(read_navigation_signed(data_bits, CNAV_M0)); + ephemeris_record.d_M_0 *= CNAV_M0_LSB; + ephemeris_record.d_e_eccentricity = static_cast(read_navigation_unsigned(data_bits, CNAV_E_ECCENTRICITY)); + ephemeris_record.d_e_eccentricity *= CNAV_E_ECCENTRICITY_LSB; + ephemeris_record.d_OMEGA = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA)); + ephemeris_record.d_OMEGA *= CNAV_OMEGA_LSB; ephemeris_record.b_integrity_status_flag = static_cast(read_navigation_bool(data_bits, CNAV_INTEGRITY_FLAG)); ephemeris_record.b_l2c_phasing_flag = static_cast(read_navigation_bool(data_bits, CNAV_L2_PHASING_FLAG)); - b_flag_ephemeris_1 = true; - break; - case 11: // Ephemeris 2/2 - ephemeris_record.d_Toe2 = static_cast(read_navigation_unsigned(data_bits, CNAV_TOE2)); - ephemeris_record.d_Toe2 *= CNAV_TOE2_LSB; - ephemeris_record.d_OMEGA0 = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA0)); - ephemeris_record.d_OMEGA0 *= CNAV_OMEGA0_LSB; - ephemeris_record.d_DELTA_OMEGA_DOT = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_OMEGA_DOT)); - ephemeris_record.d_DELTA_OMEGA_DOT *= CNAV_DELTA_OMEGA_DOT_LSB; - ephemeris_record.d_i_0 = static_cast(read_navigation_signed(data_bits, CNAV_I0)); - ephemeris_record.d_i_0 *= CNAV_I0_LSB; - ephemeris_record.d_IDOT = static_cast(read_navigation_signed(data_bits, CNAV_I0_DOT)); - ephemeris_record.d_IDOT *= CNAV_I0_DOT_LSB; - ephemeris_record.d_Cis = static_cast(read_navigation_signed(data_bits, CNAV_CIS)); - ephemeris_record.d_Cis *= CNAV_CIS_LSB; - ephemeris_record.d_Cic = static_cast(read_navigation_signed(data_bits, CNAV_CIC)); - ephemeris_record.d_Cic *= CNAV_CIC_LSB; - ephemeris_record.d_Crs = static_cast(read_navigation_signed(data_bits, CNAV_CRS)); - ephemeris_record.d_Crs *= CNAV_CRS_LSB; - ephemeris_record.d_Crc = static_cast(read_navigation_signed(data_bits, CNAV_CRC)); - ephemeris_record.d_Crc *= CNAV_CRC_LSB; - ephemeris_record.d_Cus = static_cast(read_navigation_signed(data_bits, CNAV_CUS)); - ephemeris_record.d_Cus *= CNAV_CUS_LSB; - ephemeris_record.d_Cuc = static_cast(read_navigation_signed(data_bits, CNAV_CUC)); - ephemeris_record.d_Cuc *= CNAV_CUC_LSB; - b_flag_ephemeris_2 = true; - break; - case 30: // (CLOCK, IONO, GRUP DELAY) - //clock - ephemeris_record.d_Toc = static_cast(read_navigation_unsigned(data_bits, CNAV_TOC)); - ephemeris_record.d_Toc *= CNAV_TOC_LSB; - ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA_NED0)); - ephemeris_record.d_URA1 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED1)); - ephemeris_record.d_URA2 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED2)); - ephemeris_record.d_A_f0 = static_cast(read_navigation_signed(data_bits, CNAV_AF0)); - ephemeris_record.d_A_f0 *= CNAV_AF0_LSB; - ephemeris_record.d_A_f1 = static_cast(read_navigation_signed(data_bits, CNAV_AF1)); - ephemeris_record.d_A_f1 *= CNAV_AF1_LSB; - ephemeris_record.d_A_f2 = static_cast(read_navigation_signed(data_bits, CNAV_AF2)); - ephemeris_record.d_A_f2 *= CNAV_AF2_LSB; - //group delays - //Check if the grup delay values are not available. See IS-GPS-200, Table 30-IV. - //Bit string "1000000000000" is -4096 in 2 complement - ephemeris_record.d_TGD = static_cast(read_navigation_signed(data_bits, CNAV_TGD)); - if(ephemeris_record.d_TGD < -4095.9) { ephemeris_record.d_TGD = 0.0; } - ephemeris_record.d_TGD *= CNAV_TGD_LSB; + b_flag_ephemeris_1 = true; + break; + case 11: // Ephemeris 2/2 + ephemeris_record.d_Toe2 = static_cast(read_navigation_unsigned(data_bits, CNAV_TOE2)); + ephemeris_record.d_Toe2 *= CNAV_TOE2_LSB; + ephemeris_record.d_OMEGA0 = static_cast(read_navigation_signed(data_bits, CNAV_OMEGA0)); + ephemeris_record.d_OMEGA0 *= CNAV_OMEGA0_LSB; + ephemeris_record.d_DELTA_OMEGA_DOT = static_cast(read_navigation_signed(data_bits, CNAV_DELTA_OMEGA_DOT)); + ephemeris_record.d_DELTA_OMEGA_DOT *= CNAV_DELTA_OMEGA_DOT_LSB; + ephemeris_record.d_i_0 = static_cast(read_navigation_signed(data_bits, CNAV_I0)); + ephemeris_record.d_i_0 *= CNAV_I0_LSB; + ephemeris_record.d_IDOT = static_cast(read_navigation_signed(data_bits, CNAV_I0_DOT)); + ephemeris_record.d_IDOT *= CNAV_I0_DOT_LSB; + ephemeris_record.d_Cis = static_cast(read_navigation_signed(data_bits, CNAV_CIS)); + ephemeris_record.d_Cis *= CNAV_CIS_LSB; + ephemeris_record.d_Cic = static_cast(read_navigation_signed(data_bits, CNAV_CIC)); + ephemeris_record.d_Cic *= CNAV_CIC_LSB; + ephemeris_record.d_Crs = static_cast(read_navigation_signed(data_bits, CNAV_CRS)); + ephemeris_record.d_Crs *= CNAV_CRS_LSB; + ephemeris_record.d_Crc = static_cast(read_navigation_signed(data_bits, CNAV_CRC)); + ephemeris_record.d_Crc *= CNAV_CRC_LSB; + ephemeris_record.d_Cus = static_cast(read_navigation_signed(data_bits, CNAV_CUS)); + ephemeris_record.d_Cus *= CNAV_CUS_LSB; + ephemeris_record.d_Cuc = static_cast(read_navigation_signed(data_bits, CNAV_CUC)); + ephemeris_record.d_Cuc *= CNAV_CUC_LSB; + b_flag_ephemeris_2 = true; + break; + case 30: // (CLOCK, IONO, GRUP DELAY) + //clock + ephemeris_record.d_Toc = static_cast(read_navigation_unsigned(data_bits, CNAV_TOC)); + ephemeris_record.d_Toc *= CNAV_TOC_LSB; + ephemeris_record.d_URA0 = static_cast(read_navigation_signed(data_bits, CNAV_URA_NED0)); + ephemeris_record.d_URA1 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED1)); + ephemeris_record.d_URA2 = static_cast(read_navigation_unsigned(data_bits, CNAV_URA_NED2)); + ephemeris_record.d_A_f0 = static_cast(read_navigation_signed(data_bits, CNAV_AF0)); + ephemeris_record.d_A_f0 *= CNAV_AF0_LSB; + ephemeris_record.d_A_f1 = static_cast(read_navigation_signed(data_bits, CNAV_AF1)); + ephemeris_record.d_A_f1 *= CNAV_AF1_LSB; + ephemeris_record.d_A_f2 = static_cast(read_navigation_signed(data_bits, CNAV_AF2)); + ephemeris_record.d_A_f2 *= CNAV_AF2_LSB; + //group delays + //Check if the grup delay values are not available. See IS-GPS-200, Table 30-IV. + //Bit string "1000000000000" is -4096 in 2 complement + ephemeris_record.d_TGD = static_cast(read_navigation_signed(data_bits, CNAV_TGD)); + if (ephemeris_record.d_TGD < -4095.9) + { + ephemeris_record.d_TGD = 0.0; + } + ephemeris_record.d_TGD *= CNAV_TGD_LSB; - ephemeris_record.d_ISCL1 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL1)); - if(ephemeris_record.d_ISCL1 < -4095.9) { ephemeris_record.d_ISCL1 = 0.0; } - ephemeris_record.d_ISCL1 *= CNAV_ISCL1_LSB; + ephemeris_record.d_ISCL1 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL1)); + if (ephemeris_record.d_ISCL1 < -4095.9) + { + ephemeris_record.d_ISCL1 = 0.0; + } + ephemeris_record.d_ISCL1 *= CNAV_ISCL1_LSB; - ephemeris_record.d_ISCL2 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL2)); - if(ephemeris_record.d_ISCL2 < -4095.9) { ephemeris_record.d_ISCL2 = 0.0; } - ephemeris_record.d_ISCL2 *= CNAV_ISCL2_LSB; + ephemeris_record.d_ISCL2 = static_cast(read_navigation_signed(data_bits, CNAV_ISCL2)); + if (ephemeris_record.d_ISCL2 < -4095.9) + { + ephemeris_record.d_ISCL2 = 0.0; + } + ephemeris_record.d_ISCL2 *= CNAV_ISCL2_LSB; - ephemeris_record.d_ISCL5I = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5I)); - if(ephemeris_record.d_ISCL5I < -4095.9) { ephemeris_record.d_ISCL5I = 0.0; } - ephemeris_record.d_ISCL5I *= CNAV_ISCL5I_LSB; - - ephemeris_record.d_ISCL5Q = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5Q)); - if(ephemeris_record.d_ISCL5Q < -4095.9) { ephemeris_record.d_ISCL5Q = 0.0; } - ephemeris_record.d_ISCL5Q *= CNAV_ISCL5Q_LSB; - //iono - iono_record.d_alpha0 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA0)); - iono_record.d_alpha0 = iono_record.d_alpha0 * CNAV_ALPHA0_LSB; - iono_record.d_alpha1 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA1)); - iono_record.d_alpha1 = iono_record.d_alpha1 * CNAV_ALPHA1_LSB; - iono_record.d_alpha2 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA2)); - iono_record.d_alpha2 = iono_record.d_alpha2 * CNAV_ALPHA2_LSB; - iono_record.d_alpha3 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA3)); - iono_record.d_alpha3 = iono_record.d_alpha3 * CNAV_ALPHA3_LSB; - iono_record.d_beta0 = static_cast(read_navigation_signed(data_bits, CNAV_BETA0)); - iono_record.d_beta0 = iono_record.d_beta0 * CNAV_BETA0_LSB; - iono_record.d_beta1 = static_cast(read_navigation_signed(data_bits, CNAV_BETA1)); - iono_record.d_beta1 = iono_record.d_beta1 * CNAV_BETA1_LSB; - iono_record.d_beta2 = static_cast(read_navigation_signed(data_bits, CNAV_BETA2)); - iono_record.d_beta2 = iono_record.d_beta2 * CNAV_BETA2_LSB; - iono_record.d_beta3 = static_cast(read_navigation_signed(data_bits, CNAV_BETA3)); - iono_record.d_beta3 = iono_record.d_beta3 * CNAV_BETA3_LSB; - b_flag_iono_valid = true; - break; - case 33: // (CLOCK & UTC) - ephemeris_record.d_Top = static_cast(read_navigation_unsigned(data_bits, CNAV_TOP1)); - ephemeris_record.d_Top = ephemeris_record.d_Top * CNAV_TOP1_LSB; - ephemeris_record.d_Toc = static_cast(read_navigation_unsigned(data_bits, CNAV_TOC)); - ephemeris_record.d_Toc = ephemeris_record.d_Toc * CNAV_TOC_LSB; - ephemeris_record.d_A_f0 = static_cast(read_navigation_signed(data_bits, CNAV_AF0)); - ephemeris_record.d_A_f0 = ephemeris_record.d_A_f0 * CNAV_AF0_LSB; - ephemeris_record.d_A_f1 = static_cast(read_navigation_signed(data_bits, CNAV_AF1)); - ephemeris_record.d_A_f1 = ephemeris_record.d_A_f1 * CNAV_AF1_LSB; - ephemeris_record.d_A_f2 = static_cast(read_navigation_signed(data_bits, CNAV_AF2)); - ephemeris_record.d_A_f2 = ephemeris_record.d_A_f2 * CNAV_AF2_LSB; + ephemeris_record.d_ISCL5I = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5I)); + if (ephemeris_record.d_ISCL5I < -4095.9) + { + ephemeris_record.d_ISCL5I = 0.0; + } + ephemeris_record.d_ISCL5I *= CNAV_ISCL5I_LSB; + ephemeris_record.d_ISCL5Q = static_cast(read_navigation_signed(data_bits, CNAV_ISCL5Q)); + if (ephemeris_record.d_ISCL5Q < -4095.9) + { + ephemeris_record.d_ISCL5Q = 0.0; + } + ephemeris_record.d_ISCL5Q *= CNAV_ISCL5Q_LSB; + //iono + iono_record.d_alpha0 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA0)); + iono_record.d_alpha0 = iono_record.d_alpha0 * CNAV_ALPHA0_LSB; + iono_record.d_alpha1 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA1)); + iono_record.d_alpha1 = iono_record.d_alpha1 * CNAV_ALPHA1_LSB; + iono_record.d_alpha2 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA2)); + iono_record.d_alpha2 = iono_record.d_alpha2 * CNAV_ALPHA2_LSB; + iono_record.d_alpha3 = static_cast(read_navigation_signed(data_bits, CNAV_ALPHA3)); + iono_record.d_alpha3 = iono_record.d_alpha3 * CNAV_ALPHA3_LSB; + iono_record.d_beta0 = static_cast(read_navigation_signed(data_bits, CNAV_BETA0)); + iono_record.d_beta0 = iono_record.d_beta0 * CNAV_BETA0_LSB; + iono_record.d_beta1 = static_cast(read_navigation_signed(data_bits, CNAV_BETA1)); + iono_record.d_beta1 = iono_record.d_beta1 * CNAV_BETA1_LSB; + iono_record.d_beta2 = static_cast(read_navigation_signed(data_bits, CNAV_BETA2)); + iono_record.d_beta2 = iono_record.d_beta2 * CNAV_BETA2_LSB; + iono_record.d_beta3 = static_cast(read_navigation_signed(data_bits, CNAV_BETA3)); + iono_record.d_beta3 = iono_record.d_beta3 * CNAV_BETA3_LSB; + b_flag_iono_valid = true; + break; + case 33: // (CLOCK & UTC) + ephemeris_record.d_Top = static_cast(read_navigation_unsigned(data_bits, CNAV_TOP1)); + ephemeris_record.d_Top = ephemeris_record.d_Top * CNAV_TOP1_LSB; + ephemeris_record.d_Toc = static_cast(read_navigation_unsigned(data_bits, CNAV_TOC)); + ephemeris_record.d_Toc = ephemeris_record.d_Toc * CNAV_TOC_LSB; + ephemeris_record.d_A_f0 = static_cast(read_navigation_signed(data_bits, CNAV_AF0)); + ephemeris_record.d_A_f0 = ephemeris_record.d_A_f0 * CNAV_AF0_LSB; + ephemeris_record.d_A_f1 = static_cast(read_navigation_signed(data_bits, CNAV_AF1)); + ephemeris_record.d_A_f1 = ephemeris_record.d_A_f1 * CNAV_AF1_LSB; + ephemeris_record.d_A_f2 = static_cast(read_navigation_signed(data_bits, CNAV_AF2)); + ephemeris_record.d_A_f2 = ephemeris_record.d_A_f2 * CNAV_AF2_LSB; utc_model_record.d_A0 = static_cast(read_navigation_signed(data_bits, CNAV_A0)); utc_model_record.d_A0 = utc_model_record.d_A0 * CNAV_A0_LSB; From a797bfb641635955cb68945a29386ae43e088ca0 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 12 Mar 2018 11:27:26 +0100 Subject: [PATCH 44/63] Migrate GPS L1 TOW from double to unsigned int --- .../gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc | 8 ++++---- src/core/system_parameters/GPS_L1_CA.h | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) 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 2ace76e77..580bc8bdc 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 @@ -362,12 +362,12 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__ else { d_TOW_at_current_symbol += GPS_L1_CA_CODE_PERIOD; - d_TOW_at_current_symbol_ms++; + 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.Flag_valid_word = flag_TOW_set; + 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.Flag_valid_word = flag_TOW_set; if (flag_PLL_180_deg_phase_locked == true) { diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index cf2611344..c5ae95618 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -53,6 +53,7 @@ const double GPS_L1_FREQ_HZ = FREQ1; //!< L1 [Hz] const double GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [chips/s] const double GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips] const double GPS_L1_CA_CODE_PERIOD = 0.001; //!< GPS L1 C/A code period [seconds] +const unsigned int GPS_L1_CA_CODE_PERIOD_MS = 1; //!< GPS L1 C/A code period [ms] const double GPS_L1_CA_CHIP_PERIOD = 9.7752e-07; //!< GPS L1 C/A chip period [seconds] /*! From 02deff9b24acd2f973193f002e507c31ac2e1f97 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 19 Mar 2018 16:52:36 +0100 Subject: [PATCH 45/63] Add new tracking block to GPS L2 adapter --- .../galileo_e1b_telemetry_decoder_cc.cc | 10 +-- .../adapters/gps_l2_m_dll_pll_tracking.cc | 68 +++++++++++++------ .../adapters/gps_l2_m_dll_pll_tracking.h | 3 + .../gnuradio_blocks/dll_pll_veml_tracking.cc | 13 ++-- 4 files changed, 66 insertions(+), 28 deletions(-) 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 294436c2d..5d8cbfab6 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 @@ -399,32 +399,32 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute 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) { //TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay - d_TOW_at_current_symbol = d_nav.TOW_5 + GALILEO_INAV_PAGE_PART_SECONDS + (static_cast(required_symbols)) * GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; + d_TOW_at_current_symbol = d_nav.TOW_5 + static_cast(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast(required_symbols - 1) * GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; d_nav.flag_TOW_5 = false; } else if (d_nav.flag_TOW_6 == true) //page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) { //TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay - d_TOW_at_current_symbol = d_nav.TOW_6 + GALILEO_INAV_PAGE_PART_SECONDS + (static_cast(required_symbols)) * GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; + d_TOW_at_current_symbol = d_nav.TOW_6 + static_cast(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast(required_symbols - 1) * GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND; d_nav.flag_TOW_6 = false; } else { //this page has no timing information - d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD; // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; + d_TOW_at_current_symbol += GALILEO_E1_CODE_PERIOD; // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; } } else //if there is not a new preamble, we define the TOW of the current symbol { - d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD; + d_TOW_at_current_symbol += GALILEO_E1_CODE_PERIOD; } //if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true) if (d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) //all GGTO parameters arrived { - delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64))); + delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64.0))); } if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true) diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc index 839ef334c..ac8b6af7a 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc @@ -39,6 +39,7 @@ #include "configuration_interface.h" #include "GPS_L2C.h" #include "gnss_sdr_flags.h" +#include "display.h" #include @@ -52,7 +53,6 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking( //################# CONFIGURATION PARAMETERS ######################## int fs_in; int vector_length; - int f_if; bool dump; std::string dump_filename; std::string item_type; @@ -63,31 +63,46 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking( item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - f_if = configuration->property(role + ".if", 0); dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); + pll_bw_hz = configuration->property(role + ".pll_bw_hz", 2.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.75); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); + unified_ = configuration->property(role + ".unified", false); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L2_M_CODE_RATE_HZ) / static_cast(GPS_L2_M_CODE_LENGTH_CHIPS))); - + int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); + if (symbols_extended_correlator != 1) + { + std::cout << TEXT_RED << "WARNING: Extended coherent integration is not allowed in GPS L2. Coherent integration has been set to 20 ms (1 symbol)" << TEXT_RESET << std::endl; + } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); - tracking_ = gps_l2_m_dll_pll_make_tracking_cc( - f_if, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - early_late_space_chips); + if (unified_) + { + char sig_[3] = "2S"; + item_size_ = sizeof(gr_complex); + tracking_unified_ = dll_pll_veml_make_tracking( + fs_in, vector_length, dump, dump_filename, + pll_bw_hz, dll_bw_hz, pll_bw_hz, dll_bw_hz, + early_late_space_chips, + early_late_space_chips, + early_late_space_chips, + early_late_space_chips, + 1, false, 'G', sig_); + } + else + { + tracking_ = gps_l2_m_dll_pll_make_tracking_cc( + 0, fs_in, vector_length, dump, + dump_filename, pll_bw_hz, dll_bw_hz, + early_late_space_chips); + } } else { @@ -106,7 +121,10 @@ GpsL2MDllPllTracking::~GpsL2MDllPllTracking() void GpsL2MDllPllTracking::start_tracking() { - tracking_->start_tracking(); + if (unified_) + tracking_unified_->start_tracking(); + else + tracking_->start_tracking(); } /* @@ -115,13 +133,19 @@ void GpsL2MDllPllTracking::start_tracking() void GpsL2MDllPllTracking::set_channel(unsigned int channel) { channel_ = channel; - tracking_->set_channel(channel); + if (unified_) + tracking_unified_->set_channel(channel); + else + tracking_->set_channel(channel); } void GpsL2MDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { - tracking_->set_gnss_synchro(p_gnss_synchro); + if (unified_) + tracking_unified_->set_gnss_synchro(p_gnss_synchro); + else + tracking_->set_gnss_synchro(p_gnss_synchro); } void GpsL2MDllPllTracking::connect(gr::top_block_sptr top_block) @@ -142,10 +166,16 @@ void GpsL2MDllPllTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GpsL2MDllPllTracking::get_left_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } gr::basic_block_sptr GpsL2MDllPllTracking::get_right_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.h index ae7498a40..77741e9b1 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.h @@ -40,6 +40,7 @@ #include "tracking_interface.h" #include "gps_l2_m_dll_pll_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include class ConfigurationInterface; @@ -93,11 +94,13 @@ public: private: gps_l2_m_dll_pll_tracking_cc_sptr tracking_; + dll_pll_veml_tracking_sptr tracking_unified_; size_t item_size_; unsigned int channel_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; + bool unified_; }; #endif // GNSS_SDR_gps_l2_m_dll_pll_tracking_H_ 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 9a0c97c18..9d4d0b06d 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -887,6 +887,7 @@ void dll_pll_veml_tracking::log_data(bool integrating) tmp_L = std::abs(d_L_accu); if (integrating) { + //TODO: Improve this solution! // It compensates the amplitude difference while integrating float scale_factor = static_cast(d_extend_correlation_symbols) / static_cast(d_extend_correlation_symbols_count); tmp_VE *= scale_factor; @@ -1027,7 +1028,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_Prompt_buffer_deque.pop_front(); } } - else //Signal does not have secondary code. Search a bit transition by sign change + else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change { if (d_synchonizing) { @@ -1058,6 +1059,10 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_current_symbol = 1; } } + else + { + next_state = true; + } if (next_state) { // reset extended correlator d_VE_accu = gr_complex(0.0, 0.0); @@ -1089,9 +1094,9 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) if (d_enable_extended_integration) { d_extend_correlation_symbols_count = 0; - float new_correlation_time_s = static_cast(d_extend_correlation_symbols) * static_cast(d_code_period); - d_carrier_loop_filter.set_pdi(new_correlation_time_s); - d_code_loop_filter.set_pdi(new_correlation_time_s); + float new_correlation_time = static_cast(d_extend_correlation_symbols) * static_cast(d_code_period); + d_carrier_loop_filter.set_pdi(new_correlation_time); + d_code_loop_filter.set_pdi(new_correlation_time); d_state = 3; // next state is the extended correlator integrator LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " << d_channel From e94d7296dc0349b0ad5d70e98a52cf75b15a05d3 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 20 Mar 2018 10:33:02 +0100 Subject: [PATCH 46/63] Add warnings to tracking adapters --- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 1 + .../galileo_e1_dll_pll_veml_tracking.cc | 57 ++++----- .../adapters/galileo_e5a_dll_pll_tracking.cc | 114 +++++++++++------- .../adapters/galileo_e5a_dll_pll_tracking.h | 3 + .../adapters/gps_l1_ca_dll_pll_tracking.cc | 42 +++---- .../adapters/gps_l5i_dll_pll_tracking.cc | 101 ++++++++++------ .../adapters/gps_l5i_dll_pll_tracking.h | 9 +- .../gnuradio_blocks/dll_pll_veml_tracking.cc | 47 ++++---- .../gnuradio_blocks/dll_pll_veml_tracking.h | 5 - 9 files changed, 211 insertions(+), 168 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 434f3b03a..d33a70c31 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -280,6 +280,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } else if (obs->code[j] == CODE_L5X) /* L1 + L5 */ { + //TODO } } else if (sys == SYS_GAL) /* E1 + E5a */ diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index daf6e898a..3b2331531 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -38,6 +38,7 @@ #include "configuration_interface.h" #include "Galileo_E1.h" #include "gnss_sdr_flags.h" +#include "display.h" #include @@ -49,44 +50,37 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - bool dump; - std::string dump_filename; - std::string item_type; std::string default_item_type = "gr_complex"; - float pll_bw_hz; - float pll_bw_narrow_hz; - float dll_bw_hz; - float dll_bw_narrow_hz; - float early_late_space_chips; - float very_early_late_space_chips; - float early_late_space_narrow_chips; - float very_early_late_space_narrow_chips; unified_ = configuration->property(role + ".unified", false); - item_type = configuration->property(role + ".item_type", default_item_type); + std::string item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); - fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + bool dump = configuration->property(role + ".dump", false); + float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.5); + float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.5); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); - pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); - dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); - int extend_correlation_symbols; - extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); - early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15); - very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", 0.6); - early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); - very_early_late_space_narrow_chips = configuration->property(role + ".very_early_late_space_narrow_chips", 0.6); - + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); + int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15); + float very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", 0.6); + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); + float very_early_late_space_narrow_chips = configuration->property(role + ".very_early_late_space_narrow_chips", 0.6); bool track_pilot = configuration->property(role + ".track_pilot", false); - + if (extend_correlation_symbols < 1) + { + extend_correlation_symbols = 1; + std::cout << TEXT_RED << "WARNING: Galileo E1. extend_correlation_symbols must be bigger than 0. Coherent integration has been set to 1 symbol (4 ms)" << TEXT_RESET << std::endl; + } + else if (!track_pilot and extend_correlation_symbols > 1) + { + extend_correlation_symbols = 1; + std::cout << TEXT_RED << "WARNING: Extended coherent integration is not allowed in Galileo E1 when tracking the data component. Coherent integration has been set to 4 ms (1 symbol)" << TEXT_RESET << std::endl; + } std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! - vector_length = std::round(fs_in / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS)); + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + int vector_length = std::round(fs_in / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) @@ -138,7 +132,6 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( } channel_ = 0; - DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")"; } diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc index 87c11f48a..be0030888 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc @@ -40,6 +40,7 @@ #include "configuration_interface.h" #include "Galileo_E5a.h" #include "gnss_sdr_flags.h" +#include "display.h" #include using google::LogMessage; @@ -50,55 +51,63 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - int f_if; - bool dump; - std::string dump_filename; - std::string item_type; std::string default_item_type = "gr_complex"; - float pll_bw_hz; - float dll_bw_hz; - float pll_bw_narrow_hz; - float dll_bw_narrow_hz; - int ti_ms; - float early_late_space_chips; - item_type = configuration->property(role + ".item_type", default_item_type); - //vector_length = configuration->property(role + ".vector_length", 2048); + std::string item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 12000000); - fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - f_if = configuration->property(role + ".if", 0); - dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 20.0); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + bool dump = configuration->property(role + ".dump", false); + unified_ = configuration->property(role + ".unified", false); + float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 20.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 20.0); + float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 20.0); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); - pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 5.0); - dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); - ti_ms = configuration->property(role + ".ti_ms", 3); - - early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 5.0); + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 2.0); + int ti_ms = configuration->property(role + ".ti_ms", 3); + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! - vector_length = std::round(fs_in / (Galileo_E5a_CODE_CHIP_RATE_HZ / Galileo_E5a_CODE_LENGTH_CHIPS)); - + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + int vector_length = std::round(fs_in / (Galileo_E5a_CODE_CHIP_RATE_HZ / Galileo_E5a_CODE_LENGTH_CHIPS)); + int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); + bool track_pilot = configuration->property(role + ".track_pilot", false); + if (extend_correlation_symbols < 1) + { + extend_correlation_symbols = 1; + std::cout << TEXT_RED << "WARNING: Galileo E5a. extend_correlation_symbols must be bigger than 0. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << std::endl; + } + else if (!track_pilot and extend_correlation_symbols > Galileo_E5a_I_SECONDARY_CODE_LENGTH) + { + extend_correlation_symbols = Galileo_E5a_I_SECONDARY_CODE_LENGTH; + std::cout << TEXT_RED << "WARNING: Galileo E5a. extend_correlation_symbols must be lower than 21 when tracking the data component. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; + } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); - tracking_ = galileo_e5a_dll_pll_make_tracking_cc( - f_if, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - pll_bw_narrow_hz, - dll_bw_narrow_hz, - ti_ms, - early_late_space_chips); + if (unified_) + { + char sig_[3] = "5X"; + item_size_ = sizeof(gr_complex); + tracking_unified_ = dll_pll_veml_make_tracking( + fs_in, vector_length, dump, dump_filename, + pll_bw_hz, dll_bw_hz, + pll_bw_narrow_hz, dll_bw_narrow_hz, + early_late_space_chips, + early_late_space_chips, + early_late_space_narrow_chips, + early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot, 'E', sig_); + } + else + { + tracking_ = galileo_e5a_dll_pll_make_tracking_cc( + 0, fs_in, vector_length, dump, dump_filename, + pll_bw_hz, dll_bw_hz, pll_bw_narrow_hz, + dll_bw_narrow_hz, ti_ms, + early_late_space_chips); + } } else { @@ -117,7 +126,10 @@ GalileoE5aDllPllTracking::~GalileoE5aDllPllTracking() void GalileoE5aDllPllTracking::start_tracking() { - tracking_->start_tracking(); + if (unified_) + tracking_unified_->start_tracking(); + else + tracking_->start_tracking(); } /* @@ -126,13 +138,19 @@ void GalileoE5aDllPllTracking::start_tracking() void GalileoE5aDllPllTracking::set_channel(unsigned int channel) { channel_ = channel; - tracking_->set_channel(channel); + if (unified_) + tracking_unified_->set_channel(channel); + else + tracking_->set_channel(channel); } void GalileoE5aDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { - tracking_->set_gnss_synchro(p_gnss_synchro); + if (unified_) + tracking_unified_->set_gnss_synchro(p_gnss_synchro); + else + tracking_->set_gnss_synchro(p_gnss_synchro); } void GalileoE5aDllPllTracking::connect(gr::top_block_sptr top_block) @@ -153,10 +171,16 @@ void GalileoE5aDllPllTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GalileoE5aDllPllTracking::get_left_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } gr::basic_block_sptr GalileoE5aDllPllTracking::get_right_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.h b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.h index b56cf2945..69d5600e4 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.h @@ -41,6 +41,7 @@ #include "tracking_interface.h" #include "galileo_e5a_dll_pll_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include class ConfigurationInterface; @@ -94,11 +95,13 @@ public: private: galileo_e5a_dll_pll_tracking_cc_sptr tracking_; + dll_pll_veml_tracking_sptr tracking_unified_; size_t item_size_; unsigned int channel_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; + bool unified_; }; #endif /* GNSS_SDR_GALILEO_E5A_DLL_PLL_TRACKING_H_ */ diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index e642b29a1..8834cfd73 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -40,6 +40,7 @@ #include "configuration_interface.h" #include "GPS_L1_CA.h" #include "gnss_sdr_flags.h" +#include "display.h" #include using google::LogMessage; @@ -50,16 +51,11 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - bool dump; - std::string dump_filename; - std::string item_type; std::string default_item_type = "gr_complex"; - item_type = configuration->property(role + ".item_type", default_item_type); + std::string item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); - fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - dump = configuration->property(role + ".dump", false); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + bool dump = configuration->property(role + ".dump", false); unified_ = configuration->property(role + ".unified", false); float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); @@ -70,10 +66,14 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! - vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + int vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); - if (symbols_extended_correlator < 1) symbols_extended_correlator = 1; + if (symbols_extended_correlator < 1 or symbols_extended_correlator > 20) + { + symbols_extended_correlator = 1; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be between 1 and 20. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << std::endl; + } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { @@ -82,14 +82,9 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( char sig_[3] = "1C"; item_size_ = sizeof(gr_complex); tracking_unified_ = dll_pll_veml_make_tracking( - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - pll_bw_narrow_hz, - dll_bw_narrow_hz, + fs_in, vector_length, dump, + dump_filename, pll_bw_hz, dll_bw_hz, + pll_bw_narrow_hz, dll_bw_narrow_hz, early_late_space_chips, early_late_space_chips, early_late_space_narrow_chips, @@ -101,13 +96,8 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( else { tracking_ = gps_l1_ca_dll_pll_make_tracking_cc( - 0, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, + 0, fs_in, vector_length, dump, + dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips); } } diff --git a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc index 10c180037..752c7b1ce 100644 --- a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc @@ -39,6 +39,7 @@ #include "configuration_interface.h" #include "GPS_L5.h" #include "gnss_sdr_flags.h" +#include "display.h" #include @@ -50,44 +51,61 @@ GpsL5iDllPllTracking::GpsL5iDllPllTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - int f_if; - bool dump; - std::string dump_filename; - std::string item_type; std::string default_item_type = "gr_complex"; - float pll_bw_hz; - float dll_bw_hz; - float early_late_space_chips; - item_type = configuration->property(role + ".item_type", default_item_type); + std::string item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); - fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - f_if = configuration->property(role + ".if", 0); - dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + bool dump = configuration->property(role + ".dump", false); + unified_ = configuration->property(role + ".unified", false); + float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); + float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); - early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + float pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); + float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! - vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L5i_CODE_RATE_HZ) / static_cast(GPS_L5i_CODE_LENGTH_CHIPS))); - + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + int vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L5i_CODE_RATE_HZ) / static_cast(GPS_L5i_CODE_LENGTH_CHIPS))); + int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); + float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); + bool track_pilot = configuration->property(role + ".track_pilot", false); + if (extend_correlation_symbols < 1) + { + extend_correlation_symbols = 1; + std::cout << TEXT_RED << "WARNING: GPS L5. extend_correlation_symbols must be bigger than 0. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << std::endl; + } + else if (!track_pilot and extend_correlation_symbols > GPS_L5i_NH_CODE_LENGTH) + { + extend_correlation_symbols = GPS_L5i_NH_CODE_LENGTH; + std::cout << TEXT_RED << "WARNING: GPS L5. extend_correlation_symbols must be lower than 11 when tracking the data component. Coherent integration has been set to 10 symbols (10 ms)" << TEXT_RESET << std::endl; + } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); - tracking_ = gps_l5i_dll_pll_make_tracking_cc( - f_if, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - early_late_space_chips); + if (unified_) + { + char sig_[3] = "L5"; + item_size_ = sizeof(gr_complex); + tracking_unified_ = dll_pll_veml_make_tracking( + fs_in, vector_length, dump, dump_filename, + pll_bw_hz, dll_bw_hz, + pll_bw_narrow_hz, dll_bw_narrow_hz, + early_late_space_chips, + early_late_space_chips, + early_late_space_narrow_chips, + early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot, 'G', sig_); + } + else + { + tracking_ = gps_l5i_dll_pll_make_tracking_cc( + 0, fs_in, vector_length, dump, + dump_filename, pll_bw_hz, dll_bw_hz, + early_late_space_chips); + } } else { @@ -106,7 +124,10 @@ GpsL5iDllPllTracking::~GpsL5iDllPllTracking() void GpsL5iDllPllTracking::start_tracking() { - tracking_->start_tracking(); + if (unified_) + tracking_unified_->start_tracking(); + else + tracking_->start_tracking(); } /* @@ -115,13 +136,19 @@ void GpsL5iDllPllTracking::start_tracking() void GpsL5iDllPllTracking::set_channel(unsigned int channel) { channel_ = channel; - tracking_->set_channel(channel); + if (unified_) + tracking_unified_->set_channel(channel); + else + tracking_->set_channel(channel); } void GpsL5iDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { - tracking_->set_gnss_synchro(p_gnss_synchro); + if (unified_) + tracking_unified_->set_gnss_synchro(p_gnss_synchro); + else + tracking_->set_gnss_synchro(p_gnss_synchro); } void GpsL5iDllPllTracking::connect(gr::top_block_sptr top_block) @@ -142,10 +169,16 @@ void GpsL5iDllPllTracking::disconnect(gr::top_block_sptr top_block) gr::basic_block_sptr GpsL5iDllPllTracking::get_left_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } gr::basic_block_sptr GpsL5iDllPllTracking::get_right_block() { - return tracking_; + if (unified_) + return tracking_unified_; + else + return tracking_; } diff --git a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h index c54115251..472de2466 100644 --- a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h @@ -34,11 +34,12 @@ * ------------------------------------------------------------------------- */ -#ifndef GNSS_SDR_gps_l5i_dll_pll_tracking_H_ -#define GNSS_SDR_gps_l5i_dll_pll_tracking_H_ +#ifndef GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_H_ +#define GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_H_ #include "tracking_interface.h" #include "gps_l5i_dll_pll_tracking_cc.h" +#include "dll_pll_veml_tracking.h" #include class ConfigurationInterface; @@ -92,11 +93,13 @@ public: private: gps_l5i_dll_pll_tracking_cc_sptr tracking_; + dll_pll_veml_tracking_sptr tracking_unified_; size_t item_size_; unsigned int channel_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; + bool unified_; }; -#endif // GNSS_SDR_gps_l5i_dll_pll_tracking_H_ +#endif // GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_H_ 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 9d4d0b06d..c78068c38 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -39,6 +39,7 @@ #include "lock_detectors.h" #include "control_message_factory.h" #include "MATH_CONSTANTS.h" +#include "gnss_sdr_flags.h" #include "Galileo_E1.h" #include "galileo_e1_signal_processing.h" @@ -365,11 +366,11 @@ dll_pll_veml_tracking::dll_pll_veml_tracking( // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; - d_Prompt_buffer = new gr_complex[DLL_PLL_CN0_ESTIMATION_SAMPLES]; + d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples]; d_carrier_lock_test = 1.0; d_CN0_SNV_dB_Hz = 0.0; d_carrier_lock_fail_counter = 0; - d_carrier_lock_threshold = DLL_PLL_CARRIER_LOCK_THRESHOLD; + d_carrier_lock_threshold = FLAGS_carrier_lock_th; clear_tracking_vars(); @@ -635,7 +636,7 @@ bool dll_pll_veml_tracking::acquire_secondary() bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < DLL_PLL_CN0_ESTIMATION_SAMPLES) + if (d_cn0_estimation_counter < FLAGS_cn0_samples) { // fill buffer with prompt correlator output values d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; @@ -646,11 +647,11 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { d_cn0_estimation_counter = 0; // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES, static_cast(d_fs_in), static_cast(d_code_length_chips)); + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, static_cast(d_fs_in), static_cast(d_code_length_chips)); // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, DLL_PLL_CN0_ESTIMATION_SAMPLES); + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples); // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < DLL_PLL_MINIMUM_VALID_CN0) + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min) { d_carrier_lock_fail_counter++; } @@ -658,7 +659,7 @@ bool dll_pll_veml_tracking::cn0_and_tracking_lock_status() { if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; } - if (d_carrier_lock_fail_counter > DLL_PLL_MAXIMUM_LOCK_FAIL_COUNTER) + if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail) { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; @@ -1074,25 +1075,10 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) d_Prompt_buffer_deque.clear(); d_current_symbol = 0; d_synchonizing = false; - // Set narrow taps delay values [chips] - d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); - if (d_veml) - { - d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - } - else - { - d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); - } - // UPDATE INTEGRATION TIME if (d_enable_extended_integration) { + // UPDATE INTEGRATION TIME d_extend_correlation_symbols_count = 0; float new_correlation_time = static_cast(d_extend_correlation_symbols) * static_cast(d_code_period); d_carrier_loop_filter.set_pdi(new_correlation_time); @@ -1104,6 +1090,21 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " << d_channel << " : Satellite " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN) << std::endl; + // Set narrow taps delay values [chips] + d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); + if (d_veml) + { + d_local_code_shift_chips[0] = -d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = d_early_late_spc_narrow_chips * static_cast(d_code_samples_per_chip); + } } else { 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 6d3ddef62..96ee4e288 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -32,11 +32,6 @@ #ifndef GNSS_SDR_DLL_PLL_VEML_TRACKING_H #define GNSS_SDR_DLL_PLL_VEML_TRACKING_H -#define DLL_PLL_CN0_ESTIMATION_SAMPLES 20 -#define DLL_PLL_MINIMUM_VALID_CN0 25 -#define DLL_PLL_MAXIMUM_LOCK_FAIL_COUNTER 50 -#define DLL_PLL_CARRIER_LOCK_THRESHOLD 0.85 - #include "gnss_synchro.h" #include "tracking_2nd_DLL_filter.h" #include "tracking_2nd_PLL_filter.h" From afdad07493cf81d5fed3689ac54c97fd07e77e85 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 20 Mar 2018 12:30:20 +0100 Subject: [PATCH 47/63] Clean code --- .../galileo_e1_dll_pll_veml_tracking.cc | 6 +++- .../adapters/galileo_e5a_dll_pll_tracking.cc | 4 +++ .../adapters/gps_l1_ca_dll_pll_tracking.cc | 18 +++++++++-- .../adapters/gps_l2_m_dll_pll_tracking.cc | 30 ++++++++----------- .../adapters/gps_l5i_dll_pll_tracking.cc | 4 +++ 5 files changed, 42 insertions(+), 20 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index 3b2331531..92cfaa450 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -76,7 +76,11 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( else if (!track_pilot and extend_correlation_symbols > 1) { extend_correlation_symbols = 1; - std::cout << TEXT_RED << "WARNING: Extended coherent integration is not allowed in Galileo E1 when tracking the data component. Coherent integration has been set to 4 ms (1 symbol)" << TEXT_RESET << std::endl; + std::cout << TEXT_RED << "WARNING: Galileo E1. Extended coherent integration is not allowed when tracking the data component. Coherent integration has been set to 4 ms (1 symbol)" << TEXT_RESET << std::endl; + } + if (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz) + { + std::cout << TEXT_RED << "WARNING: Galileo E1. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; } std::string default_dump_filename = "./track_ch"; std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc index be0030888..ff181cf2d 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc @@ -81,6 +81,10 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking( extend_correlation_symbols = Galileo_E5a_I_SECONDARY_CODE_LENGTH; std::cout << TEXT_RED << "WARNING: Galileo E5a. extend_correlation_symbols must be lower than 21 when tracking the data component. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; } + if (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz) + { + std::cout << TEXT_RED << "WARNING: Galileo E5a. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; + } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index 8834cfd73..9f0892222 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -69,10 +69,24 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! int vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); - if (symbols_extended_correlator < 1 or symbols_extended_correlator > 20) + if (symbols_extended_correlator < 1) { symbols_extended_correlator = 1; - std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be between 1 and 20. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << std::endl; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be bigger than 1. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << std::endl; + } + else if (symbols_extended_correlator > 20) + { + symbols_extended_correlator = 20; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be lower than 21. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; + } + bool track_pilot = configuration->property(role + ".track_pilot", false); + if (track_pilot) + { + std::cout << TEXT_RED << "WARNING: GPS L1 C/A does not have pilot signal. Data tracking has been enabled" << TEXT_RESET << std::endl; + } + if (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz) + { + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc index ac8b6af7a..4180f161f 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc @@ -51,34 +51,30 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking( { DLOG(INFO) << "role " << role; //################# CONFIGURATION PARAMETERS ######################## - int fs_in; - int vector_length; - bool dump; - std::string dump_filename; - std::string item_type; std::string default_item_type = "gr_complex"; - float pll_bw_hz; - float dll_bw_hz; - float early_late_space_chips; - item_type = configuration->property(role + ".item_type", default_item_type); + std::string item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); - fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); - dump = configuration->property(role + ".dump", false); - pll_bw_hz = configuration->property(role + ".pll_bw_hz", 2.0); + int fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + bool dump = configuration->property(role + ".dump", false); + float pll_bw_hz = configuration->property(role + ".pll_bw_hz", 2.0); if (FLAGS_pll_bw_hz != 0.0) pll_bw_hz = static_cast(FLAGS_pll_bw_hz); - dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.75); + float dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.75); if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); unified_ = configuration->property(role + ".unified", false); - early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); + float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! - vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L2_M_CODE_RATE_HZ) / static_cast(GPS_L2_M_CODE_LENGTH_CHIPS))); + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + int vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L2_M_CODE_RATE_HZ) / static_cast(GPS_L2_M_CODE_LENGTH_CHIPS))); int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); if (symbols_extended_correlator != 1) { std::cout << TEXT_RED << "WARNING: Extended coherent integration is not allowed in GPS L2. Coherent integration has been set to 20 ms (1 symbol)" << TEXT_RESET << std::endl; } + bool track_pilot = configuration->property(role + ".track_pilot", false); + if (track_pilot) + { + std::cout << TEXT_RED << "WARNING: GPS L2 does not have pilot signal. Data tracking has been enabled" << TEXT_RESET << std::endl; + } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { diff --git a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc index 752c7b1ce..23f8c62a6 100644 --- a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc @@ -80,6 +80,10 @@ GpsL5iDllPllTracking::GpsL5iDllPllTracking( extend_correlation_symbols = GPS_L5i_NH_CODE_LENGTH; std::cout << TEXT_RED << "WARNING: GPS L5. extend_correlation_symbols must be lower than 11 when tracking the data component. Coherent integration has been set to 10 symbols (10 ms)" << TEXT_RESET << std::endl; } + if (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz) + { + std::cout << TEXT_RED << "WARNING: GPS L5. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; + } //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { From 5a86de623b36207be6698ef3dd8a88960f7990d3 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 20 Mar 2018 17:24:16 +0100 Subject: [PATCH 48/63] Fix tests --- .../galileo_e1_dll_pll_veml_tracking.cc | 2 +- .../adapters/galileo_e5a_dll_pll_tracking.cc | 2 +- .../adapters/gps_l1_ca_dll_pll_tracking.cc | 2 +- .../adapters/gps_l5i_dll_pll_tracking.cc | 2 +- src/core/system_parameters/rtcm.h | 10 +- .../observables/hybrid_observables_test.cc | 208 +++++++++--------- .../gps_l1_ca_telemetry_decoder_test.cc | 3 +- .../gps_l1_ca_dll_pll_tracking_test.cc | 6 +- 8 files changed, 119 insertions(+), 116 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index 92cfaa450..6c80628f7 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -78,7 +78,7 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( extend_correlation_symbols = 1; std::cout << TEXT_RED << "WARNING: Galileo E1. Extended coherent integration is not allowed when tracking the data component. Coherent integration has been set to 4 ms (1 symbol)" << TEXT_RESET << std::endl; } - if (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz) + if ((extend_correlation_symbols > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) { std::cout << TEXT_RED << "WARNING: Galileo E1. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; } diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc index ff181cf2d..816710efc 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc @@ -81,7 +81,7 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking( extend_correlation_symbols = Galileo_E5a_I_SECONDARY_CODE_LENGTH; std::cout << TEXT_RED << "WARNING: Galileo E5a. extend_correlation_symbols must be lower than 21 when tracking the data component. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << std::endl; } - if (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz) + if ((extend_correlation_symbols > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) { std::cout << TEXT_RED << "WARNING: Galileo E5a. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; } diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index 9f0892222..5512836da 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -84,7 +84,7 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( { std::cout << TEXT_RED << "WARNING: GPS L1 C/A does not have pilot signal. Data tracking has been enabled" << TEXT_RESET << std::endl; } - if (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz) + if ((symbols_extended_correlator > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) { std::cout << TEXT_RED << "WARNING: GPS L1 C/A. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; } diff --git a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc index 23f8c62a6..aa1885fd2 100644 --- a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc @@ -80,7 +80,7 @@ GpsL5iDllPllTracking::GpsL5iDllPllTracking( extend_correlation_symbols = GPS_L5i_NH_CODE_LENGTH; std::cout << TEXT_RED << "WARNING: GPS L5. extend_correlation_symbols must be lower than 11 when tracking the data component. Coherent integration has been set to 10 symbols (10 ms)" << TEXT_RESET << std::endl; } - if (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz) + if ((extend_correlation_symbols > 1) and (pll_bw_narrow_hz > pll_bw_hz or dll_bw_narrow_hz > dll_bw_hz)) { std::cout << TEXT_RED << "WARNING: GPS L5. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; } diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h index d4ac17460..434038e21 100644 --- a/src/core/system_parameters/rtcm.h +++ b/src/core/system_parameters/rtcm.h @@ -641,7 +641,6 @@ private: { public: Rtcm_Session(boost::asio::ip::tcp::socket socket, Rtcm_Listener_Room& room) : socket_(std::move(socket)), room_(room) {} - inline void start() { room_.join(shared_from_this()); @@ -665,12 +664,13 @@ private: boost::asio::async_read(socket_, boost::asio::buffer(read_msg_.data(), Rtcm_Message::header_length), [this, self](boost::system::error_code ec, std::size_t /*length*/) { - if (!ec && read_msg_.decode_header()) + if (!ec and read_msg_.decode_header()) { do_read_message_body(); } - else if (!ec && !read_msg_.decode_header()) + else if (!ec and !read_msg_.decode_header()) { + /* TODO: The commented code throws an exception. Solve it! client_says += read_msg_.data(); bool first = true; while (client_says.length() >= 80) @@ -683,6 +683,7 @@ private: std::cout << client_says.substr(0, 80) << std::endl; client_says = client_says.substr(80, client_says.length() - 80); } + */ do_read_message_header(); } else @@ -719,8 +720,7 @@ private: { auto self(shared_from_this()); boost::asio::async_write(socket_, - boost::asio::buffer(write_msgs_.front().body(), - write_msgs_.front().body_length()), + boost::asio::buffer(write_msgs_.front().body(), write_msgs_.front().body_length()), [this, self](boost::system::error_code ec, std::size_t /*length*/) { if (!ec) { diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc index 17c140cda..5b5c9ff12 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -184,17 +184,17 @@ public: int configure_generator(); int generate_signal(); void check_results_carrier_phase( - arma::mat& true_ch0, - arma::mat& true_ch1, - arma::vec& true_tow_s, - arma::mat& measured_ch0, - arma::mat& measured_ch1); + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1); void check_results_code_psudorange( - arma::mat& true_ch0, - arma::mat& true_ch1, - arma::vec& true_tow_s, - arma::mat& measured_ch0, - arma::mat& measured_ch1); + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1); HybridObservablesTest() { @@ -281,23 +281,23 @@ void HybridObservablesTest::configure_receiver() // Set Tracking config->set_property("Tracking_1C.item_type", "gr_complex"); - config->set_property("Tracking_1C.if", "0"); config->set_property("Tracking_1C.dump", "true"); config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); config->set_property("Tracking_1C.pll_bw_hz", "35.0"); config->set_property("Tracking_1C.dll_bw_hz", "0.5"); config->set_property("Tracking_1C.early_late_space_chips", "0.5"); + config->set_property("Tracking_1C.unified", "true"); config->set_property("TelemetryDecoder_1C.dump", "true"); config->set_property("Observables.dump", "true"); } void HybridObservablesTest::check_results_carrier_phase( - arma::mat& true_ch0, - arma::mat& true_ch1, - arma::vec& true_tow_s, - arma::mat& measured_ch0, - arma::mat& measured_ch1) + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1) { //1. True value interpolation to match the measurement times @@ -358,12 +358,12 @@ void HybridObservablesTest::check_results_carrier_phase( << " [cycles]" << std::endl; std::cout.precision(ss); - ASSERT_LT(rmse_ch0, 5e-2); - ASSERT_LT(error_mean_ch0, 5e-2); + ASSERT_LT(rmse_ch0, 5e-2); + ASSERT_LT(error_mean_ch0, 5e-2); ASSERT_GT(error_mean_ch0, -5e-2); - ASSERT_LT(error_var_ch0, 5e-2); - ASSERT_LT(max_error_ch0, 5e-2); - ASSERT_GT(min_error_ch0, -5e-2); + ASSERT_LT(error_var_ch0, 5e-2); + ASSERT_LT(max_error_ch0, 5e-2); + ASSERT_GT(min_error_ch0, -5e-2); //5. report ss = std::cout.precision(); @@ -375,21 +375,21 @@ void HybridObservablesTest::check_results_carrier_phase( << " [cycles]" << std::endl; std::cout.precision(ss); - ASSERT_LT(rmse_ch1, 5e-2); - ASSERT_LT(error_mean_ch1, 5e-2); + ASSERT_LT(rmse_ch1, 5e-2); + ASSERT_LT(error_mean_ch1, 5e-2); ASSERT_GT(error_mean_ch1, -5e-2); - ASSERT_LT(error_var_ch1, 5e-2); - ASSERT_LT(max_error_ch1, 5e-2); - ASSERT_GT(min_error_ch1, -5e-2); + ASSERT_LT(error_var_ch1, 5e-2); + ASSERT_LT(max_error_ch1, 5e-2); + ASSERT_GT(min_error_ch1, -5e-2); } void HybridObservablesTest::check_results_code_psudorange( - arma::mat& true_ch0, - arma::mat& true_ch1, - arma::vec& true_tow_s, - arma::mat& measured_ch0, - arma::mat& measured_ch1) + arma::mat& true_ch0, + arma::mat& true_ch1, + arma::vec& true_tow_s, + arma::mat& measured_ch0, + arma::mat& measured_ch1) { //1. True value interpolation to match the measurement times @@ -438,12 +438,12 @@ void HybridObservablesTest::check_results_code_psudorange( << " [meters]" << std::endl; std::cout.precision(ss); - ASSERT_LT(rmse, 0.5); - ASSERT_LT(error_mean, 0.5); + ASSERT_LT(rmse, 0.5); + ASSERT_LT(error_mean, 0.5); ASSERT_GT(error_mean, -0.5); - ASSERT_LT(error_var, 0.5); - ASSERT_LT(max_error, 2.0); - ASSERT_GT(min_error, -2.0); + ASSERT_LT(error_var, 0.5); + ASSERT_LT(max_error, 2.0); + ASSERT_GT(min_error, -2.0); } @@ -543,7 +543,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults) boost::shared_ptr tlm_msg_rx_ch2 = HybridObservablesTest_tlm_msg_rx_make(); //Observables - std::shared_ptr observables(new HybridObservables(config.get(), "Observables",3, 2)); + std::shared_ptr observables(new HybridObservables(config.get(), "Observables", 3, 2)); ASSERT_NO_THROW({ tracking_ch0->set_channel(gnss_synchro_ch0.Channel_ID); @@ -604,10 +604,10 @@ TEST_F(HybridObservablesTest, ValidationOfResults) true_observables_reader true_observables; ASSERT_NO_THROW({ - if(true_observables.open_obs_file(std::string("./obs_out.bin")) == false) - { - throw std::exception(); - } + if (true_observables.open_obs_file(std::string("./obs_out.bin")) == false) + { + throw std::exception(); + } }) << "Failure opening true observables file"; unsigned int nepoch = static_cast(true_observables.num_epochs()); @@ -620,27 +620,27 @@ TEST_F(HybridObservablesTest, ValidationOfResults) true_observables.restart(); long int epoch_counter = 0; ASSERT_NO_THROW({ - while(true_observables.read_binary_obs()) - { - if(round(true_observables.prn[0]) != gnss_synchro_ch0.PRN) - { - std::cout << "True observables SV PRN does not match " << round(true_observables.prn[1]) << std::endl; - throw std::exception(); - } - if(round(true_observables.prn[1]) != gnss_synchro_ch1.PRN) - { - std::cout << "True observables SV PRN does not match " << round(true_observables.prn[1]) << std::endl; - throw std::exception(); - } - true_ch0(epoch_counter, 0) = true_observables.gps_time_sec[0]; - true_ch0(epoch_counter, 1) = true_observables.dist_m[0]; - true_ch0(epoch_counter, 2) = true_observables.doppler_l1_hz[0]; - true_ch0(epoch_counter, 3) = true_observables.acc_carrier_phase_l1_cycles[0]; + while (true_observables.read_binary_obs()) + { + if (round(true_observables.prn[0]) != gnss_synchro_ch0.PRN) + { + std::cout << "True observables SV PRN does not match " << round(true_observables.prn[1]) << std::endl; + throw std::exception(); + } + if (round(true_observables.prn[1]) != gnss_synchro_ch1.PRN) + { + std::cout << "True observables SV PRN does not match " << round(true_observables.prn[1]) << std::endl; + throw std::exception(); + } + true_ch0(epoch_counter, 0) = true_observables.gps_time_sec[0]; + true_ch0(epoch_counter, 1) = true_observables.dist_m[0]; + true_ch0(epoch_counter, 2) = true_observables.doppler_l1_hz[0]; + true_ch0(epoch_counter, 3) = true_observables.acc_carrier_phase_l1_cycles[0]; - true_ch1(epoch_counter, 0) = true_observables.gps_time_sec[1]; - true_ch1(epoch_counter, 1) = true_observables.dist_m[1]; - true_ch1(epoch_counter, 2) = true_observables.doppler_l1_hz[1]; - true_ch1(epoch_counter, 3) = true_observables.acc_carrier_phase_l1_cycles[1]; + true_ch1(epoch_counter, 0) = true_observables.gps_time_sec[1]; + true_ch1(epoch_counter, 1) = true_observables.dist_m[1]; + true_ch1(epoch_counter, 2) = true_observables.doppler_l1_hz[1]; + true_ch1(epoch_counter, 3) = true_observables.acc_carrier_phase_l1_cycles[1]; epoch_counter++; } @@ -649,10 +649,10 @@ TEST_F(HybridObservablesTest, ValidationOfResults) //read measured values observables_dump_reader estimated_observables(2); //two channels ASSERT_NO_THROW({ - if(estimated_observables.open_obs_file(std::string("./observables.dat")) == false) - { - throw std::exception(); - } + if (estimated_observables.open_obs_file(std::string("./observables.dat")) == false) + { + throw std::exception(); + } }) << "Failure opening dump observables file"; nepoch = static_cast(estimated_observables.num_epochs()); @@ -665,43 +665,51 @@ TEST_F(HybridObservablesTest, ValidationOfResults) estimated_observables.restart(); epoch_counter = 0; long int epoch_counter2 = 0; - while(estimated_observables.read_binary_obs()) - { - if(static_cast(estimated_observables.valid[0])) + while (estimated_observables.read_binary_obs()) { - measured_ch0(epoch_counter, 0) = estimated_observables.RX_time[0]; - measured_ch0(epoch_counter, 1) = estimated_observables.TOW_at_current_symbol_s[0]; - measured_ch0(epoch_counter, 2) = estimated_observables.Carrier_Doppler_hz[0]; - measured_ch0(epoch_counter, 3) = estimated_observables.Acc_carrier_phase_hz[0]; - measured_ch0(epoch_counter, 4) = estimated_observables.Pseudorange_m[0]; - epoch_counter++; + if (static_cast(estimated_observables.valid[0])) + { + measured_ch0(epoch_counter, 0) = estimated_observables.RX_time[0]; + measured_ch0(epoch_counter, 1) = estimated_observables.TOW_at_current_symbol_s[0]; + measured_ch0(epoch_counter, 2) = estimated_observables.Carrier_Doppler_hz[0]; + measured_ch0(epoch_counter, 3) = estimated_observables.Acc_carrier_phase_hz[0]; + measured_ch0(epoch_counter, 4) = estimated_observables.Pseudorange_m[0]; + epoch_counter++; + } + if (static_cast(estimated_observables.valid[1])) + { + measured_ch1(epoch_counter2, 0) = estimated_observables.RX_time[1]; + measured_ch1(epoch_counter2, 1) = estimated_observables.TOW_at_current_symbol_s[1]; + measured_ch1(epoch_counter2, 2) = estimated_observables.Carrier_Doppler_hz[1]; + measured_ch1(epoch_counter2, 3) = estimated_observables.Acc_carrier_phase_hz[1]; + measured_ch1(epoch_counter2, 4) = estimated_observables.Pseudorange_m[1]; + epoch_counter2++; + } } - if(static_cast(estimated_observables.valid[1])) - { - measured_ch1(epoch_counter2, 0) = estimated_observables.RX_time[1]; - measured_ch1(epoch_counter2, 1) = estimated_observables.TOW_at_current_symbol_s[1]; - measured_ch1(epoch_counter2, 2) = estimated_observables.Carrier_Doppler_hz[1]; - measured_ch1(epoch_counter2, 3) = estimated_observables.Acc_carrier_phase_hz[1]; - measured_ch1(epoch_counter2, 4) = estimated_observables.Pseudorange_m[1]; - epoch_counter2++; - } - } //Cut measurement tail zeros arma::uvec index = arma::find(measured_ch0.col(0) > 0.0, 1, "last"); - if((index.size() > 0) and index(0) < (nepoch - 1)) - { measured_ch0.shed_rows(index(0) + 1, nepoch - 1); } + if ((index.size() > 0) and index(0) < (nepoch - 1)) + { + measured_ch0.shed_rows(index(0) + 1, nepoch - 1); + } index = arma::find(measured_ch1.col(0) > 0.0, 1, "last"); - if((index.size() > 0) and index(0) < (nepoch - 1)) - { measured_ch1.shed_rows(index(0) + 1, nepoch - 1); } + if ((index.size() > 0) and index(0) < (nepoch - 1)) + { + measured_ch1.shed_rows(index(0) + 1, nepoch - 1); + } //Cut measurement initial transitory of the measurements index = arma::find(measured_ch0.col(0) >= true_ch0(0, 0), 1, "first"); - if((index.size() > 0) and (index(0) > 0)) - { measured_ch0.shed_rows(0, index(0)); } + if ((index.size() > 0) and (index(0) > 0)) + { + measured_ch0.shed_rows(0, index(0)); + } index = arma::find(measured_ch1.col(0) >= true_ch1(0, 0), 1, "first"); - if((index.size() > 0) and (index(0) > 0)) - { measured_ch1.shed_rows(0, index(0)); } + if ((index.size() > 0) and (index(0) > 0)) + { + measured_ch1.shed_rows(0, index(0)); + } //Correct the clock error using true values (it is not possible for a receiver to correct //the receiver clock offset error at the observables level because it is required the @@ -709,14 +717,14 @@ TEST_F(HybridObservablesTest, ValidationOfResults) //Find the reference satellite (the nearest) and compute the receiver time offset at observable level arma::vec receiver_time_offset_s; - if(measured_ch0(0, 4) < measured_ch1(0, 4)) - { - receiver_time_offset_s = true_ch0.col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; - } + if (measured_ch0(0, 4) < measured_ch1(0, 4)) + { + receiver_time_offset_s = true_ch0.col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; + } else - { - receiver_time_offset_s = true_ch1.col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; - } + { + receiver_time_offset_s = true_ch1.col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0; + } arma::vec corrected_reference_TOW_s = true_ch0.col(0) - receiver_time_offset_s; std::cout << "Receiver time offset: " << receiver_time_offset_s(0) * 1e3 << " [ms]" << std::endl; @@ -724,5 +732,5 @@ TEST_F(HybridObservablesTest, ValidationOfResults) check_results_code_psudorange(true_ch0, true_ch1, corrected_reference_TOW_s, measured_ch0, measured_ch1); check_results_carrier_phase(true_ch0, true_ch1, corrected_reference_TOW_s, measured_ch0, measured_ch1); - std::cout << "Test completed in " << elapsed_seconds.count() << " [s]" << std::endl; + std::cout << "Test completed in " << elapsed_seconds.count() << " [s]" << std::endl; } diff --git a/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc index 340e472e5..ddeeda281 100644 --- a/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc @@ -265,13 +265,12 @@ void GpsL1CATelemetryDecoderTest::configure_receiver() // Set Tracking config->set_property("Tracking_1C.item_type", "gr_complex"); - config->set_property("Tracking_1C.if", "0"); config->set_property("Tracking_1C.dump", "true"); config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); config->set_property("Tracking_1C.pll_bw_hz", "20.0"); config->set_property("Tracking_1C.dll_bw_hz", "1.5"); config->set_property("Tracking_1C.early_late_space_chips", "0.5"); - + config->set_property("Tracking_1C.unified", "true"); config->set_property("TelemetryDecoder_1C.dump", "true"); } diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc index 7ed7741da..f9357d4fd 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc @@ -225,11 +225,7 @@ void GpsL1CADllPllTrackingTest::configure_receiver() config->set_property("Tracking_1C.pll_bw_hz", "20.0"); config->set_property("Tracking_1C.dll_bw_hz", "2.0"); config->set_property("Tracking_1C.early_late_space_chips", "0.5"); - config->set_property("Tracking_1C.pll_bw_narrow_hz", "20.0"); - config->set_property("Tracking_1C.dll_bw_narrow_hz", "2.0"); - config->set_property("Tracking_1C.early_late_space_narrow_chips", "0.5"); config->set_property("Tracking_1C.unified", "true"); - config->set_property("Tracking_1C.extend_correlation_ms", "1"); config->set_property("Tracking_1C.dump", "true"); config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); } @@ -395,7 +391,7 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults) true_obs_data.restart(); std::cout << "Initial Doppler [Hz]=" << true_obs_data.doppler_l1_hz << " Initial code delay [Chips]=" << true_obs_data.prn_delay_chips << std::endl; - gnss_synchro.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * baseband_sampling_freq * GPS_L1_CA_CODE_PERIOD; + gnss_synchro.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * static_cast(baseband_sampling_freq) * GPS_L1_CA_CODE_PERIOD; gnss_synchro.Acq_doppler_hz = true_obs_data.doppler_l1_hz; gnss_synchro.Acq_samplestamp_samples = 0; From 583e74be644d971a70f5a0e15d3b7790960ae3a5 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 23 Mar 2018 15:27:31 +0100 Subject: [PATCH 49/63] Fix test --- .../gps_l1_ca_telemetry_decoder_test.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc index ddeeda281..bececcd16 100644 --- a/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc @@ -292,9 +292,9 @@ void GpsL1CATelemetryDecoderTest::check_results(arma::vec& true_time_s, arma::interp1(true_time_s, true_value, meas_time_s, true_value_interp); //2. RMSE - arma::vec err; + //arma::vec err = meas_value - true_value_interp + 0.001; + arma::vec err = meas_value - true_value_interp - 0.001; - err = meas_value - true_value_interp + 0.001; arma::vec err2 = arma::square(err); double rmse = sqrt(arma::mean(err2)); @@ -316,10 +316,10 @@ void GpsL1CATelemetryDecoderTest::check_results(arma::vec& true_time_s, << " [Seconds]" << std::endl; std::cout.precision(ss); - ASSERT_LT(rmse, 0.2E-6); - ASSERT_LT(error_mean, 0.2E-6); - ASSERT_GT(error_mean, -0.2E-6); - ASSERT_LT(error_var, 0.2E-6); + ASSERT_LT(rmse, 0.3E-6); + ASSERT_LT(error_mean, 0.3E-6); + ASSERT_GT(error_mean, -0.3E-6); + ASSERT_LT(error_var, 0.3E-6); ASSERT_LT(max_error, 0.5E-6); ASSERT_GT(min_error, -0.5E-6); } From 843679f0eddaf88ab5cb29cc96f60254a83452f6 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 3 Apr 2018 11:42:11 +0200 Subject: [PATCH 50/63] Report days, hours, mins and secs instead of secs --- .../libs/gnss_sdr_sample_counter.cc | 69 +++++++++++++++---- src/algorithms/libs/gnss_sdr_sample_counter.h | 11 ++- .../adapters/gps_l1_ca_dll_pll_tracking.cc | 36 ++++------ 3 files changed, 77 insertions(+), 39 deletions(-) diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc index 9db529625..197e5725a 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.cc +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -34,15 +34,22 @@ #include 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)), - static_cast(floor(_fs * 0.001))) + gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), + static_cast(floor(_fs * 0.001))) { message_port_register_out(pmt::mp("sample_counter")); set_max_noutput_items(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 + current_s = 0; + current_m = 0; + current_h = 0; + current_days = 0; + report_interval_ms = 1000; //default reporting 1 second + flag_enable_send_msg = false; //enable it for reporting time with asynchronous message + flag_m = false; + flag_h = false; + flag_days = false; } @@ -54,19 +61,55 @@ gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs) 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) + gr_vector_const_void_star &input_items __attribute__((unused)), + gr_vector_void_star &output_items) { - Gnss_Synchro* out = reinterpret_cast(output_items[0]); + Gnss_Synchro *out = reinterpret_cast(output_items[0]); out[0] = Gnss_Synchro(); 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) { - message_port_pub(pmt::mp("receiver_time"), pmt::from_double(static_cast(current_T_rx_ms) / 1000.0)); + current_s++; + if ((current_s % 60) == 0) + { + current_s = 0; + current_m++; + flag_m = true; + if ((current_m % 60) == 0) + { + current_m = 0; + current_h++; + flag_h = true; + if ((current_h % 24) == 0) + { + current_h = 0; + current_days++; + flag_days = true; + } + } + } + + if (flag_days) + { + std::cout << "Current receiver time: " << current_days << " [days] " << current_h << " [h] " << current_m << " [min] " << current_s << " [s]" << std::endl; + } + else if (flag_h) + { + std::cout << "Current receiver time: " << current_h << " [h] " << current_m << " [min] " << current_s << " [s]" << std::endl; + } + else if (flag_m) + { + std::cout << "Current receiver time: " << current_m << " [min] " << current_s << " [s]" << std::endl; + } + else + { + std::cout << "Current receiver time: " << current_s << " [s]" << std::endl; + } + + if (flag_enable_send_msg) + { + message_port_pub(pmt::mp("receiver_time"), pmt::from_double(static_cast(current_T_rx_ms) / 1000.0)); + } } - } current_T_rx_ms++; return 1; } diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.h b/src/algorithms/libs/gnss_sdr_sample_counter.h index 501b57057..74a525eb4 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.h +++ b/src/algorithms/libs/gnss_sdr_sample_counter.h @@ -44,14 +44,19 @@ gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs); class gnss_sdr_sample_counter : public gr::sync_decimator { private: - gnss_sdr_sample_counter(double _fs); - long long int current_T_rx_ms; + long long int current_T_rx_ms; // Receiver time in ms since the beggining of the run + unsigned int current_s; // Receiver time in seconds, modulo 60 + bool flag_m; // True if the receiver has been running for at least 1 minute + unsigned int current_m; // Receiver time in minutes, modulo 60 + bool flag_h; // True if the receiver has been running for at least 1 hour + unsigned int current_h; // Receiver time in hours, modulo 24 + bool flag_days; // True if the receiver has been running for at least 1 day + unsigned int current_days; // Receiver time in days since the beggining of the run 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, diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index f7dc4cdb2..6674422c6 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -90,29 +90,19 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) { - if (unified_) - { - char sig_[3] = "1C"; - item_size_ = sizeof(gr_complex); - tracking_unified_ = dll_pll_veml_make_tracking( - fs_in, vector_length, dump, - dump_filename, pll_bw_hz, dll_bw_hz, - pll_bw_narrow_hz, dll_bw_narrow_hz, - early_late_space_chips, - early_late_space_chips, - early_late_space_narrow_chips, - early_late_space_narrow_chips, - symbols_extended_correlator, - false, - 'G', sig_); - } - else - { - tracking_ = gps_l1_ca_dll_pll_make_tracking_cc( - 0, fs_in, vector_length, dump, - dump_filename, pll_bw_hz, dll_bw_hz, - early_late_space_chips); - } + char sig_[3] = "1C"; + item_size_ = sizeof(gr_complex); + tracking_ = dll_pll_veml_make_tracking( + fs_in, vector_length, dump, + dump_filename, pll_bw_hz, dll_bw_hz, + pll_bw_narrow_hz, dll_bw_narrow_hz, + early_late_space_chips, + early_late_space_chips, + early_late_space_narrow_chips, + early_late_space_narrow_chips, + symbols_extended_correlator, + false, + 'G', sig_); } else { From a421d2fc5adb4de1fa28ac1155f9a443f6c3f361 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 3 Apr 2018 12:45:03 +0200 Subject: [PATCH 51/63] Minor change --- src/algorithms/PVT/libs/rtklib_solver.cc | 7 +++---- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 18 ++++++++++++------ src/core/system_parameters/rtcm.h | 2 -- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index c76ed3260..4dcee81a0 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -241,6 +241,7 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); if (gps_ephemeris_iter != gps_ephemeris_map.cend()) { + /* By the moment, GPS L2 observables are not used in pseudorange computations if GPS L1 is available // 2. If found, replace the existing GPS L1 ephemeris with the GPS L2 ephemeris // (more precise!), and attach the L2 observation to the L1 observation in RTKLIB structure for (int i = 0; i < valid_obs; i++) @@ -248,18 +249,17 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ if (eph_data[i].sat == static_cast(gnss_observables_iter->second.PRN)) { eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); - /* By the moment, GPS L2 observables are not used in pseudorange computations obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs], gnss_observables_iter->second, eph_data[i].week, - 1); //Band 2 (L2) */ + 1); //Band 2 (L2) break; } } + */ } else { - /* By the moment, GPS L2 observables are not used in pseudorange computations // 3. If not found, insert the GPS L2 ephemeris and the observation //convert ephemeris from GNSS-SDR class to RTKLIB structure eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); @@ -273,7 +273,6 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ gps_cnav_ephemeris_iter->second.i_GPS_week, 1); //Band 2 (L2) valid_obs++; - */ } } else // the ephemeris are not available for this SV diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 07e7a5c73..31b60c8e6 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -191,7 +191,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } } /* fL1^2 / fL2(orL5)^2 . See IS-GPS-200, p. 103 and Galileo ICD p. 48 */ - if (sys == SYS_GPS or sys == SYS_GAL) + if (sys == SYS_GPS or sys == SYS_GAL or sys == SYS_GLO) { gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); } @@ -263,9 +263,10 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, PC = P2 + P1_P2 - ISCl5i; } } - else if (sys == SYS_GAL) // Gal. E5a single freq. + else if (sys == SYS_GAL or sys == SYS_GLO) // Gal. E5a single freq. { - //TODO + P2 += P2_C2; /* C2->P2 */ + PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); } } /* dual-frequency */ @@ -280,12 +281,17 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } else if (obs->code[j] == CODE_L5X) /* L1 + L5 */ { - //TODO + //By the moment, GPS L5 pseudoranges are not used + //PC = (P2 + ISCl5i - gamma_ * (P1 + ISCl5i)) / (1.0 - gamma_) - P1_P2; + P1 += P1_C1; /* C1->P1 */ + PC = P1 + P1_P2; } } - else if (sys == SYS_GAL) /* E1 + E5a */ + else if (sys == SYS_GAL or sys == SYS_GLO) /* E1 + E5a */ { - //TODO + P1 += P1_C1; + P2 += P2_C2; + PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); } } if (opt->sateph == EPHOPT_SBAS) diff --git a/src/core/system_parameters/rtcm.h b/src/core/system_parameters/rtcm.h index 591659c6f..1a14cc988 100644 --- a/src/core/system_parameters/rtcm.h +++ b/src/core/system_parameters/rtcm.h @@ -671,7 +671,6 @@ private: } else if (!ec and !read_msg_.decode_header()) { - /* TODO: The commented code throws an exception. Solve it! client_says += read_msg_.data(); bool first = true; while (client_says.length() >= 80) @@ -684,7 +683,6 @@ private: std::cout << client_says.substr(0, 80) << std::endl; client_says = client_says.substr(80, client_says.length() - 80); } - */ do_read_message_header(); } else From 7f521bd2bb178459139e7310cf0b477f57f50045 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 Apr 2018 19:07:05 +0200 Subject: [PATCH 52/63] Fix typo and copyright year --- src/algorithms/libs/gnss_sdr_sample_counter.cc | 4 ++-- src/algorithms/libs/gnss_sdr_sample_counter.h | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc index 197e5725a..7c52d6496 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.cc +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -1,12 +1,12 @@ /*! * \file gnss_sdr_sample_counter.cc * \brief Simple block to report the current receiver time based on the output of the tracking or telemetry blocks - * \author Javier Arribas 2017. jarribas(at)cttc.es + * \author Javier Arribas 2018. jarribas(at)cttc.es * * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.h b/src/algorithms/libs/gnss_sdr_sample_counter.h index 74a525eb4..579222ef3 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.h +++ b/src/algorithms/libs/gnss_sdr_sample_counter.h @@ -1,12 +1,12 @@ /*! * \file gnss_sdr_sample_counter.h * \brief Simple block to report the current receiver time based on the output of the tracking or telemetry blocks - * \author Javier Arribas 2017. jarribas(at)cttc.es + * \author Javier Arribas 2018. jarribas(at)cttc.es * * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -45,14 +45,14 @@ class gnss_sdr_sample_counter : public gr::sync_decimator { private: gnss_sdr_sample_counter(double _fs); - long long int current_T_rx_ms; // Receiver time in ms since the beggining of the run + long long int current_T_rx_ms; // Receiver time in ms since the beginning of the run unsigned int current_s; // Receiver time in seconds, modulo 60 bool flag_m; // True if the receiver has been running for at least 1 minute unsigned int current_m; // Receiver time in minutes, modulo 60 bool flag_h; // True if the receiver has been running for at least 1 hour unsigned int current_h; // Receiver time in hours, modulo 24 bool flag_days; // True if the receiver has been running for at least 1 day - unsigned int current_days; // Receiver time in days since the beggining of the run + unsigned int current_days; // Receiver time in days since the beginning of the run int report_interval_ms; bool flag_enable_send_msg; From d1998849f150cae864fc731a35f4e69671ae78ba Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 Apr 2018 19:15:25 +0200 Subject: [PATCH 53/63] Fix typos --- src/utils/matlab/libs/geoFunctions/deg2dms.m | 2 +- src/utils/matlab/libs/geoFunctions/dms2mat.m | 2 +- src/utils/matlab/libs/plotNavigation.m | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/utils/matlab/libs/geoFunctions/deg2dms.m b/src/utils/matlab/libs/geoFunctions/deg2dms.m index 1f925894d..948e885db 100644 --- a/src/utils/matlab/libs/geoFunctions/deg2dms.m +++ b/src/utils/matlab/libs/geoFunctions/deg2dms.m @@ -9,7 +9,7 @@ function dmsOutput = deg2dms(deg) %%% Save the sign for later processing neg_arg = false; if deg < 0 - % Only positive numbers should be used while spliting into deg/min/sec + % Only positive numbers should be used while splitting into deg/min/sec deg = -deg; neg_arg = true; end diff --git a/src/utils/matlab/libs/geoFunctions/dms2mat.m b/src/utils/matlab/libs/geoFunctions/dms2mat.m index a5c449673..6cafb2488 100644 --- a/src/utils/matlab/libs/geoFunctions/dms2mat.m +++ b/src/utils/matlab/libs/geoFunctions/dms2mat.m @@ -84,7 +84,7 @@ msign = signvec .* (d==0 & m~=0); ssign = signvec .* (d==0 & m==0 & s~=0); % In the application of signs below, the comparison with 0 is used so that -% the sign vector contains only +1 and -1. Any zero occurances causes +% the sign vector contains only +1 and -1. Any zero occurrences causes % data to be lost when the sign has been applied to a higher component % of d:m:s. Use fix function to eliminate potential round-off errors. diff --git a/src/utils/matlab/libs/plotNavigation.m b/src/utils/matlab/libs/plotNavigation.m index 9e27fc4d2..08460c702 100644 --- a/src/utils/matlab/libs/plotNavigation.m +++ b/src/utils/matlab/libs/plotNavigation.m @@ -47,7 +47,7 @@ function plotNavigation(navSolutions, settings,plot_skyplot) if (~isempty(navSolutions)) %% If reference position is not provided, then set reference position - %% to the average postion + %% to the average position if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ... || isnan(settings.truePosition.U) From 79f9810bf4a0a39112e274d55ecad0d2f3ac41a3 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 Apr 2018 19:17:44 +0200 Subject: [PATCH 54/63] Bump minimum Armadillo version to 5.300 since interp1 is required --- CMakeLists.txt | 2 +- src/tests/CMakeLists.txt | 3 --- src/tests/test_main.cc | 2 -- 3 files changed, 1 insertion(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 68c5ddb0c..9d7513d62 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -318,7 +318,7 @@ set(GNSSSDR_GNURADIO_MIN_VERSION "3.7.3") set(GNSSSDR_BOOST_MIN_VERSION "1.45") set(GNSSSDR_PYTHON_MIN_VERSION "2.7") set(GNSSSDR_MAKO_MIN_VERSION "0.4.2") -set(GNSSSDR_ARMADILLO_MIN_VERSION "4.200.0") +set(GNSSSDR_ARMADILLO_MIN_VERSION "5.300.0") set(GNSSSDR_MATIO_MIN_VERSION "1.5.3") diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 82f4e97d9..c43b47a07 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -351,9 +351,6 @@ include_directories( # Unit testing ################################################################################ if(ENABLE_UNIT_TESTING) - if( ${ARMADILLO_VERSION_STRING} STRGREATER "5.300") # make sure interp1 is present - add_definitions(-DMODERN_ARMADILLO) - endif( ${ARMADILLO_VERSION_STRING} STRGREATER "5.300") add_executable(run_tests ${CMAKE_CURRENT_SOURCE_DIR}/test_main.cc) target_link_libraries(run_tests ${CLANG_FLAGS} diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index ed4f0f4ea..0a9a9d2b0 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -146,12 +146,10 @@ DECLARE_string(log_dir); #include "unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc" #include "unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc" #include "unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc" -#if MODERN_ARMADILLO #include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc" #include "unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc" #include "unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc" #endif -#endif #include "unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc" #include "unit-tests/system-parameters/glonass_gnav_nav_message_test.cc" From c9630013f1d57ae82e3201655543ffe0860ac975 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 Apr 2018 19:28:56 +0200 Subject: [PATCH 55/63] Remove old comment --- .../tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc | 2 +- .../tracking/adapters/galileo_e1_tcp_connector_tracking.cc | 2 +- .../tracking/adapters/galileo_e5a_dll_pll_tracking.cc | 2 +- .../tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc | 3 +-- .../tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc | 2 +- .../tracking/adapters/glonass_l2_ca_dll_pll_c_aid_tracking.cc | 3 +-- .../tracking/adapters/glonass_l2_ca_dll_pll_tracking.cc | 2 +- .../tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc | 3 +-- src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc | 2 +- .../tracking/adapters/gps_l1_ca_dll_pll_tracking_gpu.cc | 3 +-- .../tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc | 2 +- src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc | 2 +- src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc | 2 +- 13 files changed, 13 insertions(+), 17 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index f846fad0a..012cc62ad 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -82,7 +82,7 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( std::cout << TEXT_RED << "WARNING: Galileo E1. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << std::endl; } std::string default_dump_filename = "./track_ch"; - std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); int vector_length = std::round(fs_in / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/galileo_e1_tcp_connector_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_tcp_connector_tracking.cc index 6f4535fe2..f2253197a 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_tcp_connector_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_tcp_connector_tracking.cc @@ -75,7 +75,7 @@ GalileoE1TcpConnectorTracking::GalileoE1TcpConnectorTracking( very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", 0.6); port_ch0 = configuration->property(role + ".port_ch0", 2060); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc index 816710efc..5a61f2007 100644 --- a/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e5a_dll_pll_tracking.cc @@ -66,7 +66,7 @@ GalileoE5aDllPllTracking::GalileoE5aDllPllTracking( int ti_ms = configuration->property(role + ".ti_ms", 3); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); int vector_length = std::round(fs_in / (Galileo_E5a_CODE_CHIP_RATE_HZ / Galileo_E5a_CODE_LENGTH_CHIPS)); int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc index 481be8080..960fd928c 100644 --- a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_c_aid_tracking.cc @@ -80,8 +80,7 @@ GlonassL1CaDllPllCAidTracking::GlonassL1CaDllPllCAidTracking( early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc index cf082d8b6..493097f27 100644 --- a/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/glonass_l1_ca_dll_pll_tracking.cc @@ -72,7 +72,7 @@ GlonassL1CaDllPllTracking::GlonassL1CaDllPllTracking( if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GLONASS_L1_CA_CODE_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_c_aid_tracking.cc b/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_c_aid_tracking.cc index 59f0dedb0..2e284bab8 100644 --- a/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_c_aid_tracking.cc +++ b/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_c_aid_tracking.cc @@ -78,8 +78,7 @@ GlonassL2CaDllPllCAidTracking::GlonassL2CaDllPllCAidTracking( early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GLONASS_L2_CA_CODE_RATE_HZ / GLONASS_L2_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_tracking.cc index 12cd75953..3b029aae3 100644 --- a/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/glonass_l2_ca_dll_pll_tracking.cc @@ -70,7 +70,7 @@ GlonassL2CaDllPllTracking::GlonassL2CaDllPllTracking( if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GLONASS_L2_CA_CODE_RATE_HZ / GLONASS_L2_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc index 87c4a26c7..1b0b9b93b 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc @@ -79,8 +79,7 @@ GpsL1CaDllPllCAidTracking::GpsL1CaDllPllCAidTracking( early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index 6674422c6..66e82aca9 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -65,7 +65,7 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking( float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.5); std::string default_dump_filename = "./track_ch"; - std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); int vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); if (symbols_extended_correlator < 1) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_gpu.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_gpu.cc index 181e07317..7a881edca 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_gpu.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking_gpu.cc @@ -72,8 +72,7 @@ GpsL1CaDllPllTrackingGPU::GpsL1CaDllPllTrackingGPU( if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast(FLAGS_dll_bw_hz); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", - default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc index 3502ec617..aab34cf76 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_tcp_connector_tracking.cc @@ -67,7 +67,7 @@ GpsL1CaTcpConnectorTracking::GpsL1CaTcpConnectorTracking( early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); port_ch0 = configuration->property(role + ".port_ch0", 2060); std::string default_dump_filename = "./track_ch"; - dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc index 4180f161f..563316bf5 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc @@ -63,7 +63,7 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking( unified_ = configuration->property(role + ".unified", false); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); int vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L2_M_CODE_RATE_HZ) / static_cast(GPS_L2_M_CODE_LENGTH_CHIPS))); int symbols_extended_correlator = configuration->property(role + ".extend_correlation_symbols", 1); if (symbols_extended_correlator != 1) diff --git a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc index aa1885fd2..d28255416 100644 --- a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc @@ -65,7 +65,7 @@ GpsL5iDllPllTracking::GpsL5iDllPllTracking( float dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); float early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5); std::string default_dump_filename = "./track_ch"; - std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! + std::string dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); int vector_length = std::round(static_cast(fs_in) / (static_cast(GPS_L5i_CODE_RATE_HZ) / static_cast(GPS_L5i_CODE_LENGTH_CHIPS))); int extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); float early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); From a3296bd71930ac4dbc13a839ec22ac1d723e3918 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 Apr 2018 19:39:35 +0200 Subject: [PATCH 56/63] Fix possible uninitialization --- .../galileo_e1_tcp_connector_tracking_cc.cc | 4 ++-- .../gps_l1_ca_tcp_connector_tracking_cc.cc | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc index e028893ca..a7c0062a2 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc @@ -267,8 +267,8 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attri gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars - float carr_error_filt_hz; - float code_error_filt_chips; + float carr_error_filt_hz = 0.0; + float code_error_filt_chips = 0.0; tcp_packet_data tcp_data; // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc index f4fe4c018..3e3515e29 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc @@ -297,10 +297,10 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attrib gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars - float carr_error; - float carr_nco; - float code_error; - float code_nco; + float carr_error = 0.0; + float carr_nco = 0.0; + float code_error = 0.0; + float code_nco = 0.0; tcp_packet_data tcp_data; // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder From ec2550f9965806304660fbc6094b52aa332296d1 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 Apr 2018 20:09:34 +0200 Subject: [PATCH 57/63] Remove unused variable --- .../gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc index 3e3515e29..cd4c86652 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc @@ -298,7 +298,6 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attrib { // process vars float carr_error = 0.0; - float carr_nco = 0.0; float code_error = 0.0; float code_nco = 0.0; From 2d347740d075936daa3413380d244a5fa90bcc36 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 Apr 2018 23:21:44 +0200 Subject: [PATCH 58/63] Fix time reporting --- .../libs/gnss_sdr_sample_counter.cc | 45 +++++++++++++------ 1 file changed, 31 insertions(+), 14 deletions(-) diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc index 7c52d6496..ac4ac67cc 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.cc +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -32,11 +32,14 @@ #include "gnss_sdr_sample_counter.h" #include "gnss_synchro.h" #include +#include +#include +#include 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)), - static_cast(floor(_fs * 0.001))) + static_cast(std::floor(_fs * 0.001))) { message_port_register_out(pmt::mp("sample_counter")); set_max_noutput_items(1); @@ -45,8 +48,8 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs) : gr::sync_decimato current_m = 0; current_h = 0; current_days = 0; - report_interval_ms = 1000; //default reporting 1 second - flag_enable_send_msg = false; //enable it for reporting time with asynchronous message + report_interval_ms = 1000; // default reporting 1 second + flag_enable_send_msg = false; // enable it for reporting time with asynchronous message flag_m = false; flag_h = false; flag_days = false; @@ -90,21 +93,35 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), if (flag_days) { - std::cout << "Current receiver time: " << current_days << " [days] " << current_h << " [h] " << current_m << " [min] " << current_s << " [s]" << std::endl; - } - else if (flag_h) - { - std::cout << "Current receiver time: " << current_h << " [h] " << current_m << " [min] " << current_s << " [s]" << std::endl; - } - else if (flag_m) - { - std::cout << "Current receiver time: " << current_m << " [min] " << current_s << " [s]" << std::endl; + std::string day; + if (current_days == 1) + { + day = " day "; + } + else + { + day = " days "; + } + std::cout << "Current receiver time: " << current_days << day << current_h << " h " << current_m << " min " << current_s << " s" << std::endl; } else { - std::cout << "Current receiver time: " << current_s << " [s]" << std::endl; + if (flag_h) + { + std::cout << "Current receiver time: " << current_h << " h " << current_m << " min " << current_s << " s" << std::endl; + } + else + { + if (flag_m) + { + std::cout << "Current receiver time: " << current_m << " min " << current_s << " s" << std::endl; + } + else + { + std::cout << "Current receiver time: " << current_s << " s" << std::endl; + } + } } - if (flag_enable_send_msg) { message_port_pub(pmt::mp("receiver_time"), pmt::from_double(static_cast(current_T_rx_ms) / 1000.0)); From 7bb4b0e065e09bc20d7714a64453318df77b9d0d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 Apr 2018 23:22:20 +0200 Subject: [PATCH 59/63] Colors for everyone --- CMakeLists.txt | 1 - src/core/system_parameters/display.h | 71 +++++++++++++++------------- 2 files changed, 38 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9d7513d62..563adfc03 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,7 +138,6 @@ set(OS_IS_LINUX "") if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") set(OperatingSystem "Linux") set(OS_IS_LINUX TRUE) - add_definitions( -DDISPLAY_COLORS=1 ) if(ARCH_64BITS) set(ARCH_ "(64 bits)") else(ARCH_64BITS) diff --git a/src/core/system_parameters/display.h b/src/core/system_parameters/display.h index b6c3c05b8..362e66f6f 100644 --- a/src/core/system_parameters/display.h +++ b/src/core/system_parameters/display.h @@ -31,47 +31,52 @@ #ifndef GNSS_SDR_DISPLAY_H_ #define GNSS_SDR_DISPLAY_H_ -#include +#include + +#ifndef NO_DISPLAY_COLORS +#define DISPLAY_COLORS 1 +#endif + #ifdef DISPLAY_COLORS -const std::string TEXT_RESET = "\033[0m"; -const std::string TEXT_BLACK = "\033[30m"; -const std::string TEXT_RED = "\033[31m"; -const std::string TEXT_GREEN = "\033[32m"; -const std::string TEXT_YELLOW = "\033[33m"; -const std::string TEXT_BLUE = "\033[34m"; -const std::string TEXT_MAGENTA = "\033[35m"; -const std::string TEXT_CYAN = "\033[36m"; -const std::string TEXT_WHITE = "\033[37m"; -const std::string TEXT_BOLD_BLACK = "\033[1m\033[30m"; -const std::string TEXT_BOLD_RED = "\033[1m\033[31m"; -const std::string TEXT_BOLD_GREEN = "\033[1m\033[32m"; -const std::string TEXT_BOLD_YELLOW = "\033[1m\033[33m"; -const std::string TEXT_BOLD_BLUE = "\033[1m\033[34m"; +const std::string TEXT_RESET = "\033[0m"; +const std::string TEXT_BLACK = "\033[30m"; +const std::string TEXT_RED = "\033[31m"; +const std::string TEXT_GREEN = "\033[32m"; +const std::string TEXT_YELLOW = "\033[33m"; +const std::string TEXT_BLUE = "\033[34m"; +const std::string TEXT_MAGENTA = "\033[35m"; +const std::string TEXT_CYAN = "\033[36m"; +const std::string TEXT_WHITE = "\033[37m"; +const std::string TEXT_BOLD_BLACK = "\033[1m\033[30m"; +const std::string TEXT_BOLD_RED = "\033[1m\033[31m"; +const std::string TEXT_BOLD_GREEN = "\033[1m\033[32m"; +const std::string TEXT_BOLD_YELLOW = "\033[1m\033[33m"; +const std::string TEXT_BOLD_BLUE = "\033[1m\033[34m"; const std::string TEXT_BOLD_MAGENTA = "\033[1m\033[35m"; -const std::string TEXT_BOLD_CYAN = "\033[1m\033[36m"; -const std::string TEXT_BOLD_WHITE = "\033[1m\033[37m"; +const std::string TEXT_BOLD_CYAN = "\033[1m\033[36m"; +const std::string TEXT_BOLD_WHITE = "\033[1m\033[37m"; #else -const std::string TEXT_RESET = ""; -const std::string TEXT_BLACK = ""; -const std::string TEXT_RED = ""; -const std::string TEXT_GREEN = ""; -const std::string TEXT_YELLOW = ""; -const std::string TEXT_BLUE = ""; -const std::string TEXT_MAGENTA = ""; -const std::string TEXT_CYAN = ""; -const std::string TEXT_WHITE = ""; -const std::string TEXT_BOLD_BLACK = ""; -const std::string TEXT_BOLD_RED = ""; -const std::string TEXT_BOLD_GREEN = ""; -const std::string TEXT_BOLD_YELLOW = ""; -const std::string TEXT_BOLD_BLUE = ""; +const std::string TEXT_RESET = ""; +const std::string TEXT_BLACK = ""; +const std::string TEXT_RED = ""; +const std::string TEXT_GREEN = ""; +const std::string TEXT_YELLOW = ""; +const std::string TEXT_BLUE = ""; +const std::string TEXT_MAGENTA = ""; +const std::string TEXT_CYAN = ""; +const std::string TEXT_WHITE = ""; +const std::string TEXT_BOLD_BLACK = ""; +const std::string TEXT_BOLD_RED = ""; +const std::string TEXT_BOLD_GREEN = ""; +const std::string TEXT_BOLD_YELLOW = ""; +const std::string TEXT_BOLD_BLUE = ""; const std::string TEXT_BOLD_MAGENTA = ""; -const std::string TEXT_BOLD_CYAN = ""; -const std::string TEXT_BOLD_WHITE = ""; +const std::string TEXT_BOLD_CYAN = ""; +const std::string TEXT_BOLD_WHITE = ""; #endif /* DISPLAY_COLORS */ #endif /* GNSS_SDR_DISPLAY_H_ */ From bc6a5684148b78c855ef596a9f7ed05a22f4357f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 3 Apr 2018 23:45:06 +0200 Subject: [PATCH 60/63] Reorder headers --- .../gnuradio_blocks/hybrid_observables_cc.cc | 17 ++++++++++++----- .../gnuradio_blocks/hybrid_observables_cc.h | 11 +++++------ 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 81daa31ca..6e95da824 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -1,6 +1,6 @@ /*! * \file hybrid_observables_cc.cc - * \brief Implementation of the pseudorange computation block + * \brief Implementation of the observables computation block * \author Javier Arribas 2017. jarribas(at)cttc.es * \author Antonio Ramos 2018. antonio.ramos(at)cttc.es * @@ -30,6 +30,7 @@ */ #include "hybrid_observables_cc.h" +#include "display.h" #include "GPS_L1_CA.h" #include #include @@ -39,7 +40,6 @@ #include #include #include -#include "display.h" using google::LogMessage; @@ -51,9 +51,12 @@ hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels_in, } -hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename) : gr::block("hybrid_observables_cc", - gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)), - gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) +hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, + unsigned int nchannels_out, + bool dump, + std::string dump_filename) : gr::block("hybrid_observables_cc", + gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)), + gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) { d_dump = dump; d_nchannels = nchannels_out; @@ -298,6 +301,7 @@ int hybrid_observables_cc::save_matfile() return 0; } + bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, std::deque &data, const double &ti) { if ((ti < data.front().RX_time) or (ti > data.back().RX_time)) @@ -334,6 +338,7 @@ bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, std::deque &data) { while (data.size() > 0) @@ -372,6 +378,7 @@ void hybrid_observables_cc::clean_history(std::deque &data) } } + void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector &data) { std::vector::iterator it; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index cf5f775d4..aedba43ab 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -36,11 +36,11 @@ #include "gnss_synchro.h" #include +#include #include #include -#include //std::vector +#include #include -#include class hybrid_observables_cc; @@ -57,9 +57,9 @@ class hybrid_observables_cc : public gr::block { public: ~hybrid_observables_cc(); - int general_work(int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - void forecast(int noutput_items, gr_vector_int &ninput_items_required); + int general_work(int noutput_items, gr_vector_int& ninput_items, + gr_vector_const_void_star& input_items, gr_vector_void_star& output_items); + void forecast(int noutput_items, gr_vector_int& ninput_items_required); private: friend hybrid_observables_cc_sptr @@ -82,7 +82,6 @@ private: unsigned int d_num_valid_channels; std::string d_dump_filename; std::ofstream d_dump_file; - }; #endif From 46bc52ce9e8ea5dc7f7bb304fbfc8b788c4ac872 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 4 Apr 2018 11:16:35 +0200 Subject: [PATCH 61/63] Sample counter now accepts any data type --- .../libs/gnss_sdr_sample_counter.cc | 12 +++---- src/algorithms/libs/gnss_sdr_sample_counter.h | 6 ++-- src/algorithms/libs/pass_through.h | 1 - src/core/receiver/gnss_flowgraph.cc | 36 +++++++++---------- 4 files changed, 26 insertions(+), 29 deletions(-) diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc index ac4ac67cc..bfe53f6af 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.cc +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -36,10 +36,10 @@ #include #include -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)), - static_cast(std::floor(_fs * 0.001))) +gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs, size_t _size) : gr::sync_decimator("sample_counter", + gr::io_signature::make(1, 1, _size), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), + static_cast(std::floor(_fs * 0.001))) { message_port_register_out(pmt::mp("sample_counter")); set_max_noutput_items(1); @@ -56,9 +56,9 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs) : gr::sync_decimato } -gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs) +gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size) { - gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter(_fs)); + gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter(_fs, _size)); return sample_counter_; } diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.h b/src/algorithms/libs/gnss_sdr_sample_counter.h index 579222ef3..761894855 100644 --- a/src/algorithms/libs/gnss_sdr_sample_counter.h +++ b/src/algorithms/libs/gnss_sdr_sample_counter.h @@ -39,12 +39,12 @@ 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, size_t _size); class gnss_sdr_sample_counter : public gr::sync_decimator { private: - gnss_sdr_sample_counter(double _fs); + gnss_sdr_sample_counter(double _fs, size_t _size); long long int current_T_rx_ms; // Receiver time in ms since the beginning of the run unsigned int current_s; // Receiver time in seconds, modulo 60 bool flag_m; // True if the receiver has been running for at least 1 minute @@ -57,7 +57,7 @@ private: bool flag_enable_send_msg; public: - friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs); + friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size); int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); diff --git a/src/algorithms/libs/pass_through.h b/src/algorithms/libs/pass_through.h index 1a9538ae4..f1d8c5f72 100644 --- a/src/algorithms/libs/pass_through.h +++ b/src/algorithms/libs/pass_through.h @@ -88,7 +88,6 @@ private: std::string role_; unsigned int in_streams_; unsigned int out_streams_; - //gr_kludge_copy_sptr kludge_copy_; gr::blocks::copy::sptr kludge_copy_; size_t item_size_; conjugate_cc_sptr conjugate_cc_; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index a1fa6855f..eec75d2f0 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -61,8 +61,6 @@ GNSSFlowgraph::GNSSFlowgraph(std::shared_ptr configurati GNSSFlowgraph::~GNSSFlowgraph() {} - - void GNSSFlowgraph::start() { if (running_) @@ -249,25 +247,25 @@ void GNSSFlowgraph::connect() //connect the signal source to sample counter //connect the sample counter to Observables try - { - double fs = static_cast(configuration_->property("GNSS-SDR.internal_fs_sps", 0)); - if(fs == 0.0) { - LOG(WARNING) << "Set GNSS-SDR.internal_fs_sps in configuration file"; - std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file" << std::endl; - throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration")); + double fs = static_cast(configuration_->property("GNSS-SDR.internal_fs_sps", 0)); + if (fs == 0.0) + { + LOG(WARNING) << "Set GNSS-SDR.internal_fs_sps in configuration file"; + std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file" << std::endl; + throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration")); + } + ch_out_sample_counter = gnss_sdr_make_sample_counter(fs, sig_conditioner_.at(0)->get_right_block()->output_signature()->sizeof_stream_item(0)); + top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter, 0); + top_block_->connect(ch_out_sample_counter, 0, observables_->get_left_block(), channels_count_); //extra port for the sample counter pulse + } + catch (const std::exception& e) + { + LOG(WARNING) << "Can't connect sample counter"; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; } - ch_out_sample_counter = gnss_sdr_make_sample_counter(fs); - top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter, 0); - top_block_->connect(ch_out_sample_counter, 0, observables_->get_left_block(), channels_count_); //extra port for the sample counter pulse - } - catch (const std::exception & e) - { - LOG(WARNING) << "Can't connect sample counter"; - LOG(ERROR) << e.what(); - top_block_->disconnect_all(); - return; - } // Signal conditioner (selected_signal_source) >> channels (i) (dependent of their associated SignalSource_ID) From fc0e973adf2291cb911b8493d4e4cc14424960eb Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 4 Apr 2018 11:29:38 +0200 Subject: [PATCH 62/63] Fix unit test --- .../observables/hybrid_observables_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc index 5b5c9ff12..541a4ce1d 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -567,7 +567,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults) gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make(); gr::blocks::null_sink::sptr sink_ch0 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); gr::blocks::null_sink::sptr sink_ch1 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); - gnss_sdr_sample_counter_sptr samp_counter = gnss_sdr_make_sample_counter(static_cast(baseband_sampling_freq)); + gnss_sdr_sample_counter_sptr samp_counter = gnss_sdr_make_sample_counter(static_cast(baseband_sampling_freq), sizeof(int8_t)); top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0); top_block->connect(gr_interleaved_char_to_complex, 0, samp_counter, 0); From 1e7d9ef79469117dda0ab0f93b27f75ee51c2416 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 4 Apr 2018 11:52:24 +0200 Subject: [PATCH 63/63] Fix test. Wrong data type --- .../observables/hybrid_observables_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc index 541a4ce1d..14e8bb760 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -567,7 +567,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults) gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make(); gr::blocks::null_sink::sptr sink_ch0 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); gr::blocks::null_sink::sptr sink_ch1 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); - gnss_sdr_sample_counter_sptr samp_counter = gnss_sdr_make_sample_counter(static_cast(baseband_sampling_freq), sizeof(int8_t)); + gnss_sdr_sample_counter_sptr samp_counter = gnss_sdr_make_sample_counter(static_cast(baseband_sampling_freq), sizeof(gr_complex)); top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0); top_block->connect(gr_interleaved_char_to_complex, 0, samp_counter, 0);