diff --git a/src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_pcps_acquisition_cc.cc index 126ec3942..5928e4d53 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_pcps_acquisition_cc.cc @@ -61,27 +61,16 @@ gps_l1_ca_pcps_acquisition_cc::gps_l1_ca_pcps_acquisition_cc( sizeof(gr_complex) * samples_per_ms), gr_make_io_signature(0, 0, sizeof(gr_complex) * samples_per_ms)) { - - // SAMPLE COUNTER - d_sample_counter = 0; - + d_sample_counter = 0; // SAMPLE COUNTER d_active = false; - d_dump = dump; d_queue = queue; - d_dump_filename = dump_filename; - d_freq = freq; d_fs_in = fs_in; - d_samples_per_ms = samples_per_ms; d_sampled_ms = sampled_ms; - d_doppler_max = doppler_max; - d_satellite = 0; - d_fft_size = d_sampled_ms * d_samples_per_ms; - d_doppler_freq = 0.0; d_code_phase = 0; d_mag = 0; @@ -97,6 +86,9 @@ gps_l1_ca_pcps_acquisition_cc::gps_l1_ca_pcps_acquisition_cc( // Inverse FFT d_ifft = new gri_fft_complex(d_fft_size, false); + d_dump = dump; + d_dump_filename = dump_filename; + DLOG(INFO) << "fs in " << d_fs_in; DLOG(INFO) << "samples per ms " << d_samples_per_ms; DLOG(INFO) << "doppler max " << d_doppler_max; @@ -121,6 +113,9 @@ gps_l1_ca_pcps_acquisition_cc::~gps_l1_ca_pcps_acquisition_cc() } } + + + void gps_l1_ca_pcps_acquisition_cc::set_satellite(unsigned int satellite) { d_satellite = satellite; @@ -136,11 +131,13 @@ void gps_l1_ca_pcps_acquisition_cc::set_satellite(unsigned int satellite) //TODO Optimize it ! try conj() for (unsigned int i = 0; i < d_fft_size; i++) { - d_fft_codes[i] = std::complex( - d_fft_if->get_outbuf()[i].real(), - -d_fft_if->get_outbuf()[i].imag()); + //d_fft_codes[i] = std::complex( + // d_fft_if->get_outbuf()[i].real(), + // -d_fft_if->get_outbuf()[i].imag()); + d_fft_codes[i] = std::complex(conj(d_fft_if->get_outbuf()[i])); d_fft_codes[i] = d_fft_codes[i] / (float)d_fft_size; //to correct the scale factor introduced by FFTW } + //memcpy(d_fft_codes,d_fft_if->get_outbuf(),sizeof(gr_complex)*d_samples_per_ms); // std::stringstream filename; @@ -156,6 +153,10 @@ void gps_l1_ca_pcps_acquisition_cc::set_satellite(unsigned int satellite) // //d_dump_file.write((char*)d_sine_if, n); //to be read with read_complex_binary() -> test OK // d_dump_file.close(); } + + + + signed int gps_l1_ca_pcps_acquisition_cc::prn_code_phase() { return d_code_phase; @@ -166,9 +167,9 @@ int gps_l1_ca_pcps_acquisition_cc::general_work(int noutput_items, gr_vector_void_star &output_items) { - /*! + /* * By J.Arribas - * Acquisition A strategy (Kay Borre book + CFAR threshold): + * Acquisition strategy (Kay Borre book + CFAR threshold): * 1. Compute the input signal power estimation * 2. Doppler serial search loop * 3. Perform the FFT-based circular convolution (parallel time search) @@ -178,159 +179,154 @@ int gps_l1_ca_pcps_acquisition_cc::general_work(int noutput_items, */ if (!d_active) - { - d_sample_counter += d_fft_size * noutput_items; // sample counter - consume_each(noutput_items); - } + { + d_sample_counter += d_fft_size * noutput_items; // sample counter + consume_each(noutput_items); + } else - { - d_sample_counter += d_fft_size; // sample counter - - //restart acquisition variables - - d_code_phase = 0; - d_doppler_freq = 0; - d_mag = 0.0; - d_input_power = 0.0; - - // initialize acquisition algorithm - - int doppler; - unsigned int indext = 0; - float magt = 0.0; - float tmp_magt = 0.0; - const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer - bool positive_acquisition = false; - int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL - - //aux vars - unsigned int i; - std::stringstream filename; - std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write - - LOG_AT_LEVEL(INFO) << "Channel: " << d_channel - << " , doing acquisition of satellite: " << d_satellite - << " ,sample stamp: " << d_sample_counter << ", threshold: " - << d_threshold << ", doppler_max: " << d_doppler_max - << ", doppler_step: " << d_doppler_step; - - // 1- Compute the input signal power estimation - //! \TODO try norm() - for (i = 0; i < d_fft_size; i++) { - d_input_power += std::abs(in[i]) * std::abs(in[i]); - } + d_sample_counter += d_fft_size; // sample counter - d_input_power = d_input_power / (float)d_fft_size; + //restart acquisition variables - // 2- Doppler frequency search loop - for (doppler = (int)(-d_doppler_max); doppler < (int)d_doppler_max; doppler - += d_doppler_step) - { // doppler search steps - //Perform the carrier wipe-off - sine_gen_complex(d_sine_if, d_freq + doppler, d_fs_in, d_fft_size); + d_code_phase = 0; + d_doppler_freq = 0; + d_mag = 0.0; + d_input_power = 0.0; + + // initialize acquisition algorithm + + int doppler; + unsigned int indext = 0; + float magt = 0.0; + float tmp_magt = 0.0; + const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer + + bool positive_acquisition = false; + int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL + + //aux vars + unsigned int i; + + DLOG(INFO) << "Channel: " << d_channel + << " , doing acquisition of satellite: " << d_satellite + << " ,sample stamp: " << d_sample_counter << ", threshold: " + << d_threshold << ", doppler_max: " << d_doppler_max + << ", doppler_step: " << d_doppler_step; + + // 1- Compute the input signal power estimation for (i = 0; i < d_fft_size; i++) - { - d_fft_if->get_inbuf()[i] = in[i] * d_sine_if[i]; - } + { + d_input_power += std::norm(in[i]); + } - //3- Perform the FFT-based circular convolution (parallel time search) - d_fft_if->execute(); + d_input_power = d_input_power / (float)d_fft_size; - //TODO Optimize me - for (i = 0; i < d_fft_size; i++) - { - d_ifft->get_inbuf()[i] = (d_fft_if->get_outbuf()[i] - * d_fft_codes[i]) / (float)d_fft_size; - } - d_ifft->execute(); + // 2- Doppler frequency search loop + for (doppler = (int)(-d_doppler_max); doppler < (int)d_doppler_max; doppler += d_doppler_step) + { + // doppler search steps + //Perform the carrier wipe-off + sine_gen_complex(d_sine_if, d_freq + doppler, d_fs_in, d_fft_size); + for (i = 0; i < d_fft_size; i++) + { + d_fft_if->get_inbuf()[i] = in[i] * d_sine_if[i]; + } - // old version - //x86_gr_complex_mag(d_ifft->get_outbuf(), d_fft_size); // d_ifft->get_outbuf()=|abs(·)|^2 and the array is converted from CPX->Float - //x86_float_max((float*)d_ifft->get_outbuf(), &indext, &magt, - // d_fft_size); // find max of |abs(·)|^2 -> index and magt + //3- Perform the FFT-based circular convolution (parallel time search) + d_fft_if->execute(); + for (i = 0; i < d_fft_size; i++) + { + d_ifft->get_inbuf()[i] = (d_fft_if->get_outbuf()[i] + * d_fft_codes[i]) / (float)d_fft_size; + } + d_ifft->execute(); - // C++ version - indext=0; - magt=0; - for (i = 0; i < d_fft_size; i++) - { - tmp_magt=std::norm(d_ifft->get_outbuf()[i]); - if (tmp_magt>magt){ - magt=tmp_magt; - indext=i; - } - } + // Search maximum + indext = 0; + magt = 0; + for (i = 0; i < d_fft_size; i++) + { + tmp_magt = std::norm(d_ifft->get_outbuf()[i]); + if (tmp_magt > magt) + { + magt = tmp_magt; + indext = i; + } + } - // Record results to files - if (d_dump) - { - filename.str(""); - filename << "./data/fft_" << doppler << "_.dat"; - std::cout << filename.str().c_str(); - std::cout << ".\n"; - d_dump_file.open(filename.str().c_str(), std::ios::out - | std::ios::binary); - d_dump_file.write((char*)d_ifft->get_outbuf(), n); //write directly |abs(·)|^2 in this Doppler bin - d_dump_file.close(); - } - // 4- record the maximum peak and the associated synchronization parameters - if (d_mag < magt) - { - d_mag = magt; - d_code_phase = indext; - d_doppler_freq = doppler; - } + // Record results to files + if (d_dump) + { + std::stringstream filename; + std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write + filename.str(""); + filename << "./data/fft_" << doppler << "_.dat"; + std::cout << filename.str().c_str(); + std::cout << ".\n"; + d_dump_file.open(filename.str().c_str(), std::ios::out + | std::ios::binary); + d_dump_file.write((char*)d_ifft->get_outbuf(), n); //write directly |abs(á)|^2 in this Doppler bin? + d_dump_file.close(); + } + + // 4- record the maximum peak and the associated synchronization parameters + if (d_mag < magt) + { + d_mag = magt; + d_code_phase = indext; + d_doppler_freq = doppler; + } + } + + // 5- Compute the test statistics and compare to the threshold + d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; + + + // 6- Declare positive or negative acquisition using a message queue + + if (d_test_statistics > d_threshold) + { + positive_acquisition = true; + d_acq_sample_stamp = d_sample_counter; + + DLOG(INFO) << "positive acquisition"; + DLOG(INFO) << "satellite " << d_satellite; + DLOG(INFO) << "sample_stamp " << d_sample_counter; + DLOG(INFO) << "test statistics value " << d_test_statistics; + DLOG(INFO) << "test statistics threshold " << d_threshold; + DLOG(INFO) << "code phase " << d_code_phase; + DLOG(INFO) << "doppler " << d_doppler_freq; + DLOG(INFO) << "magnitude " << d_mag; + DLOG(INFO) << "input signal power " << d_input_power; + } + else + { + DLOG(INFO) << "negative acquisition"; + DLOG(INFO) << "satellite " << d_satellite; + DLOG(INFO) << "sample_stamp " << d_sample_counter; + DLOG(INFO) << "test statistics value " << d_test_statistics; + DLOG(INFO) << "test statistics threshold " << d_threshold; + DLOG(INFO) << "code phase " << d_code_phase; + DLOG(INFO) << "doppler " << d_doppler_freq; + DLOG(INFO) << "magnitude " << d_mag; + DLOG(INFO) << "input signal power " << d_input_power; + } + + d_active = false; + + if (positive_acquisition) + { + acquisition_message = 1; + } + else + { + acquisition_message = 2; + } + + d_channel_internal_queue->push(acquisition_message); + + consume_each(1); } - - // 5- Compute the test statistics and compare to the threshold - d_test_statistics = 2*d_fft_size*d_mag / d_input_power; - // 6- Declare positive or negative acquisition using a message queue - - if (d_test_statistics > d_threshold) - { //0.004 - positive_acquisition = true; - d_acq_sample_stamp = d_sample_counter; - LOG_AT_LEVEL(INFO) << "positive acquisition"; - LOG_AT_LEVEL(INFO) << "satellite " << d_satellite; - LOG_AT_LEVEL(INFO) << "sample_stamp " << d_sample_counter; - LOG_AT_LEVEL(INFO) << "test statistics value " - << d_test_statistics; - LOG_AT_LEVEL(INFO) << "test statistics threshold " << d_threshold; - LOG_AT_LEVEL(INFO) << "code phase " << d_code_phase; - LOG_AT_LEVEL(INFO) << "doppler " << d_doppler_freq; - LOG_AT_LEVEL(INFO) << "magnitude " << d_mag; - LOG_AT_LEVEL(INFO) << "input signal power " << d_input_power; - - } - else - { - LOG_AT_LEVEL(INFO) << "negative acquisition"; - LOG_AT_LEVEL(INFO) << "satellite " << d_satellite; - LOG_AT_LEVEL(INFO) << "sample_stamp " << d_sample_counter; - LOG_AT_LEVEL(INFO) << "test statistics value " - << d_test_statistics; - LOG_AT_LEVEL(INFO) << "test statistics threshold " << d_threshold; - LOG_AT_LEVEL(INFO) << "code phase " << d_code_phase; - LOG_AT_LEVEL(INFO) << "doppler " << d_doppler_freq; - LOG_AT_LEVEL(INFO) << "magnitude " << d_mag; - LOG_AT_LEVEL(INFO) << "input signal power " << d_input_power; - - } - d_active = false; - - if (positive_acquisition) - { - acquisition_message = 1; - } - else - { - acquisition_message = 2; - } - - d_channel_internal_queue->push(acquisition_message); - - consume_each(1); - } return 0; } diff --git a/src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_tong_pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_tong_pcps_acquisition_cc.cc index ef758d4a3..70201a7c8 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_tong_pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_tong_pcps_acquisition_cc.cc @@ -157,11 +157,16 @@ void gps_l1_ca_tong_pcps_acquisition_cc::set_satellite(unsigned int satellite) -d_fft_if->get_outbuf()[i].imag()); } } + + + signed int gps_l1_ca_tong_pcps_acquisition_cc::prn_code_phase() { return d_code_phase; } + + int gps_l1_ca_tong_pcps_acquisition_cc::general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) diff --git a/src/algorithms/channel/adapters/channel.h b/src/algorithms/channel/adapters/channel.h index 279c2b487..0a70afdc3 100644 --- a/src/algorithms/channel/adapters/channel.h +++ b/src/algorithms/channel/adapters/channel.h @@ -71,19 +71,19 @@ public: gr_basic_block_sptr get_left_block(); gr_basic_block_sptr get_right_block(); - std::string role(){ return role_;} + std::string role(){ return role_; } - std::string implementation(){ return "Channel";} + std::string implementation(){ return "Channel"; } - size_t item_size(){ return 0;} + size_t item_size(){ return 0; } - unsigned int satellite(){ return satellite_;} + unsigned int satellite(){ return satellite_; } - AcquisitionInterface* acquisition(){ return acq_;} + AcquisitionInterface* acquisition(){ return acq_; } - TrackingInterface* tracking(){ return trk_;} + TrackingInterface* tracking(){ return trk_; } - TelemetryDecoderInterface* telemetry(){ return nav_;} + TelemetryDecoderInterface* telemetry(){ return nav_; } void start_acquisition(); void set_satellite(unsigned int satellite); diff --git a/src/algorithms/libs/gps_sdr_signal_processing.cc b/src/algorithms/libs/gps_sdr_signal_processing.cc index f7a4e9534..d397b8db4 100644 --- a/src/algorithms/libs/gps_sdr_signal_processing.cc +++ b/src/algorithms/libs/gps_sdr_signal_processing.cc @@ -7,18 +7,19 @@ /*! * The SV ID is _prn=ID -1 */ -void code_gen_conplex(std::complex* _dest, int32 _prn, unsigned int _chip_shift) { +void code_gen_conplex(std::complex* _dest, signed int _prn, unsigned int _chip_shift) +{ - uint32 G1[1023]; - uint32 G2[1023]; - uint32 G1_register[10], G2_register[10]; - uint32 feedback1, feedback2; - uint32 lcv, lcv2; - uint32 delay; - int32 prn = _prn-1; //Move the PRN code to fit an array indices + unsigned int G1[1023]; + unsigned int G2[1023]; + unsigned int G1_register[10], G2_register[10]; + unsigned int feedback1, feedback2; + unsigned int lcv, lcv2; + unsigned int delay; + signed int prn = _prn-1; //Move the PRN code to fit an array indices /* G2 Delays as defined in GPS-ISD-200D */ - int32 delays[51] = {5, 6, 7, 8, 17, 18, 139, 140, 141, 251, 252, 254 ,255, 256, 257, 258, 469, 470, 471, 472, + signed int delays[51] = {5, 6, 7, 8, 17, 18, 139, 140, 141, 251, 252, 254 ,255, 256, 257, 258, 469, 470, 471, 472, 473, 474, 509, 512, 513, 514, 515, 516, 859, 860, 861, 862, 145, 175, 52, 21, 237, 235, 886, 657, 634, 762, 355, 1012, 176, 603, 130, 359, 595, 68, 386}; @@ -43,8 +44,8 @@ void code_gen_conplex(std::complex* _dest, int32 _prn, unsigned int _chip for(lcv2 = 0; lcv2 < 9; lcv2++) { - G1_register[lcv2] = G1_register[lcv2+1]; - G2_register[lcv2] = G2_register[lcv2+1]; + G1_register[lcv2] = G1_register[lcv2 + 1]; + G2_register[lcv2] = G2_register[lcv2 + 1]; } G1_register[9] = feedback1; @@ -53,13 +54,13 @@ void code_gen_conplex(std::complex* _dest, int32 _prn, unsigned int _chip /* Set the delay */ delay = 1023 - delays[prn]; - delay+=_chip_shift; + delay += _chip_shift; delay %= 1023; /* Generate PRN from G1 and G2 Registers */ for(lcv = 0; lcv < 1023; lcv++) { - _dest[lcv] = std::complex(G1[(lcv+_chip_shift)%1023]^G2[delay], 0); - if(_dest[lcv].real()==0.0) //javi + _dest[lcv] = std::complex(G1[(lcv + _chip_shift)%1023]^G2[delay], 0); + if(_dest[lcv].real() == 0.0) //javi { _dest[lcv].real(-1.0); } @@ -75,19 +76,19 @@ void code_gen_conplex(std::complex* _dest, int32 _prn, unsigned int _chip /*! * code_gen, generate the given prn code * */ -int32 code_gen(CPX *_dest, int32 _prn) +signed int code_gen(CPX *_dest, signed int _prn) { - uint32 G1[1023]; - uint32 G2[1023]; - uint32 G1_register[10], G2_register[10]; - uint32 feedback1, feedback2; - uint32 lcv, lcv2; - uint32 delay; - int32 prn = _prn-1; //Move the PRN code to fit an array indices + unsigned int G1[1023]; + unsigned int G2[1023]; + unsigned int G1_register[10], G2_register[10]; + unsigned int feedback1, feedback2; + unsigned int lcv, lcv2; + unsigned int delay; + signed int prn = _prn-1; //Move the PRN code to fit an array indices /* G2 Delays as defined in GPS-ISD-200D */ - int32 delays[51] = {5, 6, 7, 8, 17, 18, 139, 140, 141, 251, 252, 254 ,255, 256, 257, 258, 469, 470, 471, 472, + signed int delays[51] = {5, 6, 7, 8, 17, 18, 139, 140, 141, 251, 252, 254 ,255, 256, 257, 258, 469, 470, 471, 472, 473, 474, 509, 512, 513, 514, 515, 516, 859, 860, 861, 862, 145, 175, 52, 21, 237, 235, 886, 657, 634, 762, 355, 1012, 176, 603, 130, 359, 595, 68, 386}; @@ -143,15 +144,15 @@ int32 code_gen(CPX *_dest, int32 _prn) /*! * code_gen_complex_sampled, generate GPS L1 C/A code complex for the desired SV ID and sampled to specific sampling frequency */ -void code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, int32 _fs, unsigned int _chip_shift) +void code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, signed int _fs, unsigned int _chip_shift) { // This function is based on the GNU software GPS for MATLAB in the Kay Borre book std::complex _code[1023]; - int32 _samplesPerCode,_codeValueIndex; + signed int _samplesPerCode,_codeValueIndex; float _ts; float _tc; - const int32 _codeFreqBasis=1023000; //Hz - const int32 _codeLength=1023; + const signed int _codeFreqBasis=1023000; //Hz + const signed int _codeLength=1023; //--- Find number of samples per spreading code ---------------------------- _samplesPerCode = round(_fs / (_codeFreqBasis / _codeLength)); @@ -163,7 +164,7 @@ void code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, int //std::cout<<"ts="<<_ts<> (32-N))) //!< Used in the parity check algorithm +#define _lrotl(X,N) ((X << N) ^ (X >> (32-N))) // Used in the parity check algorithm using google::LogMessage; @@ -80,19 +80,19 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(unsigned int sate d_dump = dump; d_satellite = satellite; d_vector_length = vector_length; - d_samples_per_bit = 20; // it is exactly 1000*(1/50)=20 + d_samples_per_bit = ( GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS ) / GPS_CA_TELEMETRY_RATE_BITS_SECOND; d_fs_in = fs_in; - d_preamble_duration_seconds = (1.0/(float)GPS_CA_TELEMETRY_RATE_BITS_SECOND)*(float)GPS_CA_PREAMBLE_LENGTH_BITS; + d_preamble_duration_seconds = (1.0 / GPS_CA_TELEMETRY_RATE_BITS_SECOND) * GPS_CA_PREAMBLE_LENGTH_BITS; //std::cout<<"d_preamble_duration_seconds="<d_preambles_bits, (unsigned short int*)preambles_bits, 8*sizeof(unsigned short int)); + memcpy((unsigned short int*)this->d_preambles_bits, (unsigned short int*)preambles_bits, GPS_CA_PREAMBLE_LENGTH_BITS*sizeof(unsigned short int)); // preamble bits to sampled symbols - d_preambles_symbols = (signed int*)malloc(sizeof(signed int)*8*d_samples_per_bit); + d_preambles_symbols = (signed int*)malloc(sizeof(signed int) * GPS_CA_PREAMBLE_LENGTH_BITS * d_samples_per_bit); int n = 0; - for (int i=0; i<8; i++) + for (int i=0; i *nav_queue){d_GPS_FSM.d_nav_queue=nav_queue;} + + 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_telemetry_decoder_cc_sptr @@ -112,20 +130,6 @@ private: std::string d_dump_filename; std::ofstream d_dump_file; - -public: - - ~gps_l1_ca_telemetry_decoder_cc(); - - void set_satellite(int satellite); - void set_channel(int channel); - - void set_navigation_queue(concurrent_queue *nav_queue){d_GPS_FSM.d_nav_queue=nav_queue;} - - 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 diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h index ba9fd4c38..603d9c0b4 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h @@ -83,9 +83,9 @@ public: void set_channel_queue(concurrent_queue *channel_internal_queue); void start_tracking(); - void set_acq_sample_stamp(unsigned long int sample_stamp); + private: gps_l1_ca_dll_fll_pll_tracking_cc_sptr tracking_; 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 fe2a3551c..32cfdbce7 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 @@ -41,6 +41,7 @@ #include "CN_estimators.h" #include "tracking_FLL_PLL_filter.h" #include "control_message_factory.h" +#include "gnss_flowgraph.h" #include #include #include @@ -98,7 +99,7 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(unsigned in d_fs_in = fs_in; d_vector_length = vector_length; d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) - d_dump_filename=dump_filename; + d_dump_filename = dump_filename; // Initialize tracking variables ========================================== d_carrier_loop_filter.set_params(fll_bw_hz,pll_bw_hz,order); @@ -129,6 +130,7 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(unsigned in d_CN0_SNV_dB_Hz = 0; d_carrier_lock_fail_counter = 0; d_carrier_lock_threshold = 5; + } @@ -142,7 +144,6 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking() unsigned long int acq_trk_diff_samples; float acq_trk_diff_seconds; acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length; - //std::cout<<"acq_trk_diff_samples="< MINIMUM_VALID_CN0) @@ -423,7 +425,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto } if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) { - std::cout<<"Channel "<< d_channel << " loss of lock!" << std::endl; + 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; @@ -438,15 +440,15 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto */ // ########### Output the tracking data to navigation and PVT ########## // Output channel 0: Prompt correlator output Q - *out[0]=(double)d_Prompt.real(); + *out[0] = (double)d_Prompt.real(); // Output channel 1: Prompt correlator output I - *out[1]=(double)d_Prompt.imag(); + *out[1] = (double)d_Prompt.imag(); // Output channel 2: PRN absolute delay [s] - *out[2]=d_sample_counter_seconds; + *out[2] = d_sample_counter_seconds; // Output channel 3: d_acc_carrier_phase_rad [rad] - *out[3]=(double)d_acc_carrier_phase_rad; + *out[3] = (double)d_acc_carrier_phase_rad; // Output channel 4: PRN code phase [s] - *out[4]=(double)d_code_phase_samples*(1/(float)d_fs_in); + *out[4] = (double)d_code_phase_samples * (1/(float)d_fs_in); // ########## DEBUG OUTPUT /*! @@ -459,7 +461,7 @@ 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 << " CN0=" << d_CN0_SNV_dB_Hz << " [dB-Hz]" < #include #include "concurrent_queue.h" +//#include "GPS_L1_CA.h" class Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc; typedef boost::shared_ptr diff --git a/src/core/interfaces/channel_interface.h b/src/core/interfaces/channel_interface.h index 2f3c959d2..e9868c5da 100644 --- a/src/core/interfaces/channel_interface.h +++ b/src/core/interfaces/channel_interface.h @@ -42,7 +42,7 @@ /*! * \brief This abstract class represents an interface to a channel GNSS block. * - * Abstract class for channel blocks. Since all its methods are virtual, + * Abstract class for channel blocks. Since all its methods are pure virtual, * this class cannot be instantiated directly, and a subclass can only be * instantiated directly if all inherited pure virtual methods have been * implemented by that class or a parent class. diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 979e681b0..ff18fddec 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -55,6 +55,7 @@ GNSSFlowgraph::GNSSFlowgraph(ConfigurationInterface *configuration, block_factory_ = new GNSSBlockFactory(); queue_ = queue; available_GPS_satellites_IDs_ = new std::list(); + init(); } @@ -109,7 +110,7 @@ void GNSSFlowgraph::connect() { /* Connects the blocks in the flowgraph * - * Signal Source > Signal conditioner > Channels >> Observables >> PVT > Output filter + * Signal Source > Signal conditioner >> Channels >> Observables >> PVT > Output filter */ DLOG(INFO) << "Connecting flowgraph"; if (connected_) @@ -118,7 +119,7 @@ void GNSSFlowgraph::connect() return; } - // Connect GNSS block internally + try { signal_source()->connect(top_block_); @@ -131,6 +132,7 @@ void GNSSFlowgraph::connect() return; } + // Signal Source > Signal conditioner > try { signal_conditioner()->connect(top_block_); @@ -144,6 +146,7 @@ void GNSSFlowgraph::connect() return; } + for (unsigned int i = 0; i < channels_count_; i++) { try @@ -160,6 +163,7 @@ void GNSSFlowgraph::connect() } } + try { observables()->connect(top_block_); @@ -172,6 +176,7 @@ void GNSSFlowgraph::connect() return; } + // Signal Source > Signal conditioner >> Channels >> Observables > PVT try { pvt()->connect(top_block_); @@ -184,6 +189,7 @@ void GNSSFlowgraph::connect() return; } + // Signal Source > Signal conditioner >> Channels >> Observables > PVT > Output Filter try { output_filter()->connect(top_block_); @@ -198,6 +204,7 @@ void GNSSFlowgraph::connect() DLOG(INFO) << "blocks connected internally"; + // Signal Source > Signal conditioner > try { top_block_->connect(signal_source()->get_right_block(), 0, @@ -213,6 +220,7 @@ void GNSSFlowgraph::connect() } DLOG(INFO) << "Signal source connected to signal conditioner"; + // Signal Source > Signal conditioner >> channels_count_ number of Channels in parallel for (unsigned int i = 0; i < channels_count_; i++) { try @@ -231,6 +239,7 @@ void GNSSFlowgraph::connect() DLOG(INFO) << "signal conditioner connected to channel " << i; + // Signal Source > Signal conditioner >> Channels >> Observables try { top_block_->connect(channel(i)->get_right_block(), 0, @@ -388,10 +397,13 @@ GNSSBlockInterface* GNSSFlowgraph::output_filter() return blocks_->at(4); } + + + void GNSSFlowgraph::init() { /* - * Instantiates the receiver blocks + * Instantiates the receiver blocks */ blocks_->push_back( block_factory_->GetSignalSource(configuration_, queue_)); diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index aa5ff4704..3abf591db 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -40,6 +40,7 @@ #include #include #include +#include "GPS_L1_CA.h" class GNSSBlockInterface; class ChannelInterface; diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index b4ec0ac00..bde5c4a10 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -32,19 +32,21 @@ #ifndef GNSS_SDR_GPS_L1_CA_H_ #define GNSS_SDR_GPS_L1_CA_H_ +#include + // Physical constants const double GPS_C_m_s = 299792458.0; //!< The speed of light, [m/s] const double GPS_C_m_ms = 299792.4580; //!< The speed of light, [m/ms] -const double GPS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E -const double OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Earth rotation rate, [rad/s] -const double GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] -const double F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)] +const double GPS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E +const double OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Earth rotation rate, [rad/s] +const double GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] +const double F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)] // carrier and code frequencies -const float GPS_L1_FREQ_HZ = 1.57542e9; //!< L1 [Hz] -const float GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [chips/s] -const float GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips] +const double GPS_L1_FREQ_HZ = 1.57542e9; //!< L1 [Hz] +const double GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [chips/s] +const double GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips] /*! * \brief Maximum Time-Of-Arrival (TOA) difference between satellites for a receiver operated on Earth surface is 20 ms @@ -54,19 +56,19 @@ const float GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length * [1] J. Bao-Yen Tsui, Fundamentals of Global Positioning System Receivers. A Software Approach, John Wiley & Sons, * Inc., Hoboken, NJ, 2nd edition, 2005. */ -const double MAX_TOA_DELAY_MS=20; +const double MAX_TOA_DELAY_MS = 20; #define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here -const float GPS_STARTOFFSET_ms= 68.802; //[ms] Initial sign. travel time (this cannot go here) +const float GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) // NAVIGATION MESSAGE DEMODULATION AND DECODING #define GPS_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1} -#define GPS_CA_PREAMBLE_LENGTH_BITS 8 -#define GPS_CA_TELEMETRY_RATE_BITS_SECOND 50 //!< NAV message bit rate [bits/s] +const int GPS_CA_PREAMBLE_LENGTH_BITS = 8; +const int GPS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] #define GPS_WORD_LENGTH 4 // CRC + GPS WORD (-2 -1 0 ... 29) Bits = 4 bytes #define GPS_SUBFRAME_LENGTH 40 // GPS_WORD_LENGTH x 10 = 40 bytes const int GPS_SUBFRAME_BITS=300; //!< Number of bits per subframe in the NAV message [bits] @@ -81,14 +83,15 @@ const int GPS_WORD_BITS=30; //!< Number of bits per word in t /*! * \brief Navigation message bits slice structure: A portion of bits is indicated by * the start position inside the subframe and the length in number of bits */ -typedef struct bits_slice{ - int position; - int length; - bits_slice(int p,int l) - { - position=p; - length=l; - } +typedef struct bits_slice +{ + int position; + int length; + bits_slice(int p,int l) + { + position=p; + length=l; + } } bits_slice; @@ -341,6 +344,72 @@ const bits_slice HEALTH_SV24[]={{259,6}}; +inline void ca_code_generator_complex(std::complex* _dest, signed int _prn, unsigned int _chip_shift) +{ + + unsigned int G1[1023]; + unsigned int G2[1023]; + unsigned int G1_register[10], G2_register[10]; + unsigned int feedback1, feedback2; + unsigned int lcv, lcv2; + unsigned int delay; + signed int prn = _prn-1; //Move the PRN code to fit an array indices + + /* G2 Delays as defined in IS-GPS-200E */ + signed int delays[32] = {5, 6, 7, 8, 17, 18, 139, 140, 141, 251, + 252, 254, 255, 256, 257, 258, 469, 470, 471, 472, + 473, 474, 509, 512, 513, 514, 515, 516, 859, 860, + 861, 862}; + // PRN sequences 33 through 37 are reserved for other uses (e.g. ground transmitters) + + /* A simple error check */ + if((prn < 0) || (prn > 32)) + return; + + for(lcv = 0; lcv < 10; lcv++) + { + G1_register[lcv] = 1; + G2_register[lcv] = 1; + } + + /* Generate G1 & G2 Register */ + for(lcv = 0; lcv < 1023; lcv++) + { + G1[lcv] = G1_register[0]; + G2[lcv] = G2_register[0]; + + feedback1 = G1_register[7]^G1_register[0]; + feedback2 = (G2_register[8] + G2_register[7] + G2_register[4] + G2_register[2] + G2_register[1] + G2_register[0]) & 0x1; + + for(lcv2 = 0; lcv2 < 9; lcv2++) + { + G1_register[lcv2] = G1_register[lcv2 + 1]; + G2_register[lcv2] = G2_register[lcv2 + 1]; + } + + G1_register[9] = feedback1; + G2_register[9] = feedback2; + } + + /* Set the delay */ + delay = 1023 - delays[prn]; + delay += _chip_shift; + delay %= 1023; + /* Generate PRN from G1 and G2 Registers */ + for(lcv = 0; lcv < 1023; lcv++) + { + _dest[lcv] = std::complex(G1[(lcv + _chip_shift)%1023]^G2[delay], 0); + if(_dest[lcv].real() == 0.0) //javi + { + _dest[lcv].real(-1.0); + } + delay++; + delay %= 1023; + //std::cout<<_dest[lcv].real(); //OK + } +} + + #endif /* GNSS_SDR_GPS_L1_CA_H_ */ diff --git a/src/core/system_parameters/gps_navigation_message.h b/src/core/system_parameters/gps_navigation_message.h index a2fc0b028..d449f51a1 100644 --- a/src/core/system_parameters/gps_navigation_message.h +++ b/src/core/system_parameters/gps_navigation_message.h @@ -70,7 +70,7 @@ private: public: //broadcast orbit 1 - double d_TOW; + double d_TOW; //!< Time of GPS Week [s] double d_IODE_SF2; double d_IODE_SF3; double d_Crs; //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m]