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 b2aed28f1..bc7b0c11a 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 @@ -134,8 +134,8 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(unsigned in -void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking(){ - +void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking() +{ /* * correct the code phase according to the delay between acq and trk */ @@ -207,6 +207,10 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking(){ std::cout<<"PULL-IN Doppler [Hz]= "<3) + if (((float)d_sample_counter - (float)d_acq_sample_stamp)/(float)d_fs_in>3) { - d_FLL_discriminator_hz=0; //disconnect the FLL after the initial lock + d_FLL_discriminator_hz = 0; //disconnect the FLL after the initial lock } /*! * DLL and FLL+PLL filter and get current carrier Doppler and code frequency */ - carr_nco_hz=d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz,PLL_discriminator_hz,correlation_time_s); + carr_nco_hz = d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz, PLL_discriminator_hz, correlation_time_s); d_carrier_doppler_hz = (float)d_if_freq + carr_nco_hz; - d_code_freq_hz= GPS_L1_CA_CODE_RATE_HZ- (((d_carrier_doppler_hz - (float)d_if_freq)*GPS_L1_CA_CODE_RATE_HZ)/GPS_L1_FREQ_HZ)-code_error_chips; + d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ- (((d_carrier_doppler_hz - (float)d_if_freq)*GPS_L1_CA_CODE_RATE_HZ)/GPS_L1_FREQ_HZ) - code_error_chips; /*! * \todo Improve the lock detection algorithm! */ // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counterMINIMUM_VALID_CN0) - { - d_carrier_lock_fail_counter++; - }else{ - if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--; - } - if (d_carrier_lock_fail_counter>MAXIMUM_LOCK_FAIL_COUNTER) - { - std::cout<<"Channel "<push(tracking_message); - d_carrier_lock_fail_counter=0; - d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine + } + else + { + d_cn0_estimation_counter = 0; + d_CN0_SNV_dB_Hz=gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES,d_fs_in); + d_carrier_lock_test=carrier_lock_detector(d_Prompt_buffer,CN0_ESTIMATION_SAMPLES); + // ###### TRACKING UNLOCK NOTIFICATION ##### + int tracking_message; + if (d_carrier_lock_test < d_carrier_lock_threshold or d_carrier_lock_test > MINIMUM_VALID_CN0) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout<<"Channel "<< d_channel << " loss of lock!" << std::endl; + tracking_message = 3; //loss of lock + d_channel_internal_queue->push(tracking_message); + d_carrier_lock_fail_counter = 0; + d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine - } - //std::cout<<"d_carrier_lock_fail_counter"<(d_channel)); d_dump_filename.append(".dat"); d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit ); d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - std::cout<<"Tracking dump enabled on channel "< *channel_internal_queue) { d_channel_internal_queue = channel_internal_queue; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h index dd10f233b..abe6db96c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h @@ -73,6 +73,37 @@ gps_l1_ca_dll_fll_pll_make_tracking_cc(unsigned int satellite, */ class Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc: public gr_block { +public: + + ~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(); + + void set_satellite(unsigned int satellite); + void set_channel(unsigned int channel); + void set_acq_code_phase(float code_phase); + void set_acq_doppler(float doppler); + void start_tracking(); + void update_local_code(); + void update_local_carrier(); + void set_FLL_and_PLL_BW(float fll_bw_hz,float pll_bw_hz); + void set_acq_sample_stamp(unsigned long int sample_stamp); + void set_channel_queue(concurrent_queue *channel_internal_queue); + + /* + * \brief just like gr_block::general_work, only this arranges to call consume_each for you + * + * The user must override work to define the signal processing code + */ + //virtual int work (int noutput_items, + // gr_vector_const_void_star &input_items, + // gr_vector_void_star &output_items) = 0; + + //int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + + int general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + + void forecast (int noutput_items, gr_vector_int &ninput_items_required); + private: @@ -171,38 +202,6 @@ private: std::string d_dump_filename; std::ofstream d_dump_file; - -public: - - ~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(); - - void set_satellite(unsigned int satellite); - void set_channel(unsigned int channel); - void set_acq_code_phase(float code_phase); - void set_acq_doppler(float doppler); - void start_tracking(); - void update_local_code(); - void update_local_carrier(); - void set_FLL_and_PLL_BW(float fll_bw_hz,float pll_bw_hz); - void set_acq_sample_stamp(unsigned long int sample_stamp); - void set_channel_queue(concurrent_queue *channel_internal_queue); - - /*! - * \brief just like gr_block::general_work, only this arranges to call consume_each for you - * - * The user must override work to define the signal processing code - */ - //virtual int work (int noutput_items, - // gr_vector_const_void_star &input_items, - // gr_vector_void_star &output_items) = 0; - - //int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - - int general_work (int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - - void forecast (int noutput_items, gr_vector_int &ninput_items_required); - }; #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 d3ee87526..123c730ea 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 @@ -212,7 +212,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() // DEBUG OUTPUT std::cout << "Tracking start on channel " << d_channel << " for satellite ID* " << this->d_satellite << std::endl; - DLOG(INFO) << "Start tracking for satellite "<< this->d_satellite << " received" << std::endl; + DLOG(INFO) << "Start tracking for satellite " << this->d_satellite << " received" << std::endl; // enable tracking d_pull_in = true; 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 49ab3b965..2f7c94bc7 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 @@ -70,29 +70,57 @@ gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq, */ class Gps_L1_Ca_Dll_Pll_Tracking_cc: public gr_block { +public: + + ~Gps_L1_Ca_Dll_Pll_Tracking_cc(); + + void set_satellite(unsigned int satellite); + void set_channel(unsigned int channel); + void set_acq_code_phase(float code_phase); + void set_acq_doppler(float doppler); + void start_tracking(); + void set_acq_sample_stamp(unsigned long int sample_stamp); + void set_channel_queue(concurrent_queue *channel_internal_queue); + + /* + * \brief just like gr_block::general_work, only this arranges to call consume_each for you + * + * The user must override work to define the signal processing code + */ + //virtual int work (int noutput_items, + // gr_vector_const_void_star &input_items, + // gr_vector_void_star &output_items) = 0; + + //int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + + int general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + + void forecast (int noutput_items, gr_vector_int &ninput_items_required); + private: friend gps_l1_ca_dll_pll_tracking_cc_sptr gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, 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 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); Gps_L1_Ca_Dll_Pll_Tracking_cc(unsigned int satellite, 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 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); void update_local_code(); void update_local_carrier(); @@ -118,11 +146,11 @@ private: gr_complex* d_prompt_code; gr_complex* d_carr_sign; - gr_complex d_Early; - gr_complex d_Prompt; - gr_complex d_Late; + gr_complex d_Early; + gr_complex d_Prompt; + gr_complex d_Late; - // remaining code phase and carrier phase between tracking loops + // remaining code phase and carrier phase between tracking loops float d_rem_code_phase_samples; float d_next_rem_code_phase_samples; float d_rem_carr_phase_rad; @@ -142,11 +170,11 @@ private: float d_code_phase_samples; //PRN period in samples - int d_current_prn_length_samples; - int d_next_prn_length_samples; - double d_sample_counter_seconds; + int d_current_prn_length_samples; + int d_next_prn_length_samples; + double d_sample_counter_seconds; - //processing samples counters + //processing samples counters unsigned long int d_sample_counter; unsigned long int d_acq_sample_stamp; @@ -165,35 +193,6 @@ private: // file dump std::string d_dump_filename; std::ofstream d_dump_file; - -public: - - ~Gps_L1_Ca_Dll_Pll_Tracking_cc(); - - void set_satellite(unsigned int satellite); - void set_channel(unsigned int channel); - void set_acq_code_phase(float code_phase); - void set_acq_doppler(float doppler); - void start_tracking(); - void set_acq_sample_stamp(unsigned long int sample_stamp); - void set_channel_queue(concurrent_queue *channel_internal_queue); - - /*! - * \brief just like gr_block::general_work, only this arranges to call consume_each for you - * - * The user must override work to define the signal processing code - */ - //virtual int work (int noutput_items, - // gr_vector_const_void_star &input_items, - // gr_vector_void_star &output_items) = 0; - - //int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - - int general_work (int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - - void forecast (int noutput_items, gr_vector_int &ninput_items_required); - }; #endif //GNSS_SDR_GPS_L1_CA_DLL_PLL_TRACKING_CC_H