2011-10-01 18:45:20 +00:00
/*!
2012-07-12 21:17:37 +00:00
* \ file pcps_acquisition_cc . cc
2012-09-12 15:03:38 +00:00
* \ brief This class implements a Parallel Code Phase Search Acquisition
* \ authors < ul >
* < li > Javier Arribas , 2011. jarribas ( at ) cttc . es
* < li > Luis Esteve , 2012. luis ( at ) epsilon - formacion . com
* < / ul >
2011-10-01 18:45:20 +00:00
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*
2012-10-19 16:00:40 +00:00
* Copyright ( C ) 2010 - 2012 ( see AUTHORS file for a list of contributors )
2011-10-01 18:45:20 +00:00
*
* GNSS - SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS - SDR .
*
* GNSS - SDR is free software : you can redistribute it and / or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation , either version 3 of the License , or
* at your option ) any later version .
*
* GNSS - SDR is distributed in the hope that it will be useful ,
* but WITHOUT ANY WARRANTY ; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
* GNU General Public License for more details .
*
* You should have received a copy of the GNU General Public License
* along with GNSS - SDR . If not , see < http : //www.gnu.org/licenses/>.
*
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
*/
2012-07-12 21:17:37 +00:00
# include "pcps_acquisition_cc.h"
# include "gnss_signal_processing.h"
2011-10-01 18:45:20 +00:00
# include "control_message_factory.h"
2013-07-04 13:47:40 +00:00
# include <gnuradio/io_signature.h>
2011-10-01 18:45:20 +00:00
# include <sstream>
# include <glog/log_severity.h>
# include <glog/logging.h>
2012-09-04 14:29:07 +00:00
# include <volk/volk.h>
2011-10-01 18:45:20 +00:00
using google : : LogMessage ;
2012-07-12 21:17:37 +00:00
pcps_acquisition_cc_sptr pcps_make_acquisition_cc (
2011-10-01 18:45:20 +00:00
unsigned int sampled_ms , unsigned int doppler_max , long freq ,
2013-07-04 13:47:40 +00:00
long fs_in , int samples_per_ms , gr : : msg_queue : : sptr queue , bool dump ,
2011-10-01 18:45:20 +00:00
std : : string dump_filename )
{
2012-07-12 21:17:37 +00:00
return pcps_acquisition_cc_sptr (
new pcps_acquisition_cc ( sampled_ms , doppler_max , freq ,
2011-10-01 18:45:20 +00:00
fs_in , samples_per_ms , queue , dump , dump_filename ) ) ;
}
2012-10-19 16:00:40 +00:00
2012-07-12 21:17:37 +00:00
pcps_acquisition_cc : : pcps_acquisition_cc (
2011-10-01 18:45:20 +00:00
unsigned int sampled_ms , unsigned int doppler_max , long freq ,
2013-07-04 13:47:40 +00:00
long fs_in , int samples_per_ms , gr : : msg_queue : : sptr queue , bool dump ,
2011-10-01 18:45:20 +00:00
std : : string dump_filename ) :
2013-07-04 13:47:40 +00:00
gr : : block ( " pcps_acquisition_cc " ,
gr : : io_signature : : make ( 1 , 1 , sizeof ( gr_complex ) * sampled_ms * samples_per_ms ) ,
gr : : io_signature : : make ( 0 , 0 , sizeof ( gr_complex ) * sampled_ms * samples_per_ms ) )
2011-10-01 18:45:20 +00:00
{
2012-01-16 18:27:31 +00:00
d_sample_counter = 0 ; // SAMPLE COUNTER
2011-10-01 18:45:20 +00:00
d_active = false ;
d_queue = queue ;
d_freq = freq ;
d_fs_in = fs_in ;
d_samples_per_ms = samples_per_ms ;
d_sampled_ms = sampled_ms ;
d_doppler_max = doppler_max ;
d_fft_size = d_sampled_ms * d_samples_per_ms ;
d_mag = 0 ;
d_input_power = 0.0 ;
2012-10-18 08:33:59 +00:00
//todo: do something if posix_memalign fails
if ( posix_memalign ( ( void * * ) & d_carrier , 16 , d_fft_size * sizeof ( gr_complex ) ) = = 0 ) { } ;
if ( posix_memalign ( ( void * * ) & d_fft_codes , 16 , d_fft_size * sizeof ( gr_complex ) ) = = 0 ) { } ;
2011-10-01 18:45:20 +00:00
// Direct FFT
2012-11-25 11:15:11 +00:00
d_fft_if = new gr : : fft : : fft_complex ( d_fft_size , true ) ;
2011-10-01 18:45:20 +00:00
// Inverse FFT
2012-11-25 11:15:11 +00:00
d_ifft = new gr : : fft : : fft_complex ( d_fft_size , false ) ;
2011-10-01 18:45:20 +00:00
2012-10-19 16:00:40 +00:00
// For dumping samples into a file
2012-01-16 18:27:31 +00:00
d_dump = dump ;
d_dump_filename = dump_filename ;
2011-10-01 18:45:20 +00:00
}
2012-01-27 11:58:55 +00:00
2012-10-19 16:00:40 +00:00
pcps_acquisition_cc : : ~ pcps_acquisition_cc ( )
{
free ( d_carrier ) ;
free ( d_fft_codes ) ;
delete d_ifft ;
delete d_fft_if ;
2011-10-01 18:45:20 +00:00
if ( d_dump )
2012-10-19 16:00:40 +00:00
{
d_dump_file . close ( ) ;
}
2011-10-01 18:45:20 +00:00
}
2012-10-19 16:00:40 +00:00
2012-07-12 21:17:37 +00:00
void pcps_acquisition_cc : : set_local_code ( std : : complex < float > * code )
{
2013-07-04 13:47:40 +00:00
memcpy ( d_fft_if - > get_inbuf ( ) , code , sizeof ( gr_complex ) * d_fft_size ) ;
2012-07-12 21:17:37 +00:00
}
2012-01-16 18:27:31 +00:00
2012-10-19 16:00:40 +00:00
2012-07-12 21:17:37 +00:00
void pcps_acquisition_cc : : init ( )
2011-10-01 18:45:20 +00:00
{
2012-10-19 16:00:40 +00:00
d_gnss_synchro - > Acq_delay_samples = 0.0 ;
d_gnss_synchro - > Acq_doppler_hz = 0.0 ;
d_gnss_synchro - > Acq_samplestamp_samples = 0 ;
2011-11-26 14:20:47 +00:00
d_mag = 0.0 ;
2011-10-01 18:45:20 +00:00
d_input_power = 0.0 ;
2012-07-12 21:17:37 +00:00
d_fft_if - > execute ( ) ; // We need the FFT of local code
2012-01-23 00:52:05 +00:00
2011-10-01 18:45:20 +00:00
//Conjugate the local code
for ( unsigned int i = 0 ; i < d_fft_size ; i + + )
{
2012-01-16 18:27:31 +00:00
d_fft_codes [ i ] = std : : complex < float > ( conj ( d_fft_if - > get_outbuf ( ) [ i ] ) ) ;
2011-10-01 18:45:20 +00:00
}
}
2012-01-16 18:27:31 +00:00
2012-07-12 21:17:37 +00:00
int pcps_acquisition_cc : : general_work ( int noutput_items ,
2011-10-01 18:45:20 +00:00
gr_vector_int & ninput_items , gr_vector_const_void_star & input_items ,
gr_vector_void_star & output_items )
{
2012-01-16 18:27:31 +00:00
/*
2012-07-12 21:17:37 +00:00
* By J . Arribas and L . Esteve
2012-01-16 18:27:31 +00:00
* Acquisition strategy ( Kay Borre book + CFAR threshold ) :
2011-12-27 21:21:12 +00:00
* 1. Compute the input signal power estimation
* 2. Doppler serial search loop
* 3. Perform the FFT - based circular convolution ( parallel time search )
* 4. Record the maximum peak and the associated synchronization parameters
* 5. Compute the test statistics and compare to the threshold
* 6. Declare positive or negative acquisition using a message queue
2011-10-01 18:45:20 +00:00
*/
if ( ! d_active )
{
2012-01-16 18:27:31 +00:00
d_sample_counter + = d_fft_size * noutput_items ; // sample counter
consume_each ( noutput_items ) ;
2011-10-01 18:45:20 +00:00
}
2012-01-16 18:27:31 +00:00
else
{
// initialize acquisition algorithm
int doppler ;
unsigned int indext = 0 ;
float magt = 0.0 ;
float tmp_magt = 0.0 ;
const gr_complex * in = ( const gr_complex * ) input_items [ 0 ] ; //Get the input samples pointer
bool positive_acquisition = false ;
int acquisition_message = - 1 ; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
//aux vars
unsigned int i ;
2012-09-04 14:29:07 +00:00
float fft_normalization_factor ;
2012-10-19 16:00:40 +00:00
d_sample_counter + = d_fft_size ; // sample counter
//restart acquisition variables
d_gnss_synchro - > Acq_delay_samples = 0.0 ;
d_gnss_synchro - > Acq_doppler_hz = 0.0 ;
d_mag = 0.0 ;
d_input_power = 0.0 ;
2012-01-16 18:27:31 +00:00
DLOG ( INFO ) < < " Channel: " < < d_channel
2013-07-04 13:47:40 +00:00
< < " , doing acquisition of satellite: " < < d_gnss_synchro - > System
< < " " < < d_gnss_synchro - > PRN
< < " ,sample stamp: " < < d_sample_counter < < " , threshold: "
< < d_threshold < < " , doppler_max: " < < d_doppler_max
< < " , doppler_step: " < < d_doppler_step ;
2011-10-01 18:45:20 +00:00
2012-01-16 18:27:31 +00:00
// 1- Compute the input signal power estimation
for ( i = 0 ; i < d_fft_size ; i + + )
{
d_input_power + = std : : norm ( in [ i ] ) ;
}
d_input_power = d_input_power / ( float ) d_fft_size ;
// 2- Doppler frequency search loop
2013-05-16 16:25:51 +00:00
for ( doppler = ( int ) ( - d_doppler_max ) ; doppler < = ( int ) d_doppler_max ; doppler + = d_doppler_step )
2012-01-16 18:27:31 +00:00
{
2012-10-19 16:00:40 +00:00
// doppler search steps
2013-05-26 23:59:04 +00:00
//TODO: Create a run time lookup table with all the required Doppler wipe-off LO signals, by allocating a memory space (aligned) and by initializing it when the Doppler step is assigned
// then, use the arrays to multiply the incomming signal, instead of computing the sine and cosine online.
2012-10-19 16:00:40 +00:00
// Perform the carrier wipe-off
2012-10-18 10:24:41 +00:00
complex_exp_gen_conj ( d_carrier , d_freq + doppler , d_fs_in , d_fft_size ) ;
2012-10-18 08:33:59 +00:00
if ( is_unaligned ( ) = = true )
2012-10-19 16:00:40 +00:00
{
volk_32fc_x2_multiply_32fc_u ( d_fft_if - > get_inbuf ( ) , in , d_carrier , d_fft_size ) ;
}
else
{
//use directly the input vector
volk_32fc_x2_multiply_32fc_a ( d_fft_if - > get_inbuf ( ) , in , d_carrier , d_fft_size ) ;
}
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
2012-01-16 18:27:31 +00:00
d_fft_if - > execute ( ) ;
2012-09-04 14:29:07 +00:00
2012-10-19 16:00:40 +00:00
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library
2012-09-04 14:29:07 +00:00
volk_32fc_x2_multiply_32fc_a ( d_ifft - > get_inbuf ( ) , d_fft_if - > get_outbuf ( ) , d_fft_codes , d_fft_size ) ;
2012-10-19 16:00:40 +00:00
// compute the inverse FFT
2012-01-16 18:27:31 +00:00
d_ifft - > execute ( ) ;
// Search maximum
indext = 0 ;
magt = 0 ;
2012-10-19 16:00:40 +00:00
fft_normalization_factor = ( float ) d_fft_size * ( float ) d_fft_size ;
2012-01-16 18:27:31 +00:00
for ( i = 0 ; i < d_fft_size ; i + + )
{
tmp_magt = std : : norm ( d_ifft - > get_outbuf ( ) [ i ] ) ;
if ( tmp_magt > magt )
{
magt = tmp_magt ;
indext = i ;
}
}
2012-09-04 14:29:07 +00:00
// Normalize the maximum value to correct the scale factor introduced by FFTW
2012-10-19 16:00:40 +00:00
magt = magt / ( fft_normalization_factor * fft_normalization_factor ) ;
// 4- record the maximum peak and the associated synchronization parameters
if ( d_mag < magt )
{
d_mag = magt ;
d_gnss_synchro - > Acq_delay_samples = ( double ) indext ;
d_gnss_synchro - > Acq_doppler_hz = ( double ) doppler ;
}
2012-09-04 14:29:07 +00:00
2012-10-19 16:00:40 +00:00
// Record results to file if required
2012-01-16 18:27:31 +00:00
if ( d_dump )
{
std : : stringstream filename ;
std : : streamsize n = 2 * sizeof ( float ) * ( d_fft_size ) ; // complex file write
filename . str ( " " ) ;
2012-10-19 16:00:40 +00:00
filename < < " ../data/test_statistics_ " < < d_gnss_synchro - > System
2013-07-04 13:47:40 +00:00
< < " _ " < < d_gnss_synchro - > Signal < < " _sat_ "
< < d_gnss_synchro - > PRN < < " _doppler_ " < < doppler < < " .dat " ;
d_dump_file . open ( filename . str ( ) . c_str ( ) , std : : ios : : out | std : : ios : : binary ) ;
2012-07-12 21:17:37 +00:00
d_dump_file . write ( ( char * ) d_ifft - > get_outbuf ( ) , n ) ; //write directly |abs(x)|^2 in this Doppler bin?
2012-01-16 18:27:31 +00:00
d_dump_file . close ( ) ;
}
}
// 5- Compute the test statistics and compare to the threshold
2013-05-26 23:59:04 +00:00
//d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
d_test_statistics = d_mag / d_input_power ;
2012-01-16 18:27:31 +00:00
// 6- Declare positive or negative acquisition using a message queue
if ( d_test_statistics > d_threshold )
{
positive_acquisition = true ;
2012-01-27 18:01:17 +00:00
d_gnss_synchro - > Acq_samplestamp_samples = d_sample_counter ;
2012-01-16 18:27:31 +00:00
DLOG ( INFO ) < < " positive acquisition " ;
2012-10-19 16:00:40 +00:00
DLOG ( INFO ) < < " satellite " < < d_gnss_synchro - > System < < " " < < d_gnss_synchro - > PRN ;
2012-01-16 18:27:31 +00:00
DLOG ( INFO ) < < " sample_stamp " < < d_sample_counter ;
DLOG ( INFO ) < < " test statistics value " < < d_test_statistics ;
DLOG ( INFO ) < < " test statistics threshold " < < d_threshold ;
2012-01-27 11:58:55 +00:00
DLOG ( INFO ) < < " code phase " < < d_gnss_synchro - > Acq_delay_samples ;
DLOG ( INFO ) < < " doppler " < < d_gnss_synchro - > Acq_doppler_hz ;
2012-01-16 18:27:31 +00:00
DLOG ( INFO ) < < " magnitude " < < d_mag ;
DLOG ( INFO ) < < " input signal power " < < d_input_power ;
}
else
{
DLOG ( INFO ) < < " negative acquisition " ;
2012-10-19 16:00:40 +00:00
DLOG ( INFO ) < < " satellite " < < d_gnss_synchro - > System < < " " < < d_gnss_synchro - > PRN ;
2012-01-16 18:27:31 +00:00
DLOG ( INFO ) < < " sample_stamp " < < d_sample_counter ;
DLOG ( INFO ) < < " test statistics value " < < d_test_statistics ;
DLOG ( INFO ) < < " test statistics threshold " < < d_threshold ;
2012-01-27 11:58:55 +00:00
DLOG ( INFO ) < < " code phase " < < d_gnss_synchro - > Acq_delay_samples ;
DLOG ( INFO ) < < " doppler " < < d_gnss_synchro - > Acq_doppler_hz ;
2012-01-16 18:27:31 +00:00
DLOG ( INFO ) < < " magnitude " < < d_mag ;
DLOG ( INFO ) < < " input signal power " < < d_input_power ;
}
d_active = false ;
if ( positive_acquisition )
{
acquisition_message = 1 ;
}
else
{
acquisition_message = 2 ;
}
d_channel_internal_queue - > push ( acquisition_message ) ;
consume_each ( 1 ) ;
2011-10-01 18:45:20 +00:00
}
return 0 ;
}