Merge branch 'antonioramosdet-update_pcps_acq_sc' into next

src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc

@@ -35,17 +35,14 @@
 
 
 #include "pcps_acquisition_sc.h"
-#include <sstream>
+#include <cstring>
-#include <boost/filesystem.hpp>
 #include <gnuradio/io_signature.h>
 #include <glog/logging.h>
+#include <matio.h>
 #include <volk/volk.h>
-#include "control_message_factory.h"
 #include "GPS_L1_CA.h" //GPS_TWO_PI
 #include "GLONASS_L1_CA.h" //GLONASS_TWO_PI
 
-
-
 using google::LogMessage;
 
 pcps_acquisition_sc_sptr pcps_make_acquisition_sc(
@@ -88,7 +85,7 @@ pcps_acquisition_sc::pcps_acquisition_sc(
     d_well_count = 0;
     d_doppler_max = doppler_max;
     d_fft_size = d_sampled_ms * d_samples_per_ms;
-    d_mag = 0;
+    d_mag = 0.0;
     d_input_power = 0.0;
     d_num_doppler_bins = 0;
     d_bit_transition_flag = bit_transition_flag;
@@ -98,9 +95,6 @@ pcps_acquisition_sc::pcps_acquisition_sc(
     d_code_phase = 0;
     d_test_statistics = 0.0;
     d_channel = 0;
-    d_doppler_freq = 0.0;
-
-    //set_relative_rate( 1.0/d_fft_size );
 
     // COD:
     // Experimenting with the overlap/save technique for handling bit trannsitions
@@ -133,15 +127,12 @@ pcps_acquisition_sc::pcps_acquisition_sc(
     // 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<lv_16sc_t*>(volk_gnsssdr_malloc(d_fft_size * sizeof(lv_16sc_t), volk_gnsssdr_get_alignment()));
+    grid_ = arma::fmat();
 }
 
 
@@ -155,32 +146,12 @@ pcps_acquisition_sc::~pcps_acquisition_sc()
                 }
             delete[] d_grid_doppler_wipeoffs;
         }
-
     volk_gnsssdr_free(d_fft_codes);
     volk_gnsssdr_free(d_magnitude);
     volk_gnsssdr_free(d_in_32fc);
-
     delete d_ifft;
     delete d_fft_if;
-
+    volk_gnsssdr_free(d_data_buffer);
-    if (d_dump)
-        {
-            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<std::mutex> lk( d_mutex );
-                d_done = true;
-                d_cond.notify_one();
-            }
-
-            d_worker_thread.join();
-        }
-
-    volk_gnsssdr_free( d_data_buffer );
 }
 
 
@@ -263,10 +234,13 @@ void pcps_acquisition_sc::init()
             int doppler = -static_cast<int>(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;
+
+    if(d_dump)
+        {
+            unsigned int effective_fft_size = (d_bit_transition_flag ? (d_fft_size / 2) : d_fft_size);
+            grid_ = arma::fmat(effective_fft_size, d_num_doppler_bins, arma::fill::zeros);
+        }
 }
 
 void pcps_acquisition_sc::update_grid_doppler_wipeoffs()
@@ -296,6 +270,7 @@ void pcps_acquisition_sc::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)
         {}
@@ -342,7 +317,7 @@ void pcps_acquisition_sc::send_negative_acquisition()
 }
 
 
-int pcps_acquisition_sc::general_work(int noutput_items,
+int pcps_acquisition_sc::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)))
 {
@@ -357,11 +332,17 @@ int pcps_acquisition_sc::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;
@@ -372,76 +353,37 @@ int pcps_acquisition_sc::general_work(int noutput_items,
             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<std::mutex> lk( d_mutex );
-
-            int num_items_consumed = 1;
-
-            if( d_worker_active )
-                {
-                    if( d_blocking )
-                        {
-                            // Should never get here:
-                            std::string msg = "pcps_acquisition_sc: 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;
-                        }
-                }
-            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( lv_16sc_t ) );
+            memcpy(d_data_buffer, input_items[0], d_fft_size * sizeof(lv_16sc_t));
-                    d_new_data_available = true;
+            if(d_blocking)
-                    d_cond.notify_one();
-
-                    if( d_blocking )
                 {
-                            d_cond.wait( lk, [&]{ return !this->d_new_data_available; } );
+                    lk.unlock();
-
+                    acquisition_core(d_sample_counter);
                 }
+            else
+                {
+                    gr::thread::thread d_worker(&pcps_acquisition_sc::acquisition_core, this, d_sample_counter);
+                    d_worker_active = true;
                 }
-
+            d_sample_counter += d_fft_size;
-            consume_each(num_items_consumed);
+            consume_each(1);
-
             break;
-        } // case 1, switch d_state
+        }
-
+    }
-    } // switch d_state
+    return 0;
-
-    return noutput_items;
 }
 
 
