diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index a0461f9c5..897532983 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -127,13 +127,9 @@ pcps_acquisition_cc::pcps_acquisition_cc( // For dumping samples into a file d_dump = dump; d_dump_filename = dump_filename; - d_gnss_synchro = 0; d_grid_doppler_wipeoffs = 0; - - d_done = false; d_blocking = blocking; - d_new_data_available = false; d_worker_active = false; d_data_buffer = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); } @@ -160,19 +156,6 @@ pcps_acquisition_cc::~pcps_acquisition_cc() { d_dump_file.close(); } - - // Let the worker thread know that we are done and then wait to join - if( d_worker_thread.joinable() ) - { - { - std::lock_guard lk( d_mutex ); - d_done = true; - d_cond.notify_one(); - } - - d_worker_thread.join(); - } - volk_gnsssdr_free( d_data_buffer ); } @@ -233,9 +216,6 @@ void pcps_acquisition_cc::init() int doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, d_freq + doppler); } - - d_new_data_available = false; - d_done = false; d_worker_active = false; } @@ -253,6 +233,7 @@ void pcps_acquisition_cc::set_state(int state) d_mag = 0.0; d_input_power = 0.0; d_test_statistics = 0.0; + d_active = true; } else if (d_state == 0) {} @@ -299,7 +280,7 @@ void pcps_acquisition_cc::send_negative_acquisition() } -int pcps_acquisition_cc::general_work(int noutput_items, +int pcps_acquisition_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused))) { @@ -314,193 +295,157 @@ int pcps_acquisition_cc::general_work(int noutput_items, * 6. Declare positive or negative acquisition using a message port */ - switch (d_state) + gr::thread::scoped_lock lk(d_setlock); + if(!d_active || d_worker_active) + { + d_sample_counter += d_fft_size * ninput_items[0]; + consume_each(ninput_items[0]); + return 0; + } + + switch(d_state) { case 0: { - if (d_active) - { - //restart acquisition variables - d_gnss_synchro->Acq_delay_samples = 0.0; - d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; - d_well_count = 0; - d_mag = 0.0; - d_input_power = 0.0; - d_test_statistics = 0.0; - d_state = 1; - } - + //restart acquisition variables + d_gnss_synchro->Acq_delay_samples = 0.0; + d_gnss_synchro->Acq_doppler_hz = 0.0; + d_gnss_synchro->Acq_samplestamp_samples = 0; + d_well_count = 0; + d_mag = 0.0; + d_input_power = 0.0; + d_test_statistics = 0.0; + d_state = 1; d_sample_counter += d_fft_size * ninput_items[0]; // sample counter consume_each(ninput_items[0]); - break; } case 1: { - std::unique_lock lk( d_mutex ); - - int num_items_consumed = 1; - - if( d_worker_active ) + // Copy the data to the core and let it know that new data is available + memcpy(d_data_buffer, input_items[0], d_fft_size * sizeof(gr_complex)); + if(d_blocking) { - if( d_blocking ) - { - // Should never get here: - std::string msg = "pcps_acquisition_cc: Entered general work with worker active in blocking mode, should never happen"; - LOG(WARNING) << msg; - std::cout << msg << std::endl; - d_cond.wait( lk, [&]{ return !this->d_worker_active; } ); - } - else - { - num_items_consumed = ninput_items[0]; - d_sample_counter += d_fft_size * num_items_consumed; - } + lk.unlock(); + acquisition_core(d_sample_counter); } else { - // Copy the data to the core and let it know that new data is available - memcpy( d_data_buffer, input_items[0], d_fft_size * sizeof( gr_complex ) ); - d_new_data_available = true; - d_cond.notify_one(); - - if( d_blocking ) - { - d_cond.wait( lk, [&]{ return !this->d_new_data_available; } ); - - } + gr::thread::thread d_worker(&pcps_acquisition_cc::acquisition_core, this, d_sample_counter); + d_worker_active = true; } - - consume_each(num_items_consumed); - + d_sample_counter += d_fft_size; + consume_each(1); break; - } // case 1, switch d_state - - } // switch d_state - - return noutput_items; + } + } + return 0; } -void pcps_acquisition_cc::acquisition_core( void ) +void pcps_acquisition_cc::acquisition_core( unsigned long int samp_count ) { - d_worker_active = false; - while( 1 ) + gr::thread::scoped_lock lk(d_setlock); + + // initialize acquisition algorithm + int doppler; + uint32_t indext = 0; + float magt = 0.0; + const gr_complex *in = d_data_buffer; //Get the input samples pointer + + int effective_fft_size = ( d_bit_transition_flag ? d_fft_size/2 : d_fft_size ); + + float fft_normalization_factor = static_cast(d_fft_size) * static_cast(d_fft_size); + + d_input_power = 0.0; + d_mag = 0.0; + d_well_count++; + + DLOG(INFO) << "Channel: " << d_channel + << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN + << " ,sample stamp: " << samp_count << ", threshold: " + << d_threshold << ", doppler_max: " << d_doppler_max + << ", doppler_step: " << d_doppler_step + << ", use_CFAR_algorithm_flag: " << ( d_use_CFAR_algorithm_flag ? "true" : "false" ); + + lk.unlock(); + if (d_use_CFAR_algorithm_flag) { - std::unique_lock lk( d_mutex ); - d_cond.wait( lk, [&]{ return this->d_new_data_available or this->d_done; } ); - d_worker_active = !d_done; - unsigned long int sample_counter = d_sample_counter; // sample counter - lk.unlock(); + // 1- (optional) 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 + for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) + { + // doppler search steps + doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; - if( d_done ) + 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(); + + // Multiply carrier wiped--off, Fourier transformed incoming signal + // with the local FFT'd code reference using SIMD operations with VOLK library + volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); + + // compute the inverse FFT + d_ifft->execute(); + + // Search maximum + size_t offset = ( d_bit_transition_flag ? effective_fft_size : 0 ); + volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf() + offset, effective_fft_size); + volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, effective_fft_size); + magt = d_magnitude[indext]; + + if (d_use_CFAR_algorithm_flag) { - break; + // Normalize the maximum value to correct the scale factor introduced by FFTW + magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); } - - // initialize acquisition algorithm - int doppler; - uint32_t indext = 0; - float magt = 0.0; - const gr_complex *in = d_data_buffer; //Get the input samples pointer - - int effective_fft_size = ( d_bit_transition_flag ? d_fft_size/2 : d_fft_size ); - - float fft_normalization_factor = static_cast(d_fft_size) * static_cast(d_fft_size); - - d_input_power = 0.0; - d_mag = 0.0; - d_well_count++; - - DLOG(INFO) << "Channel: " << d_channel - << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN - << " ,sample stamp: " << sample_counter << ", threshold: " - << d_threshold << ", doppler_max: " << d_doppler_max - << ", doppler_step: " << d_doppler_step - << ", use_CFAR_algorithm_flag: " << ( d_use_CFAR_algorithm_flag ? "true" : "false" ); - - if (d_use_CFAR_algorithm_flag == true) + // 4- record the maximum peak and the associated synchronization parameters + if (d_mag < magt) { - // 1- (optional) 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 - for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) - { - // doppler search steps - doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; + d_mag = magt; - 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(); - - // Multiply carrier wiped--off, Fourier transformed incoming signal - // with the local FFT'd code reference using SIMD operations with VOLK library - volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), - d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); - - // compute the inverse FFT - d_ifft->execute(); - - // Search maximum - size_t offset = ( d_bit_transition_flag ? effective_fft_size : 0 ); - volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf() + offset, effective_fft_size); - volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, effective_fft_size); - magt = d_magnitude[indext]; - - if (d_use_CFAR_algorithm_flag == true) + 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); - } - // 4- record the maximum peak and the associated synchronization parameters - if (d_mag < magt) - { - d_mag = magt; - - if (d_use_CFAR_algorithm_flag == false) - { - // Search grid noise floor approximation for this doppler line - volk_32f_accumulator_s32f(&d_input_power, d_magnitude, effective_fft_size); - d_input_power = (d_input_power - d_mag) / (effective_fft_size - 1); - } - - // In case that d_bit_transition_flag = true, we compare the potentially - // new maximum test statistics (d_mag/d_input_power) with the value in - // d_test_statistics. When the second dwell is being processed, the value - // of d_mag/d_input_power could be lower than d_test_statistics (i.e, - // the maximum test statistics in the previous dwell is greater than - // current d_mag/d_input_power). Note that d_test_statistics is not - // restarted between consecutive dwells in multidwell operation. - - if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) - { - d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); - d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); - d_gnss_synchro->Acq_samplestamp_samples = sample_counter; - - // 5- Compute the test statistics and compare to the threshold - //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; - d_test_statistics = d_mag / d_input_power; - } + // Search grid noise floor approximation for this doppler line + volk_32f_accumulator_s32f(&d_input_power, d_magnitude, effective_fft_size); + d_input_power = (d_input_power - d_mag) / (effective_fft_size - 1); } - // Record results to file if required - if (d_dump) - { - std::stringstream filename; - std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write - filename.str(""); + // In case that d_bit_transition_flag = true, we compare the potentially + // new maximum test statistics (d_mag/d_input_power) with the value in + // d_test_statistics. When the second dwell is being processed, the value + // of d_mag/d_input_power could be lower than d_test_statistics (i.e, + // the maximum test statistics in the previous dwell is greater than + // current d_mag/d_input_power). Note that d_test_statistics is not + // restarted between consecutive dwells in multidwell operation. - boost::filesystem::path p = d_dump_filename; - filename << p.parent_path().string() + if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) + { + d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); + d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); + d_gnss_synchro->Acq_samplestamp_samples = samp_count; + + // 5- Compute the test statistics and compare to the threshold + //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; + d_test_statistics = d_mag / d_input_power; + } + } + // Record results to file if required + if (d_dump) + { + std::stringstream filename; + std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write + filename.str(""); + boost::filesystem::path p = d_dump_filename; + filename << p.parent_path().string() << boost::filesystem::path::preferred_separator << p.stem().string() << "_" << d_gnss_synchro->System @@ -509,15 +454,32 @@ void pcps_acquisition_cc::acquisition_core( void ) << doppler << p.extension().string(); - DLOG(INFO) << "Writing ACQ out to " << filename.str(); + DLOG(INFO) << "Writing ACQ out to " << filename.str(); - d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); - d_dump_file.write(reinterpret_cast(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin? - d_dump_file.close(); - } + d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); + d_dump_file.write(reinterpret_cast(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin? + d_dump_file.close(); } - - if (!d_bit_transition_flag) + } + lk.lock(); + if (!d_bit_transition_flag) + { + if (d_test_statistics > d_threshold) + { + d_state = 0; // Positive acquisition + d_active = false; + send_positive_acquisition(); + } + else if (d_well_count == d_max_dwells) + { + d_state = 0; + d_active = false; + send_negative_acquisition(); + } + } + else + { + if (d_well_count == d_max_dwells) // d_max_dwells = 2 { if (d_test_statistics > d_threshold) { @@ -525,66 +487,13 @@ void pcps_acquisition_cc::acquisition_core( void ) d_active = false; send_positive_acquisition(); } - else if (d_well_count == d_max_dwells) + else { - d_state = 0; + d_state = 0; // Negative acquisition d_active = false; send_negative_acquisition(); } } - else - { - if (d_well_count == d_max_dwells) // d_max_dwells = 2 - { - if (d_test_statistics > d_threshold) - { - d_state = 0; // Positive acquisition - d_active = false; - send_positive_acquisition(); - } - else - { - d_state = 0; // Negative acquisition - d_active = false; - send_negative_acquisition(); - } - } - } - - lk.lock(); - d_worker_active = false; - d_new_data_available = false; - lk.unlock(); - d_cond.notify_one(); } -} - - -bool pcps_acquisition_cc::start( void ) -{ d_worker_active = false; - d_done = false; - - // Start the worker thread and wait for it to acknowledge: - d_worker_thread = std::move( std::thread( &pcps_acquisition_cc::acquisition_core, this ) ); - - return gr::block::start(); } - - -bool pcps_acquisition_cc::stop( void ) -{ - // Let the worker thread know that we are done and then wait to join - if( d_worker_thread.joinable() ) - { - { - std::lock_guard lk( d_mutex ); - d_done = true; - d_cond.notify_one(); - } - - d_worker_thread.join(); - } - return gr::block::stop(); -} - diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index 486468379..bb3db1e43 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -21,6 +21,7 @@ *
  • Luis Esteve, 2012. luis(at)epsilon-formacion.com *
  • Marc Molina, 2013. marc.molina.pena@gmail.com *
  • Cillian O'Driscoll, 2017. cillian(at)ieee.org + *
  • Antonio Ramos, 2017. antonio.ramos@cttc.es * * * ------------------------------------------------------------------------- @@ -53,9 +54,6 @@ #include #include -#include -#include -#include #include #include #include @@ -99,7 +97,7 @@ private: void update_local_carrier(gr_complex* carrier_vector, int correlator_length_samples, float freq); - void acquisition_core( void ); + void acquisition_core( unsigned long int samp_count ); void send_negative_acquisition(); void send_positive_acquisition(); @@ -109,7 +107,6 @@ private: int d_samples_per_code; //unsigned int d_doppler_resolution; float d_threshold; - std::string d_satellite_str; unsigned int d_doppler_max; unsigned int d_doppler_step; unsigned int d_sampled_ms; @@ -137,16 +134,8 @@ private: bool d_dump; unsigned int d_channel; std::string d_dump_filename; - - std::thread d_worker_thread; - std::mutex d_mutex; - - std::condition_variable d_cond; - bool d_done; - bool d_new_data_available; bool d_worker_active; bool d_blocking; - gr_complex *d_data_buffer; public: @@ -251,15 +240,6 @@ public: gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - /*! - * Called by the flowgraph when processing is about to start. - */ - bool start( void ); - - /*! - * Called by the flowgraph when processing is done. - */ - bool stop( void ); }; #endif /* GNSS_SDR_PCPS_ACQUISITION_CC_H_*/