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 a69d57f87..b9095bc13 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 @@ -93,11 +93,11 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::forecast (int noutput_items, gr_vector_i Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - gr_msg_queue_sptr queue, - bool dump, + long if_freq, + long fs_in, + unsigned int vector_length, + gr_msg_queue_sptr queue, + bool dump, std::string dump_filename, int order, float fll_bw_hz, @@ -159,6 +159,12 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc( d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; d_carrier_lock_threshold = 5; + + systemName["G"] = std::string("GPS"); + systemName["R"] = std::string("GLONASS"); + systemName["S"] = std::string("SBAS"); + systemName["E"] = std::string("Galileo"); + systemName["C"] = std::string("Compass"); } @@ -170,9 +176,9 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking() * correct the code phase according to the delay between acq and trk */ - d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; - d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; - d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; + d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; + d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; unsigned long int acq_trk_diff_samples; float acq_trk_diff_seconds; @@ -230,17 +236,25 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking() d_code_phase_samples = d_acq_code_phase_samples; + std::string sys_ = &d_acquisition_gnss_synchro->System; + sys = sys_.substr(0,1); + // DEBUG OUTPUT - std::cout << "Tracking start on channel " << d_channel << " for satellite " << d_acquisition_gnss_synchro->System << " "<< d_acquisition_gnss_synchro->PRN << std::endl; + std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; DLOG(INFO) << "Start tracking for satellite " << d_acquisition_gnss_synchro->System << " "<< d_acquisition_gnss_synchro->PRN << " received "; - // enable tracking + // enable tracking Gnss_Satellite(systemName[&d_acquisition_gnss_synchro->System], d_acquisition_gnss_synchro->PRN) d_pull_in = true; d_enable_tracking = true; std::cout << "PULL-IN Doppler [Hz]= " << d_carrier_doppler_hz << " Code Phase correction [samples]=" << delay_correction_samples << " PULL-IN Code Phase [samples]= " << d_acq_code_phase_samples << std::endl; + + + + + } @@ -346,10 +360,10 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto return 1; } - // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder - Gnss_Synchro current_synchro_data; + // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder + Gnss_Synchro current_synchro_data; // Fill the acquisition data - current_synchro_data=*d_acquisition_gnss_synchro; + current_synchro_data=*d_acquisition_gnss_synchro; // get the sample in and out pointers const gr_complex* in = (gr_complex*) input_items[0]; //block input samples pointer @@ -377,14 +391,14 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto // perform Early, Prompt and Late correlation d_correlator.Carrier_wipeoff_and_EPL_volk(d_current_prn_length_samples, - in, - d_carr_sign, - d_early_code, - d_prompt_code, - d_late_code, - d_Early, - d_Prompt, - d_Late); + in, + d_carr_sign, + d_early_code, + d_prompt_code, + d_late_code, + d_Early, + d_Prompt, + d_Late); /* * DLL, FLL, and PLL discriminators @@ -479,7 +493,9 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto { d_last_seg = floor(d_sample_counter/d_fs_in); std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl; - std::cout << "Tracking CH " << d_channel << ": Satellite " << d_acquisition_gnss_synchro->System << " "<< d_acquisition_gnss_synchro->PRN << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; + //std::string sys = &d_acquisition_gnss_synchro->System; + //std::cout << sys << ", " << sys.substr(0,1) << std::endl; + std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; //std::cout<<"TRK CH "<System << " "<< d_acquisition_gnss_synchro->PRN << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; + //std::string sys2 = &d_acquisition_gnss_synchro->System; + //std::cout << sys2 << ", " << sys2.substr(0,1) << std::endl; + std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; //std::cout<<"TRK CH "< systemName; + std::string sys; }; #endif //GNSS_SDR_GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H 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 68c9cfd19..317b64672 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 @@ -41,7 +41,7 @@ #include "CN_estimators.h" #include "GPS_L1_CA.h" #ifdef GNSS_SDR_USE_BOOST_ROUND - #include +#include #endif #include "control_message_factory.h" #include @@ -65,24 +65,24 @@ using google::LogMessage; gps_l1_ca_dll_pll_tracking_cc_sptr gps_l1_ca_dll_pll_make_tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - gr_msg_queue_sptr queue, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips) + long if_freq, + long fs_in, + unsigned int vector_length, + gr_msg_queue_sptr queue, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips) { - return gps_l1_ca_dll_pll_tracking_cc_sptr(new Gps_L1_Ca_Dll_Pll_Tracking_cc(if_freq, - fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips)); + return gps_l1_ca_dll_pll_tracking_cc_sptr(new Gps_L1_Ca_Dll_Pll_Tracking_cc(if_freq, + fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips)); } void Gps_L1_Ca_Dll_Pll_Tracking_cc::forecast (int noutput_items, - gr_vector_int &ninput_items_required) + gr_vector_int &ninput_items_required) { ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call } @@ -90,15 +90,15 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::forecast (int noutput_items, Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( - long if_freq, - long fs_in, - unsigned int vector_length, - gr_msg_queue_sptr queue, - bool dump, - std::string dump_filename, - float pll_bw_hz, - float dll_bw_hz, - float early_late_space_chips) : + long if_freq, + long fs_in, + unsigned int vector_length, + gr_msg_queue_sptr queue, + bool dump, + std::string dump_filename, + float pll_bw_hz, + float dll_bw_hz, + float early_late_space_chips) : gr_block ("Gps_L1_Ca_Dll_Pll_Tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)), gr_make_io_signature(1, 1, sizeof(Gnss_Synchro))) { @@ -173,6 +173,12 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; d_carrier_lock_threshold = 5; + + systemName["G"] = std::string("GPS"); + systemName["R"] = std::string("GLONASS"); + systemName["S"] = std::string("SBAS"); + systemName["E"] = std::string("Galileo"); + systemName["C"] = std::string("Compass"); } void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() @@ -181,9 +187,9 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() * correct the code phase according to the delay between acq and trk */ - d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; - d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; - d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; + d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; + d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; unsigned long int acq_trk_diff_samples; float acq_trk_diff_seconds; @@ -244,15 +250,20 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() d_code_phase_samples = d_acq_code_phase_samples; + std::string sys_ = &d_acquisition_gnss_synchro->System; + sys = sys_.substr(0,1); + // DEBUG OUTPUT - std::cout << "Tracking start on channel " << d_channel << " for satellite " << d_acquisition_gnss_synchro->System << " "<< d_acquisition_gnss_synchro->PRN << std::endl; - DLOG(INFO) << "Start tracking for satellite " << d_acquisition_gnss_synchro->System << " "<< d_acquisition_gnss_synchro->PRN << " received" << std::endl; + std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; + DLOG(INFO) << "Start tracking for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " received" << std::endl; // enable tracking d_pull_in = true; d_enable_tracking = true; - std::cout << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz << " Code Phase correction [samples]=" << delay_correction_samples << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples << std::endl; + std::cout << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz + << " Code Phase correction [samples]=" << delay_correction_samples + << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples << std::endl; } @@ -361,11 +372,11 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_trk_shif_correction_samples = d_next_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_next_prn_length_samples); //std::cout<<"acq_trk_shif_correction="<telemetry_decoder - Gnss_Synchro current_synchro_data; + // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder + Gnss_Synchro current_synchro_data; // Fill the acquisition data - current_synchro_data=*d_acquisition_gnss_synchro; + current_synchro_data = *d_acquisition_gnss_synchro; const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; @@ -405,14 +416,14 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in // perform Early, Prompt and Late correlation d_correlator.Carrier_wipeoff_and_EPL_volk(d_current_prn_length_samples, - in, - d_carr_sign, - d_early_code, - d_prompt_code, - d_late_code, - d_Early, - d_Prompt, - d_Late); + in, + d_carr_sign, + d_early_code, + d_prompt_code, + d_late_code, + d_Early, + d_Prompt, + d_Late); // Compute PLL error and update carrier NCO - carr_error = pll_cloop_two_quadrant_atan(*d_Prompt) / (float)TWO_PI; @@ -498,13 +509,13 @@ 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).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); - current_synchro_data.CN0_dB_hz=(double)d_CN0_SNV_dB_Hz; - *out[0]=current_synchro_data; + 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); + current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz; + *out[0] = current_synchro_data; // ########## DEBUG OUTPUT /*! @@ -517,7 +528,8 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in { d_last_seg = floor(d_sample_counter / d_fs_in); std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl; - std::cout << "Tracking CH " << d_channel << ": Satellite " << d_acquisition_gnss_synchro->System << " "<< d_acquisition_gnss_synchro->PRN << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; + std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) + << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; //std::cout<<"TRK CH "<System << " "<< d_acquisition_gnss_synchro->PRN << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; + std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) + << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; //std::cout<<"TRK CH "< systemName; + std::string sys; }; #endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_CC_H