diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index bcfdc6859..cce3d6c4c 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -198,7 +198,7 @@ int pcps_acquisition_cc::general_work(int noutput_items, { //doppler search steps //Perform the carrier wipe-off - complex_exp_gen(d_carrier, d_freq + doppler, d_fs_in, d_fft_size); + complex_exp_gen_conj(d_carrier, d_freq + doppler, d_fs_in, d_fft_size); if (is_unaligned()==true) { diff --git a/src/algorithms/libs/gnss_signal_processing.cc b/src/algorithms/libs/gnss_signal_processing.cc index f22bb9e4e..d69efc4ad 100644 --- a/src/algorithms/libs/gnss_signal_processing.cc +++ b/src/algorithms/libs/gnss_signal_processing.cc @@ -61,6 +61,32 @@ void complex_exp_gen(std::complex* _dest, double _f, double _fs, unsigned } } +void complex_exp_gen_conj(std::complex* _dest, double _f, double _fs, unsigned int _samps) +{ + //old + //double phase = 0; + //const double phase_step = (GPS_TWO_PI * _f) / _fs; + + //new Fixed Point NCO (faster) + int phase_i=0; + int phase_step_i; + float phase_step_f =(float)((GPS_TWO_PI * _f) / _fs); + phase_step_i=gr_fxpt::float_to_fixed(phase_step_f); + float sin_f,cos_f; + + for(unsigned int i = 0; i < _samps; i++) + { + //old + //_dest[i] = std::complex(cos(phase), sin(phase)); + //phase += phase_step; + + //new Fixed Point NCO (faster) + gr_fxpt::sincos(phase_i,&sin_f,&cos_f); + _dest[i] = std::complex(cos_f, -sin_f); + phase_i += phase_step_i; + } +} + void hex_to_binary_converter(int * _dest, char _from) { diff --git a/src/algorithms/libs/gnss_signal_processing.h b/src/algorithms/libs/gnss_signal_processing.h index d6e9f1145..df7458406 100644 --- a/src/algorithms/libs/gnss_signal_processing.h +++ b/src/algorithms/libs/gnss_signal_processing.h @@ -46,6 +46,14 @@ void complex_exp_gen(std::complex* _dest, double _f, double _fs, unsigned int _samps); +/*! + * \brief This function generates a conjugate complex exponential in _dest. + * + */ +void complex_exp_gen_conj(std::complex* _dest, double _f, double _fs, + unsigned int _samps); + + /*! * \brief This function makes a conversion from hex (the input is a char) * to binary (the output are 4 ints with +1 or -1 values). 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 697f4b656..e5ad7d5ec 100644 --- 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 @@ -286,7 +286,7 @@ void galileo_e1_dll_pll_veml_tracking_cc::update_local_carrier() phase_rad = d_rem_carr_phase_rad; for(int i = 0; i < d_current_prn_length_samples; i++) { - d_carr_sign[i] = gr_complex(cos(phase_rad), sin(phase_rad)); + d_carr_sign[i] = gr_complex(cos(phase_rad), -sin(phase_rad)); phase_rad += phase_step_rad; } d_rem_carr_phase_rad = fmod(phase_rad, GPS_TWO_PI); @@ -460,8 +460,8 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect // ########### Output the tracking results to Telemetry block ########## - current_synchro_data.Prompt_I = (double)(*d_Prompt).imag(); // ??????? - current_synchro_data.Prompt_Q = (double)(*d_Prompt).real(); // ??????? + current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); + current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); // Tracking_timestamp_secs is aligned with the PRN start sample current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_next_prn_length_samples + (double)d_next_rem_code_phase_samples) / (double)d_fs_in; @@ -517,8 +517,8 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; float tmp_float; double tmp_double; - prompt_I = (*d_Prompt).imag(); - prompt_Q = (*d_Prompt).real(); + prompt_I = (*d_Prompt).real(); + prompt_Q = (*d_Prompt).imag(); tmp_VE = std::abs(*d_Very_Early); tmp_E = std::abs(*d_Early); tmp_P = std::abs(*d_Prompt); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc index 935bf5bef..c188d4d4a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc @@ -310,7 +310,7 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_carrier() phase = d_rem_carr_phase; for(int i = 0; i < d_current_prn_length_samples; i++) { - d_carr_sign[i] = gr_complex(cos(phase), sin(phase)); + d_carr_sign[i] = gr_complex(cos(phase), -sin(phase)); phase += phase_step; } d_rem_carr_phase = fmod(phase, GPS_TWO_PI); @@ -319,8 +319,6 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_carrier() - - Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc() { d_dump_file.close(); @@ -452,19 +450,12 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto // Compute PLL error PLL_discriminator_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GPS_TWO_PI; - /* - * \todo Update FLL assistance algorithm! - */ - if ((((double)d_sample_counter - (double)d_acq_sample_stamp) / d_fs_in) > 3) - { - d_FLL_discriminator_hz = 0; //disconnect the FLL after the initial lock - } /* * DLL and FLL+PLL filter and get current carrier Doppler and code frequency */ carr_nco_hz = d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz, PLL_discriminator_hz, correlation_time_s); d_carrier_doppler_hz = d_if_freq + carr_nco_hz; - d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ - (((d_carrier_doppler_hz - d_if_freq) * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ) - code_error_chips; + d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ + (((d_carrier_doppler_hz + d_if_freq) * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ) - code_error_chips; /*! * \todo Improve the lock detection algorithm! @@ -539,8 +530,8 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I=(double)(*d_Prompt).imag(); - current_synchro_data.Prompt_Q=(double)(*d_Prompt).real(); + current_synchro_data.Prompt_I=(double)(*d_Prompt).real(); + current_synchro_data.Prompt_Q=(double)(*d_Prompt).imag(); // Tracking_timestamp_secs is aligned with the PRN start sample current_synchro_data.Tracking_timestamp_secs=((double)d_sample_counter+(double)d_current_prn_length_samples+d_rem_code_phase_samples)/d_fs_in; // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, Code_phase_secs=0 @@ -568,8 +559,8 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto float tmp_E, tmp_P, tmp_L; float tmp_float; double tmp_double; - prompt_I = (*d_Prompt).imag(); - prompt_Q = (*d_Prompt).real(); + prompt_I = (*d_Prompt).real(); + prompt_Q = (*d_Prompt).imag(); tmp_E=std::abs(*d_Early); tmp_P=std::abs(*d_Prompt); tmp_L=std::abs(*d_Late); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc index a1d09810b..ed01a2794 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc @@ -82,7 +82,7 @@ gps_l1_ca_dll_pll_make_optim_tracking_cc( void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required) { - ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call + ninput_items_required[0] = d_gnuradio_forecast_samples ; //set the required available samples in each call } @@ -109,6 +109,7 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc( d_if_freq = if_freq; d_fs_in = fs_in; d_vector_length = vector_length; + d_gnuradio_forecast_samples=(int)d_vector_length*2; d_dump_filename = dump_filename; // Initialize tracking ========================================== @@ -339,14 +340,14 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::update_local_carrier() { //using temp variables gr_fxpt::sincos(phase_rad_i,&sin_f,&cos_f); - d_carr_sign[i] = gr_complex(cos_f, sin_f); + d_carr_sign[i] = gr_complex(cos_f, -sin_f); //using references (may be it can be a problem for c++11 standard //gr_fxpt::sincos(phase_rad_i,&d_carr_sign[i].imag(),&d_carr_sign[i].real()); phase_rad_i += phase_step_rad_i; // Using std::cos and std::sin - //d_carr_sign[i] = gr_complex(cos(phase_rad), sin(phase_rad)); + //d_carr_sign[i] = gr_complex(cos(phase_rad), -sin(phase_rad)); } d_rem_carr_phase_rad = fmod(gr_fxpt::fixed_to_float(phase_rad_i), GPS_TWO_PI); @@ -425,7 +426,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec // variable code PRN sample block size d_current_prn_length_samples = d_next_prn_length_samples; - //update_local_code(); + update_local_code(); update_local_carrier(); // perform Early, Prompt and Late correlation @@ -535,8 +536,8 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = (double)(*d_Prompt).imag(); - current_synchro_data.Prompt_Q = (double)(*d_Prompt).real(); + current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); + current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); // Tracking_timestamp_secs is aligned with the PRN start sample current_synchro_data.Tracking_timestamp_secs=((double)d_sample_counter+(double)d_next_prn_length_samples+(double)d_next_rem_code_phase_samples)/(double)d_fs_in; // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 @@ -593,8 +594,8 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec float tmp_E, tmp_P, tmp_L; float tmp_float; double tmp_double; - prompt_I = (*d_Prompt).imag(); - prompt_Q = (*d_Prompt).real(); + prompt_I = (*d_Prompt).real(); + prompt_Q = (*d_Prompt).imag(); tmp_E = std::abs(*d_Early); tmp_P = std::abs(*d_Prompt); tmp_L = std::abs(*d_Late); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h index 1f7c5fb3d..487b15119 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h @@ -135,7 +135,6 @@ private: long d_fs_in; double d_early_late_spc_chips; - double d_code_phase_step_chips; gr_complex* d_ca_code; @@ -190,6 +189,7 @@ private: // control vars bool d_enable_tracking; bool d_pull_in; + int d_gnuradio_forecast_samples; // file dump std::string d_dump_filename; 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 f515099f3..ed3fbc99e 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 @@ -307,7 +307,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_carrier() phase_rad = d_rem_carr_phase_rad; for(int i = 0; i < d_current_prn_length_samples; i++) { - d_carr_sign[i] = gr_complex(cos(phase_rad), sin(phase_rad)); + d_carr_sign[i] = gr_complex(cos(phase_rad), -sin(phase_rad)); phase_rad += phase_step_rad; } d_rem_carr_phase_rad = fmod(phase_rad, GPS_TWO_PI); @@ -496,8 +496,8 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = (double)(*d_Prompt).imag(); - current_synchro_data.Prompt_Q = (double)(*d_Prompt).real(); + current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); + current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); // Tracking_timestamp_secs is aligned with the PRN start sample current_synchro_data.Tracking_timestamp_secs=((double)d_sample_counter+(double)d_next_prn_length_samples+(double)d_next_rem_code_phase_samples)/(double)d_fs_in; // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 @@ -554,8 +554,8 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in float tmp_E, tmp_P, tmp_L; float tmp_float; double tmp_double; - prompt_I = (*d_Prompt).imag(); - prompt_Q = (*d_Prompt).real(); + prompt_I = (*d_Prompt).real(); + prompt_Q = (*d_Prompt).imag(); tmp_E = std::abs(*d_Early); tmp_P = std::abs(*d_Prompt); tmp_L = std::abs(*d_Late); 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 a33af890b..16a808dc2 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 @@ -310,10 +310,6 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::update_local_code() { associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips)); d_early_code[i] = d_ca_code[associated_chip_index]; - //associated_chip_index = 1 + round(fmod(tcode_chips, code_length_chips)); - //d_prompt_code[i] = d_ca_code[associated_chip_index]; - //associated_chip_index = 1 + round(fmod(tcode_chips+d_early_late_spc_chips, code_length_chips)); - //d_late_code[i] = d_ca_code[associated_chip_index]; tcode_chips = tcode_chips + d_code_phase_step_chips; } @@ -332,7 +328,7 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::update_local_carrier() phase_rad = d_rem_carr_phase_rad; for(int i = 0; i < d_current_prn_length_samples; i++) { - d_carr_sign[i] = gr_complex(cos(phase_rad), sin(phase_rad)); + d_carr_sign[i] = gr_complex(cos(phase_rad), -sin(phase_rad)); phase_rad += phase_step_rad; } d_rem_carr_phase_rad = fmod(phase_rad, GPS_TWO_PI); @@ -466,7 +462,7 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec d_control_id++; //! Send and receive a TCP packet - boost::array tx_variables_array = {{d_control_id,(*d_Early).imag(),(*d_Early).real(),(*d_Late).imag(),(*d_Late).real(),(*d_Prompt).imag(),(*d_Prompt).real(),d_acq_carrier_doppler_hz,1}}; + boost::array tx_variables_array = {{d_control_id,(*d_Early).real(),(*d_Early).imag(),(*d_Late).real(),(*d_Late).imag(),(*d_Prompt).real(),(*d_Prompt).imag(),d_acq_carrier_doppler_hz,1}}; d_tcp_com.send_receive_tcp_packet(tx_variables_array, &tcp_data); //! Recover the tracking data @@ -545,8 +541,8 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = (double)(*d_Prompt).imag(); - current_synchro_data.Prompt_Q = (double)(*d_Prompt).real(); + current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); + current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); current_synchro_data.Tracking_timestamp_secs = d_sample_counter_seconds; current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad; current_synchro_data.Code_phase_secs = (double)d_code_phase_samples * (1/(float)d_fs_in); @@ -600,8 +596,8 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec float prompt_Q; float tmp_E, tmp_P, tmp_L; float tmp_float; - prompt_I = (*d_Prompt).imag(); - prompt_Q = (*d_Prompt).real(); + prompt_I = (*d_Prompt).real(); + prompt_Q = (*d_Prompt).imag(); tmp_E = std::abs(*d_Early); tmp_P = std::abs(*d_Prompt); tmp_L = std::abs(*d_Late); diff --git a/src/algorithms/tracking/libs/CN_estimators.cc b/src/algorithms/tracking/libs/CN_estimators.cc index c69963c56..0ef26047a 100644 --- a/src/algorithms/tracking/libs/CN_estimators.cc +++ b/src/algorithms/tracking/libs/CN_estimators.cc @@ -72,19 +72,17 @@ float gps_l1_ca_CN0_SNV(gr_complex* Prompt_buffer, int length, long fs_in) { // estimate CN0 using buffered values // MATLAB CODE - // Psig=((1/N)*sum(abs(imag(x((n-N+1):n)))))^2; - // Ptot=(1/N)*sum(abs(x((n-N+1):n)).^2); // SNR_SNV(count)=Psig/(Ptot-Psig); // CN0_SNV_dB=10*log10(SNR_SNV)+10*log10(BW)-10*log10(PRN_length); float SNR, SNR_dB_Hz; - float tmp_abs_imag; + float tmp_abs_real; float Psig, Ptot; Psig = 0; Ptot = 0; for (int i=0; i