diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc index c2aef9a46..f90d5e542 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc @@ -57,6 +57,10 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(const Acq_Con d_well_count(0), d_n_samples_in_buffer(0), d_fft_size(static_cast(conf_.samples_per_ms)), + d_fine_doppler_zero_padding_factor(8), + d_fine_doppler_prn_replicas(10), + d_fine_doppler_signal_samples(d_fine_doppler_prn_replicas * d_fft_size), + d_fine_doppler_fft_size(d_fine_doppler_signal_samples * d_fine_doppler_zero_padding_factor), d_gnuradio_forecast_samples(d_fft_size), d_channel(0), d_dump_channel(0), @@ -64,14 +68,19 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(const Acq_Con d_dump(conf_.dump), d_fft_if(gnss_fft_fwd_make_unique(d_fft_size)), d_ifft(gnss_fft_rev_make_unique(d_fft_size)), + d_fine_doppler_fft(gnss_fft_fwd_make_unique(d_fine_doppler_fft_size)), d_grid_data(d_num_doppler_points, volk_gnsssdr::vector(d_fft_size)), d_fft_codes(d_fft_size), d_10_ms_buffer(50 * d_fft_size), - d_magnitude(d_fft_size) + d_magnitude(d_fft_size), + d_fine_doppler_code_replica(d_fine_doppler_signal_samples), + d_fine_doppler_magnitude(d_fine_doppler_fft_size), + d_fine_doppler_freq_bins(d_fine_doppler_fft_size) { this->message_port_register_out(pmt::mp("events")); update_carrier_wipeoff(); + update_fine_doppler_freq_bins(); // this implementation can only produce dumps in channel 0 // todo: migrate config parameters to the unified acquisition config class @@ -179,6 +188,23 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff() } +void pcps_acquisition_fine_doppler_cc::update_fine_doppler_freq_bins() +{ + int counter = 0; + for (int k = 0; k < (d_fine_doppler_fft_size / 2); k++) + { + d_fine_doppler_freq_bins[counter] = ((static_cast(d_acq_params.fs_in) / 2.0) * static_cast(k)) / (static_cast(d_fine_doppler_fft_size) / 2.0); + counter++; + } + + for (int k = d_fine_doppler_fft_size / 2; k > 0; k--) + { + d_fine_doppler_freq_bins[counter] = ((-static_cast(d_acq_params.fs_in) / 2.0) * static_cast(k)) / (static_cast(d_fine_doppler_fft_size) / 2.0); + counter++; + } +} + + float pcps_acquisition_fine_doppler_cc::compute_CAF() { float firstPeak = 0.0; @@ -275,9 +301,6 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons << ", doppler_max: " << d_acq_params.doppler_max << ", doppler_step: " << d_acq_params.doppler_step; - // 2- Doppler frequency search loop - volk_gnsssdr::vector p_tmp_vector(d_fft_size); - for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) { // doppler search steps @@ -296,9 +319,9 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons d_ifft->execute(); // save the grid matrix delay file - volk_32fc_magnitude_squared_32f(p_tmp_vector.data(), d_ifft->get_outbuf(), d_fft_size); + volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size); // accumulate grid values - volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), d_grid_data[doppler_index].data(), p_tmp_vector.data(), d_fft_size); + volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), d_grid_data[doppler_index].data(), d_magnitude.data(), d_fft_size); } return d_fft_size; @@ -315,73 +338,45 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons int pcps_acquisition_fine_doppler_cc::estimate_Doppler() { - // Direct FFT - int zero_padding_factor = 8; - int prn_replicas = 10; - int signal_samples = prn_replicas * d_fft_size; - // int fft_size_extended = nextPowerOf2(signal_samples * zero_padding_factor); - int fft_size_extended = signal_samples * zero_padding_factor; - - auto fft_operator = gnss_fft_fwd_make_unique(fft_size_extended); - // zero padding the entire vector - std::fill_n(fft_operator->get_inbuf(), fft_size_extended, gr_complex(0.0, 0.0)); + std::fill_n(d_fine_doppler_fft->get_inbuf(), d_fine_doppler_fft_size, gr_complex(0.0, 0.0)); // 1. generate local code aligned with the acquisition code phase estimation - volk_gnsssdr::vector code_replica(signal_samples); - - gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_acq_params.fs_in, 0); + gps_l1_ca_code_gen_complex_sampled(d_fine_doppler_code_replica, d_gnss_synchro->PRN, d_acq_params.fs_in, 0); int shift_index = static_cast(d_gnss_synchro->Acq_delay_samples); // Rotate to align the local code replica using acquisition time delay estimation if (shift_index != 0) { - std::rotate(code_replica.data(), code_replica.data() + (d_fft_size - shift_index), code_replica.data() + d_fft_size - 1); + std::rotate(d_fine_doppler_code_replica.data(), d_fine_doppler_code_replica.data() + (d_fft_size - shift_index), d_fine_doppler_code_replica.data() + d_fft_size - 1); } - for (int n = 0; n < prn_replicas - 1; n++) + for (int n = 0; n < d_fine_doppler_prn_replicas - 1; n++) { - std::copy_n(code_replica.data(), d_fft_size, &code_replica[(n + 1) * d_fft_size]); + std::copy_n(d_fine_doppler_code_replica.data(), d_fft_size, &d_fine_doppler_code_replica[(n + 1) * d_fft_size]); } // 2. Perform code wipe-off - volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), d_10_ms_buffer.data(), code_replica.data(), signal_samples); + volk_32fc_x2_multiply_32fc(d_fine_doppler_fft->get_inbuf(), d_10_ms_buffer.data(), d_fine_doppler_code_replica.data(), d_fine_doppler_signal_samples); // 3. Perform the FFT (zero padded!) - fft_operator->execute(); + d_fine_doppler_fft->execute(); // 4. Compute the magnitude and find the maximum - volk_gnsssdr::vector p_tmp_vector(fft_size_extended); - volk_32fc_magnitude_squared_32f(p_tmp_vector.data(), fft_operator->get_outbuf(), fft_size_extended); + volk_32fc_magnitude_squared_32f(d_fine_doppler_magnitude.data(), d_fine_doppler_fft->get_outbuf(), d_fine_doppler_fft_size); uint32_t tmp_index_freq = 0; - volk_gnsssdr_32f_index_max_32u(&tmp_index_freq, p_tmp_vector.data(), fft_size_extended); - - // case even - int counter = 0; - volk_gnsssdr::vector fftFreqBins(fft_size_extended); - - for (int k = 0; k < (fft_size_extended / 2); k++) - { - fftFreqBins[counter] = ((static_cast(d_acq_params.fs_in) / 2.0) * static_cast(k)) / (static_cast(fft_size_extended) / 2.0); - counter++; - } - - for (int k = fft_size_extended / 2; k > 0; k--) - { - fftFreqBins[counter] = ((-static_cast(d_acq_params.fs_in) / 2.0) * static_cast(k)) / (static_cast(fft_size_extended) / 2.0); - counter++; - } + volk_gnsssdr_32f_index_max_32u(&tmp_index_freq, d_fine_doppler_magnitude.data(), d_fine_doppler_fft_size); // 5. Update the Doppler estimation in Hz - if (std::abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz) < 1000) + if (std::abs(d_fine_doppler_freq_bins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz) < 1000) { - d_gnss_synchro->Acq_doppler_hz = static_cast(fftFreqBins[tmp_index_freq]); - // std::cout << "FFT maximum present at " << fftFreqBins[tmp_index_freq] << " [Hz]\n"; + d_gnss_synchro->Acq_doppler_hz = static_cast(d_fine_doppler_freq_bins[tmp_index_freq]); + // std::cout << "FFT maximum present at " << d_fine_doppler_freq_bins[tmp_index_freq] << " [Hz]\n"; } else { - DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << std::abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz); + DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << std::abs(d_fine_doppler_freq_bins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz); DLOG(INFO) << "Error estimating fine frequency Doppler"; } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h index 97e471bdc..09c26c004 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h @@ -163,6 +163,7 @@ private: float compute_CAF(); void reset_grid(); void update_carrier_wipeoff(); + void update_fine_doppler_freq_bins(); bool start() override; arma::fmat grid_; @@ -185,6 +186,10 @@ private: int d_well_count; int d_n_samples_in_buffer; const int d_fft_size; + const int d_fine_doppler_zero_padding_factor; + const int d_fine_doppler_prn_replicas; + const int d_fine_doppler_signal_samples; + const int d_fine_doppler_fft_size; int d_gnuradio_forecast_samples; unsigned int d_channel; unsigned int d_dump_channel; @@ -195,12 +200,16 @@ private: std::weak_ptr d_channel_fsm; std::unique_ptr d_fft_if; std::unique_ptr d_ifft; + std::unique_ptr d_fine_doppler_fft; volk_gnsssdr::vector>> d_grid_doppler_wipeoffs; volk_gnsssdr::vector> d_grid_data; volk_gnsssdr::vector d_fft_codes; volk_gnsssdr::vector d_10_ms_buffer; volk_gnsssdr::vector d_magnitude; + volk_gnsssdr::vector d_fine_doppler_code_replica; + volk_gnsssdr::vector d_fine_doppler_magnitude; + volk_gnsssdr::vector d_fine_doppler_freq_bins; }; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc index bbac7bf02..28fc7f586 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc @@ -66,7 +66,8 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(const Acq_Conf &conf) d_disable_assist(false), d_fft_if(gnss_fft_fwd_make_unique(d_fft_size)), d_ifft(gnss_fft_rev_make_unique(d_fft_size)), - d_fft_codes(d_fft_size) + d_fft_codes(d_fft_size), + d_magnitude(d_fft_size) { this->message_port_register_out(pmt::mp("events")); } @@ -231,16 +232,15 @@ float pcps_assisted_acquisition_cc::search_maximum() } -float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_star &input_items) const +float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_star &input_items) { const auto *in = reinterpret_cast(input_items[0]); // Get the input samples pointer // 1- Compute the input signal power estimation - std::vector p_tmp_vector(d_fft_size); - volk_32fc_magnitude_squared_32f(p_tmp_vector.data(), in, d_fft_size); + volk_32fc_magnitude_squared_32f(d_magnitude.data(), in, d_fft_size); float power; - volk_32f_accumulator_s32f(&power, p_tmp_vector.data(), d_fft_size); + volk_32f_accumulator_s32f(&power, d_magnitude.data(), d_fft_size); return (power / static_cast(d_fft_size)); } @@ -258,8 +258,6 @@ int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_cons << ", doppler_step: " << d_acq_params.doppler_step; // 2- Doppler frequency search loop - std::vector p_tmp_vector(d_fft_size); - for (int32_t doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) { // doppler search steps @@ -277,9 +275,9 @@ int32_t pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_cons d_ifft->execute(); // save the grid matrix delay file - volk_32fc_magnitude_squared_32f(p_tmp_vector.data(), d_ifft->get_outbuf(), d_fft_size); + volk_32fc_magnitude_squared_32f(d_magnitude.data(), d_ifft->get_outbuf(), d_fft_size); const float *old_vector = d_grid_data[doppler_index].data(); - volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), old_vector, p_tmp_vector.data(), d_fft_size); + volk_32f_x2_add_32f(d_grid_data[doppler_index].data(), old_vector, d_magnitude.data(), d_fft_size); } return d_fft_size; } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h index 6a0a1f557..55faced3b 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h @@ -148,7 +148,7 @@ private: void calculate_magnitudes(gr_complex* fft_begin, int32_t doppler_shift, int32_t doppler_offset); int32_t compute_and_accumulate_grid(gr_vector_const_void_star& input_items); - float estimate_input_power(gr_vector_const_void_star& input_items) const; + float estimate_input_power(gr_vector_const_void_star& input_items); float search_maximum(); void get_assistance(); void reset_grid(); @@ -187,6 +187,7 @@ private: std::vector>> d_grid_doppler_wipeoffs; std::vector> d_grid_data; std::vector d_fft_codes; + std::vector d_magnitude; };