From ddcf1b1dee1d546ff074307f75bca0e92347da5d Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 27 Apr 2017 12:20:24 +0200 Subject: [PATCH] New observables generation algorithm that accepts multirate inputs from different GNSS systems --- .../gnuradio_blocks/hybrid_observables_cc.cc | 406 +++++++++++------- .../gnuradio_blocks/hybrid_observables_cc.h | 9 +- 2 files changed, 247 insertions(+), 168 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 09a32ceae..95f8f6d0b 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -1,8 +1,7 @@ /*! * \file hybrid_observables_cc.cc * \brief Implementation of the pseudorange computation block for Galileo E1 - * \author Mara Branzanti 2013. mara.branzanti(at)gmail.com - * \author Javier Arribas 2013. jarribas(at)cttc.es + * \author Javier Arribas 2017. jarribas(at)cttc.es * * ------------------------------------------------------------------------- * @@ -39,7 +38,6 @@ #include #include #include -#include "gnss_synchro.h" #include "Galileo_E1.h" #include "GPS_L1_CA.h" @@ -56,39 +54,43 @@ hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_f 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))) + gr::block("hybrid_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), + gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro))) { // initialize internal vars d_dump = dump; d_nchannels = nchannels; 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 for (unsigned int i = 0; i < d_nchannels; i++) - { - d_acc_carrier_phase_queue_rads.push_back(std::deque(d_nchannels)); - d_carrier_doppler_queue_hz.push_back(std::deque(d_nchannels)); - d_symbol_TOW_queue_s.push_back(std::deque(d_nchannels)); - } + { + d_gnss_synchro_history_queue.push_back(std::deque()); + } + //todo: this is a gnuradio scheduler hack. + // Migrate the queues to gnuradio set_history to see if the scheduler can handle + // the multiple output flow + d_max_noutputs=100; + this->set_min_noutput_items(100); // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) + { + if (d_dump_file.is_open() == false) { - 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(); - } - } + 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(); + } } + } } @@ -98,163 +100,219 @@ hybrid_observables_cc::~hybrid_observables_cc() } -bool Hybrid_pairCompare_gnss_synchro_d_TOW_at_current_symbol(const std::pair& a, const std::pair& b) +bool Hybrid_pairCompare_gnss_synchro_sample_counter(const std::pair& a, const std::pair& b) { - return (a.second.TOW_at_current_symbol_s) < (b.second.TOW_at_current_symbol_s); + 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 ((double)a.Tracking_sample_counter/(double)a.fs) < (b); +} + +bool Hybrid_pairCompare_gnss_synchro_d_TOW(const std::pair& a, const std::pair& b) +{ + return (a.second.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); +} 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) + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) { Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; // Get the input pointer Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; // Get the output pointer + int n_outputs=0; + int n_consume[d_nchannels]; + double past_history_s=100e-3; Gnss_Synchro current_gnss_synchro[d_nchannels]; - std::map current_gnss_synchro_map; - std::map::iterator gnss_synchro_iter; - - if (d_nchannels != ninput_items.size()) - { - LOG(WARNING) << "The Observables block is not well connected"; - } /* - * 1. Read the GNSS SYNCHRO objects from available channels + * 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(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); - //################### SAVE DOPPLER AND ACC CARRIER PHASE HISTORIC DATA FOR INTERPOLATION IN OBSERVABLE MODULE ####### - d_carrier_doppler_queue_hz[i].push_back(current_gnss_synchro[i].Carrier_Doppler_hz); - d_acc_carrier_phase_queue_rads[i].push_back(current_gnss_synchro[i].Carrier_phase_rads); - // save TOW history - d_symbol_TOW_queue_s[i].push_back(current_gnss_synchro[i].TOW_at_current_symbol_s); - if (d_carrier_doppler_queue_hz[i].size() > history_deep) - { - d_carrier_doppler_queue_hz[i].pop_front(); - } - if (d_acc_carrier_phase_queue_rads[i].size() > history_deep) - { - d_acc_carrier_phase_queue_rads[i].pop_front(); - } - if (d_symbol_TOW_queue_s[i].size() > history_deep) - { - d_symbol_TOW_queue_s[i].pop_front(); - } - } - else - { - // Clear the observables history for this channel - if (d_symbol_TOW_queue_s[i].size() > 0) - { - d_symbol_TOW_queue_s[i].clear(); - d_carrier_doppler_queue_hz[i].clear(); - d_acc_carrier_phase_queue_rads[i].clear(); - } - } + d_gnss_synchro_history_queue[i].push_back(in[i][j]); } + //std::cout<<"push["< 0) + bool channel_history_ok; + do{ + channel_history_ok=true; + for (unsigned int i = 0; i < d_nchannels; i++) { - /* - * 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_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), Hybrid_pairCompare_gnss_synchro_d_TOW_at_current_symbol); - double d_TOW_reference = gnss_synchro_iter->second.TOW_at_current_symbol_s; - double d_ref_PRN_phase_samples = gnss_synchro_iter->second.Code_phase_samples; - //std::cout<<"OBS SV REF SAT: "<second.PRN<second.Tracking_sample_counter; + if (d_gnss_synchro_history_queue[i].size()::iterator gnss_synchro_map_iter; + std::deque::iterator gnss_synchro_deque_iter; - for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_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++) + { + 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.begin(), + gnss_synchro_map.end(), + Hybrid_pairCompare_gnss_synchro_sample_counter); + T_rx_s = (double)gnss_synchro_map_iter->second.Tracking_sample_counter/(double)gnss_synchro_map_iter->second.fs; + 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].begin(), + d_gnss_synchro_history_queue[i].end(), + T_rx_s, + Hybrid_valueCompare_gnss_synchro_receiver_time); + if (gnss_synchro_deque_iter!=d_gnss_synchro_history_queue[i].end()) + { + if (gnss_synchro_deque_iter->Flag_valid_word==true) + { + double T_rx_channel=(double)gnss_synchro_deque_iter->Tracking_sample_counter/(double)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<(double)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].begin(), gnss_synchro_deque_iter); + if (distance>0) + { + double T_rx_channel_prev=(double)d_gnss_synchro_history_queue[i].at(distance-1).Tracking_sample_counter/(double)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)( + 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)); + } + + }else{ + //std::cout<<"ch["< this will be the reference symbol + gnss_synchro_map_iter = max_element(realigned_gnss_synchro_map.begin(), + realigned_gnss_synchro_map.end(), + Hybrid_pairCompare_gnss_synchro_d_TOW); + double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; + double d_ref_PRN_phase_samples = gnss_synchro_map_iter->second.Code_phase_samples; + //std::cout<<"OBS SV REF SAT: "<second.PRN<second.Tracking_sample_counter - d_ref_PRN_sample_counter); - delta_sample_counter_s=(double)delta_sample_counter/(double)gnss_synchro_iter->second.fs; - delta_PRN_phase_s = (gnss_synchro_iter->second.Code_phase_samples - d_ref_PRN_phase_samples)/(double)gnss_synchro_iter->second.fs; + delta_T_rx_s = ((double)gnss_synchro_map_iter->second.Tracking_sample_counter/(double)gnss_synchro_map_iter->second.fs - T_rx_s); + delta_PRN_phase_s = (gnss_synchro_map_iter->second.Code_phase_samples - d_ref_PRN_phase_samples)/(double)gnss_synchro_map_iter->second.fs; //compute the pseudorange (no rx time offset correction) - traveltime_ms = (d_TOW_reference - gnss_synchro_iter->second.TOW_at_current_symbol_s) * 1000.0 - + delta_sample_counter_s*1000.0 + delta_PRN_phase_s*1000.0 - + GPS_STARTOFFSET_ms; + traveltime_ms = (d_TOW_reference - gnss_synchro_map_iter->second.TOW_at_current_symbol_s) * 1000.0 + + delta_T_rx_s*1000.0 + delta_PRN_phase_s*1000.0 + + GPS_STARTOFFSET_ms; //convert to meters pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] - //std::cout<<"["<second.PRN<<"] delta_rx_t: "<second.d_TOW_at_current_symbol) * 1000.0 - // <<" Pr: "<second.Channel_ID] = gnss_synchro_iter->second; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true; + 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_iter->second.Channel_ID].RX_time = d_TOW_reference + GPS_STARTOFFSET_ms / 1000.0; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = d_TOW_reference + GPS_STARTOFFSET_ms / 1000.0; - if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= history_deep) - { - arma::vec symbol_TOW_vec_s; - arma::vec dopper_vec_hz; - arma::vec dopper_vec_interp_hz; - arma::vec acc_phase_vec_rads; - arma::vec acc_phase_vec_interp_rads; - arma::vec desired_symbol_TOW(1); - // compute interpolated observation values for Doppler and Accumulate carrier phase - symbol_TOW_vec_s = arma::vec(std::vector(d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].begin(), d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].end())); - acc_phase_vec_rads = arma::vec(std::vector(d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].begin(), d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].end())); - dopper_vec_hz = arma::vec(std::vector(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end())); + // compute interpolated observation values for Doppler and Accumulate carrier phase + // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) + int element_key=gnss_synchro_map_iter->second.Channel_ID; + Gnss_Synchro adj_obs=adjacent_gnss_synchro_map.at(element_key); + double adj_delta_T_rx_s=((double)adj_obs.Tracking_sample_counter/(double)adj_obs.fs - T_rx_s); + double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads+adj_delta_T_rx_s + *(gnss_synchro_map_iter->second.Carrier_phase_rads-adj_obs.Carrier_phase_rads)/(delta_T_rx_s-adj_delta_T_rx_s); + double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz+adj_delta_T_rx_s + *(gnss_synchro_map_iter->second.Carrier_Doppler_hz-adj_obs.Carrier_Doppler_hz)/(delta_T_rx_s-adj_delta_T_rx_s); - desired_symbol_TOW[0] = symbol_TOW_vec_s[history_deep - 1] + delta_sample_counter_s+delta_PRN_phase_s; - // arma::interp1(symbol_TOW_vec_s,dopper_vec_hz,desired_symbol_TOW,dopper_vec_interp_hz); - // arma::interp1(symbol_TOW_vec_s,acc_phase_vec_rads,desired_symbol_TOW,acc_phase_vec_interp_rads); - // Curve fitting to quadratic function - arma::mat A = arma::ones (history_deep, 2); - A.col(1) = symbol_TOW_vec_s; + 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; + //debug +// double delta_T_rx_s_previous=((double)adjacent_gnss_synchro_map.at(gnss_synchro_map_iter->second.Channel_ID).Tracking_sample_counter/(double)gnss_synchro_map_iter->second.fs - T_rx_s); + +// std::cout<<"["<second.PRN<<"] delta_rx_t: "<second.TOW_at_current_symbol_s) * 1000.0 +// <<" Pr: "<second.Carrier_Doppler_hz +// <<" Doppler inter: "<second.Channel_ID].Carrier_phase_rads = acc_phase_lin[0]; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz = carrier_doppler_lin[0]; - } } - } - if(d_dump == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - for (unsigned int i = 0; i < d_nchannels; i++) + //std::cout<n_outputs); + + //Multi-rate consume! + for (unsigned int i=0; i #include #include +#include "gnss_synchro.h" class hybrid_observables_cc; @@ -61,11 +62,11 @@ private: hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); //Tracking observable history - std::vector> d_acc_carrier_phase_queue_rads; - std::vector> d_carrier_doppler_queue_hz; - std::vector> d_symbol_TOW_queue_s; + std::vector> d_gnss_synchro_history_queue; - // class private vars + double T_rx_s; + double T_rx_step_s; + int d_max_noutputs; bool d_dump; unsigned int d_nchannels; unsigned int history_deep;