From 8885333aa78ba98f64bea8083dab0b0f66d0d51a Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 13 Feb 2018 18:16:03 +0100 Subject: [PATCH 01/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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/61] 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 cbf26e7dd592ff7215546a8246bd39f488ce883b Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 30 Mar 2018 12:46:21 +0200 Subject: [PATCH 50/61] Make documentation reproducible --- .../libs/volk_gnsssdr_module/volk_gnsssdr/Doxyfile.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/Doxyfile.in b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/Doxyfile.in index 2c0a87a3c..8fa50f08d 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/Doxyfile.in +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/Doxyfile.in @@ -145,7 +145,7 @@ INLINE_INHERITED_MEMB = NO # shortest path that makes the file name unique will be used # The default value is: YES. -FULL_PATH_NAMES = YES +FULL_PATH_NAMES = NO # The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. # Stripping is only done if one of the specified strings matches the left-hand From 2785a751c1239e07b142599b4df5f10bc7c52b76 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 2 Apr 2018 01:36:21 +0200 Subject: [PATCH 51/61] Fix typos) --- CONTRIBUTING.md | 4 +-- README.md | 32 +++++++++---------- ...gnss-sdr_GPS_L1_2ch_fmcomms2_realtime.conf | 2 -- .../volk_gnsssdr/README.md | 4 +-- 4 files changed, 20 insertions(+), 22 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 8af901f52..7a87240cb 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -67,7 +67,7 @@ GitHub](https://github.com/join). GitHub](https://github.com/gnss-sdr/gnss-sdr/fork). This will copy the whole gnss-sdr repository to your personal account. - 3. Then, go to your favourite working folder in your computer and + 3. Then, go to your favorite working folder in your computer and clone your forked repository by typing (replacing ```YOUR_USERNAME``` by the actual username of your GitHub account): @@ -128,7 +128,7 @@ $ git pull --rebase upstream next ### How to submit a pull request -Before submitting you code, please be sure to [apply clang-format](http://gnss-sdr.org/coding-style/#use-tools-for-automated-code-formatting). +Before submitting your code, please be sure to [apply clang-format](http://gnss-sdr.org/coding-style/#use-tools-for-automated-code-formatting). When the contribution is ready, you can [submit a pull request](https://github.com/gnss-sdr/gnss-sdr/compare/). Head to your diff --git a/README.md b/README.md index 3ebdf9c22..270eaf8d5 100644 --- a/README.md +++ b/README.md @@ -304,7 +304,7 @@ $ cmake ../ $ make ~~~~~~ -By default, CMake will build the Release version, meaning that the compiler will generate a fast, optimized executable. This is the recommended build type when using a RF front-end and you need to attain real time. If working with a file (and thus without real-time constraints), you may want to obtain more information about the internals of the receiver, as well as more fine-grained logging. This can be done by building the Debug version, by doing: +By default, CMake will build the Release version, meaning that the compiler will generate a fast, optimized executable. This is the recommended build type when using an RF front-end and you need to attain real time. If working with a file (and thus without real-time constraints), you may want to obtain more information about the internals of the receiver, as well as more fine-grained logging. This can be done by building the Debug version, by doing: ~~~~~~ $ cmake -DCMAKE_BUILD_TYPE=Debug ../ @@ -698,7 +698,7 @@ Getting started We use a [DBSRX2](https://www.ettus.com/product/details/DBSRX2) to do the task, but you can try the newer Ettus' daughter boards as well. 3. The easiest way to capture a signal file is to use the GNU Radio Companion GUI. Only two blocks are needed: a USRP signal source connected to complex float file sink. You need to tune the USRP central frequency and decimation factor using USRP signal source properties box. We suggest using a decimation factor of 20 if you use the USRP2. This will give you 100/20 = 5 MSPS which will be enough to receive GPS L1 C/A signals. The front-end gain should also be configured. In our test with the DBSRX2 we obtained good results with ```G=50```. 4. Capture at least 80 seconds of signal in open sky conditions. During the process, be aware of USRP driver buffer underruns messages. If your hard disk is not fast enough to write data at this speed you can capture to a virtual RAM drive. 80 seconds of signal at 5 MSPS occupies less than 3 Gbytes using ```gr_complex```. - 5. If you have no access to a RF front-end, you can download a sample raw data file (that contains GPS and Galileo signals) from [here](http://sourceforge.net/projects/gnss-sdr/files/data/). + 5. If you have no access to an RF front-end, you can download a sample raw data file (that contains GPS and Galileo signals) from [here](http://sourceforge.net/projects/gnss-sdr/files/data/). 3. You are ready to configure the receiver to use your captured file among other parameters: 1. The default configuration file resides at [/usr/local/share/gnss-sdr/conf/default.conf](./conf/gnss-sdr.conf). 2. You need to review/modify at least the following settings: @@ -718,7 +718,7 @@ For more information, check out our [quick start guide](http://gnss-sdr.org/quic Using GNSS-SDR ============== -With GNSS-SDR, you can define you own receiver, work with captured raw data or from a RF front-end, dump into files intermediate signals, or tune every single algorithm used in the signal processing. All the configuration is done in a single file. Those configuration files reside at the [gnss-sdr/conf/](./conf/) folder (or at /usr/local/share/gnss-sdr/conf if you installed the program). By default, the executable ```gnss-sdr``` will read the configuration available at ```gnss-sdr/conf/gnss-sdr.conf``` (or at (usr/local/share/gnss-sdr/conf/default.conf if you installed the program). You can edit that file to fit your needs, or even better, define a new ```my_receiver.conf``` file with your own configuration. This new receiver can be generated by invoking gnss-sdr with the ```--config_file``` flag pointing to your configuration file: +With GNSS-SDR, you can define your own receiver, work with captured raw data or from an RF front-end, dump into files intermediate signals, or tune every single algorithm used in the signal processing. All the configuration is done in a single file. Those configuration files reside at the [gnss-sdr/conf/](./conf/) folder (or at /usr/local/share/gnss-sdr/conf if you installed the program). By default, the executable ```gnss-sdr``` will read the configuration available at ```gnss-sdr/conf/gnss-sdr.conf``` (or at (usr/local/share/gnss-sdr/conf/default.conf if you installed the program). You can edit that file to fit your needs, or even better, define a new ```my_receiver.conf``` file with your own configuration. This new receiver can be generated by invoking gnss-sdr with the ```--config_file``` flag pointing to your configuration file: ~~~~~~ $ gnss-sdr --config_file=/path/to/my_receiver.conf @@ -770,7 +770,7 @@ Since the configuration is just a set of property names and values without any m ### GNSS block factory -Hence, the application defines a simple accessor class to fetch the configuration pairs of values and passes them to a factory class called [GNSSBlockFactory](./src/core/receiver/gnss_block_factory.h). This factory decides, according to the configuration, which class needs to be instantiated and which parameters should be passed to the constructor. Hence, the factory encapsulates the complexity of blocks' instantiation. With that approach, adding a new block that requires new parameters will be as simple as adding the block class and modifying the factory to be able to instantiate it. This loose coupling between the blocks' implementations and the syntax of the configuration enables extending the application capacities in a high degree. It also allows to produce fully customized receivers, for instance a testbed for acquisition algorithms, and to place observers at any point of the receiver chain. +Hence, the application defines a simple accessor class to fetch the configuration pairs of values and passes them to a factory class called [GNSSBlockFactory](./src/core/receiver/gnss_block_factory.h). This factory decides, according to the configuration, which class needs to be instantiated and which parameters should be passed to the constructor. Hence, the factory encapsulates the complexity of blocks' instantiation. With that approach, adding a new block that requires new parameters will be as simple as adding the block class and modifying the factory to be able to instantiate it. This loose coupling between the blocks' implementations and the syntax of the configuration enables extending the application capacities in a high degree. It also allows producing fully customized receivers, for instance a testbed for acquisition algorithms, and to place observers at any point of the receiver chain. More information can be found at the [Control Plane page](http://gnss-sdr.org/docs/control-plane/). @@ -784,9 +784,9 @@ GNU Radio's class ```gr::basic_block``` is the abstract base class for all signa A signal processing flow is constructed by creating a tree of hierarchical blocks, which at any level may also contain terminal nodes that actually implement signal processing functions. -Class ```gr::top_block``` is the top-level hierarchical block representing a flowgraph. It defines GNU Radio runtime functions used during the execution of the program: run(), start(), stop(), wait(), etc. A a subclass called [GNSSBlockInterface](./src/core/interfaces/gnss_block_interface.h) is the common interface for all the GNSS-SDR modules. It defines pure virtual methods, that are required to be implemented by a derived class. +Class ```gr::top_block``` is the top-level hierarchical block representing a flowgraph. It defines GNU Radio runtime functions used during the execution of the program: run(), start(), stop(), wait(), etc. A subclass called [GNSSBlockInterface](./src/core/interfaces/gnss_block_interface.h) is the common interface for all the GNSS-SDR modules. It defines pure virtual methods, that are required to be implemented by a derived class. -Subclassing GNSSBlockInterface, we defined interfaces for the GNSS receiver blocks depicted in the figure above. This hierarchy provides the definition of different algorithms and different implementations, which will be instantiated according to the configuration. This strategy allows multiple implementations sharing a common interface, achieving the objective of decoupling interfaces from implementations: it defines a family of algorithms, encapsulates each one, and makes them interchangeable. Hence, we let the algorithm vary independently from the program that uses it. +Subclassing GNSSBlockInterface, we defined interfaces for the GNSS receiver blocks depicted in the figure above. This hierarchy provides the definition of different algorithms and different implementations, which will be instantiated according to the configuration. This strategy allows multiple implementations sharing a common interface, achieving the objective of decoupling interfaces from implementations: it defines a family of algorithms, encapsulates each one, and makes them interchangeable. Hence, we let the algorithm vary independently of the program that uses it. Internally, GNSS-SDR makes use of the complex data types defined by [VOLK](http://libvolk.org/ "Vector-Optimized Library of Kernels home"). They are fundamental for handling sample streams in which samples are complex numbers with real and imaginary components of 8, 16 or 32 bits, common formats delivered by GNSS (and generic SDR) radio frequency front-ends. The following list shows the data type names that GNSS-SDR exposes through the configuration file: @@ -806,7 +806,7 @@ More information about the available processing blocks and their configuration p The input of a software receiver are the raw bits that come out from the front-end's analog-to-digital converter (ADC). Those bits can be read from a file stored in the hard disk or directly in real-time from a hardware device through USB or Ethernet buses. -The Signal Source module is in charge of implementing the hardware driver, that is, the portion of the code that communicates with the RF front-end and receives the samples coming from the ADC. This communication is usually performed through USB or Ethernet buses. Since real-time processing requires a highly optimized implementation of the whole receiver, this module also allows to read samples from a file stored in a hard disk, and thus processing without time constraints. Relevant parameters of those samples are the intermediate frequency (or baseband I&Q components), the sampling rate and number of bits per sample, that must be specified by the user in the configuration file. +The Signal Source module is in charge of implementing the hardware driver, that is, the portion of the code that communicates with the RF front-end and receives the samples coming from the ADC. This communication is usually performed through USB or Ethernet buses. Since real-time processing requires a highly optimized implementation of the whole receiver, this module also allows reading samples from a file stored in a hard disk, and thus processing without time constraints. Relevant parameters of those samples are the intermediate frequency (or baseband I&Q components), the sampling rate and number of bits per sample, that must be specified by the user in the configuration file. This module also performs bit-depth adaptation, since most of the existing RF front-ends provide samples quantized with 2 or 3 bits, while operations inside the processor are performed on 32- or 64-bit words, depending on its architecture. Although there are implementations of the most intensive computational processes (mainly correlation) that take advantage of specific data types and architectures for the sake of efficiency, the approach is processor-specific and hardly portable. We suggest to keep signal samples in standard data types and letting the compiler select the best library version (implemented using SIMD or any other processor-specific technology) of the required routines for a given processor. @@ -822,7 +822,7 @@ SignalSource.item_type=gr_complex SignalSource.sampling_frequency=4000000 ; Sampling frequency in samples per second (Sps) ~~~~~~ -Type ```gr_complex``` refers to a GNU Radio typedef equivalent to ```std::complex```. In order to save some storage space, you might wanted to store your signal in a more efficient format such as an I/Q interleaved ```short`` integer sample stream. In that case, change the corresponding line to: +Type ```gr_complex``` refers to a GNU Radio typedef equivalent to ```std::complex```. In order to save some storage space, you might want to store your signal in a more efficient format such as an I/Q interleaved ```short`` integer sample stream. In that case, change the corresponding line to: ~~~~~~ SignalSource.item_type=ishort @@ -846,7 +846,7 @@ Sometimes, samples are stored in files with a format which is not in the list of Within a byte the samples may be packed in big endian ```big_endian_bytes=true``` (if the most significant byte value is stored at the memory location with the lowest address, the next byte value in significance is stored at the following memory location, and so on) or little endian ```big_endian_bytes=false``` (if the least significant byte value is at the lowest address, and the other bytes follow in increasing order of significance). If the order is big endian then the most significant two bits will form the first sample output, otherwise the least significant two bits will be used. -Additionally the samples may be either real ```sample_type=real```, or complex. If the sample type is complex, then the samples are either stored in the order: real, imag, real, imag, ... ```sample_type=iq``` or in the order: imag, real, imag, real, ... ```sample_type=qi```. +Additionally, the samples may be either real ```sample_type=real```, or complex. If the sample type is complex, then the samples are either stored in the order: real, imag, real, imag, ... ```sample_type=iq``` or in the order: imag, real, imag, real, ... ```sample_type=qi```. Finally, if the data is stored as shorts ```item_type=short```, then it may be stored in either big endian ```big_endian_items=true``` or little endian ```big_endian_items=false```. If the shorts are big endian then the 2nd byte in each short is output first. @@ -1008,7 +1008,7 @@ If your signal source is providing baseband signal samples of type ```gr_complex SignalConditioner.implementation=Pass_Through ~~~~~~ -If you need to adapt some aspect of you signal, you can enable the Signal Conditioner and configure three internal blocks: a data type adpater, an input signal and a resampler. +If you need to adapt some aspect of your signal, you can enable the Signal Conditioner and configure three internal blocks: a data type adapter, an input signal and a resampler. ~~~~~~ ;#[Signal_Conditioner] enables this block. Then you have to configure [DataTypeAdapter], [InputFilter] and [Resampler] blocks @@ -1104,7 +1104,7 @@ More documentation at the [Resampler Blocks page](http://gnss-sdr.org/docs/sp-bl ### Channel -A channel encapsulates all signal processing devoted to a single satellite. Thus, it is a large composite object which encapsulates the acquisition, tracking and navigation data decoding modules. As a composite object, it can be treated as a single entity, meaning that it can be easily replicated. Since the number of channels is selectable by the user in the configuration file, this approach helps improving the scalability and maintainability of the receiver. +A channel encapsulates all signal processing devoted to a single satellite. Thus, it is a large composite object which encapsulates the acquisition, tracking and navigation data decoding modules. As a composite object, it can be treated as a single entity, meaning that it can be easily replicated. Since the number of channels is selectable by the user in the configuration file, this approach helps to improve the scalability and maintainability of the receiver. Each channel must be assigned to a GNSS signal, according to the following identifiers: @@ -1146,7 +1146,7 @@ Channel6.signal=1B ; Channel7.signal=1B ; ~~~~~~ -This module is also in charge of managing the interplay between acquisition and tracking. Acquisition can be initialized in several ways, depending on the prior information available (called cold start when the receiver has no information about its position nor the satellites almanac; warm start when a rough location and the approximate time of day are available, and the receiver has a recently recorded almanac broadcast; or hot start when the receiver was tracking a satellite and the signal line of sight broke for a short period of time, but the ephemeris and almanac data is still valid, or this information is provided by other means), and an acquisition process can finish deciding that the satellite is not present, that longer integration is needed in order to confirm the presence of the satellite, or declaring the satellite present. In the latter case, acquisition process should stop and trigger the tracking module with coarse estimations of the synchronization parameters. The mathematical abstraction used to design this logic is known as finite state machine (FSM), that is a behavior model composed of a finite number of states, transitions between those states, and actions. For the implementation, we use the [Boost.Statechart library](http://www.boost.org/libs/statechart/doc/tutorial.html), which provides desirable features such as support for asynchronous state machines, multi-threading, type-safety, error handling and compile-time validation. +This module is also in charge of managing the interplay between acquisition and tracking. Acquisition can be initialized in several ways, depending on the prior information available (called cold start when the receiver has no information about its position nor the satellites' almanac; warm start when a rough location and the approximate time of day are available, and the receiver has a recently recorded almanac broadcast; or hot start when the receiver was tracking a satellite and the signal line of sight broke for a short period of time, but the ephemeris and almanac data is still valid, or this information is provided by other means), and an acquisition process can finish deciding that the satellite is not present, that longer integration is needed in order to confirm the presence of the satellite, or declaring the satellite present. In the latter case, acquisition process should stop and trigger the tracking module with coarse estimations of the synchronization parameters. The mathematical abstraction used to design this logic is known as finite state machine (FSM), that is a behavior model composed of a finite number of states, transitions between those states, and actions. The abstract class [ChannelInterface](./src/core/interfaces/channel_interface.h) represents an interface to a channel GNSS block. Check [Channel](./src/algorithms/channel/adapters/channel.h) for an actual implementation. @@ -1256,7 +1256,7 @@ More documentation at the [Tracking Blocks page](http://gnss-sdr.org/docs/sp-blo #### Decoding of the navigation message -Most of GNSS signal links are modulated by a navigation message containing the time the message was transmitted, orbital parameters of satellites (also known as ephemeris) and an almanac (information about the general system health, rough orbits of all satellites in the network as well as data related to error correction). Navigation data bits are structured in words, pages, subframes, frames and superframes. Sometimes, bits corresponding to a single parameter are spread over different words, and values extracted from different frames are required for proper decoding. Some words are for synchronization purposes, others for error control an others contain actual information. There are also error control mechanisms, from parity checks to forward error correction (FEC) encoding and interleaving, depending on the system. All this decoding complexity is managed by a finite state machine implemented with the [Boost.Statechart library](http://www.boost.org/libs/statechart/doc/tutorial.html). +Most of GNSS signal links are modulated by a navigation message containing the time the message was transmitted, orbital parameters of satellites (also known as ephemeris) and an almanac (information about the general system health, rough orbits of all satellites in the network as well as data related to error correction). Navigation data bits are structured in words, pages, subframes, frames and superframes. Sometimes, bits corresponding to a single parameter are spread over different words, and values extracted from different frames are required for proper decoding. Some words are for synchronization purposes, others for error control and others contain actual information. There are also error control mechanisms, from parity checks to forward error correction (FEC) encoding and interleaving, depending on the system. All this decoding complexity is managed by a finite state machine. The common interface is [TelemetryDecoderInterface](./src/core/interfaces/telemetry_decoder_interface.h). Check [GpsL1CaTelemetryDecoder](./src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.h) for an example of the GPS L1 NAV message decoding adapter, and [gps_l1_ca_telemetry_decoder_cc](./src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h) for an actual implementation of a signal processing block. Configuration example: @@ -1343,7 +1343,7 @@ PVT.rtcm_MT1077_rate_ms=1000 PVT.rinex_version=2 ~~~~~~ -* **RTCM SC-104** provides standards that define the data structure for differential GNSS correction information for a variety of differential correction applications. Developed by the Radio Technical Commission for Maritime Services ([RTCM](http://www.rtcm.org/overview.php#Standards "Radio Technical Commission for Maritime Services")), they have become an industry standard for communication of correction information. GNSS-SDR implements RTCM version 3.2, defined in the document *RTCM 10403.2, Differential GNSS (Global Navigation Satellite Systems) Services - Version 3* (February 1, 2013), which can be [purchased online](https://ssl29.pair.com/dmarkle/puborder.php?show=3 "RTCM Online Publication Order Form"). By default, the generated RTCM binary messages are dumped into a text file in hexadecimal format. However, GNSS-SDR is equipped with a TCP/IP server, acting as an NTRIP source that can feed an NTRIP server. NTRIP (Networked Transport of RTCM via Internet Protocol) is an open standard protocol that can be freely download from [BKG](http://igs.bkg.bund.de/root_ftp/NTRIP/documentation/NtripDocumentation.pdf "Networked Transport of RTCM via Internet Protocol (Ntrip) Version 1.0"), and it is designed for disseminating differential correction data (*e.g.* in the RTCM-104 format) or other kinds of GNSS streaming data to stationary or mobile users over the Internet. The TCP/IP server can be enabled by setting ```PVT.flag_rtcm_server=true``` in the configuration file, and will be active during the execution of the software receiver. By default, the server will operate on port 2101 (which is the recommended port for RTCM services according to the Internet Assigned Numbers Authority, [IANA](http://www.iana.org/assignments/service-names-port-numbers "Service Name and Transport Protocol Port Number Registry")), and will identify the Reference Station with ID=1234. This behaviour can be changed in the configuration file: +* **RTCM SC-104** provides standards that define the data structure for differential GNSS correction information for a variety of differential correction applications. Developed by the Radio Technical Commission for Maritime Services ([RTCM](http://www.rtcm.org/overview.php#Standards "Radio Technical Commission for Maritime Services")), they have become an industry standard for communication of correction information. GNSS-SDR implements RTCM version 3.2, defined in the document *RTCM 10403.2, Differential GNSS (Global Navigation Satellite Systems) Services - Version 3* (February 1, 2013), which can be [purchased online](https://ssl29.pair.com/dmarkle/puborder.php?show=3 "RTCM Online Publication Order Form"). By default, the generated RTCM binary messages are dumped into a text file in hexadecimal format. However, GNSS-SDR is equipped with a TCP/IP server, acting as an NTRIP source that can feed an NTRIP server. NTRIP (Networked Transport of RTCM via Internet Protocol) is an open standard protocol that can be freely downloaded from [BKG](http://igs.bkg.bund.de/root_ftp/NTRIP/documentation/NtripDocumentation.pdf "Networked Transport of RTCM via Internet Protocol (Ntrip) Version 1.0"), and it is designed for disseminating differential correction data (*e.g.* in the RTCM-104 format) or other kinds of GNSS streaming data to stationary or mobile users over the Internet. The TCP/IP server can be enabled by setting ```PVT.flag_rtcm_server=true``` in the configuration file, and will be active during the execution of the software receiver. By default, the server will operate on port 2101 (which is the recommended port for RTCM services according to the Internet Assigned Numbers Authority, [IANA](http://www.iana.org/assignments/service-names-port-numbers "Service Name and Transport Protocol Port Number Registry")), and will identify the Reference Station with ID=1234. This behaviour can be changed in the configuration file: ~~~~~~ PVT.flag_rtcm_server=true PVT.rtcm_tcp_port=2102 @@ -1400,9 +1400,9 @@ There is a list of papers related to GNSS-SDR in our [publications page](http:// Ok, now what? ============= -In order to start using GNSS-SDR, you may want to populate ```gnss-sdr/data``` folder (or anywhere else on your system) with raw data files. By "raw data" we mean the output of a Radio Frequency front-end's Analog-to-Digital converter. GNSS-SDR needs signal samples already in baseband or in passband, at a suitable intemediate frequency (on the order of MHz). Prepare your configuration file, and then you are ready for running ```gnss-sdr --config_file=your_configuration.conf```, and seeing how the file is processed. +In order to start using GNSS-SDR, you may want to populate ```gnss-sdr/data``` folder (or anywhere else on your system) with raw data files. By "raw data" we mean the output of a Radio Frequency front-end's Analog-to-Digital converter. GNSS-SDR needs signal samples already in baseband or in passband, at a suitable intermediate frequency (on the order of MHz). Prepare your configuration file, and then you are ready for running ```gnss-sdr --config_file=your_configuration.conf```, and seeing how the file is processed. -Another interesting option is working in real-time with a RF front-end. We provide drivers for UHD-compatible hardware such as the [USRP family](http://www.ettus.com/product), for OsmoSDR and other front-ends (HackRF, bladeRF, LimeSDR), for the GN3S v2 USB dongle and for some DVB-T USB dongles. Start with a low number of channels and then increase it in order to test how many channels your processor can handle in real-time. +Another interesting option is working in real-time with an RF front-end. We provide drivers for UHD-compatible hardware such as the [USRP family](http://www.ettus.com/product), for OsmoSDR and other front-ends (HackRF, bladeRF, LimeSDR), for the GN3S v2 USB dongle and for some DVB-T USB dongles. Start with a low number of channels and then increase it in order to test how many channels your processor can handle in real-time. You can find more information at the [GNSS-SDR Documentation page](http://gnss-sdr.org/docs/) or directly asking to the [GNSS-SDR Developers mailing list](http://lists.sourceforge.net/lists/listinfo/gnss-sdr-developers). diff --git a/conf/gnss-sdr_GPS_L1_2ch_fmcomms2_realtime.conf b/conf/gnss-sdr_GPS_L1_2ch_fmcomms2_realtime.conf index 80814ad1d..79a57f712 100644 --- a/conf/gnss-sdr_GPS_L1_2ch_fmcomms2_realtime.conf +++ b/conf/gnss-sdr_GPS_L1_2ch_fmcomms2_realtime.conf @@ -6,8 +6,6 @@ ;######### GLOBAL OPTIONS ################## ;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [Sps]. -;FOR USE GNSS-SDR WITH RTLSDR DONGLES USER MUST SET THE CALIBRATED SAMPLE RATE HERE -; i.e. using front-end-cal as reported here:http://www.cttc.es/publication/turning-a-television-into-a-gnss-receiver/ GNSS-SDR.internal_fs_sps=7000000 ;######### SIGNAL_SOURCE CONFIG ############ diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/README.md b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/README.md index ac23478a0..3b5e14513 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/README.md +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/README.md @@ -7,7 +7,7 @@ and contact information about the original VOLK library. The boilerplate of this code was initially generated with ```volk_modtool```, an application provided by VOLK that creates the -skeleton than can then be filled with custom kernels. Some modifications +skeleton that can then be filled with custom kernels. Some modifications were added to accommodate the specificities of Global Navigation Satellite Systems (GNSS) signal processing. Those changes are clearly indicated in the source code, and do not break compatibility. @@ -39,7 +39,7 @@ This library is automatically built and installed along with GNSS-SDR if it is not found by CMake on your system at configure time. However, you can install and use VOLK_GNSSSDR kernels as you use VOLK's, -independently from GNSS-SDR. +independently of GNSS-SDR. First, make sure that the required dependencies are installed in your machine: From 843679f0eddaf88ab5cb29cc96f60254a83452f6 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 3 Apr 2018 11:42:11 +0200 Subject: [PATCH 52/61] 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 53/61] 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 54/61] 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 55/61] 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 56/61] 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 57/61] 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 58/61] 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 59/61] 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 60/61] 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 61/61] 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_ */