diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index e99b48989..97db35e1f 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -59,16 +59,16 @@ gps_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, boost::shared_ptr< -void gps_l1_ca_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required) -{ - if (noutput_items != 0) - { - for (unsigned i = 0; i < 3; i++) - { - ninput_items_required[i] = d_samples_per_bit * 8; //set the required sample history - } - } -} +//void gps_l1_ca_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required) +//{ +// if (noutput_items != 0) +// { +// for (unsigned i = 0; i < 3; i++) +// { +// ninput_items_required[i] = d_samples_per_bit * 8; //set the required sample history +// } +// } +//} @@ -181,114 +181,101 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i // ########### Output the tracking data to navigation and PVT ########## const Gnss_Synchro **in = (const Gnss_Synchro **) &input_items[0]; //Get the input samples pointer - // TODO Optimize me! - if (in[0][d_samples_per_bit*8 - 1].symbol_integration_enabled==false) + // keep the last valid symbols + if (in[0]->Flag_valid_symbol_output==true) { - //******* preamble correlation ******** - for (unsigned int i = 0; i < d_samples_per_bit*8; i++) + d_symbol_history.push_back(in[0]->Prompt_I); + if (static_cast(d_symbol_history.size())>GPS_CA_PREAMBLE_LENGTH_SYMBOLS) { - if (in[0][i].Prompt_I < 0) // symbols clipping - { - corr_value -= d_preambles_symbols[i]; - } - else - { - corr_value += d_preambles_symbols[i]; - } + d_symbol_history.pop_front(); } - }else{ - //******* preamble correlation ******** - for (unsigned int i = 0; i < d_samples_per_bit*8; i++) - { - if (in[0][i].Flag_valid_symbol_output==true) - { - if (in[0][i].Prompt_I < 0) // symbols clipping + + if (d_symbol_history.size()==GPS_CA_PREAMBLE_LENGTH_SYMBOLS) + { + //******* preamble correlation ******** + int current_symbol_index=0; + for (int i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS/in[0]->correlation_length_ms; i++) + { + current_symbol_index=i*in[0]->correlation_length_ms;//+static_cast(floor(in[0]->correlation_length_ms/2)); + if (d_symbol_history.at(i) < 0) // symbols clipping { - corr_value -= d_preambles_symbols[i]*d_samples_per_bit; + //symbol weight expansion using the current tracking correlation length + corr_value -= d_preambles_symbols[current_symbol_index]*in[0]->correlation_length_ms; } else { - corr_value += d_preambles_symbols[i]*d_samples_per_bit; + //symbol weight expansion using the current tracking correlation length + corr_value += d_preambles_symbols[current_symbol_index]*in[0]->correlation_length_ms; } - } - } + } + } + if (abs(corr_value)>120) + { + std::cout<d_satellite; - d_symbol_accumulator = 0; //sync the symbol to bits integrator - d_symbol_accumulator_counter = 0; - d_frame_bit_index = 7; - d_stat = 1; // enter into frame pre-detection status - } - else if (d_stat == 1) //check 6 seconds of preamble separation - { - preamble_diff = d_sample_counter - d_preamble_index; - if (abs(preamble_diff - 6000) < 1) - { - d_GPS_FSM.Event_gps_word_preamble(); - d_flag_preamble = true; - d_preamble_index = d_sample_counter; //record the preamble sample stamp (t_P) - d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs;// - d_preamble_duration_seconds; //record the PRN start sample index associated to the preamble - d_frame_bit_index = 7; - if (!d_flag_frame_sync) - { - //send asynchronous message to tracking to inform of frame sync and extend correlation time - pmt::pmt_t value = pmt::from_long(d_preamble_index-1); - this->message_port_pub(pmt::mp("preamble_index"),value); + if (abs(corr_value) == GPS_CA_PREAMBLE_LENGTH_SYMBOLS) + { + if (d_stat == 0) + { + d_GPS_FSM.Event_gps_word_preamble(); + d_preamble_index = d_sample_counter;//record the preamble sample stamp + std::cout << "Preamble detection for SAT " << this->d_satellite; + d_symbol_accumulator = 0; //sync the symbol to bits integrator + d_symbol_accumulator_counter = 0; + d_frame_bit_index = 7; + d_stat = 1; // enter into frame pre-detection status + } + else if (d_stat == 1) //check 6 seconds of preamble separation + { + preamble_diff = d_sample_counter - d_preamble_index; + if (abs(preamble_diff - 6000) < 1) + { + std::cout <<"preamble! corr lenght="<correlation_length_ms<message_port_pub(pmt::mp("preamble_index"),value); - d_flag_frame_sync = true; - if (corr_value < 0) - { - flag_PLL_180_deg_phase_locked = true; //PLL is locked to opposite phase! - DLOG(INFO) << " PLL in opposite phase for Sat "<< this->d_satellite.get_PRN(); - } - else - { - flag_PLL_180_deg_phase_locked = false; - } - DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]"; - } - } - } - } - else - { - if (d_stat == 1) - { - preamble_diff = d_sample_counter - d_preamble_index; - if (preamble_diff > 6001) - { - DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff; - d_stat = 0; //lost of frame sync - d_flag_frame_sync = false; - flag_TOW_set = false; - } - } - } + d_flag_frame_sync = true; + if (corr_value < 0) + { + flag_PLL_180_deg_phase_locked = true; //PLL is locked to opposite phase! + std::cout << " PLL in opposite phase for Sat "<< this->d_satellite.get_PRN(); + } + else + { + flag_PLL_180_deg_phase_locked = false; + } + std::cout << " Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]"; + } + }else{ + if (preamble_diff > 6001) + { + std::cout << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff; + d_stat = 0; //lost of frame sync + d_flag_frame_sync = false; + flag_TOW_set = false; + } + } + } + } //******* SYMBOL TO BIT ******* - if (in[0][d_samples_per_bit*8 - 1].Flag_valid_symbol_output==true) + if (in[0]->Flag_valid_symbol_output==true) { - if (in[0][d_samples_per_bit*8 - 1].symbol_integration_enabled==true) - { - // extended correlation to bit period is enabled in tracking! - // 1 symbol = 1 bit - d_symbol_accumulator = in[0][d_samples_per_bit*8 - 1].Prompt_I; // accumulate the input value in d_symbol_accumulator - d_symbol_accumulator_counter=20; - }else{ - // 20 symbols = 1 bit: do symbols integration in telemetry decoder - d_symbol_accumulator += in[0][d_samples_per_bit*8 - 1].Prompt_I; // accumulate the input value in d_symbol_accumulator - d_symbol_accumulator_counter++; - } + // extended correlation to bit period is enabled in tracking! + d_symbol_accumulator += in[0]->Prompt_I; // accumulate the input value in d_symbol_accumulator + d_symbol_accumulator_counter+=in[0]->correlation_length_ms; } if (d_symbol_accumulator_counter == 20 ) @@ -342,8 +329,6 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i { d_GPS_frame_4bytes <<= 1; //shift 1 bit left the telemetry word } - - } // output the frame consume_each(1); //one by one @@ -381,8 +366,8 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i if (flag_PLL_180_deg_phase_locked == true) { - //correct the accumulated phase for the costas loop phase shift, if required - current_synchro_data.Carrier_phase_rads += GPS_PI; + //correct the accumulated phase for the costas loop phase shift, if required + current_synchro_data.Carrier_phase_rads += GPS_PI; } if(d_dump == true) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h index bd6713ff4..303684fe1 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h @@ -85,7 +85,7 @@ public: * \brief Function which tells the scheduler how many input items * are required to produce noutput_items output items. */ - void forecast (int noutput_items, gr_vector_int &ninput_items_required); + //void forecast (int noutput_items, gr_vector_int &ninput_items_required); private: friend gps_l1_ca_telemetry_decoder_cc_sptr @@ -107,6 +107,7 @@ private: bool d_flag_frame_sync; // symbols + std::deque d_symbol_history; double d_symbol_accumulator; short int d_symbol_accumulator_counter; @@ -138,10 +139,6 @@ private: double d_TOW_at_Preamble; double d_TOW_at_current_symbol; - std::deque d_symbol_TOW_queue_s; - // Doppler and Phase accumulator queue for interpolation in Observables - std::deque d_carrier_doppler_queue_hz; - std::deque d_acc_carrier_phase_queue_rads; double Prn_timestamp_at_preamble_ms; bool flag_TOW_set; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc index 4377c55f6..7dbe01681 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -347,6 +347,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec d_pull_in = false; // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; + current_synchro_data.correlation_length_ms=1; + current_synchro_data.Flag_valid_symbol_output = false; *out[0] = current_synchro_data; consume_each(samples_offset); //shift input to perform alignment with local replica d_symbol_counter++; @@ -361,13 +363,13 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec 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_correlation_length_samples); - // ####### 20ms coherent intergration extension (experimental) - // keep the last 40 symbols (2 bits to detect transitions) + // ####### coherent intergration extension + // keep the last symbols d_E_history.push_back(d_correlator_outs[0]); // save early output d_P_history.push_back(d_correlator_outs[1]); // save prompt output d_L_history.push_back(d_correlator_outs[2]); // save late output - if (d_P_history.size()>d_extend_correlation_ms) + if (static_cast(d_P_history.size())>d_extend_correlation_ms) { d_E_history.pop_front(); d_P_history.pop_front(); @@ -382,7 +384,6 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec { // compute coherent integration and enable tracking loop // perform coherent integration using correlator output history - //gr_complex d_correlator_outs_2[3]; //std::cout<<"##### RESET COHERENT INTEGRATION ####"<PRN) + DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; //if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock! } @@ -607,7 +614,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items, gr_vec if (floor(d_sample_counter / d_fs_in) != d_last_seg) { d_last_seg = floor(d_sample_counter / d_fs_in); - std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) + DLOG(INFO) << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]"; } } diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index bbe96db49..26920175a 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -74,6 +74,7 @@ const int GPS_L1_CA_HISTORY_DEEP = 100; #define GPS_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1} const int GPS_CA_PREAMBLE_LENGTH_BITS = 8; +const int GPS_CA_PREAMBLE_LENGTH_SYMBOLS = 160; const int GPS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] const int GPS_CA_TELEMETRY_SYMBOLS_PER_BIT = 20; const int GPS_CA_TELEMETRY_RATE_SYMBOLS_SECOND = GPS_CA_TELEMETRY_RATE_BITS_SECOND*20; //!< NAV message bit rate [symbols/s] diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index d70c21b1b..5aea0d21b 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -61,7 +61,7 @@ public: double Tracking_timestamp_secs; //!< Set by Tracking processing block bool Flag_valid_tracking; bool Flag_valid_symbol_output; - bool symbol_integration_enabled; //!< Set by Tracking processing block + int correlation_length_ms; //!< Set by Tracking processing block //Telemetry Decoder double Prn_timestamp_ms; //!< Set by Telemetry Decoder processing block