From 89c00560f298ebc0c5d75dc115c014a43114384e Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Sat, 7 Jul 2018 15:39:09 +0200 Subject: [PATCH] Fix Acquisition fine Doppler implementation --- .../pcps_acquisition_fine_doppler_cc.cc | 41 ++++++++----- .../pcps_acquisition_fine_doppler_cc.h | 2 - src/tests/common-files/tracking_tests_flags.h | 1 + ...gps_l1_ca_dll_pll_tracking_pull-in_test.cc | 61 +++++++++++++------ 4 files changed, 72 insertions(+), 33 deletions(-) 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 629f53c75..69fedeb6f 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 @@ -63,10 +63,8 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(const Acq_Con d_active = false; d_fs_in = conf_.fs_in; d_samples_per_ms = conf_.samples_per_ms; - d_sampled_ms = conf_.sampled_ms; d_config_doppler_max = conf_.doppler_max; - d_config_doppler_min = -conf_.doppler_max; - d_fft_size = d_sampled_ms * d_samples_per_ms; + d_fft_size = d_samples_per_ms; // HS Acquisition d_max_dwells = conf_.max_dwells; d_gnuradio_forecast_samples = d_fft_size; @@ -75,7 +73,7 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(const Acq_Con d_fft_codes = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); d_magnitude = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment())); - d_10_ms_buffer = static_cast(volk_gnsssdr_malloc(10 * d_samples_per_ms * sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_10_ms_buffer = static_cast(volk_gnsssdr_malloc(50 * d_samples_per_ms * sizeof(gr_complex), volk_gnsssdr_get_alignment())); // Direct FFT d_fft_if = new gr::fft::fft_complex(d_fft_size, true); @@ -126,7 +124,7 @@ void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_ste d_doppler_step = doppler_step; // Create the search grid array - d_num_doppler_points = floor(std::abs(d_config_doppler_max - d_config_doppler_min) / d_doppler_step); + d_num_doppler_points = floor(std::abs(2 * d_config_doppler_max) / d_doppler_step); d_grid_data = new float *[d_num_doppler_points]; for (int i = 0; i < d_num_doppler_points; i++) @@ -222,7 +220,7 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff() d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_points]; for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) { - doppler_hz = d_config_doppler_min + d_doppler_step * doppler_index; + doppler_hz = d_doppler_step * doppler_index - d_config_doppler_max; // doppler search steps // compute the carrier doppler wipe-off signal and store it phase_step_rad = static_cast(GPS_TWO_PI) * doppler_hz / static_cast(d_fs_in); @@ -295,7 +293,7 @@ double pcps_acquisition_fine_doppler_cc::compute_CAF() // 4- record the maximum peak and the associated synchronization parameters d_gnss_synchro->Acq_delay_samples = static_cast(index_time); - d_gnss_synchro->Acq_doppler_hz = static_cast(index_doppler * d_doppler_step + d_config_doppler_min); + d_gnss_synchro->Acq_doppler_hz = static_cast(index_doppler * d_doppler_step - d_config_doppler_max); d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; return d_test_statistics; @@ -333,6 +331,7 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons // doppler search steps // Perform the carrier wipe-off 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) // Compute the FFT of the carrier wiped--off incoming signal d_fft_if->execute(); @@ -352,6 +351,14 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons volk_gnsssdr_free(p_tmp_vector); return d_fft_size; + //debug + // std::cout << "iff=["; + // for (int n = 0; n < d_fft_size; n++) + // { + // std::cout << std::real(d_ifft->get_outbuf()[n]) << "+" << std::imag(d_ifft->get_outbuf()[n]) << "i,"; + // } + // std::cout << "]\n"; + // getchar(); } @@ -495,6 +502,7 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items, if (d_active == true) { reset_grid(); + d_n_samples_in_buffer = 0; d_state = 1; } if (!acq_parameters.blocking_on_standby) @@ -505,6 +513,8 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items, break; case 1: // S1. ComputeGrid compute_and_accumulate_grid(input_items); + memcpy(&d_10_ms_buffer[d_n_samples_in_buffer], reinterpret_cast(input_items[0]), d_fft_size * sizeof(gr_complex)); + d_n_samples_in_buffer += d_fft_size; d_well_count++; if (d_well_count >= d_max_dwells) { @@ -522,10 +532,9 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items, else { d_state = 5; //negative acquisition + d_n_samples_in_buffer = 0; } - d_n_samples_in_buffer = 0; - d_sample_counter += d_fft_size; // sample counter - consume_each(d_fft_size); + break; case 3: // Fine doppler estimation samples_remaining = 10 * d_samples_per_ms - d_n_samples_in_buffer; @@ -539,10 +548,14 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items, } else { - memcpy(&d_10_ms_buffer[d_n_samples_in_buffer], reinterpret_cast(input_items[0]), samples_remaining * sizeof(gr_complex)); - estimate_Doppler(); //disabled in repo - d_sample_counter += samples_remaining; // sample counter - consume_each(samples_remaining); + if (samples_remaining > 0) + { + memcpy(&d_10_ms_buffer[d_n_samples_in_buffer], reinterpret_cast(input_items[0]), samples_remaining * sizeof(gr_complex)); + d_sample_counter += samples_remaining; // sample counter + consume_each(samples_remaining); + } + estimate_Doppler(); //disabled in repo + d_n_samples_in_buffer = 0; d_state = 4; } break; 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 69a5ff2cc..4bdf583d5 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 @@ -94,11 +94,9 @@ private: float d_threshold; std::string d_satellite_str; int d_config_doppler_max; - int d_config_doppler_min; int d_num_doppler_points; int d_doppler_step; - unsigned int d_sampled_ms; unsigned int d_fft_size; unsigned long int d_sample_counter; gr_complex* d_carrier; diff --git a/src/tests/common-files/tracking_tests_flags.h b/src/tests/common-files/tracking_tests_flags.h index dc54da91e..275c90017 100644 --- a/src/tests/common-files/tracking_tests_flags.h +++ b/src/tests/common-files/tracking_tests_flags.h @@ -61,6 +61,7 @@ DEFINE_double(acq_Delay_error_chips_start, 2.0, "Acquisition Code Delay error st DEFINE_double(acq_Delay_error_chips_stop, -2.0, "Acquisition Code Delay error stop sweep value [Chips]"); DEFINE_double(acq_Delay_error_chips_step, -0.1, "Acquisition Code Delay error sweep step value [Chips]"); +DEFINE_int64(skip_samples, 0, "Skip an initial transitory in the processed signal file capture [samples]"); DEFINE_int32(plot_detail_level, 0, "Specify the desired plot detail (0,1,2): 0 - Minimum plots (default) 2 - Plot all tracking parameters"); diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_pull-in_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_pull-in_test.cc index e449fa2e0..0ca01547d 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_pull-in_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_pull-in_test.cc @@ -40,10 +40,12 @@ #include #include #include +#include #include #include "GPS_L1_CA.h" #include "gnss_block_factory.h" #include "tracking_interface.h" +#include "gps_l1_ca_pcps_acquisition.h" #include "gps_l1_ca_pcps_acquisition_fine_doppler.h" #include "in_memory_configuration.h" #include "tracking_true_obs_reader.h" @@ -71,6 +73,7 @@ private: public: int rx_message; + gr::top_block_sptr top_block; ~Acquisition_msg_rx(); //!< Default destructor }; @@ -87,6 +90,7 @@ void Acquisition_msg_rx::msg_handler_events(pmt::pmt_t msg) { long int message = pmt::to_long(msg); rx_message = message; + top_block->stop(); //stop the flowgraph } catch (boost::bad_any_cast& e) { @@ -335,16 +339,36 @@ bool GpsL1CADllPllTrackingPullInTest::acquire_GPS_L1CA_signal(int SV_ID) config->set_property("GNSS-SDR.internal_fs_sps", std::to_string(baseband_sampling_freq)); config->set_property("Acquisition.max_dwells", "10"); + config->set_property("Acquisition.blocking_on_standby", "true"); config->set_property("Acquisition.dump", "true"); - GNSSBlockFactory block_factory; + config->set_property("Acquisition.dump_filename", "./data/acquisition.dat"); + + config->set_property("Acquisition.use_CFAR_algorithm", "false"); GpsL1CaPcpsAcquisitionFineDoppler* acquisition; - acquisition = new GpsL1CaPcpsAcquisitionFineDoppler(config.get(), "Acquisition", 1, 1); + acquisition = new GpsL1CaPcpsAcquisitionFineDoppler(config.get(), "Acquisition", 1, 0); + + //GpsL1CaPcpsAcquisition* acquisition; + //acquisition = new GpsL1CaPcpsAcquisition(config.get(), "Acquisition", 1, 0); acquisition->set_channel(1); acquisition->set_gnss_synchro(&tmp_gnss_synchro); acquisition->set_threshold(config->property("Acquisition.threshold", FLAGS_external_signal_acquisition_threshold)); - acquisition->set_doppler_max(config->property("Acquisition.doppler_max", 10000)); + acquisition->set_doppler_max(config->property("Acquisition.doppler_max", 25000)); acquisition->set_doppler_step(config->property("Acquisition.doppler_step", 500)); + acquisition->connect(top_block); + + + gr::blocks::file_source::sptr file_source; + std::string file = FLAGS_signal_file; + const char* file_name = file.c_str(); + file_source = gr::blocks::file_source::make(sizeof(int8_t), file_name, false); + file_source->seek(2 * FLAGS_skip_samples, 0); //skip head. ibyte, two bytes per complex sample + gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make(); + gr::blocks::head::sptr head_samples = gr::blocks::head::make(sizeof(gr_complex), baseband_sampling_freq * FLAGS_duration); + + top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0); + top_block->connect(gr_interleaved_char_to_complex, 0, head_samples, 0); + top_block->connect(head_samples, 0, acquisition->get_left_block(), 0); boost::shared_ptr msg_rx; try @@ -357,15 +381,8 @@ bool GpsL1CADllPllTrackingPullInTest::acquire_GPS_L1CA_signal(int SV_ID) exit(0); } - gr::blocks::file_source::sptr file_source; - std::string file = FLAGS_signal_file; - const char* file_name = file.c_str(); - file_source = gr::blocks::file_source::make(sizeof(int8_t), file_name, false); - gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make(); - gr::blocks::null_sink::sptr sink = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); - top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0); - top_block->connect(gr_interleaved_char_to_complex, 0, acquisition->get_left_block(), 0); - top_block->msg_connect(acquisition->get_left_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); + msg_rx->top_block = top_block; + top_block->msg_connect(acquisition->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); // 5. Run the flowgraph // Get visible GPS satellites (positive acquisitions with Doppler measurements) @@ -380,6 +397,7 @@ bool GpsL1CADllPllTrackingPullInTest::acquire_GPS_L1CA_signal(int SV_ID) code_delay_measurements_map.clear(); acq_samplestamp_map.clear(); + for (unsigned int PRN = 1; PRN < 33; PRN++) { tmp_gnss_synchro.PRN = PRN; @@ -387,6 +405,7 @@ bool GpsL1CADllPllTrackingPullInTest::acquire_GPS_L1CA_signal(int SV_ID) acquisition->init(); acquisition->set_local_code(); acquisition->reset(); + acquisition->set_state(1); msg_rx->rx_message = 0; top_block->run(); if (start_msg == true) @@ -412,10 +431,17 @@ bool GpsL1CADllPllTrackingPullInTest::acquire_GPS_L1CA_signal(int SV_ID) std::cout << " . "; } top_block->stop(); - file_source->seek(0, 0); + file_source->seek(2 * FLAGS_skip_samples, 0); //skip head. ibyte, two bytes per complex sample + head_samples.reset(); std::cout.flush(); } std::cout << "]" << std::endl; + std::cout << "-------------------------------------------\n"; + + for (auto& x : doppler_measurements_map) + { + std::cout << "DETECTED PRN: " << x.first << " with Doppler: " << x.second << " [Hz], code phase: " << code_delay_measurements_map.at(x.first) << " [samples] at signal SampleStamp " << acq_samplestamp_map.at(x.first) << "\n"; + } // report the elapsed time end = std::chrono::system_clock::now(); @@ -587,19 +613,20 @@ TEST_F(GpsL1CADllPllTrackingPullInTest, ValidationOfResults) gr::blocks::file_source::sptr file_source = gr::blocks::file_source::make(sizeof(int8_t), file_name, false); gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make(); gr::blocks::null_sink::sptr sink = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); + gr::blocks::head::sptr head_samples = gr::blocks::head::make(sizeof(gr_complex), baseband_sampling_freq * FLAGS_duration); top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0); - top_block->connect(gr_interleaved_char_to_complex, 0, tracking->get_left_block(), 0); + top_block->connect(gr_interleaved_char_to_complex, 0, head_samples, 0); + top_block->connect(head_samples, 0, tracking->get_left_block(), 0); top_block->connect(tracking->get_right_block(), 0, sink, 0); top_block->msg_connect(tracking->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events")); - - file_source->seek(acq_samplestamp_samples, 0); + file_source->seek(2 * FLAGS_skip_samples + acq_samplestamp_samples, 0); //skip head. ibyte, two bytes per complex sample }) << "Failure connecting the blocks of tracking test."; //******************************************************************** //***** STEP 5: Perform the signal tracking and read the results ***** //******************************************************************** - std::cout << "------------ START TRACKING -------------" << std::endl; + std::cout << "--- START TRACKING WITH PULL-IN ERROR: " << acq_doppler_error_hz_values.at(current_acq_doppler_error_idx) << " [Hz] and " << acq_delay_error_chips_values.at(current_acq_doppler_error_idx).at(current_acq_code_error_idx) << " [Chips] ---" << std::endl; tracking->start_tracking(); std::chrono::time_point start, end; EXPECT_NO_THROW({