2017-02-13 16:20:49 +00:00
/*!
2019-03-01 09:11:36 +00:00
* \ file fpga_multicorrelator . cc
* \ brief FPGA vector correlator class
2017-02-13 16:20:49 +00:00
* \ authors < ul >
2019-02-27 12:30:09 +00:00
* < li > Marc Majoral , 2019. mmajoral ( at ) cttc . cat
2019-03-01 09:11:36 +00:00
* < li > Javier Arribas , 2019. jarribas ( at ) cttc . es
2017-03-13 19:54:25 +00:00
* < / ul >
2017-02-13 16:20:49 +00:00
*
2019-03-01 09:11:36 +00:00
* Class that controls and executes a highly optimized vector correlator
2017-03-13 08:59:16 +00:00
* class in the FPGA
2017-02-13 16:20:49 +00:00
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2017-02-13 16:20:49 +00:00
*
2020-12-30 12:35:06 +00:00
* GNSS - SDR is a Global Navigation Satellite System software - defined receiver .
2017-02-13 16:20:49 +00:00
* This file is part of GNSS - SDR .
*
2020-12-30 12:35:06 +00:00
* Copyright ( C ) 2010 - 2020 ( see AUTHORS file for a list of contributors )
2020-02-08 00:20:02 +00:00
* SPDX - License - Identifier : GPL - 3.0 - or - later
2017-02-13 16:20:49 +00:00
*
2020-07-28 14:57:15 +00:00
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
2017-02-13 16:20:49 +00:00
*/
2018-05-03 08:38:21 +00:00
# include "fpga_multicorrelator.h"
2019-03-02 23:15:20 +00:00
# include <volk_gnsssdr/volk_gnsssdr.h>
2019-02-27 16:27:31 +00:00
# include <cmath>
2019-03-02 23:15:20 +00:00
# include <fcntl.h> // for O_RDWR, O_RSYNC
2019-02-27 16:27:31 +00:00
# include <string>
2019-03-02 23:15:20 +00:00
# include <sys/mman.h> // for PROT_READ, PROT_WRITE, MAP_SHARED
2018-12-10 21:59:10 +00:00
# include <utility>
2017-05-23 16:12:22 +00:00
2024-04-29 06:27:33 +00:00
# if USE_GLOG_AND_GFLAGS
# include <glog/logging.h>
# else
# include <absl/log/log.h>
# endif
2019-03-01 14:29:43 +00:00
# ifndef TEMP_FAILURE_RETRY
# define TEMP_FAILURE_RETRY(exp) \
( { \
decltype ( exp ) _rc ; \
do \
{ \
_rc = ( exp ) ; \
} \
while ( _rc = = - 1 & & errno = = EINTR ) ; \
_rc ; \
} )
# endif
2017-02-13 16:20:49 +00:00
2019-08-14 00:34:00 +00:00
// floating point math constants related to the parameters that are written in the FPGA
const float PHASE_CARR_MAX_DIV_PI = 683565275.5764316 ; // 2^(31)/pi
const float TWO_PI = 6.283185307179586 ;
2019-03-01 09:11:36 +00:00
Fpga_Multicorrelator_8sc : : Fpga_Multicorrelator_8sc ( int32_t n_correlators ,
2019-10-27 20:34:19 +00:00
int32_t * ca_codes ,
int32_t * data_codes ,
uint32_t code_length_chips ,
bool track_pilot ,
2019-07-22 17:58:37 +00:00
uint32_t code_samples_per_chip )
2021-10-11 13:13:12 +00:00
: d_initial_sample_counter ( 0 ) ,
d_corr_out ( nullptr ) ,
d_Prompt_Data ( nullptr ) ,
d_shifts_chips ( nullptr ) ,
d_prompt_data_shift ( nullptr ) ,
2023-12-21 12:57:41 +00:00
d_rem_code_phase_chips ( 0.0 ) ,
d_code_phase_step_chips ( 0.0 ) ,
d_code_phase_rate_step_chips ( 0.0 ) ,
d_rem_carrier_phase_in_rad ( 0.0 ) ,
d_phase_step_rad ( 0.0 ) ,
d_carrier_phase_rate_step_rad ( 0.0 ) ,
2021-10-11 13:13:12 +00:00
d_code_length_samples ( code_length_chips * code_samples_per_chip ) ,
d_n_correlators ( n_correlators ) ,
d_device_descriptor ( 0 ) ,
d_map_base ( nullptr ) ,
d_correlator_length_samples ( 0 ) ,
d_code_phase_step_chips_num ( 0 ) ,
2023-12-21 12:57:41 +00:00
d_code_phase_rate_step_chips_num ( 0 ) ,
2021-10-11 13:13:12 +00:00
d_rem_carr_phase_rad_int ( 0 ) ,
d_phase_step_rad_int ( 0 ) ,
2023-12-21 12:57:41 +00:00
d_carrier_phase_rate_step_rad_int ( 0 ) ,
2021-10-11 13:13:12 +00:00
d_ca_codes ( ca_codes ) ,
d_data_codes ( data_codes ) ,
2023-12-21 12:57:41 +00:00
d_secondary_code_0_length ( 0 ) ,
d_secondary_code_1_length ( 0 ) ,
2021-10-11 13:13:12 +00:00
d_track_pilot ( track_pilot ) ,
d_secondary_code_enabled ( false )
2019-03-01 09:11:36 +00:00
{
// instantiate variable length vectors
if ( d_track_pilot )
{
2021-10-01 10:52:29 +00:00
d_initial_index = volk_gnsssdr : : vector < uint32_t > ( n_correlators + 1 ) ;
d_initial_interp_counter = volk_gnsssdr : : vector < uint32_t > ( n_correlators + 1 ) ;
2019-03-01 09:11:36 +00:00
}
else
{
2021-10-01 10:52:29 +00:00
d_initial_index = volk_gnsssdr : : vector < uint32_t > ( n_correlators ) ;
d_initial_interp_counter = volk_gnsssdr : : vector < uint32_t > ( n_correlators ) ;
2019-03-01 09:11:36 +00:00
}
2019-06-18 16:22:01 +00:00
2019-03-01 09:11:36 +00:00
DLOG ( INFO ) < < " TRACKING FPGA CLASS CREATED " ;
}
Fpga_Multicorrelator_8sc : : ~ Fpga_Multicorrelator_8sc ( )
{
close_device ( ) ;
}
2019-02-28 20:45:30 +00:00
uint64_t Fpga_Multicorrelator_8sc : : read_sample_counter ( )
2018-03-02 16:40:13 +00:00
{
2019-08-12 22:19:31 +00:00
uint64_t sample_counter_tmp ;
uint64_t sample_counter_msw_tmp ;
2019-07-22 17:58:37 +00:00
sample_counter_tmp = d_map_base [ sample_counter_reg_addr_lsw ] ;
sample_counter_msw_tmp = d_map_base [ sample_counter_reg_addr_msw ] ;
2018-08-11 10:56:52 +00:00
sample_counter_msw_tmp = sample_counter_msw_tmp < < 32 ;
sample_counter_tmp = sample_counter_tmp + sample_counter_msw_tmp ; // 2^32
return sample_counter_tmp ;
2018-03-02 16:40:13 +00:00
}
2019-03-01 09:11:36 +00:00
2019-02-28 20:45:30 +00:00
void Fpga_Multicorrelator_8sc : : set_initial_sample ( uint64_t samples_offset )
2017-03-13 08:59:16 +00:00
{
2017-03-13 19:54:25 +00:00
d_initial_sample_counter = samples_offset ;
2019-07-22 17:58:37 +00:00
d_map_base [ initial_counter_value_reg_addr_lsw ] = ( d_initial_sample_counter & 0xFFFFFFFF ) ;
d_map_base [ initial_counter_value_reg_addr_msw ] = ( d_initial_sample_counter > > 32 ) & 0xFFFFFFFF ;
2017-03-13 08:59:16 +00:00
}
2018-04-05 13:05:46 +00:00
2019-03-01 09:11:36 +00:00
2019-02-28 20:45:30 +00:00
void Fpga_Multicorrelator_8sc : : set_local_code_and_taps ( float * shifts_chips , float * prompt_data_shift , int32_t PRN )
2018-08-11 10:56:52 +00:00
{
2017-02-13 16:20:49 +00:00
d_shifts_chips = shifts_chips ;
2018-08-01 13:55:40 +00:00
d_prompt_data_shift = prompt_data_shift ;
2019-02-28 20:45:30 +00:00
Fpga_Multicorrelator_8sc : : fpga_configure_tracking_gps_local_code ( PRN ) ;
2017-02-13 16:20:49 +00:00
}
2019-03-01 09:11:36 +00:00
2019-02-28 20:45:30 +00:00
void Fpga_Multicorrelator_8sc : : set_output_vectors ( gr_complex * corr_out , gr_complex * Prompt_Data )
2017-02-13 16:20:49 +00:00
{
d_corr_out = corr_out ;
2018-05-22 10:25:14 +00:00
d_Prompt_Data = Prompt_Data ;
2017-02-13 16:20:49 +00:00
}
2019-03-01 09:11:36 +00:00
2019-03-25 18:45:54 +00:00
void Fpga_Multicorrelator_8sc : : update_local_code ( )
2017-02-13 16:20:49 +00:00
{
2019-02-28 20:45:30 +00:00
Fpga_Multicorrelator_8sc : : fpga_compute_code_shift_parameters ( ) ;
Fpga_Multicorrelator_8sc : : fpga_configure_code_parameters_in_fpga ( ) ;
2017-02-13 16:20:49 +00:00
}
2017-08-18 10:45:47 +00:00
2019-02-28 20:45:30 +00:00
void Fpga_Multicorrelator_8sc : : Carrier_wipeoff_multicorrelator_resampler (
2019-04-10 09:31:34 +00:00
float rem_carrier_phase_in_rad , // nco phase initial position
float phase_step_rad , // nco phase step
float carrier_phase_rate_step_rad __attribute__ ( ( unused ) ) , // nco phase step rate change
float rem_code_phase_chips , // code resampler initial position
float code_phase_step_chips __attribute__ ( ( unused ) ) , // code resampler step
float code_phase_rate_step_chips __attribute__ ( ( unused ) ) , // code resampler step rate
int32_t signal_length_samples ) // number of samples
2017-02-13 16:20:49 +00:00
{
2019-04-10 09:31:34 +00:00
d_rem_carrier_phase_in_rad = rem_carrier_phase_in_rad ; // nco phase initial position
d_phase_step_rad = phase_step_rad ; // nco phase step
d_carrier_phase_rate_step_rad = carrier_phase_rate_step_rad ; // nco phase step rate
d_rem_code_phase_chips = rem_code_phase_chips ; // code resampler initial position
d_code_phase_step_chips = code_phase_step_chips ; // code resampler step
d_code_phase_rate_step_chips = code_phase_rate_step_chips ; // code resampler step rate
d_correlator_length_samples = signal_length_samples ; // number of samples
2019-03-25 18:45:54 +00:00
Fpga_Multicorrelator_8sc : : update_local_code ( ) ;
2019-02-28 20:45:30 +00:00
Fpga_Multicorrelator_8sc : : fpga_compute_signal_parameters_in_fpga ( ) ;
Fpga_Multicorrelator_8sc : : fpga_configure_signal_parameters_in_fpga ( ) ;
Fpga_Multicorrelator_8sc : : fpga_launch_multicorrelator_fpga ( ) ;
2018-08-11 10:56:52 +00:00
int32_t irq_count ;
2017-03-13 08:59:16 +00:00
ssize_t nb ;
2017-06-08 15:29:45 +00:00
nb = read ( d_device_descriptor , & irq_count , sizeof ( irq_count ) ) ;
2017-03-13 19:54:25 +00:00
if ( nb ! = sizeof ( irq_count ) )
{
2020-07-07 16:53:50 +00:00
std : : cout < < " Tracking_module Read failed to retrieve 4 bytes! \n " ;
std : : cout < < " Tracking_module Interrupt number " < < irq_count < < ' \n ' ;
2017-03-13 19:54:25 +00:00
}
2019-06-18 16:22:01 +00:00
// release secondary code indices, keep channel locked
if ( d_secondary_code_enabled = = true )
2019-07-19 09:55:53 +00:00
{
2019-07-22 17:58:37 +00:00
d_map_base [ drop_samples_reg_addr ] = enable_secondary_code ; // keep secondary code enabled
2019-07-19 09:55:53 +00:00
}
2019-06-18 16:22:01 +00:00
else
2019-07-19 09:55:53 +00:00
{
2019-07-22 17:58:37 +00:00
d_map_base [ drop_samples_reg_addr ] = 0 ; // block samples
2019-07-19 09:55:53 +00:00
}
2019-02-28 20:45:30 +00:00
Fpga_Multicorrelator_8sc : : read_tracking_gps_results ( ) ;
2017-02-13 16:20:49 +00:00
}
2017-08-18 10:45:47 +00:00
2019-02-28 20:45:30 +00:00
bool Fpga_Multicorrelator_8sc : : free ( )
2017-02-13 16:20:49 +00:00
{
2018-03-02 16:40:13 +00:00
// unlock the channel
2019-02-28 20:45:30 +00:00
Fpga_Multicorrelator_8sc : : unlock_channel ( ) ;
2017-02-13 16:20:49 +00:00
return true ;
}
2017-08-18 10:45:47 +00:00
2021-01-25 23:14:50 +00:00
void Fpga_Multicorrelator_8sc : : open_channel ( const std : : string & device_io_name , uint32_t channel )
2017-03-13 08:59:16 +00:00
{
2020-07-07 16:53:50 +00:00
std : : cout < < " trk device_io_name = " < < device_io_name < < ' \n ' ;
2018-08-01 13:55:40 +00:00
2020-11-27 09:51:06 +00:00
if ( ( d_device_descriptor = open ( device_io_name . c_str ( ) , O_RDWR | O_SYNC ) ) = = - 1 )
2017-06-08 15:29:45 +00:00
{
LOG ( WARNING ) < < " Cannot open deviceio " < < device_io_name ;
2020-07-07 16:53:50 +00:00
std : : cout < < " Cannot open deviceio " < < device_io_name < < ' \n ' ;
2017-06-08 15:29:45 +00:00
}
2022-04-19 13:53:00 +00:00
d_map_base = reinterpret_cast < volatile uint32_t * > ( mmap ( nullptr , FPGA_PAGE_SIZE ,
2018-08-11 10:56:52 +00:00
PROT_READ | PROT_WRITE , MAP_SHARED , d_device_descriptor , 0 ) ) ;
if ( d_map_base = = reinterpret_cast < void * > ( - 1 ) )
2017-03-13 19:54:25 +00:00
{
2017-06-08 15:29:45 +00:00
LOG ( WARNING ) < < " Cannot map the FPGA tracking module "
2020-11-27 09:51:06 +00:00
< < channel < < " into user memory " ;
2020-07-07 16:53:50 +00:00
std : : cout < < " Cannot map deviceio " < < device_io_name < < ' \n ' ;
2017-03-13 19:54:25 +00:00
}
2017-03-13 08:59:16 +00:00
2019-03-02 23:15:20 +00:00
// sanity check: check test register
2019-07-23 13:20:59 +00:00
uint32_t writeval = test_register_track_writeval ;
2018-08-11 10:56:52 +00:00
uint32_t readval ;
2019-02-28 20:45:30 +00:00
readval = Fpga_Multicorrelator_8sc : : fpga_acquisition_test_register ( writeval ) ;
2017-03-13 08:59:16 +00:00
if ( writeval ! = readval )
2017-03-13 19:54:25 +00:00
{
LOG ( WARNING ) < < " Test register sanity check failed " ;
2020-07-07 16:53:50 +00:00
std : : cout < < " Tracking test register sanity check failed \n " ;
2017-03-13 19:54:25 +00:00
}
2017-03-13 08:59:16 +00:00
else
2017-03-13 19:54:25 +00:00
{
LOG ( INFO ) < < " Test register sanity check success ! " ;
}
2017-03-13 08:59:16 +00:00
}
2017-08-18 10:45:47 +00:00
2019-10-27 21:42:58 +00:00
uint32_t Fpga_Multicorrelator_8sc : : fpga_acquisition_test_register ( uint32_t writeval )
2017-03-13 08:59:16 +00:00
{
2018-08-11 10:56:52 +00:00
uint32_t readval = 0 ;
2017-03-13 19:54:25 +00:00
// write value to test register
2019-07-22 17:58:37 +00:00
d_map_base [ test_reg_addr ] = writeval ;
2017-03-13 19:54:25 +00:00
// read value from test register
2019-07-22 17:58:37 +00:00
readval = d_map_base [ test_reg_addr ] ;
2017-03-13 19:54:25 +00:00
// return read value
return readval ;
2017-03-13 08:59:16 +00:00
}
2017-08-18 10:45:47 +00:00
2019-02-28 20:45:30 +00:00
void Fpga_Multicorrelator_8sc : : fpga_configure_tracking_gps_local_code ( int32_t PRN )
2017-03-13 08:59:16 +00:00
{
2018-08-11 10:56:52 +00:00
uint32_t k ;
2019-07-23 13:20:59 +00:00
d_map_base [ prog_mems_addr ] = local_code_fpga_clear_address_counter ;
2019-03-25 18:45:54 +00:00
for ( k = 0 ; k < d_code_length_samples ; k + + )
2018-08-11 10:56:52 +00:00
{
2019-07-22 17:58:37 +00:00
d_map_base [ prog_mems_addr ] = d_ca_codes [ ( d_code_length_samples * ( PRN - 1 ) ) + k ] ;
2018-08-11 10:56:52 +00:00
}
2018-08-01 13:55:40 +00:00
if ( d_track_pilot )
2017-03-13 19:54:25 +00:00
{
2019-07-23 13:20:59 +00:00
d_map_base [ prog_mems_addr ] = local_code_fpga_clear_address_counter ;
2019-03-25 18:45:54 +00:00
for ( k = 0 ; k < d_code_length_samples ; k + + )
2017-03-13 19:54:25 +00:00
{
2019-07-22 17:58:37 +00:00
d_map_base [ prog_mems_addr ] = d_data_codes [ ( d_code_length_samples * ( PRN - 1 ) ) + k ] ;
2017-03-13 19:54:25 +00:00
}
}
2019-03-25 18:45:54 +00:00
2019-10-27 21:42:58 +00:00
d_map_base [ code_length_minus_1_reg_addr ] = d_code_length_samples - 1 ; // number of samples - 1
2017-03-13 08:59:16 +00:00
}
2017-08-18 10:45:47 +00:00
2019-08-14 00:14:29 +00:00
void Fpga_Multicorrelator_8sc : : fpga_compute_code_shift_parameters ( )
2017-03-13 08:59:16 +00:00
{
2019-03-25 18:45:54 +00:00
float frac_part ; // decimal part
int32_t dec_part ; // fractional part
2019-04-04 17:55:02 +00:00
for ( uint32_t i = 0 ; i < d_n_correlators ; i + + )
2017-03-13 19:54:25 +00:00
{
2019-09-13 06:56:37 +00:00
dec_part = std : : floor ( d_shifts_chips [ i ] - d_rem_code_phase_chips ) ;
2018-08-01 13:55:40 +00:00
2019-03-25 18:45:54 +00:00
if ( dec_part < 0 )
2017-03-13 19:54:25 +00:00
{
2019-03-25 18:45:54 +00:00
dec_part = dec_part + d_code_length_samples ; // % operator does not work as in Matlab with negative numbers
2017-03-13 19:54:25 +00:00
}
2019-03-25 18:45:54 +00:00
d_initial_index [ i ] = dec_part ;
frac_part = fmod ( d_shifts_chips [ i ] - d_rem_code_phase_chips , 1.0 ) ;
if ( frac_part < 0 )
2017-03-13 19:54:25 +00:00
{
2020-07-10 19:13:17 +00:00
frac_part = frac_part + 1.0F ; // fmod operator does not work as in Matlab with negative numbers
2017-03-13 19:54:25 +00:00
}
2018-08-01 13:55:40 +00:00
2019-09-13 06:56:37 +00:00
d_initial_interp_counter [ i ] = static_cast < uint32_t > ( std : : floor ( max_code_resampler_counter * frac_part ) ) ;
2017-03-13 19:54:25 +00:00
}
2018-08-01 13:55:40 +00:00
if ( d_track_pilot )
{
2019-09-13 06:56:37 +00:00
dec_part = std : : floor ( d_prompt_data_shift [ 0 ] - d_rem_code_phase_chips ) ;
2018-08-01 13:55:40 +00:00
2019-03-25 18:45:54 +00:00
if ( dec_part < 0 )
2018-08-01 13:55:40 +00:00
{
2019-03-25 18:45:54 +00:00
dec_part = dec_part + d_code_length_samples ; // % operator does not work as in Matlab with negative numbers
2018-08-01 13:55:40 +00:00
}
2019-03-25 18:45:54 +00:00
d_initial_index [ d_n_correlators ] = dec_part ;
frac_part = fmod ( d_prompt_data_shift [ 0 ] - d_rem_code_phase_chips , 1.0 ) ;
if ( frac_part < 0 )
2018-08-01 13:55:40 +00:00
{
2020-07-10 19:13:17 +00:00
frac_part = frac_part + 1.0F ; // fmod operator does not work as in Matlab with negative numbers
2018-08-01 13:55:40 +00:00
}
2019-09-13 06:56:37 +00:00
d_initial_interp_counter [ d_n_correlators ] = static_cast < uint32_t > ( std : : floor ( max_code_resampler_counter * frac_part ) ) ;
2017-03-13 19:54:25 +00:00
}
2017-03-13 08:59:16 +00:00
}
2017-08-18 10:45:47 +00:00
2019-08-14 00:14:29 +00:00
void Fpga_Multicorrelator_8sc : : fpga_configure_code_parameters_in_fpga ( )
2017-03-13 08:59:16 +00:00
{
2019-04-04 17:55:02 +00:00
for ( uint32_t i = 0 ; i < d_n_correlators ; i + + )
2017-03-13 19:54:25 +00:00
{
2019-07-22 17:58:37 +00:00
d_map_base [ initial_index_reg_base_addr + i ] = d_initial_index [ i ] ;
d_map_base [ initial_interp_counter_reg_base_addr + i ] = d_initial_interp_counter [ i ] ;
2017-03-13 19:54:25 +00:00
}
2018-08-01 13:55:40 +00:00
if ( d_track_pilot )
{
2019-07-22 17:58:37 +00:00
d_map_base [ initial_index_reg_base_addr + d_n_correlators ] = d_initial_index [ d_n_correlators ] ;
d_map_base [ initial_interp_counter_reg_base_addr + d_n_correlators ] = d_initial_interp_counter [ d_n_correlators ] ;
2017-03-13 19:54:25 +00:00
}
2017-03-13 08:59:16 +00:00
}
2017-08-18 10:45:47 +00:00
2019-08-14 00:14:29 +00:00
void Fpga_Multicorrelator_8sc : : fpga_compute_signal_parameters_in_fpga ( )
2017-03-13 08:59:16 +00:00
{
2017-03-13 19:54:25 +00:00
float d_rem_carrier_phase_in_rad_temp ;
2019-10-27 21:42:58 +00:00
d_code_phase_step_chips_num = static_cast < uint32_t > ( std : : roundf ( max_code_resampler_counter * d_code_phase_step_chips ) ) ;
d_code_phase_rate_step_chips_num = static_cast < uint32_t > ( std : : roundf ( max_code_resampler_counter * d_code_phase_rate_step_chips ) ) ;
2018-08-01 13:55:40 +00:00
2017-03-13 19:54:25 +00:00
if ( d_rem_carrier_phase_in_rad > M_PI )
{
2019-03-25 18:45:54 +00:00
d_rem_carrier_phase_in_rad_temp = - TWO_PI + d_rem_carrier_phase_in_rad ;
2017-03-13 19:54:25 +00:00
}
2017-06-08 15:29:45 +00:00
else if ( d_rem_carrier_phase_in_rad < - M_PI )
2017-03-13 19:54:25 +00:00
{
2019-03-25 18:45:54 +00:00
d_rem_carrier_phase_in_rad_temp = TWO_PI + d_rem_carrier_phase_in_rad ;
2017-03-13 19:54:25 +00:00
}
else
{
d_rem_carrier_phase_in_rad_temp = d_rem_carrier_phase_in_rad ;
}
2019-10-27 21:42:58 +00:00
d_rem_carr_phase_rad_int = static_cast < int32_t > ( std : : roundf ( d_rem_carrier_phase_in_rad_temp * PHASE_CARR_MAX_DIV_PI ) ) ;
d_phase_step_rad_int = static_cast < int32_t > ( std : : roundf ( d_phase_step_rad * PHASE_CARR_MAX_DIV_PI ) ) ; // the FPGA accepts a range for the phase step between -pi and +pi
d_carrier_phase_rate_step_rad_int = static_cast < int32_t > ( std : : roundf ( d_carrier_phase_rate_step_rad * PHASE_CARR_MAX_DIV_PI ) ) ;
2017-03-13 08:59:16 +00:00
}
2017-03-13 19:54:25 +00:00
2017-08-18 10:45:47 +00:00
2019-08-14 00:14:29 +00:00
void Fpga_Multicorrelator_8sc : : fpga_configure_signal_parameters_in_fpga ( )
2017-03-13 08:59:16 +00:00
{
2019-07-22 17:58:37 +00:00
d_map_base [ code_phase_step_chips_num_reg_addr ] = d_code_phase_step_chips_num ; // code phase step
2019-04-10 09:31:34 +00:00
2019-07-22 17:58:37 +00:00
d_map_base [ code_phase_step_chips_rate_reg_addr ] = d_code_phase_rate_step_chips_num ; // code phase step rate
2019-04-10 09:31:34 +00:00
2019-07-22 17:58:37 +00:00
d_map_base [ nsamples_minus_1_reg_addr ] = d_correlator_length_samples - 1 ; // number of samples
2018-08-01 13:55:40 +00:00
2019-07-22 17:58:37 +00:00
d_map_base [ rem_carr_phase_rad_reg_addr ] = d_rem_carr_phase_rad_int ; // initial nco phase
2018-08-01 13:55:40 +00:00
2019-07-22 17:58:37 +00:00
d_map_base [ phase_step_rad_reg_addr ] = d_phase_step_rad_int ; // nco phase step
2018-08-01 13:55:40 +00:00
2019-07-22 17:58:37 +00:00
d_map_base [ phase_step_rate_reg_addr ] = d_carrier_phase_rate_step_rad_int ; // nco phase step rate
2017-03-13 08:59:16 +00:00
}
2017-08-18 10:45:47 +00:00
2019-08-14 00:14:29 +00:00
void Fpga_Multicorrelator_8sc : : fpga_launch_multicorrelator_fpga ( )
2017-03-13 08:59:16 +00:00
{
2019-04-25 13:52:23 +00:00
// enable interrupts
int32_t reenable = 1 ;
ssize_t nbytes = TEMP_FAILURE_RETRY ( write ( d_device_descriptor , reinterpret_cast < void * > ( & reenable ) , sizeof ( int32_t ) ) ) ;
if ( nbytes ! = sizeof ( int32_t ) )
{
2020-07-07 16:53:50 +00:00
std : : cerr < < " Error launching the FPGA multicorrelator \n " ;
2019-04-25 13:52:23 +00:00
}
2018-08-11 10:56:52 +00:00
// writing 1 to reg 14 launches the tracking
2019-07-22 17:58:37 +00:00
d_map_base [ start_flag_addr ] = 1 ;
2017-03-13 08:59:16 +00:00
}
2017-08-18 10:45:47 +00:00
2019-08-14 00:14:29 +00:00
void Fpga_Multicorrelator_8sc : : read_tracking_gps_results ( )
2017-03-13 08:59:16 +00:00
{
2018-08-11 10:56:52 +00:00
int32_t readval_real ;
int32_t readval_imag ;
2017-03-13 19:54:25 +00:00
2019-04-04 17:55:02 +00:00
for ( uint32_t k = 0 ; k < d_n_correlators ; k + + )
2017-03-13 19:54:25 +00:00
{
2019-07-22 17:58:37 +00:00
readval_real = d_map_base [ result_reg_real_base_addr + k ] ;
readval_imag = d_map_base [ result_reg_imag_base_addr + k ] ;
2018-08-11 10:56:52 +00:00
d_corr_out [ k ] = gr_complex ( readval_real , readval_imag ) ;
2017-03-13 19:54:25 +00:00
}
2018-08-01 13:55:40 +00:00
if ( d_track_pilot )
{
2019-07-22 17:58:37 +00:00
readval_real = d_map_base [ result_reg_real_base_addr + d_n_correlators ] ;
readval_imag = d_map_base [ result_reg_imag_base_addr + d_n_correlators ] ;
2018-08-11 10:56:52 +00:00
d_Prompt_Data [ 0 ] = gr_complex ( readval_real , readval_imag ) ;
2017-03-13 19:54:25 +00:00
}
2017-03-13 08:59:16 +00:00
}
2017-08-18 10:45:47 +00:00
2019-08-14 00:14:29 +00:00
void Fpga_Multicorrelator_8sc : : unlock_channel ( )
2017-03-13 08:59:16 +00:00
{
2017-03-13 19:54:25 +00:00
// unlock the channel to let the next samples go through
2019-07-22 17:58:37 +00:00
d_map_base [ drop_samples_reg_addr ] = drop_samples ; // unlock the channel and disable secondary codes
d_map_base [ stop_tracking_reg_addr ] = 1 ; // set the tracking module back to idle
2019-07-19 09:55:53 +00:00
d_secondary_code_enabled = false ;
2017-03-13 08:59:16 +00:00
}
2017-05-18 15:10:28 +00:00
2019-03-01 09:11:36 +00:00
2019-02-28 20:45:30 +00:00
void Fpga_Multicorrelator_8sc : : close_device ( )
2018-03-02 16:40:13 +00:00
{
2018-12-10 18:05:12 +00:00
auto * aux = const_cast < uint32_t * > ( d_map_base ) ;
2022-04-19 13:53:00 +00:00
if ( munmap ( static_cast < void * > ( aux ) , FPGA_PAGE_SIZE ) = = - 1 )
2018-03-02 16:40:13 +00:00
{
2020-07-07 16:53:50 +00:00
std : : cout < < " Failed to unmap memory uio \n " ;
2018-03-02 16:40:13 +00:00
}
close ( d_device_descriptor ) ;
}
2018-08-11 10:56:52 +00:00
2017-08-18 10:45:47 +00:00
2019-08-14 00:14:29 +00:00
void Fpga_Multicorrelator_8sc : : lock_channel ( )
2017-05-18 15:10:28 +00:00
{
2017-06-08 15:29:45 +00:00
// lock the channel for processing
2019-07-22 17:58:37 +00:00
d_map_base [ drop_samples_reg_addr ] = 0 ; // lock the channel
2017-05-18 15:10:28 +00:00
}
2019-06-18 16:22:01 +00:00
void Fpga_Multicorrelator_8sc : : set_secondary_code_lengths ( uint32_t secondary_code_0_length , uint32_t secondary_code_1_length )
{
2019-07-19 09:55:53 +00:00
d_secondary_code_0_length = secondary_code_0_length ;
d_secondary_code_1_length = secondary_code_1_length ;
2019-06-18 16:22:01 +00:00
2019-07-19 09:55:53 +00:00
uint32_t secondary_code_length_0_minus_1 = d_secondary_code_0_length - 1 ;
uint32_t secondary_code_length_1_minus_1 = d_secondary_code_1_length - 1 ;
2019-06-18 16:22:01 +00:00
2019-07-22 17:58:37 +00:00
d_map_base [ secondary_code_lengths_reg_addr ] = secondary_code_length_1_minus_1 * 256 + secondary_code_length_0_minus_1 ;
2019-06-18 16:22:01 +00:00
}
2019-10-27 20:34:19 +00:00
2019-07-23 17:05:43 +00:00
void Fpga_Multicorrelator_8sc : : update_prn_code_length ( uint32_t first_prn_length , uint32_t next_prn_length )
2019-06-18 16:22:01 +00:00
{
2019-07-23 17:05:43 +00:00
d_map_base [ first_prn_length_minus_1_reg_addr ] = first_prn_length - 1 ;
d_map_base [ next_prn_length_minus_1_reg_addr ] = next_prn_length - 1 ;
2019-06-18 16:22:01 +00:00
}
2019-10-27 20:34:19 +00:00
2019-06-18 16:22:01 +00:00
void Fpga_Multicorrelator_8sc : : initialize_secondary_code ( uint32_t secondary_code , std : : string * secondary_code_string )
{
2019-07-19 09:55:53 +00:00
uint32_t secondary_code_length ;
uint32_t reg_addr ;
if ( secondary_code = = 0 )
{
secondary_code_length = d_secondary_code_0_length ;
2019-07-22 17:58:37 +00:00
reg_addr = prog_secondary_code_0_data_reg_addr ;
2019-07-19 09:55:53 +00:00
}
else
{
secondary_code_length = d_secondary_code_1_length ;
2019-07-22 17:58:37 +00:00
reg_addr = prog_secondary_code_1_data_reg_addr ;
2019-07-19 09:55:53 +00:00
}
Fpga_Multicorrelator_8sc : : write_secondary_code ( secondary_code_length , secondary_code_string , reg_addr ) ;
2019-06-18 16:22:01 +00:00
}
void Fpga_Multicorrelator_8sc : : write_secondary_code ( uint32_t secondary_code_length , std : : string * secondary_code_string , uint32_t reg_addr )
{
2019-09-13 06:56:37 +00:00
uint32_t num_words = std : : ceil ( static_cast < float > ( secondary_code_length ) / secondary_code_word_size ) ;
2019-07-22 17:58:37 +00:00
uint32_t last_word_size = secondary_code_length % secondary_code_word_size ;
2019-06-18 16:22:01 +00:00
2019-07-19 09:55:53 +00:00
if ( last_word_size = = 0 )
{
2019-07-22 17:58:37 +00:00
last_word_size = secondary_code_word_size ;
2019-07-19 09:55:53 +00:00
}
2019-06-18 16:22:01 +00:00
2019-07-19 09:55:53 +00:00
uint32_t write_val = 0U ;
uint32_t pow_k ;
uint32_t mem_addr ;
if ( num_words > 1 )
{
for ( mem_addr = 0 ; mem_addr < num_words - 1 ; mem_addr + + )
{
write_val = 0U ;
pow_k = 1 ;
2019-07-22 17:58:37 +00:00
for ( unsigned int k = 0 ; k < secondary_code_word_size ; k + + )
2019-07-19 09:55:53 +00:00
{
2019-10-27 21:42:58 +00:00
std : : string string_tmp ( 1 , ( * secondary_code_string ) [ mem_addr * secondary_code_word_size + k ] ) ;
2019-07-19 09:55:53 +00:00
write_val = write_val | std : : stoi ( string_tmp ) * pow_k ;
pow_k = pow_k * 2 ;
}
2019-07-22 17:58:37 +00:00
write_val = write_val | mem_addr * secondary_code_addr_bits | secondary_code_wr_strobe ;
2019-07-19 09:55:53 +00:00
d_map_base [ reg_addr ] = write_val ;
}
}
write_val = 0U ;
pow_k = 1 ;
mem_addr = num_words - 1 ;
for ( unsigned int k = 0 ; k < last_word_size ; k + + )
{
2019-10-27 21:42:58 +00:00
std : : string string_tmp ( 1 , ( * secondary_code_string ) [ mem_addr * secondary_code_word_size + k ] ) ;
2019-07-19 09:55:53 +00:00
write_val = write_val | std : : stoi ( string_tmp ) * pow_k ;
pow_k = pow_k * 2 ;
}
2019-07-22 17:58:37 +00:00
write_val = write_val | ( mem_addr * secondary_code_addr_bits ) | ( secondary_code_wr_strobe ) ;
2019-07-19 09:55:53 +00:00
d_map_base [ reg_addr ] = write_val ;
2019-06-18 16:22:01 +00:00
}
2019-10-27 20:34:19 +00:00
2019-06-18 16:22:01 +00:00
void Fpga_Multicorrelator_8sc : : enable_secondary_codes ( )
{
2019-07-22 17:58:37 +00:00
d_map_base [ drop_samples_reg_addr ] = init_secondary_code_addresses | enable_secondary_code ; // enable secondary codes and clear secondary code indices
2019-07-19 09:55:53 +00:00
d_secondary_code_enabled = true ;
2019-06-18 16:22:01 +00:00
}
2019-10-27 20:34:19 +00:00
2019-06-18 16:22:01 +00:00
void Fpga_Multicorrelator_8sc : : disable_secondary_codes ( )
{
2019-07-19 09:55:53 +00:00
// this function is to be called before starting the tracking process in order to disable the secondary codes by default
2019-07-22 17:58:37 +00:00
d_map_base [ drop_samples_reg_addr ] = drop_samples ;
2019-06-18 16:22:01 +00:00
}