diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index 3a8c3cdf6..5beb35253 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -133,7 +133,9 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu d_data_buffer_sc = nullptr; } grid_ = arma::fmat(); + narrow_grid_ = arma::fmat(); d_step_two = false; + d_num_doppler_bins_step2 = acq_parameters.num_doppler_bins_step2; d_dump_number = 0; d_dump_channel = acq_parameters.dump_channel; d_samplesPerChip = acq_parameters.samples_per_chip; @@ -164,7 +166,7 @@ pcps_acquisition::~pcps_acquisition() } if (acq_parameters.make_2_steps) { - for (unsigned int i = 0; i < acq_parameters.num_doppler_bins_step2; i++) + for (unsigned int i = 0; i < d_num_doppler_bins_step2; i++) { volk_gnsssdr_free(d_grid_doppler_wipeoffs_step_two[i]); } @@ -272,8 +274,8 @@ void pcps_acquisition::init() d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; if (acq_parameters.make_2_steps) { - d_grid_doppler_wipeoffs_step_two = new gr_complex*[acq_parameters.num_doppler_bins_step2]; - for (unsigned int doppler_index = 0; doppler_index < acq_parameters.num_doppler_bins_step2; doppler_index++) + d_grid_doppler_wipeoffs_step_two = new gr_complex*[d_num_doppler_bins_step2]; + for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins_step2; doppler_index++) { d_grid_doppler_wipeoffs_step_two[doppler_index] = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); } @@ -298,6 +300,7 @@ void pcps_acquisition::init() { unsigned int effective_fft_size = (acq_parameters.bit_transition_flag ? (d_fft_size / 2) : d_fft_size); grid_ = arma::fmat(effective_fft_size, d_num_doppler_bins, arma::fill::zeros); + narrow_grid_ = arma::fmat(effective_fft_size, d_num_doppler_bins_step2, arma::fill::zeros); } } @@ -314,9 +317,9 @@ void pcps_acquisition::update_grid_doppler_wipeoffs() void pcps_acquisition::update_grid_doppler_wipeoffs_step2() { - for (unsigned int doppler_index = 0; doppler_index < acq_parameters.num_doppler_bins_step2; doppler_index++) + for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins_step2; doppler_index++) { - float doppler = (static_cast(doppler_index) - static_cast(acq_parameters.num_doppler_bins_step2) / 2.0) * acq_parameters.doppler_step2; + float doppler = (static_cast(doppler_index) - static_cast(floor(d_num_doppler_bins_step2 / 2.0))) * acq_parameters.doppler_step2; update_local_carrier(d_grid_doppler_wipeoffs_step_two[doppler_index], d_fft_size, d_doppler_center_step_two + doppler); } } @@ -456,6 +459,30 @@ void pcps_acquisition::dump_results(int effective_fft_size) Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); + matvar = Mat_VarCreate("num_dwells", MAT_C_UINT32, MAT_T_UINT32, 1, dims, &d_num_noncoherent_integrations_counter, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + if (acq_parameters.make_2_steps) + { + dims[0] = static_cast(effective_fft_size); + dims[1] = static_cast(d_num_doppler_bins_step2); + matvar = Mat_VarCreate("acq_grid_narrow", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, narrow_grid_.memptr(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + dims[0] = static_cast(1); + dims[1] = static_cast(1); + matvar = Mat_VarCreate("doppler_step_narrow", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &acq_parameters.doppler_step2, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + aux = d_doppler_center_step_two - static_cast(floor(d_num_doppler_bins_step2 / 2.0)) * acq_parameters.doppler_step2; + matvar = Mat_VarCreate("doppler_grid_narrow_min", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &aux, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); } } @@ -487,7 +514,7 @@ float pcps_acquisition::max_to_input_power_statistic(uint32_t& indext, int& dopp } else { - doppler = static_cast(d_doppler_center_step_two + (index_doppler - (acq_parameters.num_doppler_bins_step2 / 2.0) * acq_parameters.doppler_step2)); + doppler = static_cast(d_doppler_center_step_two + (static_cast(index_doppler) - static_cast(floor(d_num_doppler_bins_step2 / 2.0))) * acq_parameters.doppler_step2); } float magt = grid_maximum / (fft_normalization_factor * fft_normalization_factor); @@ -525,7 +552,7 @@ float pcps_acquisition::first_vs_second_peak_statistic(uint32_t& indext, int& do } else { - doppler = static_cast(d_doppler_center_step_two + (index_doppler - (acq_parameters.num_doppler_bins_step2 / 2.0) * acq_parameters.doppler_step2)); + doppler = static_cast(d_doppler_center_step_two + (static_cast(index_doppler) - static_cast(floor(d_num_doppler_bins_step2 / 2.0))) * acq_parameters.doppler_step2); } // Find 1 chip wide code phase exclude range around the peak @@ -655,7 +682,7 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) } else { - for (unsigned int doppler_index = 0; doppler_index < acq_parameters.num_doppler_bins_step2; doppler_index++) + for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins_step2; doppler_index++) { volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs_step_two[doppler_index], d_fft_size); @@ -680,15 +707,20 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) volk_32fc_magnitude_squared_32f(d_tmp_buffer, d_ifft->get_outbuf() + offset, effective_fft_size); volk_32f_x2_add_32f(d_magnitude_grid[doppler_index], d_magnitude_grid[doppler_index], d_tmp_buffer, effective_fft_size); } + // Record results to file if required + if (acq_parameters.dump and d_channel == d_dump_channel) + { + memcpy(narrow_grid_.colptr(doppler_index), d_magnitude_grid[doppler_index], sizeof(float) * effective_fft_size); + } } // Compute the test statistic if (d_use_CFAR_algorithm_flag) { - d_test_statistics = max_to_input_power_statistic(indext, doppler, d_input_power, acq_parameters.num_doppler_bins_step2, static_cast(d_doppler_center_step_two - (static_cast(acq_parameters.num_doppler_bins_step2) / 2.0) * acq_parameters.doppler_step2), acq_parameters.doppler_step2); + d_test_statistics = max_to_input_power_statistic(indext, doppler, d_input_power, d_num_doppler_bins_step2, static_cast(d_doppler_center_step_two - (static_cast(d_num_doppler_bins_step2) / 2.0) * acq_parameters.doppler_step2), acq_parameters.doppler_step2); } else { - d_test_statistics = first_vs_second_peak_statistic(indext, doppler, acq_parameters.num_doppler_bins_step2, static_cast(d_doppler_center_step_two - (static_cast(acq_parameters.num_doppler_bins_step2) / 2.0) * acq_parameters.doppler_step2), acq_parameters.doppler_step2); + d_test_statistics = first_vs_second_peak_statistic(indext, doppler, d_num_doppler_bins_step2, static_cast(d_doppler_center_step_two - (static_cast(d_num_doppler_bins_step2) / 2.0) * acq_parameters.doppler_step2), acq_parameters.doppler_step2); } d_gnss_synchro->Acq_delay_samples = static_cast(std::fmod(static_cast(indext), acq_parameters.samples_per_code)); d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); @@ -712,6 +744,8 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) else { d_step_two = true; // Clear input buffer and make small grid acquisition + d_num_noncoherent_integrations_counter = 0; + d_positive_acq = 0; d_state = 0; } } @@ -721,6 +755,11 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) d_state = 0; // Positive acquisition } } + else + { + d_buffer_count = 0; + d_state = 1; + } if (d_num_noncoherent_integrations_counter == acq_parameters.max_dwells) { @@ -746,6 +785,7 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count) else { d_step_two = true; // Clear input buffer and make small grid acquisition + d_num_noncoherent_integrations_counter = 0; d_state = 0; } } @@ -877,7 +917,6 @@ int pcps_acquisition::general_work(int noutput_items __attribute__((unused)), consume_each(buff_increment); break; } - case 2: { // Copy the data to the core and let it know that new data is available diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h index af64cda40..382a76faf 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h @@ -133,6 +133,8 @@ private: gr::fft::fft_complex* d_ifft; Gnss_Synchro* d_gnss_synchro; arma::fmat grid_; + arma::fmat narrow_grid_; + unsigned int d_num_doppler_bins_step2; long int d_dump_number; unsigned int d_dump_channel; unsigned int d_buffer_count; diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_dump_reader.cc b/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_dump_reader.cc index 992c6a528..8875b87f2 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_dump_reader.cc +++ b/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_dump_reader.cc @@ -109,6 +109,10 @@ bool acquisition_dump_reader::read_binary_acq() positive_acq = *static_cast(var2_->data); Mat_VarFree(var2_); + var2_ = Mat_VarRead(matfile, "num_dwells"); + num_dwells = *static_cast(var2_->data); + Mat_VarFree(var2_); + var2_ = Mat_VarRead(matfile, "PRN"); PRN = *static_cast(var2_->data); Mat_VarFree(var2_); @@ -211,6 +215,7 @@ acquisition_dump_reader::acquisition_dump_reader(const std::string& basename, threshold = 0.0; positive_acq = 0; sample_counter = 0; + num_dwells = 0; PRN = 0; if (d_doppler_step == 0) d_doppler_step = 1; d_num_doppler_bins = static_cast(ceil(static_cast(static_cast(d_doppler_max) - static_cast(-d_doppler_max)) / static_cast(d_doppler_step))); diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_dump_reader.h b/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_dump_reader.h index 3958b98b2..bb6e7cc53 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_dump_reader.h +++ b/src/tests/unit-tests/signal-processing-blocks/libs/acquisition_dump_reader.h @@ -64,6 +64,7 @@ public: float threshold; int positive_acq; unsigned int PRN; + unsigned int num_dwells; long unsigned int sample_counter; private: