diff --git a/conf/gnss-sdr_Galileo_E1_nsr.conf b/conf/gnss-sdr_Galileo_E1_nsr.conf index 476460802..c9b35724c 100644 --- a/conf/gnss-sdr_Galileo_E1_nsr.conf +++ b/conf/gnss-sdr_Galileo_E1_nsr.conf @@ -16,7 +16,7 @@ ControlThread.wait_for_flowgraph=false ;######### SIGNAL_SOURCE CONFIG ############ SignalSource.implementation=Nsr_File_Signal_Source -SignalSource.filename=/datalogger/signals/ifen/E1L1_FE0_Band0.stream +SignalSource.filename=/Users/javier/signals/ifen/E1L1_FE0_Band0.stream SignalSource.item_type=byte SignalSource.sampling_frequency=20480000 SignalSource.freq=1575420000 @@ -110,15 +110,15 @@ TelemetryDecoder_1B.dump=false ;######### OBSERVABLES CONFIG ############ Observables.implementation=Galileo_E1B_Observables -Observables.dump=false +Observables.dump=true Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ PVT.implementation=GALILEO_E1_PVT -PVT.averaging_depth=10 +PVT.averaging_depth=1 PVT.flag_averaging=false -PVT.output_rate_ms=10 +PVT.output_rate_ms=100 PVT.display_rate_ms=500 PVT.dump=true PVT.dump_filename=./PVT diff --git a/conf/gnss-sdr_Hybrid_byte_sim.conf b/conf/gnss-sdr_Hybrid_byte_sim.conf index 403afb9f2..2f8cb0654 100644 --- a/conf/gnss-sdr_Hybrid_byte_sim.conf +++ b/conf/gnss-sdr_Hybrid_byte_sim.conf @@ -233,7 +233,7 @@ Acquisition_1B.doppler_step=125 ;######### TRACKING GPS CONFIG ############ ;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_FLL_PLL_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] -Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_C_Aid_Tracking +Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Artemisa_Tracking ;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version. Tracking_1C.item_type=gr_complex @@ -247,7 +247,7 @@ Tracking_1C.dump=true Tracking_1C.dump_filename=../data/epl_tracking_ch_ ;#pll_bw_hz: PLL loop filter bandwidth [Hz] -Tracking_1C.pll_bw_hz=20.0; +Tracking_1C.pll_bw_hz=15.0; ;#dll_bw_hz: DLL loop filter bandwidth [Hz] Tracking_1C.dll_bw_hz=1.5; diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index a65142a0d..1f2ede3af 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -2992,7 +2992,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); // Galileo E1B PHASE - lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Carrier_phase_rads / (2 * GALILEO_PI), 3), 14); + lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14); if (lli == 0) { lineObs += std::string(1, ' '); diff --git a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc index ea542db87..7dd820708 100644 --- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc @@ -41,6 +41,7 @@ #include #include "control_message_factory.h" #include "gnss_synchro.h" +#include "Galileo_E1.h" using google::LogMessage; @@ -65,6 +66,13 @@ galileo_e1_observables_cc::galileo_e1_observables_cc(unsigned int nchannels, boo d_dump_filename = dump_filename; d_flag_averaging = flag_averaging; + for (int i=0;i(d_nchannels)); + d_carrier_doppler_queue_hz.push_back(std::deque(d_nchannels)); + d_symbol_TOW_queue_s.push_back(std::deque(d_nchannels)); + } + // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) { @@ -129,11 +137,39 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n */ current_gnss_synchro[i].Flag_valid_pseudorange = false; current_gnss_synchro[i].Pseudorange_m = 0.0; - if (current_gnss_synchro[i].Flag_valid_word) - { - //record the word structure in a map for pseudorange computation - current_gnss_synchro_map.insert(std::pair(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); - } + + if (current_gnss_synchro[i].Flag_valid_word) //if this channel have valid word + { + //record the word structure in a map for pseudorange computation + current_gnss_synchro_map.insert(std::pair(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].d_TOW_at_current_symbol); + + if (d_carrier_doppler_queue_hz[i].size()>GALILEO_E1_HISTORY_DEEP) + { + d_carrier_doppler_queue_hz[i].pop_front(); + } + if (d_acc_carrier_phase_queue_rads[i].size()>GALILEO_E1_HISTORY_DEEP) + { + d_acc_carrier_phase_queue_rads[i].pop_front(); + } + if (d_symbol_TOW_queue_s[i].size()>GALILEO_E1_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(); + } + } } /* @@ -155,18 +191,47 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n double traveltime_ms; double pseudorange_m; double delta_rx_time_ms; + 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); for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++) { - // compute the required symbol history shift in order to match the reference symbol - delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms-d_ref_PRN_rx_time_ms; - //compute the pseudorange - traveltime_ms = (d_TOW_reference - gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GALILEO_STARTOFFSET_ms; - pseudorange_m = traveltime_ms * GALILEO_C_m_ms; // [m] - // update the pseudorange object - //current_gnss_synchro[gnss_synchro_iter->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_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000)/1000 + GALILEO_STARTOFFSET_ms/1000.0; + // compute the required symbol history shift in order to match the reference symbol + delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms - d_ref_PRN_rx_time_ms; + //compute the pseudorange + traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GALILEO_STARTOFFSET_ms; + pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] + // update the pseudorange object + current_gnss_synchro[gnss_synchro_iter->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_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000.0)/1000.0 + GALILEO_STARTOFFSET_ms/1000.0; + + if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size()>=GPS_L1_CA_HISTORY_DEEP) + { + // 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())); + desired_symbol_TOW[0]=symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP-1]+delta_rx_time_ms/1000.0; + // Curve fitting to cuadratic function + arma::mat A=arma::ones (GPS_L1_CA_HISTORY_DEEP,2); + A.col(1)=symbol_TOW_vec_s; + //A.col(2)=symbol_TOW_vec_s % symbol_TOW_vec_s; + arma::mat coef_acc_phase(1,3); + coef_acc_phase=arma::pinv(A.t()*A)*A.t()*acc_phase_vec_rads; + arma::mat coef_doppler(1,3); + coef_doppler=arma::pinv(A.t()*A)*A.t()*dopper_vec_hz; + arma::vec acc_phase_lin; + arma::vec carrier_doppler_lin; + acc_phase_lin=coef_acc_phase[0]+coef_acc_phase[1]*desired_symbol_TOW[0];//+coef_acc_phase[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0]; + carrier_doppler_lin=coef_doppler[0]+coef_doppler[1]*desired_symbol_TOW[0];//+coef_doppler[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0]; + current_gnss_synchro[gnss_synchro_iter->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]; + } } } diff --git a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h index 131d76fb5..e626d2567 100644 --- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h @@ -41,6 +41,7 @@ #include #include #include +#include #include "concurrent_queue.h" #include "galileo_navigation_message.h" #include "rinex_printer.h" @@ -70,6 +71,11 @@ private: galileo_e1_make_observables_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging); galileo_e1_observables_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging); + //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; + // class private vars boost::shared_ptr d_queue; bool d_dump; diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc index 4e867738d..8a254a375 100644 --- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc @@ -198,16 +198,15 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni // compute the required symbol history shift in order to match the reference symbol delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms - d_ref_PRN_rx_time_ms; //compute the pseudorange - traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GPS_STARTOFFSET_ms; + traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol) * 1000.0 + delta_rx_time_ms + GPS_STARTOFFSET_ms; pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] // update the pseudorange object current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].debug_var1 = delta_rx_time_ms; 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_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000.0)/1000.0 + GPS_STARTOFFSET_ms/1000.0; - if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= GPS_L1_CA_HISTORY_DEEP) + if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size()>=GPS_L1_CA_HISTORY_DEEP) { // 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())); @@ -219,14 +218,14 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni //std::cout<<"acc_phase_vec_rads="<(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + samples_offset = std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples d_pull_in = false; consume_each(samples_offset); //shift input to perform alignment with local replica @@ -365,18 +364,18 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect // ################## PLL ########################################################## // PLL discriminator - carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast(GPS_TWO_PI); + carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GALILEO_TWO_PI; // Carrier discriminator filter carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); // New carrier Doppler frequency estimation d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; // New code Doppler frequency estimation d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ); - //carrier phase accumulator for (K) Doppler estimation - d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; + //carrier phase accumulator for (K) Doppler estimation- + d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples)/static_cast(d_fs_in); //remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; - d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * d_current_prn_length_samples/static_cast(d_fs_in); + d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); // ################## DLL ########################################################## // DLL discriminator @@ -400,7 +399,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; T_prn_samples = T_prn_seconds * static_cast(d_fs_in); K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples + d_current_prn_length_samples = std::round(K_blk_samples); //round to a discrete samples //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### @@ -473,9 +472,9 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect */ // stream to collect cout calls to improve thread safety std::stringstream tmp_str_stream; - if (floor(d_sample_counter / d_fs_in) != d_last_seg) + if (std::floor(d_sample_counter / d_fs_in) != d_last_seg) { - d_last_seg = floor(d_sample_counter / d_fs_in); + d_last_seg = std::floor(d_sample_counter / d_fs_in); if (d_channel == 0) { @@ -498,9 +497,9 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect */ // stream to collect cout calls to improve thread safety std::stringstream tmp_str_stream; - if (floor(d_sample_counter / d_fs_in) != d_last_seg) + if (std::floor(d_sample_counter / d_fs_in) != d_last_seg) { - d_last_seg = floor(d_sample_counter / d_fs_in); + d_last_seg = std::floor(d_sample_counter / d_fs_in); if (d_channel == 0) { 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 90d7f1bd3..9f989e262 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 @@ -404,6 +404,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_ double carr_error_filt_hz; double code_error_chips; double code_error_filt_chips; + // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc index 8201d8066..714674bea 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -218,7 +218,8 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking() d_acq_code_phase_samples = corrected_acq_phase_samples; d_carrier_doppler_hz = d_acq_carrier_doppler_hz; - d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz/static_cast(d_fs_in); + + d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); // DLL/PLL filter initialization d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); //The carrier loop filter implements the Doppler accumulator @@ -239,7 +240,6 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking() d_rem_code_phase_chips = 0.0; d_acc_carrier_phase_cycles = 0.0; d_pll_to_dll_assist_secs_Ti = 0.0; - d_code_phase_samples = d_acq_code_phase_samples; std::string sys_ = &d_acquisition_gnss_synchro->System; 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 b75505e62..cd7bb2b5a 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 @@ -297,7 +297,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_code() void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_carrier() { float sin_f, cos_f; - float phase_step_rad = static_cast(GPS_TWO_PI) * ( d_if_freq + d_carrier_doppler_hz ) / static_cast(d_fs_in); + float phase_step_rad = static_cast(GPS_TWO_PI) * static_cast( d_if_freq + d_carrier_doppler_hz ) / static_cast(d_fs_in); int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad); int phase_rad_i = gr::fxpt::float_to_fixed(d_rem_carr_phase_rad); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc index 592d35161..fb3875936 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc @@ -122,16 +122,16 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc( //pinned memory mode - use special function to get OS-pinned memory int N_CORRELATORS = 3; // Get space for a vector with the C/A code replica sampled 1x/chip - cudaHostAlloc((void**)&d_ca_code, (GPS_L1_CA_CODE_LENGTH_CHIPS* sizeof(gr_complex)), cudaHostAllocMapped || cudaHostAllocWriteCombined); + cudaHostAlloc((void**)&d_ca_code, (GPS_L1_CA_CODE_LENGTH_CHIPS* sizeof(gr_complex)), cudaHostAllocMapped || cudaHostAllocWriteCombined); // Get space for the resampled early / prompt / late local replicas - cudaHostAlloc((void**)&d_local_code_shift_chips, N_CORRELATORS * sizeof(float), cudaHostAllocMapped || cudaHostAllocWriteCombined); - cudaHostAlloc((void**)&in_gpu, 2 * d_vector_length * sizeof(gr_complex), cudaHostAllocMapped || cudaHostAllocWriteCombined); - // correlator outputs (scalar) - cudaHostAlloc((void**)&d_corr_outs_gpu ,sizeof(gr_complex)*N_CORRELATORS, cudaHostAllocMapped || cudaHostAllocWriteCombined ); + cudaHostAlloc((void**)&d_local_code_shift_chips, N_CORRELATORS * sizeof(float), cudaHostAllocMapped || cudaHostAllocWriteCombined); + cudaHostAlloc((void**)&in_gpu, 2 * d_vector_length * sizeof(gr_complex), cudaHostAllocMapped || cudaHostAllocWriteCombined); + // correlator outputs (scalar) + cudaHostAlloc((void**)&d_corr_outs_gpu ,sizeof(gr_complex)*N_CORRELATORS, cudaHostAllocMapped || cudaHostAllocWriteCombined ); - //map to EPL pointers + //map to EPL pointers d_Early = &d_corr_outs_gpu[0]; - d_Prompt = &d_corr_outs_gpu[1]; + d_Prompt = &d_corr_outs_gpu[1]; d_Late = &d_corr_outs_gpu[2]; //--- Perform initializations ------------------------------ @@ -139,6 +139,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc( //local code resampler on GPU multicorrelator_gpu->init_cuda_integrated_resampler(2 * d_vector_length, GPS_L1_CA_CODE_LENGTH_CHIPS, 3); multicorrelator_gpu->set_input_output_vectors(d_corr_outs_gpu, in_gpu); + // define initial code frequency basis of NCO d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ; // define residual code phase (in chips) @@ -168,6 +169,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc( systemName["G"] = std::string("GPS"); systemName["S"] = std::string("SBAS"); + set_relative_rate(1.0/((double)d_vector_length*2)); d_channel_internal_queue = 0; @@ -234,9 +236,9 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking() // generate local reference ALWAYS starting at chip 1 (1 sample per chip) gps_l1_ca_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN, 0); - d_local_code_shift_chips[0]=-d_early_late_spc_chips; - d_local_code_shift_chips[1]=0.0; - d_local_code_shift_chips[2]=d_early_late_spc_chips; + d_local_code_shift_chips[0] = - d_early_late_spc_chips; + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_early_late_spc_chips; multicorrelator_gpu->set_local_code_and_taps(GPS_L1_CA_CODE_LENGTH_CHIPS, d_ca_code, d_local_code_shift_chips, 3); @@ -273,7 +275,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::~Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc() cudaFreeHost(d_corr_outs_gpu); cudaFreeHost(d_local_code_shift_chips); cudaFreeHost(d_ca_code); - multicorrelator_gpu->free_cuda(); delete(multicorrelator_gpu); delete[] d_Prompt_buffer; @@ -285,10 +286,10 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vecto gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars - float carr_error_hz = 0.0; - float carr_error_filt_hz = 0.0; - float code_error_chips = 0.0; - float code_error_filt_chips = 0.0; + float carr_error_hz=0.0; + float carr_error_filt_hz=0.0; + float code_error_chips=0.0; + float code_error_filt_chips=0.0; // Block input data and block output stream pointers const gr_complex* in = (gr_complex*) input_items[0]; diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator.cc b/src/algorithms/tracking/libs/cpu_multicorrelator.cc index befac3170..268aeb3cd 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator.cc +++ b/src/algorithms/tracking/libs/cpu_multicorrelator.cc @@ -63,6 +63,7 @@ bool cpu_multicorrelator::init( } + bool cpu_multicorrelator::set_local_code_and_taps( int code_length_chips, const std::complex* local_code_in, @@ -85,6 +86,7 @@ bool cpu_multicorrelator::set_input_output_vectors(std::complex* corr_out } + void cpu_multicorrelator::update_local_code(int correlator_length_samples,float rem_code_phase_chips, float code_phase_step_chips) { float local_code_chip_index; @@ -93,7 +95,7 @@ void cpu_multicorrelator::update_local_code(int correlator_length_samples,float for (int n = 0; n < correlator_length_samples; n++) { // resample code for current tap - local_code_chip_index = fmod(code_phase_step_chips*static_cast(n)+ d_shifts_chips[current_correlator_tap] - rem_code_phase_chips, d_code_length_chips); + local_code_chip_index = std::fmod(code_phase_step_chips*static_cast(n)+ d_shifts_chips[current_correlator_tap] - rem_code_phase_chips, d_code_length_chips); //Take into account that in multitap correlators, the shifts can be negative! if (local_code_chip_index < 0.0) local_code_chip_index += d_code_length_chips; d_local_codes_resampled[current_correlator_tap][n] = d_local_code_in[static_cast(round(local_code_chip_index))]; diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator.h b/src/algorithms/tracking/libs/cpu_multicorrelator.h index bce16fc5b..2c8c9667c 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator.h +++ b/src/algorithms/tracking/libs/cpu_multicorrelator.h @@ -35,6 +35,7 @@ #ifndef GNSS_SDR_CPU_MULTICORRELATOR_H_ #define GNSS_SDR_CPU_MULTICORRELATOR_H_ + #include /*! @@ -51,6 +52,7 @@ public: void update_local_carrier(int correlator_length_samples, float rem_carr_phase_rad, float phase_step_rad); bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float rem_code_phase_chips, float code_phase_step_chips, int signal_length_samples); bool free(); + private: // Allocate the device input vectors const std::complex *d_sig_in; diff --git a/src/algorithms/tracking/libs/cuda_multicorrelator.h b/src/algorithms/tracking/libs/cuda_multicorrelator.h index 2f8610c38..161cbfe4e 100644 --- a/src/algorithms/tracking/libs/cuda_multicorrelator.h +++ b/src/algorithms/tracking/libs/cuda_multicorrelator.h @@ -131,6 +131,7 @@ public: std::complex* corr_out, std::complex* sig_in ); + bool free_cuda(); bool Carrier_wipeoff_multicorrelator_resampler_cuda( float rem_carrier_phase_in_rad, diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index bc345ff4a..c5c74f35b 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -1312,6 +1312,12 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams, queue)); block = std::move(block_); } + else if (implementation.compare("GPS_L1_CA_DLL_PLL_C_Aid_Tracking") == 0) + { + std::unique_ptr block_(new GpsL1CaDllPllCAidTracking(configuration.get(), role, in_streams, + out_streams, queue)); + block = std::move(block_); + } else if (implementation.compare("GPS_L1_CA_DLL_PLL_Optim_Tracking") == 0) { std::unique_ptr block_(new GpsL1CaDllPllOptimTracking(configuration.get(), role, in_streams, @@ -1577,7 +1583,7 @@ std::unique_ptr GNSSBlockFactory::GetTrkBlock( out_streams, queue)); block = std::move(block_); } - if (implementation.compare("GPS_L1_CA_DLL_PLL_C_Aid_Tracking") == 0) + else if (implementation.compare("GPS_L1_CA_DLL_PLL_C_Aid_Tracking") == 0) { std::unique_ptr block_(new GpsL1CaDllPllCAidTracking(configuration.get(), role, in_streams, out_streams, queue)); diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index 16b6b2ac4..e6aa63b83 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -71,6 +71,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 = 100; + // NAVIGATION MESSAGE DEMODULATION AND DECODING #define GPS_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1} diff --git a/src/core/system_parameters/Galileo_E1.h b/src/core/system_parameters/Galileo_E1.h index 8d27405d7..1e18be660 100644 --- a/src/core/system_parameters/Galileo_E1.h +++ b/src/core/system_parameters/Galileo_E1.h @@ -42,6 +42,7 @@ // Physical constants const double GALILEO_PI = 3.1415926535898; //!< Pi as defined in GALILEO ICD +const double GALILEO_TWO_PI = 6.283185307179600 ; //!< 2*Pi as defined in GALILEO ICD const double GALILEO_GM = 3.986004418e14; //!< Geocentric gravitational constant[m^3/s^2] const double GALILEO_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Mean angular velocity of the Earth [rad/s] const double GALILEO_C_m_s = 299792458.0; //!< The speed of light, [m/s] @@ -61,6 +62,10 @@ const int Galileo_E1_NUMBER_OF_CODES = 50; const double GALILEO_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) + +// OBSERVABLE HISTORY DEEP FOR INTERPOLATION +const int GALILEO_E1_HISTORY_DEEP=100; + // Galileo INAV Telemetry structure #define GALILEO_INAV_PREAMBLE {0, 1, 0, 1, 1, 0, 0, 0, 0, 0}