-void pcps_acquisition_sc::acquisition_core( void )
+void pcps_acquisition_sc::acquisition_core( unsigned long int samp_count )
 {
-    d_worker_active = false;
+    gr::thread::scoped_lock lk(d_setlock);
-    while( 1 )
-        {
-            std::unique_lock<std::mutex> 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();
-
-            if( d_done )
-                {
-                    break;
-                }
 
     // initialize acquisition algorithm
     int doppler;
@@ -451,23 +393,22 @@ void pcps_acquisition_sc::acquisition_core( void )
 
     int effective_fft_size = ( d_bit_transition_flag ? d_fft_size/2 : d_fft_size );
 
-            //TODO: optimize the signal processing chain to not use gr_complex. This is a temporary solution
-            volk_gnsssdr_16ic_convert_32fc(d_in_32fc, in, effective_fft_size);
-
     float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
 
     d_input_power = 0.0;
     d_mag = 0.0;
     d_well_count++;
+    volk_gnsssdr_16ic_convert_32fc(d_in_32fc, in, effective_fft_size);
 
     DLOG(INFO) << "Channel: " << d_channel
                << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN
-                       << " ,sample stamp: " << sample_counter << ", threshold: "
+               << " ,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" );
 
-            if (d_use_CFAR_algorithm_flag == true)
+    lk.unlock();
+    if (d_use_CFAR_algorithm_flag)
         {
             // 1- (optional) Compute the input signal power estimation
             volk_32fc_magnitude_squared_32f(d_magnitude, d_in_32fc, d_fft_size);
@@ -480,8 +421,7 @@ void pcps_acquisition_sc::acquisition_core( void )
             // doppler search steps
             doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
 
-                    volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), d_in_32fc,
+            volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), d_in_32fc, d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
-                            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
@@ -489,8 +429,7 @@ void pcps_acquisition_sc::acquisition_core( void )
 
             // 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(),
+            volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
-                            d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
 
             // compute the inverse FFT
             d_ifft->execute();
@@ -501,7 +440,7 @@ void pcps_acquisition_sc::acquisition_core( void )
             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);
@@ -511,7 +450,7 @@ void pcps_acquisition_sc::acquisition_core( void )
                 {
                     d_mag = magt;
 
-                            if (d_use_CFAR_algorithm_flag == false)
+                    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);
@@ -530,39 +469,57 @@ void pcps_acquisition_sc::acquisition_core( void )
                         {
                             d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
                             d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
-                                    d_gnss_synchro->Acq_samplestamp_samples = sample_counter;
+                            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;
+                    memcpy(grid_.colptr(doppler_index), d_magnitude, sizeof(float) * effective_fft_size);
-                            std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
+                    if(doppler_index == (d_num_doppler_bins - 1))
-                            filename.str("");
+                        {
+                            std::string filename = d_dump_filename;
+                            filename.append("_");
+                            filename.append(1, d_gnss_synchro->System);
+                            filename.append("_");
+                            filename.append(1, d_gnss_synchro->Signal[0]);
+                            filename.append(1, d_gnss_synchro->Signal[1]);
+                            filename.append("_sat_");
+                            filename.append(std::to_string(d_gnss_synchro->PRN));
+                            filename.append(".mat");
+                            mat_t* matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
+                            if(matfp == NULL)
+                                {
+                                    std::cout << "Unable to create or open Acquisition dump file" << std::endl;
+                                    d_dump = false;
+                                }
+                            else
+                                {
+                                    size_t dims[2] = {static_cast<size_t>(effective_fft_size), static_cast<size_t>(d_num_doppler_bins)};
+                                    matvar_t* matvar = Mat_VarCreate("grid", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, grid_.memptr(), 0);
+                                    Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+                                    Mat_VarFree(matvar);
 
-                            boost::filesystem::path p = d_dump_filename;
+                                    dims[0] = static_cast<size_t>(1);
-                            filename << p.parent_path().string()
+                                    dims[1] = static_cast<size_t>(1);
-                            << boost::filesystem::path::preferred_separator
+                                    matvar = Mat_VarCreate("doppler_max", MAT_C_SINGLE, MAT_T_UINT32, 1, dims, &d_doppler_max, 0);
-                            << p.stem().string()
+                                    Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
-                            << "_" << d_gnss_synchro->System
+                                    Mat_VarFree(matvar);
-                            <<"_" << d_gnss_synchro->Signal << "_sat_"
-                            << d_gnss_synchro->PRN << "_doppler_"
-                            <<  doppler
-                            << p.extension().string();
 
-                            DLOG(INFO) << "Writing ACQ out to " << filename.str();
+                                    matvar = Mat_VarCreate("doppler_step", MAT_C_SINGLE, MAT_T_UINT32, 1, dims, &d_doppler_step, 0);
+                                    Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
+                                    Mat_VarFree(matvar);
 
-                            d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
+                                    Mat_Close(matfp);
-                            d_dump_file.write(reinterpret_cast<char*>(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin?
-                            d_dump_file.close();
                                 }
                         }
-
+                }
+        }
+    lk.lock();
     if (!d_bit_transition_flag)
         {
             if (d_test_statistics > d_threshold)
@@ -596,41 +553,5 @@ void pcps_acquisition_sc::acquisition_core( void )
                         }
                 }
         }
