diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index 276a72db7..d1e4375d1 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -97,7 +97,7 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu if (acq_parameters.bit_transition_flag) { d_fft_size *= 2; - acq_parameters.max_dwells = 1; //Activation of acq_parameters.bit_transition_flag invalidates the value of acq_parameters.max_dwells + acq_parameters.max_dwells = 1; // Activation of acq_parameters.bit_transition_flag invalidates the value of acq_parameters.max_dwells } d_fft_codes = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); @@ -128,6 +128,15 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu d_dump_number = 0; d_dump_channel = acq_parameters.dump_channel; samplesPerChip = acq_parameters.samples_per_chip; + // todo: CFAR statistic not available for non-coherent integration + if (acq_parameters.max_dwells == 1) + { + d_use_CFAR_algorithm_flag = acq_parameters.use_CFAR_algorithm_flag; + } + else + { + d_use_CFAR_algorithm_flag = false; + } } @@ -427,17 +436,49 @@ void pcps_acquisition::dump_results(int effective_fft_size) } } -float pcps_acquisition::first_vs_second_peak_statistics(uint32_t& indext, int& doppler) + +float pcps_acquisition::max_to_input_power_statistic(uint32_t& indext, int& doppler, float input_power) { - float firstPeak = 0.0; + float grid_maximum = 0.0; int index_doppler = 0; uint32_t tmp_intex_t = 0; uint32_t index_time = 0; + float fft_normalization_factor = static_cast(d_fft_size) * static_cast(d_fft_size); // Look for correlation peaks in the results ============================== // Find the highest peak and compare it to the second highest peak // The second peak is chosen not closer than 1 chip to the highest peak //--- Find the correlation peak and the carrier frequency -------------- + for (int i = 0; i < d_num_doppler_bins; i++) + { + volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_magnitude_grid[i], d_fft_size); + if (d_magnitude_grid[i][tmp_intex_t] > grid_maximum) + { + grid_maximum = d_magnitude_grid[i][tmp_intex_t]; + index_doppler = i; + index_time = tmp_intex_t; + } + } + indext = index_time; + doppler = -static_cast(acq_parameters.doppler_max) + d_doppler_step * index_doppler; + + float magt = grid_maximum / (fft_normalization_factor * fft_normalization_factor); + return magt / input_power; +} + + +float pcps_acquisition::first_vs_second_peak_statistic(uint32_t& indext, int& doppler) +{ + // Look for correlation peaks in the results + // Find the highest peak and compare it to the second highest peak + // The second peak is chosen not closer than 1 chip to the highest peak + + float firstPeak = 0.0; + int index_doppler = 0; + uint32_t tmp_intex_t = 0; + uint32_t index_time = 0; + + // Find the correlation peak and the carrier frequency for (int i = 0; i < d_num_doppler_bins; i++) { volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_magnitude_grid[i], d_fft_size); @@ -451,12 +492,11 @@ float pcps_acquisition::first_vs_second_peak_statistics(uint32_t& indext, int& d indext = index_time; doppler = -static_cast(acq_parameters.doppler_max) + d_doppler_step * index_doppler; - // -- - Find 1 chip wide code phase exclude range around the peak - //uint32_t samplesPerChip = ceil(GPS_L1_CA_CHIP_PERIOD * static_cast(this->d_fs_in)); + // Find 1 chip wide code phase exclude range around the peak int32_t excludeRangeIndex1 = index_time - samplesPerChip; int32_t excludeRangeIndex2 = index_time + samplesPerChip; - // -- - Correct code phase exclude range if the range includes array boundaries + // Correct code phase exclude range if the range includes array boundaries if (excludeRangeIndex1 < 0) { excludeRangeIndex1 = d_fft_size + excludeRangeIndex1; @@ -476,11 +516,11 @@ float pcps_acquisition::first_vs_second_peak_statistics(uint32_t& indext, int& d } while (idx != excludeRangeIndex2); - //--- Find the second highest correlation peak in the same freq. bin --- + // Find the second highest correlation peak in the same freq. bin --- volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_tmp_buffer, d_fft_size); float secondPeak = d_tmp_buffer[tmp_intex_t]; - // 5- Compute the test statistics and compare to the threshold + // Compute the test statistics and compare to the threshold return firstPeak / secondPeak; } @@ -510,36 +550,37 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) << " ,sample stamp: " << samp_count << ", threshold: " << d_threshold << ", doppler_max: " << acq_parameters.doppler_max << ", doppler_step: " << d_doppler_step - << ", use_CFAR_algorithm_flag: " << (acq_parameters.use_CFAR_algorithm_flag ? "true" : "false"); + << ", use_CFAR_algorithm_flag: " << (d_use_CFAR_algorithm_flag ? "true" : "false"); lk.unlock(); - if (acq_parameters.use_CFAR_algorithm_flag) + + if (d_use_CFAR_algorithm_flag) { - // 1- (optional) Compute the input signal power estimation + // Compute the input signal power estimation volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size); d_input_power /= static_cast(d_fft_size); } - // 2- Doppler frequency search loop + + // Doppler frequency grid loop if (!d_step_two) { for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) { - // Remove doppler + // Remove Doppler volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size); - // 3- Perform the FFT-based convolution (parallel time search) + // Perform the FFT-based convolution (parallel time search) // Compute the FFT of the carrier wiped--off incoming signal d_fft_if->execute(); - // Multiply carrier wiped--off, Fourier transformed incoming signal - // with the local FFT'd code reference using SIMD operations with VOLK library + // Multiply carrier wiped--off, Fourier transformed incoming signal with the local FFT'd code reference volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); - // compute the inverse FFT + // Compute the inverse FFT d_ifft->execute(); - // compute squared magnitude (and accumulate in case of non-coherent integration) + // Compute squared magnitude (and accumulate in case of non-coherent integration) size_t offset = (acq_parameters.bit_transition_flag ? effective_fft_size : 0); if (d_num_noncoherent_integrations_counter == 1) { @@ -556,15 +597,19 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) memcpy(grid_.colptr(doppler_index), d_magnitude_grid[doppler_index], sizeof(float) * effective_fft_size); } } - // 5- Compute the test statistics and compare to the threshold - float computed_statistic = first_vs_second_peak_statistics(indext, doppler); - if (d_test_statistics < computed_statistic or !acq_parameters.bit_transition_flag) + + // Compute the test statistic + if (d_use_CFAR_algorithm_flag) { - d_gnss_synchro->Acq_delay_samples = static_cast(indext % acq_parameters.samples_per_code); - d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); - d_gnss_synchro->Acq_samplestamp_samples = samp_count; - d_test_statistics = computed_statistic; + d_test_statistics = max_to_input_power_statistic(indext, doppler, d_input_power); } + else + { + d_test_statistics = first_vs_second_peak_statistic(indext, doppler); + } + d_gnss_synchro->Acq_delay_samples = static_cast(indext % acq_parameters.samples_per_code); + d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); + d_gnss_synchro->Acq_samplestamp_samples = samp_count; } else { @@ -592,7 +637,7 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, effective_fft_size); magt = d_magnitude[indext]; - if (acq_parameters.use_CFAR_algorithm_flag) + if (d_use_CFAR_algorithm_flag) { // Normalize the maximum value to correct the scale factor introduced by FFTW magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); @@ -602,7 +647,7 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) { d_mag = magt; - if (!acq_parameters.use_CFAR_algorithm_flag) + if (!d_use_CFAR_algorithm_flag) { // Search grid noise floor approximation for this doppler line volk_32f_accumulator_s32f(&d_input_power, d_magnitude, effective_fft_size); @@ -635,6 +680,7 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) } } } + lk.lock(); if (!acq_parameters.bit_transition_flag) { diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h index 5d648ebf3..4ca381ce0 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h @@ -95,13 +95,15 @@ private: void dump_results(int effective_fft_size); - float first_vs_second_peak_statistics(uint32_t& indext, int& doppler); + float first_vs_second_peak_statistic(uint32_t& indext, int& doppler); + float max_to_input_power_statistic(uint32_t& indext, int& doppler, float input_power); Acq_Conf acq_parameters; bool d_active; bool d_worker_active; bool d_cshort; bool d_step_two; + bool d_use_CFAR_algorithm_flag; int d_positive_acq; float d_threshold; float d_mag;