From c8f7e0812710085d91a4fc50acccef166a86a277 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 26 Nov 2015 18:44:04 +0100 Subject: [PATCH] Updated all GPS and Galileo trackings for double floating point internal computations and bug fixes in the carrier phase accumulator. (all, except Matlab-Simulink linked trackings) --- CMakeLists.txt | 59 +++++++--- conf/gnss-sdr_Hybrid_byte_sim.conf | 12 +- .../gps_l1_ca_observables_cc.cc | 47 ++------ .../tracking/gnuradio_blocks/CMakeLists.txt | 13 +- .../galileo_e1_dll_pll_veml_tracking_cc.cc | 61 ++++++---- .../galileo_e1_dll_pll_veml_tracking_cc.h | 18 +-- .../galileo_e1_tcp_connector_tracking_cc.cc | 2 +- .../galileo_e5a_dll_pll_tracking_cc.cc | 111 +++++++++--------- .../galileo_e5a_dll_pll_tracking_cc.h | 34 +++--- ...alileo_volk_e1_dll_pll_veml_tracking_cc.cc | 61 ++++++---- ...galileo_volk_e1_dll_pll_veml_tracking_cc.h | 18 +-- .../gps_l1_ca_dll_fll_pll_tracking_cc.cc | 9 +- .../gps_l1_ca_dll_pll_artemisa_tracking_cc.cc | 47 ++++---- .../gps_l1_ca_dll_pll_artemisa_tracking_cc.h | 4 +- .../gps_l1_ca_dll_pll_optim_tracking_cc.cc | 63 +++++----- .../gps_l1_ca_dll_pll_optim_tracking_cc.h | 20 ++-- .../gps_l1_ca_dll_pll_tracking_cc.cc | 104 ++++++++-------- .../gps_l1_ca_dll_pll_tracking_cc.h | 20 ++-- .../gps_l1_ca_dll_pll_tracking_gpu_cc.cc | 2 +- .../gps_l2_m_dll_pll_tracking_cc.cc | 106 ++++++++--------- .../gps_l2_m_dll_pll_tracking_cc.h | 20 ++-- src/algorithms/tracking/libs/CMakeLists.txt | 28 ++--- .../tracking/libs/tracking_discriminators.cc | 16 +-- .../tracking/libs/tracking_discriminators.h | 10 +- src/core/system_parameters/GPS_L1_CA.h | 2 +- src/main/CMakeLists.txt | 4 + 26 files changed, 460 insertions(+), 431 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ea860622b..e728ba8a5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -733,8 +733,8 @@ if(NOT ARMADILLO_FOUND) message(STATUS " Armadillo will be downloaded and built automatically ") message(STATUS " when doing 'make'. ") - set(armadillo_RELEASE 5.200.2) - set(armadillo_MD5 "ef57ba4c473a3b67c672441a7face09e") + set(armadillo_RELEASE 6.200.2) + set(armadillo_MD5 "e07910be1a79b20fa2efe1006a274390") ExternalProject_Add( armadillo-${armadillo_RELEASE} @@ -781,21 +781,42 @@ endif(NOT ARMADILLO_FOUND) # GnuTLS - http://www.gnutls.org/ ################################################################################ find_package(GnuTLS) -if(NOT GNUTLS_FOUND) - message(" The GnuTLS library has not been found.") - message(" You can try to install it by typing:") - if(OS_IS_LINUX) - if(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "Red Hat") - message(" sudo yum install libgnutls-openssl-devel") - else(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "Red Hat") - message(" sudo apt-get install libgnutls-openssl-dev") - endif(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "Red Hat") - endif(OS_IS_LINUX) - if(OS_IS_MACOSX) - message(" sudo port install gnutls") - endif(OS_IS_MACOSX) - message(FATAL_ERROR "GnuTLS libraries are required to build gnss-sdr") -endif(NOT GNUTLS_FOUND) +find_library(GNUTLS_OPENSSL_LIBRARY NAMES gnutls-openssl libgnutls-openssl.so.27 + HINTS /usr/lib + /usr/lib64 + /usr/local/lib + /usr/local/lib64 + /opt/local/lib + /usr/lib/x86_64-linux-gnu + /usr/lib/aarch64-linux-gnu + /usr/lib/arm-linux-gnueabihf + /usr/lib/arm-linux-gnueabi + /usr/lib/i386-linux-gnu + ) + +if(NOT GNUTLS_OPENSSL_LIBRARY) + message(STATUS "Looking for OpenSSL instead...") + find_package(OpenSSL) + if(OPENSSL_FOUND) + set(GNUTLS_INCLUDE_DIR ${OPENSSL_INCLUDE_DIR}) + set(GNUTLS_LIBRARIES "") + set(GNUTLS_OPENSSL_LIBRARY ${OPENSSL_SSL_LIBRARY}) + else(OPENSSL_FOUND) + message(" The GnuTLS library with openssl compatibility enabled has not been found.") + message(" You can try to install the required libraries by typing:") + if(OS_IS_LINUX) + if(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "Red Hat") + message(" sudo yum install openssl-devel") + else(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "Red Hat") + message(" sudo apt-get install libgnutls-openssl-dev") + endif(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "Red Hat") + endif(OS_IS_LINUX) + if(OS_IS_MACOSX) + message(" sudo port install gnutls") + endif(OS_IS_MACOSX) + message(FATAL_ERROR "GnuTLS libraries with openssl compatibility are required to build gnss-sdr") + endif(OPENSSL_FOUND) +endif(NOT GNUTLS_OPENSSL_LIBRARY) ################################################################################ @@ -1085,9 +1106,9 @@ endif(ENABLE_GPROF) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${MY_CXX_FLAGS}") if(OS_IS_LINUX) - if(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "openSUSE") + if(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "openSUSE" OR ${LINUX_DISTRIBUTION} MATCHES "ArchLinux") link_libraries(pthread) - endif(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "openSUSE") + endif(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "openSUSE" OR ${LINUX_DISTRIBUTION} MATCHES "ArchLinux") endif(OS_IS_LINUX) diff --git a/conf/gnss-sdr_Hybrid_byte_sim.conf b/conf/gnss-sdr_Hybrid_byte_sim.conf index 126dfba41..2f8cb0654 100644 --- a/conf/gnss-sdr_Hybrid_byte_sim.conf +++ b/conf/gnss-sdr_Hybrid_byte_sim.conf @@ -7,7 +7,7 @@ ;######### GLOBAL OPTIONS ################## ;internal_fs_hz: Internal signal sampling frequency after the signal conditioning stage [Hz]. -GNSS-SDR.internal_fs_hz=2600000 +GNSS-SDR.internal_fs_hz=4000000 ;######### CONTROL_THREAD CONFIG ############ ControlThread.wait_for_flowgraph=false @@ -23,7 +23,7 @@ SignalSource.filename=/home/javier/ClionProjects/gnss-sim/build/signal_out.bin SignalSource.item_type=byte ;#sampling_frequency: Original Signal sampling frequency in [Hz] -SignalSource.sampling_frequency=2600000 +SignalSource.sampling_frequency=4000000 ;#freq: RF front-end center frequency in [Hz] SignalSource.freq=1575420000 @@ -127,7 +127,7 @@ InputFilter.grid_density=16 ;#The following options are used only in Freq_Xlating_Fir_Filter implementation. ;#InputFilter.IF is the intermediate frequency (in Hz) shifted down to zero Hz -InputFilter.sampling_frequency=2600000 +InputFilter.sampling_frequency=4000000 InputFilter.IF=0 @@ -150,10 +150,10 @@ Resampler.dump_filename=../data/resampler.dat Resampler.item_type=gr_complex ;#sample_freq_in: the sample frequency of the input signal -Resampler.sample_freq_in=2600000 +Resampler.sample_freq_in=4000000 ;#sample_freq_out: the desired sample frequency of the output signal -Resampler.sample_freq_out=2600000 +Resampler.sample_freq_out=4000000 ;######### CHANNELS GLOBAL CONFIG ############ @@ -253,7 +253,7 @@ Tracking_1C.pll_bw_hz=15.0; Tracking_1C.dll_bw_hz=1.5; ;#fll_bw_hz: FLL loop filter bandwidth [Hz] -Tracking_1C.fll_bw_hz=10.0; +Tracking_1C.fll_bw_hz=2.0; ;#order: PLL/DLL loop filter order [2] or [3] Tracking_1C.order=3; 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 5e7cc382a..6c52dcf0a 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 @@ -211,7 +211,6 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni 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())); - //std::cout<<"symbol_TOW_vec_s[0]="<(Galileo_E1_B_CODE_LENGTH_CHIPS) * 2; double code_phase_step_chips; @@ -246,11 +246,11 @@ void galileo_e1_dll_pll_veml_tracking_cc::update_local_code() int epl_loop_length_samples; // unified loop for VE, E, P, L, VL code vectors - code_phase_step_chips = (static_cast(d_code_freq_chips)) / (static_cast(d_fs_in)); - code_phase_step_half_chips = (2.0 * static_cast(d_code_freq_chips)) / (static_cast(d_fs_in)); + code_phase_step_chips = d_code_freq_chips / (static_cast(d_fs_in)); + code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast(d_fs_in)); rem_code_phase_half_chips = d_rem_code_phase_samples * (2*d_code_freq_chips / d_fs_in); - tcode_half_chips = - static_cast(rem_code_phase_half_chips); + tcode_half_chips = - rem_code_phase_half_chips; early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); very_early_late_spc_samples = round(d_very_early_late_spc_chips / code_phase_step_chips); @@ -310,10 +310,14 @@ galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc() int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { - float carr_error_hz; - float carr_error_filt_hz; - float code_error_chips; - float code_error_filt_chips; + double carr_error_hz; + carr_error_hz=0.0; + double carr_error_filt_hz; + carr_error_filt_hz=0.0; + double code_error_chips; + code_error_chips=0.0; + double code_error_filt_chips; + code_error_filt_chips=0.0; if (d_enable_tracking == true) { @@ -323,7 +327,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect * Signal alignment (skip samples until the incoming signal is aligned with local replica) */ int samples_offset; - float acq_trk_shif_correction_samples; + double acq_trk_shif_correction_samples; int acq_to_trk_delay_samples; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); @@ -372,7 +376,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect // 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 = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; + d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; //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); @@ -383,7 +387,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect // Code discriminator filter code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] //Code phase accumulator - float code_error_filt_secs; + double code_error_filt_secs; code_error_filt_secs = (Galileo_E1_CODE_PERIOD * code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] //code_error_filt_secs=T_prn_seconds*code_error_filt_chips*T_chip_seconds*static_cast(d_fs_in); //[seconds] d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; @@ -395,7 +399,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect double T_prn_samples; double K_blk_samples; // Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1 / static_cast(d_code_freq_chips); + T_chip_seconds = 1.0 / d_code_freq_chips; 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); @@ -460,9 +464,9 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 current_synchro_data.Code_phase_secs = 0; - current_synchro_data.Carrier_phase_rads = static_cast(d_acc_carrier_phase_rad); - current_synchro_data.Carrier_Doppler_hz = static_cast(d_carrier_doppler_hz); - current_synchro_data.CN0_dB_hz = static_cast(d_CN0_SNV_dB_Hz); + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.Flag_valid_pseudorange = false; *out[0] = current_synchro_data; @@ -547,19 +551,28 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect // PRN start sample stamp d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); + tmp_float=d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(float)); + tmp_float=d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float=d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + tmp_float=carr_error_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float=carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + tmp_float=code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float=code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); + tmp_float=d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float=d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // AUX vars (for debug purposes) tmp_float = d_rem_code_phase_samples; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h index 10b073802..c42eb048b 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h @@ -126,8 +126,8 @@ private: long d_if_freq; long d_fs_in; - float d_early_late_spc_chips; - float d_very_early_late_spc_chips; + double d_early_late_spc_chips; + double d_very_early_late_spc_chips; gr_complex* d_ca_code; @@ -146,22 +146,22 @@ private: // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars double d_code_freq_chips; - float d_carrier_doppler_hz; + double d_carrier_doppler_hz; double d_acc_carrier_phase_rad; double d_acc_code_phase_secs; @@ -175,9 +175,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars 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 74a399d18..ec5119e48 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 @@ -387,7 +387,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve // 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 = d_acc_carrier_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD; + d_acc_carrier_phase_rad -= GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD; //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); 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 0a988a1af..fccccfb21 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 @@ -217,18 +217,18 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::start_tracking() d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; long int acq_trk_diff_samples; - float acq_trk_diff_seconds; + double acq_trk_diff_seconds; acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp);//-d_vector_length; LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); //doppler effect // Fd=(C/(C+Vr))*F - float radial_velocity; + double radial_velocity; radial_velocity = (Galileo_E5a_FREQ_HZ + d_acq_carrier_doppler_hz)/Galileo_E5a_FREQ_HZ; // new chip and prn sequence periods based on acq Doppler - float T_chip_mod_seconds; - float T_prn_mod_seconds; - float T_prn_mod_samples; + double T_chip_mod_seconds; + double T_prn_mod_seconds; + double T_prn_mod_samples; d_code_freq_chips = radial_velocity * Galileo_E5a_CODE_CHIP_RATE_HZ; T_chip_mod_seconds = 1/d_code_freq_chips; T_prn_mod_seconds = T_chip_mod_seconds * Galileo_E5a_CODE_LENGTH_CHIPS; @@ -236,13 +236,13 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::start_tracking() d_current_prn_length_samples = round(T_prn_mod_samples); - float T_prn_true_seconds = Galileo_E5a_CODE_LENGTH_CHIPS / Galileo_E5a_CODE_CHIP_RATE_HZ; - float T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); - float T_prn_diff_seconds; + double T_prn_true_seconds = Galileo_E5a_CODE_LENGTH_CHIPS / Galileo_E5a_CODE_CHIP_RATE_HZ; + double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_diff_seconds; T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; - float N_prn_diff; + double N_prn_diff; N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; - float corrected_acq_phase_samples, delay_correction_samples; + double corrected_acq_phase_samples, delay_correction_samples; corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); if (corrected_acq_phase_samples < 0) { @@ -358,7 +358,7 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::update_local_code() int epl_loop_length_samples; // unified loop for E, P, L code vectors - code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); + code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in); tcode_chips = -rem_code_phase_chips; @@ -383,7 +383,7 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::update_local_code() void Galileo_E5a_Dll_Pll_Tracking_cc::update_local_carrier() { float sin_f, cos_f; - float phase_step_rad = static_cast(2 * GALILEO_PI) * d_carrier_doppler_hz / static_cast(d_fs_in); + float phase_step_rad = static_cast(2.0 * GALILEO_PI * 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); @@ -400,10 +400,10 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_ gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars - float carr_error_hz; - float carr_error_filt_hz; - float code_error_chips; - float code_error_filt_chips; + double carr_error_hz; + 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 @@ -451,7 +451,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_ case 1: { int samples_offset; - float acq_trk_shif_correction_samples; + double acq_trk_shif_correction_samples; int acq_to_trk_delay_samples; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); @@ -561,11 +561,11 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_ { if (d_secondary_lock == true) { - carr_error_hz = pll_four_quadrant_atan(d_Prompt) / static_cast(GALILEO_PI) * 2; + carr_error_hz = pll_four_quadrant_atan(d_Prompt) / GALILEO_PI * 2.0; } else { - carr_error_hz = pll_cloop_two_quadrant_atan(d_Prompt) / static_cast(GALILEO_PI) * 2; + carr_error_hz = pll_cloop_two_quadrant_atan(d_Prompt) / GALILEO_PI * 2.0; } // Carrier discriminator filter @@ -576,10 +576,10 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_ d_code_freq_chips = Galileo_E5a_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E5a_CODE_CHIP_RATE_HZ) / Galileo_E5a_FREQ_HZ); } //carrier phase accumulator for (K) doppler estimation - d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + 2*GALILEO_PI * d_carrier_doppler_hz * GALILEO_E5a_CODE_PERIOD; + d_acc_carrier_phase_rad -= 2*GALILEO_PI * d_carrier_doppler_hz * GALILEO_E5a_CODE_PERIOD; //remanent carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + 2*GALILEO_PI * d_carrier_doppler_hz * GALILEO_E5a_CODE_PERIOD; - d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, 2*GALILEO_PI); + d_rem_carr_phase_rad = d_rem_carr_phase_rad + 2.0*GALILEO_PI * d_carrier_doppler_hz * GALILEO_E5a_CODE_PERIOD; + d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, 2.0*GALILEO_PI); // ################## DLL ########################################################## if (d_integration_counter == d_current_ti_ms) @@ -600,7 +600,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_ double T_prn_samples; double K_blk_samples; // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1 / static_cast(d_code_freq_chips); + T_chip_seconds = 1.0 / d_code_freq_chips; T_prn_seconds = T_chip_seconds * Galileo_E5a_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 + d_code_error_filt_secs * static_cast(d_fs_in); @@ -694,9 +694,9 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_ current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_current_prn_length_samples) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 current_synchro_data.Code_phase_secs = 0; - current_synchro_data.Carrier_phase_rads = static_cast(d_acc_carrier_phase_rad); - current_synchro_data.Carrier_Doppler_hz = static_cast(d_carrier_doppler_hz); - current_synchro_data.CN0_dB_hz = static_cast(d_CN0_SNV_dB_Hz); + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.Flag_valid_tracking = false; @@ -781,39 +781,42 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_ } try { - // EPR - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); - // PRN start sample stamp - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); - // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); - // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(float)); + // EPR + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + //tmp_float=(float)d_sample_counter; + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + //PLL commands + d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(double)); - // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); + //DLL commands + d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); + + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + + // AUX vars (for debug purposes) + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } catch (std::ifstream::failure e) { diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h index 5b40a7c73..7257f80d8 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h @@ -137,10 +137,10 @@ private: long d_fs_in; double d_early_late_spc_chips; - float d_dll_bw_hz; - float d_pll_bw_hz; - float d_dll_bw_init_hz; - float d_pll_bw_init_hz; + double d_dll_bw_hz; + double d_pll_bw_hz; + double d_dll_bw_init_hz; + double d_pll_bw_init_hz; gr_complex* d_codeQ; gr_complex* d_codeI; @@ -160,26 +160,26 @@ private: float tmp_P; float tmp_L; // remaining code phase and carrier phase between tracking loops - float d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_code_phase_samples; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars - float d_code_freq_chips; - float d_carrier_doppler_hz; - float d_acc_carrier_phase_rad; - float d_code_phase_samples; - float d_acc_code_phase_secs; - float d_code_error_filt_secs; + double d_code_freq_chips; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_code_phase_samples; + double d_acc_code_phase_secs; + double d_code_error_filt_secs; //PRN period in samples int d_current_prn_length_samples; @@ -191,9 +191,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc index d9ae8bd9c..820b99830 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc @@ -253,7 +253,7 @@ void galileo_volk_e1_dll_pll_veml_tracking_cc::start_tracking() void galileo_volk_e1_dll_pll_veml_tracking_cc::update_local_code() { double tcode_half_chips; - float rem_code_phase_half_chips; + double rem_code_phase_half_chips; int code_length_half_chips = static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS) * 2; double code_phase_step_chips; double code_phase_step_half_chips; @@ -262,11 +262,11 @@ void galileo_volk_e1_dll_pll_veml_tracking_cc::update_local_code() int epl_loop_length_samples; // unified loop for VE, E, P, L, VL code vectors - code_phase_step_chips = (static_cast(d_code_freq_chips)) / (static_cast(d_fs_in)); - code_phase_step_half_chips = (2.0 * static_cast(d_code_freq_chips)) / (static_cast(d_fs_in)); + code_phase_step_chips = (d_code_freq_chips) / (static_cast(d_fs_in)); + code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast(d_fs_in)); rem_code_phase_half_chips = d_rem_code_phase_samples * (2*d_code_freq_chips / d_fs_in); - tcode_half_chips = - static_cast(rem_code_phase_half_chips); + tcode_half_chips = - rem_code_phase_half_chips; early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); very_early_late_spc_samples = round(d_very_early_late_spc_chips / code_phase_step_chips); @@ -287,9 +287,9 @@ void galileo_volk_e1_dll_pll_veml_tracking_cc::update_local_carrier() { float phase_rad, phase_step_rad; // Compute the carrier phase step for the K-1 carrier doppler estimation - phase_step_rad = static_cast(GPS_TWO_PI) * d_carrier_doppler_hz / static_cast(d_fs_in); + phase_step_rad = static_cast (GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in)); // Initialize the carrier phase with the remanent carrier phase of the K-2 loop - phase_rad = d_rem_carr_phase_rad; + phase_rad = static_cast (d_rem_carr_phase_rad); //HERE YOU CAN CHOOSE THE DESIRED VOLK IMPLEMENTATION //volk_gnsssdr_s32f_x2_update_local_carrier_32fc_manual(d_carr_sign, phase_rad, phase_step_rad, d_current_prn_length_samples, "generic"); @@ -340,10 +340,10 @@ galileo_volk_e1_dll_pll_veml_tracking_cc::~galileo_volk_e1_dll_pll_veml_tracking int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { - float carr_error_hz; - float carr_error_filt_hz; - float code_error_chips; - float code_error_filt_chips; + double carr_error_hz; + double carr_error_filt_hz; + double code_error_chips; + double code_error_filt_chips; if (d_enable_tracking == true) { @@ -353,7 +353,7 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr * Signal alignment (skip samples until the incoming signal is aligned with local replica) */ int samples_offset; - float acq_trk_shif_correction_samples; + double acq_trk_shif_correction_samples; int acq_to_trk_delay_samples; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); @@ -419,7 +419,7 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr // 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 = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; + d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; //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); @@ -430,7 +430,7 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr // Code discriminator filter code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] //Code phase accumulator - float code_error_filt_secs; + double code_error_filt_secs; code_error_filt_secs = (Galileo_E1_CODE_PERIOD * code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] //code_error_filt_secs=T_prn_seconds*code_error_filt_chips*T_chip_seconds*static_cast(d_fs_in); //[seconds] d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; @@ -442,7 +442,7 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr double T_prn_samples; double K_blk_samples; // Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1 / static_cast(d_code_freq_chips); + T_chip_seconds = 1.0 / d_code_freq_chips; 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); @@ -507,9 +507,9 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 current_synchro_data.Code_phase_secs = 0; - current_synchro_data.Carrier_phase_rads = static_cast(d_acc_carrier_phase_rad); - current_synchro_data.Carrier_Doppler_hz = static_cast(d_carrier_doppler_hz); - current_synchro_data.CN0_dB_hz = static_cast(d_CN0_SNV_dB_Hz); + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.Flag_valid_pseudorange = false; *out[0] = current_synchro_data; @@ -594,19 +594,28 @@ int galileo_volk_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr // PRN start sample stamp d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); + tmp_float=d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(float)); + tmp_float=d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float=d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + tmp_float=carr_error_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float=carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + tmp_float=code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float=code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); + tmp_float=d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float=d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); // AUX vars (for debug purposes) tmp_float = d_rem_code_phase_samples; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h index 7f118abc5..ead8d0502 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.h @@ -126,8 +126,8 @@ private: long d_if_freq; long d_fs_in; - float d_early_late_spc_chips; - float d_very_early_late_spc_chips; + double d_early_late_spc_chips; + double d_very_early_late_spc_chips; gr_complex* d_ca_code; @@ -162,22 +162,22 @@ private: // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars double d_code_freq_chips; - float d_carrier_doppler_hz; + double d_carrier_doppler_hz; double d_acc_carrier_phase_rad; double d_acc_code_phase_secs; @@ -191,9 +191,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars 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 c56242560..6db945272 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 @@ -315,7 +315,7 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_carrier() phase += phase_step; } d_rem_carr_phase = fmod(phase, GPS_TWO_PI); - d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + phase; + d_acc_carrier_phase_rad -= d_acc_carrier_phase_rad + phase; } @@ -439,6 +439,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto if (d_FLL_wait == 1) { d_Prompt_prev = *d_Prompt; + d_FLL_discriminator_hz=0.0; d_FLL_wait = 0; } else @@ -454,7 +455,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto /* * DLL and FLL+PLL filter and get current carrier Doppler and code frequency */ - carr_nco_hz = d_carrier_loop_filter.get_carrier_error(0.0, PLL_discriminator_hz, GPS_L1_CA_CODE_PERIOD); + 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); @@ -528,9 +529,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto double T_prn_samples; double K_blk_samples; T_chip_seconds = 1 / static_cast(d_code_freq_hz); - T_chip_seconds=GPS_L1_CA_CHIP_PERIOD; - //T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - T_prn_seconds = GPS_L1_CA_CODE_PERIOD; + T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_samples = T_prn_seconds * d_fs_in; float code_error_filt_samples; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc index 6c287a4c2..17a4e9ac4 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_artemisa_tracking_cc.cc @@ -102,7 +102,7 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_tracking_cc( d_fs_in = fs_in; d_vector_length = vector_length; d_dump_filename = dump_filename; - d_current_prn_length_samples = static_cast(d_vector_length); + d_correlation_length_samples = static_cast(d_vector_length); // Initialize tracking ========================================== d_code_loop_filter.set_DLL_BW(dll_bw_hz); @@ -128,7 +128,7 @@ gps_l1_ca_dll_pll_artemisa_tracking_cc::gps_l1_ca_dll_pll_artemisa_tracking_cc( d_local_code_shift_chips[1]=0.0; d_local_code_shift_chips[2]=d_early_late_spc_chips; - multicorrelator_cpu.init(2*d_current_prn_length_samples,d_n_correlator_taps); + multicorrelator_cpu.init(2*d_correlation_length_samples,d_n_correlator_taps); //--- Perform initializations ------------------------------ // define initial code frequency basis of NCO @@ -202,7 +202,7 @@ void gps_l1_ca_dll_pll_artemisa_tracking_cc::start_tracking() T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); - d_current_prn_length_samples = round(T_prn_mod_samples); + d_correlation_length_samples = round(T_prn_mod_samples); double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); @@ -291,6 +291,7 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_ double code_error_filt_chips=0.0; double code_error_filt_secs_Ti=0.0; double CURRENT_INTEGRATION_TIME_S; + double CORRECTED_INTEGRATION_TIME_S; double dll_code_error_secs_Ti=0.0; double carr_phase_error_secs_Ti=0.0; double old_d_rem_code_phase_samples; @@ -303,9 +304,9 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_ double acq_trk_shif_correction_samples; int acq_to_trk_delay_samples; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; - acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_correlation_length_samples)); samples_offset = 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_sample_counter += samples_offset; //count for the processed samples d_pull_in = false; // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; @@ -320,16 +321,10 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_ // ################# CARRIER WIPEOFF AND CORRELATORS ############################## // perform carrier wipe-off and compute Early, Prompt and Late correlation multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,in); - multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carrier_phase_rad,d_carrier_phase_step_rad,d_rem_code_phase_chips,d_code_phase_step_chips,d_current_prn_length_samples); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carrier_phase_rad,d_carrier_phase_step_rad,d_rem_code_phase_chips,d_code_phase_step_chips,d_correlation_length_samples); // UPDATE INTEGRATION TIME - CURRENT_INTEGRATION_TIME_S=(static_cast(d_current_prn_length_samples)/static_cast(d_fs_in)); - // UPDATE REMNANT CARRIER PHASE - //remnant carrier phase [rad] - d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S,GPS_TWO_PI); - // UPDATE CARRIER PHASE ACCUULATOR - //carrier phase accumulator prior to update the PLL estimators (accumulated carrier in this loop depends on the old estimations!) - d_acc_carrier_phase_cycles -= d_carrier_doppler_hz*CURRENT_INTEGRATION_TIME_S; + CURRENT_INTEGRATION_TIME_S=(static_cast(d_correlation_length_samples)/static_cast(d_fs_in)); // ################## PLL ########################################################## // Update PLL discriminator [rads/Ti -> Secs/Ti] @@ -351,7 +346,8 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_ code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips_Ti); //input [chips/Ti] -> output [chips/second] code_error_filt_secs_Ti = code_error_filt_chips*CURRENT_INTEGRATION_TIME_S/d_code_freq_chips; // [s/Ti] // DLL code error estimation [s/Ti] - dll_code_error_secs_Ti=-code_error_filt_secs_Ti+d_pll_to_dll_assist_secs_Ti; + // TODO: PLL carrier aid to DLL is disabled. Re-enable it and measure performance + dll_code_error_secs_Ti=-code_error_filt_secs_Ti;//+d_pll_to_dll_assist_secs_Ti; // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // keep alignment parameters for the next input buffer @@ -364,9 +360,20 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_ T_prn_seconds = T_chip_seconds * GPS_L1_CA_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 - dll_code_error_secs_Ti * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples + + d_correlation_length_samples = round(K_blk_samples); //round to a discrete samples old_d_rem_code_phase_samples=d_rem_code_phase_samples; - d_rem_code_phase_samples = K_blk_samples - static_cast(d_current_prn_length_samples); //rounding error < 1 sample + d_rem_code_phase_samples = K_blk_samples - static_cast(d_correlation_length_samples); //rounding error < 1 sample + + + // UPDATE REMNANT CARRIER PHASE + CORRECTED_INTEGRATION_TIME_S=(static_cast(d_correlation_length_samples)/static_cast(d_fs_in)); + //remnant carrier phase [rad] + d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S,GPS_TWO_PI); + // UPDATE CARRIER PHASE ACCUULATOR + //carrier phase accumulator prior to update the PLL estimators (accumulated carrier in this loop depends on the old estimations!) + d_acc_carrier_phase_cycles -= d_carrier_doppler_hz*CORRECTED_INTEGRATION_TIME_S; + //################### PLL COMMANDS ################################################# //carrier phase step (NCO phase increment per sample) [rads/sample] @@ -530,7 +537,7 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_ // AUX vars (for debug purposes) tmp_double = d_rem_code_phase_samples; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + tmp_double = static_cast(d_sample_counter + d_correlation_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } catch (const std::ifstream::failure* e) @@ -539,9 +546,9 @@ int gps_l1_ca_dll_pll_artemisa_tracking_cc::general_work (int noutput_items, gr_ } } - consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates - d_sample_counter += d_current_prn_length_samples; //count for the processed samples - //LOG(INFO)<<"GPS tracking output end on CH="<d_channel << " SAMPLE STAMP="<Acq_samplestamp_samples; long int acq_trk_diff_samples; - float acq_trk_diff_seconds; + double acq_trk_diff_seconds; acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); //doppler effect // Fd=(C/(C+Vr))*F - float radial_velocity; + double radial_velocity; radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ; // new chip and prn sequence periods based on acq Doppler - float T_chip_mod_seconds; - float T_prn_mod_seconds; - float T_prn_mod_samples; + double T_chip_mod_seconds; + double T_prn_mod_seconds; + double T_prn_mod_samples; d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; T_chip_mod_seconds = 1/d_code_freq_chips; T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); d_current_prn_length_samples = round(T_prn_mod_samples); - float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; - float T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); - float T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; - float N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; - float corrected_acq_phase_samples, delay_correction_samples; + double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; + double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; + double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; + double corrected_acq_phase_samples, delay_correction_samples; corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); if (corrected_acq_phase_samples < 0) { @@ -338,10 +338,10 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec { // stream to collect cout calls to improve thread safety std::stringstream tmp_str_stream; - float carr_error_hz; - float carr_error_filt_hz; - float code_error_chips; - float code_error_filt_chips; + double carr_error_hz; + double carr_error_filt_hz; + double code_error_chips; + double code_error_filt_chips; if (d_enable_tracking == true) { @@ -398,7 +398,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec #endif // ################## 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) / GPS_TWO_PI; // Carrier discriminator filter carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); // New carrier Doppler frequency estimation @@ -406,7 +406,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec // New code Doppler frequency estimation d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); //carrier phase accumulator for (K) doppler estimation - d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD; + d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD; //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 * GPS_L1_CA_CODE_PERIOD; d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); @@ -417,7 +417,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec // Code discriminator filter code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] //Code phase accumulator - float code_error_filt_secs; + double code_error_filt_secs; code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; //[seconds] d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; @@ -428,7 +428,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec double T_prn_samples; double K_blk_samples; // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1 / static_cast(d_code_freq_chips); + T_chip_seconds = 1.0 / d_code_freq_chips; T_prn_seconds = T_chip_seconds * GPS_L1_CA_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); @@ -563,23 +563,32 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec //tmp_float=(float)d_sample_counter; d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int)); // accumulated carrier phase - d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float)); + tmp_float=d_acc_carrier_phase_rad; + d_dump_file.write((char*)&tmp_float, sizeof(float)); // carrier and code frequency - d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float)); - d_dump_file.write((char*)&d_code_freq_chips, sizeof(float)); + tmp_float=d_carrier_doppler_hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float=d_code_freq_chips; + d_dump_file.write((char*)&tmp_float, sizeof(float)); //PLL commands - d_dump_file.write((char*)&carr_error_hz, sizeof(float)); - d_dump_file.write((char*)&carr_error_filt_hz, sizeof(float)); + tmp_float=carr_error_hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float=carr_error_filt_hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); //DLL commands - d_dump_file.write((char*)&code_error_chips, sizeof(float)); - d_dump_file.write((char*)&code_error_filt_chips, sizeof(float)); + tmp_float=code_error_chips; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float=code_error_filt_chips; + d_dump_file.write((char*)&tmp_float, sizeof(float)); // CN0 and carrier lock test - d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float)); - d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float)); + tmp_float=d_CN0_SNV_dB_Hz; + d_dump_file.write((char*)&tmp_float, sizeof(float)); + tmp_float=d_carrier_lock_test; + d_dump_file.write((char*)&tmp_float, sizeof(float)); // AUX vars (for debug purposes) tmp_float = d_rem_code_phase_samples; 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 0aee4ce8b..52ce88efc 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,24 +135,24 @@ private: // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars double d_code_freq_chips; - float d_carrier_doppler_hz; - float d_acc_carrier_phase_rad; - float d_code_phase_samples; - float d_acc_code_phase_secs; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_code_phase_samples; + double d_acc_code_phase_secs; //PRN period in samples int d_current_prn_length_samples; @@ -164,9 +164,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars 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 3193d335e..07d64b945 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 @@ -190,17 +190,17 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; long int acq_trk_diff_samples; - float acq_trk_diff_seconds; + double acq_trk_diff_seconds; acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp);//-d_vector_length; DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); //doppler effect // Fd=(C/(C+Vr))*F - float radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ; + double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ; // new chip and prn sequence periods based on acq Doppler - float T_chip_mod_seconds; - float T_prn_mod_seconds; - float T_prn_mod_samples; + double T_chip_mod_seconds; + double T_prn_mod_seconds; + double T_prn_mod_samples; d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; T_chip_mod_seconds = 1/d_code_freq_chips; T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; @@ -208,11 +208,11 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() d_current_prn_length_samples = round(T_prn_mod_samples); - float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; - float T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); - float T_prn_diff_seconds= T_prn_true_seconds - T_prn_mod_seconds; - float N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; - float corrected_acq_phase_samples, delay_correction_samples; + double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; + double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_diff_seconds= T_prn_true_seconds - T_prn_mod_seconds; + double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; + double corrected_acq_phase_samples, delay_correction_samples; corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); if (corrected_acq_phase_samples < 0) { @@ -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_carrier_doppler_hz / static_cast(d_fs_in); + float phase_step_rad = static_cast(GPS_TWO_PI) * static_cast(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); @@ -336,10 +336,10 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars - float carr_error_hz; - float carr_error_filt_hz; - float code_error_chips; - float code_error_filt_chips; + double carr_error_hz; + double carr_error_filt_hz; + double code_error_chips; + double code_error_filt_chips; // Block input data and block output stream pointers const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignment @@ -355,7 +355,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in if (d_pull_in == true) { int samples_offset; - float acq_trk_shif_correction_samples; + double acq_trk_shif_correction_samples; int acq_to_trk_delay_samples; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); @@ -414,7 +414,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in // ################## 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) / GPS_TWO_PI; // Carrier discriminator filter carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); // New carrier Doppler frequency estimation @@ -422,7 +422,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in // New code Doppler frequency estimation d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); //carrier phase accumulator for (K) doppler estimation - d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD; + d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD; //remanent 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 * GPS_L1_CA_CODE_PERIOD; d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); @@ -433,7 +433,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in // Code discriminator filter code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] //Code phase accumulator - float code_error_filt_secs; + double code_error_filt_secs; code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; //[seconds] d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; @@ -504,9 +504,9 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter)/static_cast(d_fs_in); // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 current_synchro_data.Code_phase_secs = 0; - current_synchro_data.Carrier_phase_rads = static_cast(d_acc_carrier_phase_rad); - current_synchro_data.Carrier_Doppler_hz = static_cast(d_carrier_doppler_hz); - current_synchro_data.CN0_dB_hz = static_cast(d_CN0_SNV_dB_Hz); + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.Flag_valid_pseudorange = false; *out[0] = current_synchro_data; @@ -579,41 +579,41 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in tmp_L = std::abs(*d_Late); try { - // EPR - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); - // PRN start sample stamp - //tmp_float=(float)d_sample_counter; - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); - // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); - // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - tmp_float=d_code_freq_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // EPR + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + //tmp_float=(float)d_sample_counter; + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + //PLL commands + d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); + //DLL commands + d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + + // AUX vars (for debug purposes) + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } catch (std::ifstream::failure e) { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h index 216e63955..5cf55fa56 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h @@ -139,24 +139,24 @@ private: // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars double d_code_freq_chips; - float d_carrier_doppler_hz; - float d_acc_carrier_phase_rad; - float d_code_phase_samples; - float d_acc_code_phase_secs; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_code_phase_samples; + double d_acc_code_phase_secs; //PRN period in samples int d_current_prn_length_samples; @@ -168,9 +168,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars 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 9d0a563d0..3927e0364 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 @@ -356,7 +356,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items, gr_vecto // New code Doppler frequency estimation d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); //carrier phase accumulator for (K) doppler estimation - d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD; + d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD; //remanent 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 * GPS_L1_CA_CODE_PERIOD; d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); 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 018fd6486..709097760 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 @@ -199,11 +199,11 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking() acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); //doppler effect // Fd=(C/(C+Vr))*F - float radial_velocity = (GPS_L2_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L2_FREQ_HZ; + double radial_velocity = (GPS_L2_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L2_FREQ_HZ; // new chip and prn sequence periods based on acq Doppler - float T_chip_mod_seconds; - float T_prn_mod_seconds; - float T_prn_mod_samples; + double T_chip_mod_seconds; + double T_prn_mod_seconds; + double T_prn_mod_samples; d_code_freq_chips = radial_velocity * GPS_L2_M_CODE_RATE_HZ; T_chip_mod_seconds = 1/d_code_freq_chips; T_prn_mod_seconds = T_chip_mod_seconds * GPS_L2_M_CODE_LENGTH_CHIPS; @@ -211,11 +211,11 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking() d_current_prn_length_samples = round(T_prn_mod_samples); - float T_prn_true_seconds = GPS_L2_M_CODE_LENGTH_CHIPS / GPS_L2_M_CODE_RATE_HZ; - float T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); - float T_prn_diff_seconds= T_prn_true_seconds - T_prn_mod_seconds; - float N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; - float corrected_acq_phase_samples, delay_correction_samples; + double T_prn_true_seconds = GPS_L2_M_CODE_LENGTH_CHIPS / GPS_L2_M_CODE_RATE_HZ; + double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_diff_seconds= T_prn_true_seconds - T_prn_mod_seconds; + double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; + double corrected_acq_phase_samples, delay_correction_samples; corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); if (corrected_acq_phase_samples < 0) { @@ -276,7 +276,7 @@ void gps_l2_m_dll_pll_tracking_cc::update_local_code() int epl_loop_length_samples; // unified loop for E, P, L code vectors - code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); + code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in); tcode_chips = -rem_code_phase_chips; @@ -301,7 +301,7 @@ void gps_l2_m_dll_pll_tracking_cc::update_local_carrier() { float phase_rad, phase_step_rad; - phase_step_rad = static_cast(GPS_L2_TWO_PI) * d_carrier_doppler_hz / static_cast(d_fs_in); + phase_step_rad = GPS_L2_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); phase_rad = d_rem_carr_phase_rad; for(int i = 0; i < d_current_prn_length_samples; i++) { @@ -337,10 +337,10 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { // process vars - float carr_error_hz=0; - float carr_error_filt_hz=0; - float code_error_chips=0; - float code_error_filt_chips=0; + double carr_error_hz=0; + double carr_error_filt_hz=0; + double code_error_chips=0; + double code_error_filt_chips=0; // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder Gnss_Synchro current_synchro_data = Gnss_Synchro(); @@ -355,7 +355,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int if (d_pull_in == true) { int samples_offset; - float acq_trk_shif_correction_samples; + double acq_trk_shif_correction_samples; int acq_to_trk_delay_samples; acq_to_trk_delay_samples = (d_sample_counter - (d_acq_sample_stamp-d_current_prn_length_samples)); acq_trk_shif_correction_samples = -fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); @@ -419,7 +419,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int // ################## PLL ########################################################## // PLL discriminator - carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast(GPS_L2_TWO_PI); + carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GPS_L2_TWO_PI; // Carrier discriminator filter carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); // New carrier Doppler frequency estimation @@ -427,7 +427,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int // New code Doppler frequency estimation d_code_freq_chips = GPS_L2_M_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L2_M_CODE_RATE_HZ) / GPS_L2_FREQ_HZ); //carrier phase accumulator for (K) doppler estimation - d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_L2_TWO_PI * d_carrier_doppler_hz * GPS_L2_M_PERIOD; + d_acc_carrier_phase_rad -= GPS_L2_TWO_PI * d_carrier_doppler_hz * GPS_L2_M_PERIOD; //remanent carrier phase to prevent overflow in the code NCO d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_L2_TWO_PI * d_carrier_doppler_hz * GPS_L2_M_PERIOD; d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_L2_TWO_PI); @@ -438,7 +438,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int // Code discriminator filter code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] //Code phase accumulator - float code_error_filt_secs; + double code_error_filt_secs; code_error_filt_secs = (GPS_L2_M_PERIOD * code_error_filt_chips) / GPS_L2_M_CODE_RATE_HZ; //[seconds] d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; @@ -449,7 +449,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int double T_prn_samples; double K_blk_samples; // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1 / static_cast(d_code_freq_chips); + T_chip_seconds = 1.0 / d_code_freq_chips; T_prn_seconds = T_chip_seconds * GPS_L2_M_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); @@ -502,16 +502,16 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples)/static_cast(d_fs_in); // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??) - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_rem_code_phase_samples) / static_cast(d_fs_in); //compute remnant code phase samples AFTER the Tracking timestamp d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter)/static_cast(d_fs_in); // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 current_synchro_data.Code_phase_secs = 0; - current_synchro_data.Carrier_phase_rads = static_cast(d_acc_carrier_phase_rad); - current_synchro_data.Carrier_Doppler_hz = static_cast(d_carrier_doppler_hz); - current_synchro_data.CN0_dB_hz = static_cast(d_CN0_SNV_dB_Hz); + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.Flag_valid_tracking = true; *out[0] = current_synchro_data; @@ -585,40 +585,40 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int tmp_L = std::abs(*d_Late); try { - // EPR - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); - // PRN start sample stamp - //tmp_float=(float)d_sample_counter; - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); - // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(float)); + // EPR + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + //tmp_float=(float)d_sample_counter; + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(float)); + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(float)); + //PLL commands + d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(float)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(float)); + //DLL commands + d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(float)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(float)); + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // AUX vars (for debug purposes) + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } catch (std::ifstream::failure& e) { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h index 615df8cfd..4fc35f44b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h @@ -137,24 +137,24 @@ private: // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; - float d_rem_carr_phase_rad; + double d_rem_carr_phase_rad; // PLL and DLL filter library Tracking_2nd_DLL_filter d_code_loop_filter; Tracking_2nd_PLL_filter d_carrier_loop_filter; // acquisition - float d_acq_code_phase_samples; - float d_acq_carrier_doppler_hz; + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; // correlator Correlator d_correlator; // tracking vars double d_code_freq_chips; - float d_carrier_doppler_hz; - float d_acc_carrier_phase_rad; - float d_code_phase_samples; - float d_acc_code_phase_secs; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_code_phase_samples; + double d_acc_code_phase_secs; //PRN period in samples int d_current_prn_length_samples; @@ -166,9 +166,9 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; gr_complex* d_Prompt_buffer; - float d_carrier_lock_test; - float d_CN0_SNV_dB_Hz; - float d_carrier_lock_threshold; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; // control vars diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index cc53a9184..665ebeacf 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -18,21 +18,15 @@ if(ENABLE_CUDA) - FIND_PACKAGE(CUDA REQUIRED) - - # Append current NVCC flags by something, eg comput capability - # set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} --gpu-architecture sm_30) - - list(APPEND CUDA_NVCC_FLAGS "-gencode arch=compute_30,code=sm_30; -std=c++11;-O3; -use_fast_math -default-stream per-thread") - SET(CUDA_PROPAGATE_HOST_FLAGS OFF) - - CUDA_INCLUDE_DIRECTORIES( - ${CMAKE_CURRENT_SOURCE_DIR} - #${CMAKE_CURRENT_SOURCE_DIR}/cudahelpers - ) - - SET(LIB_TYPE STATIC) #set the lib type - CUDA_ADD_LIBRARY(CUDA_CORRELATOR_LIB ${LIB_TYPE} cuda_multicorrelator.h cuda_multicorrelator.cu) + # Append current NVCC flags by something, eg comput capability + # set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} --gpu-architecture sm_30) + list(APPEND CUDA_NVCC_FLAGS "-gencode arch=compute_30,code=sm_30; -std=c++11;-O3; -use_fast_math -default-stream per-thread") + set(CUDA_PROPAGATE_HOST_FLAGS OFF) + CUDA_INCLUDE_DIRECTORIES( ${CMAKE_CURRENT_SOURCE_DIR}) + set(LIB_TYPE STATIC) #set the lib type + CUDA_ADD_LIBRARY(CUDA_CORRELATOR_LIB ${LIB_TYPE} cuda_multicorrelator.h cuda_multicorrelator.cu) + set(OPT_TRACKING_LIBRARIES ${OPT_TRACKING_LIBRARIES} CUDA_CORRELATOR_LIB) + set(OPT_TRACKING_INCLUDES ${OPT_TRACKING_INCLUDES} ${CUDA_INCLUDE_DIRS} ) endif(ENABLE_CUDA) @@ -54,7 +48,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/interfaces ${CMAKE_SOURCE_DIR}/src/core/receiver ${VOLK_INCLUDE_DIRS} - ${CUDA_INCLUDE_DIRS} + ${OPT_TRACKING_INCLUDES} ) if(ENABLE_GENERIC_ARCH) @@ -69,4 +63,4 @@ endif(SSE3_AVAILABLE) file(GLOB TRACKING_LIB_HEADERS "*.h") add_library(tracking_lib ${TRACKING_LIB_SOURCES} ${TRACKING_LIB_HEADERS}) source_group(Headers FILES ${TRACKING_LIB_HEADERS}) -target_link_libraries(tracking_lib ${CUDA_CORRELATOR_LIB} ${VOLK_LIBRARIES} ${GNURADIO_RUNTIME_LIBRARIES}) \ No newline at end of file +target_link_libraries(tracking_lib ${OPT_TRACKING_LIBRARIES} ${VOLK_LIBRARIES} ${GNURADIO_RUNTIME_LIBRARIES}) diff --git a/src/algorithms/tracking/libs/tracking_discriminators.cc b/src/algorithms/tracking/libs/tracking_discriminators.cc index e82df1bc2..1aaa10004 100644 --- a/src/algorithms/tracking/libs/tracking_discriminators.cc +++ b/src/algorithms/tracking/libs/tracking_discriminators.cc @@ -46,9 +46,9 @@ * \f$I_{PS2},Q_{PS2}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_2\f$. The output is in [radians/second]. */ -float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t1, float t2) +double fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, double t1, double t2) { - float cross, dot; + double cross, dot; dot = prompt_s1.real()*prompt_s2.real() + prompt_s1.imag()*prompt_s2.imag(); cross = prompt_s1.real()*prompt_s2.imag() - prompt_s2.real()*prompt_s1.imag(); return atan2(cross, dot) / (t2-t1); @@ -62,7 +62,7 @@ float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t * \f} * where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians]. */ -float pll_four_quadrant_atan(gr_complex prompt_s1) +double pll_four_quadrant_atan(gr_complex prompt_s1) { return atan2(prompt_s1.imag(), prompt_s1.real()); } @@ -75,7 +75,7 @@ float pll_four_quadrant_atan(gr_complex prompt_s1) * \f} * where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians]. */ -float pll_cloop_two_quadrant_atan(gr_complex prompt_s1) +double pll_cloop_two_quadrant_atan(gr_complex prompt_s1) { if (prompt_s1.real() != 0.0) { @@ -96,9 +96,9 @@ float pll_cloop_two_quadrant_atan(gr_complex prompt_s1) * where \f$E=\sqrt{I_{ES}^2+Q_{ES}^2}\f$ is the Early correlator output absolute value and * \f$L=\sqrt{I_{LS}^2+Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips]. */ -float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1) +double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1) { - float P_early, P_late; + double P_early, P_late; P_early = std::abs(early_s1); P_late = std::abs(late_s1); return 0.5*(P_early - P_late) / ((P_early + P_late)); @@ -113,9 +113,9 @@ float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1) * where \f$E=\sqrt{I_{VE}^2+Q_{VE}^2+I_{E}^2+Q_{E}^2}\f$ and * \f$L=\sqrt{I_{VL}^2+Q_{VL}^2+I_{L}^2+Q_{L}^2}\f$ . The output is in [chips]. */ -float dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1) +double dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1) { - float P_early, P_late; + double P_early, P_late; P_early = std::sqrt(std::norm(very_early_s1) + std::norm(early_s1)); P_late = std::sqrt(std::norm(very_late_s1) + std::norm(late_s1)); return (P_early - P_late) / ((P_early + P_late)); diff --git a/src/algorithms/tracking/libs/tracking_discriminators.h b/src/algorithms/tracking/libs/tracking_discriminators.h index 8a78f9c1c..87fe3abab 100644 --- a/src/algorithms/tracking/libs/tracking_discriminators.h +++ b/src/algorithms/tracking/libs/tracking_discriminators.h @@ -50,7 +50,7 @@ * \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_1\f$, and * \f$I_{PS2},Q_{PS2}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_2\f$. The output is in [radians/second]. */ -float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t1, float t2); +double fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, double t1, double t2); /*! \brief PLL four quadrant arctan discriminator @@ -61,7 +61,7 @@ float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2, float t * \f} * where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians]. */ -float pll_four_quadrant_atan(gr_complex prompt_s1); +double pll_four_quadrant_atan(gr_complex prompt_s1); /*! \brief PLL Costas loop two quadrant arctan discriminator @@ -72,7 +72,7 @@ float pll_four_quadrant_atan(gr_complex prompt_s1); * \f} * where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians]. */ -float pll_cloop_two_quadrant_atan(gr_complex prompt_s1); +double pll_cloop_two_quadrant_atan(gr_complex prompt_s1); /*! \brief DLL Noncoherent Early minus Late envelope normalized discriminator @@ -84,7 +84,7 @@ float pll_cloop_two_quadrant_atan(gr_complex prompt_s1); * where \f$E=\sqrt{I_{ES}^2+Q_{ES}^2}\f$ is the Early correlator output absolute value and * \f$L=\sqrt{I_{LS}^2+Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips]. */ -float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1); +double dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1); /*! \brief DLL Noncoherent Very Early Minus Late Power (VEMLP) normalized discriminator @@ -97,7 +97,7 @@ float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1); * where \f$E=\sqrt{I_{VE}^2+Q_{VE}^2+I_{E}^2+Q_{E}^2}\f$ and * \f$L=\sqrt{I_{VL}^2+Q_{VL}^2+I_{L}^2+Q_{L}^2}\f$ . The output is in [chips]. */ -float dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1); +double dll_nc_vemlp_normalized(gr_complex very_early_s1, gr_complex early_s1, gr_complex late_s1, gr_complex very_late_s1); #endif diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index a2cef3d95..cb63bb303 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -70,7 +70,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=200; +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/main/CMakeLists.txt b/src/main/CMakeLists.txt index 6b139b97d..5b6e56405 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -33,6 +33,10 @@ if(ENABLE_UHD) set(GNSS_SDR_OPTIONAL_HEADERS ${GNSS_SDR_OPTIONAL_HEADERS} ${UHD_INCLUDE_DIRS}) endif(ENABLE_UHD) +if(OPENSSL_FOUND) + add_definitions( -DUSE_OPENSSL_FALLBACK=1 ) +endif(OPENSSL_FOUND) + if(ENABLE_CUDA) add_definitions(-DCUDA_GPU_ACCEL=1) set(GNSS_SDR_OPTIONAL_LIBS ${GNSS_SDR_OPTIONAL_LIBS} ${CUDA_LIBRARIES})