-
-            lk.lock();
     d_worker_active = false;
-            d_new_data_available = false;
-            lk.unlock();
-            d_cond.notify_one();
-        }
 }
-
-
-bool pcps_acquisition_sc::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_sc::acquisition_core, this ) );
-
-    return gr::block::start();
-}
-
-
-bool pcps_acquisition_sc::stop( void )
-{
-    // Let the worker thread know that we are done and then wait to join
-    if( d_worker_thread.joinable() )
-        {
-            {
-                std::lock_guard<std::mutex> lk( d_mutex );
-                d_done = true;
-                d_cond.notify_one();
-            }
-
-            d_worker_thread.join();
-        }
-    return gr::block::stop();
-}
-

src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.h

@@ -51,13 +51,9 @@
 #ifndef GNSS_SDR_PCPS_ACQUISITION_SC_H_
 #define GNSS_SDR_PCPS_ACQUISITION_SC_H_
 
-#include <fstream>
 #include <string>
-#include <mutex>
+#include <armadillo>
-#include <thread>
-#include <condition_variable>
 #include <gnuradio/block.h>
-#include <gnuradio/gr_complex.h>
 #include <gnuradio/fft/fft.h>
 #include <volk_gnsssdr/volk_gnsssdr.h>
 #include "gnss_synchro.h"
@@ -101,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 update_grid_doppler_wipeoffs();
     bool is_fdma();
@@ -109,53 +105,42 @@ private:
     void send_negative_acquisition();
     void send_positive_acquisition();
 
+    bool d_bit_transition_flag;
+    bool d_use_CFAR_algorithm_flag;
+    bool d_active;
+    bool d_dump;
+    bool d_worker_active;
+    bool d_blocking;
+    float d_threshold;
+    float d_mag;
+    float d_input_power;
+    float d_test_statistics;
+    float* d_magnitude;
     long d_fs_in;
     long d_freq;
     long d_old_freq;
     int d_samples_per_ms;
     int d_samples_per_code;
-    //unsigned int d_doppler_resolution;
+    int d_state;
-    float d_threshold;
+    unsigned int d_channel;
-    std::string d_satellite_str;
     unsigned int d_doppler_max;
     unsigned int d_doppler_step;
     unsigned int d_sampled_ms;
     unsigned int d_max_dwells;
     unsigned int d_well_count;
     unsigned int d_fft_size;
-    unsigned long int d_sample_counter;
-    gr_complex** d_grid_doppler_wipeoffs;
     unsigned int d_num_doppler_bins;
+    unsigned int d_code_phase;
+    unsigned long int d_sample_counter;
+    lv_16sc_t* d_data_buffer;
+    gr_complex** d_grid_doppler_wipeoffs;
     gr_complex* d_fft_codes;
     gr_complex* d_in_32fc;
     gr::fft::fft_complex* d_fft_if;
     gr::fft::fft_complex* d_ifft;
-    Gnss_Synchro *d_gnss_synchro;
+    Gnss_Synchro* d_gnss_synchro;
-    unsigned int d_code_phase;
-    float d_doppler_freq;
-    float d_mag;
-    float* d_magnitude;
-    float d_input_power;
-    float d_test_statistics;
-    bool d_bit_transition_flag;
-    bool d_use_CFAR_algorithm_flag;
-    std::ofstream d_dump_file;
-    bool d_active;
-    int d_state;
-    bool d_dump;
-    unsigned int d_channel;
     std::string d_dump_filename;
-
+    arma::fmat grid_;
-    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;
-
-    lv_16sc_t *d_data_buffer;
 
 public:
     /*!
@@ -259,15 +244,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_SC_H_*/