mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-15 14:25:00 +00:00
Code Issues Releases Wiki Activity

currently optimizing the FPGA-related code

Browse Source
This commit is contained in:
Marc Majoral 2019-03-25 19:45:54 +01:00
parent 2bae20d2fd
commit 905a85670c
8 changed files with 83 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition_fpga.cc
View File

@ -159,6 +159,9 @@ GalileoE5aPcpsAcquisitionFpga::GalileoE5aPcpsAcquisitionFpga(ConfigurationInterf
{ {
d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast<int32_t>(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)), d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast<int32_t>(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)),
static_cast<int32_t>(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max))); static_cast<int32_t>(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)));
// d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast<int32_t>(256 * floor(fft_codes_padded[i].real() * (pow(2, 7 - 1) - 1) / max)),
// static_cast<int32_t>(256 * floor(fft_codes_padded[i].imag() * (pow(2, 7 - 1) - 1) / max)));
} }
} }

3
src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition_fpga.cc
View File

@ -141,6 +141,9 @@ GpsL5iPcpsAcquisitionFpga::GpsL5iPcpsAcquisitionFpga(
{ {
d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast<int32_t>(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)), d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast<int32_t>(floor(fft_codes_padded[i].real() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)),
static_cast<int32_t>(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max))); static_cast<int32_t>(floor(fft_codes_padded[i].imag() * (pow(2, QUANT_BITS_LOCAL_CODE - 1) - 1) / max)));
// d_all_fft_codes_[i + nsamples_total * (PRN - 1)] = lv_16sc_t(static_cast<int32_t>(128 * floor(fft_codes_padded[i].real() * (pow(2, 7 - 1) - 1) / max)),
// static_cast<int32_t>(128 * floor(fft_codes_padded[i].imag() * (pow(2, 7 - 1) - 1) / max)));
} }
} }

10
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fpga.cc
View File

@ -220,11 +220,11 @@ void pcps_acquisition_fpga::acquisition_core(uint32_t num_doppler_bins, uint32_t
} }
// debug // debug
//if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
// { {
// printf("firstpeak = %f, secondpeak = %f, test_statistics = %f reported block exp = %d PRN = %d inext = %d, initial_sample = %ld doppler = %d\n", firstpeak, secondpeak, d_test_statistics, (int)total_block_exp, (int)d_gnss_synchro->PRN, (int)indext, (long int)initial_sample, (int)doppler); printf("firstpeak = %f, secondpeak = %f, test_statistics = %f reported block exp = %d PRN = %d inext = %d, initial_sample = %ld doppler = %d\n", firstpeak, secondpeak, d_test_statistics, (int)total_block_exp, (int)d_gnss_synchro->PRN, (int)indext, (long int)initial_sample, (int)doppler);
// printf("doppler_min = %d doppler_step = %d num_doppler_bins = %d\n", (int)doppler_min, (int)doppler_step, (int)num_doppler_bins); printf("doppler_min = %d doppler_step = %d num_doppler_bins = %d\n", (int)doppler_min, (int)doppler_step, (int)num_doppler_bins);
// } }
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler); d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_sample_counter = initial_sample; d_sample_counter = initial_sample;

83
src/algorithms/acquisition/libs/fpga_acquisition.cc
View File

@ -53,6 +53,7 @@
#define MEM_LOCAL_CODE_WR_ENABLE 0x0C000000 // command to enable the ENA and WR pins of the internal memory of the multicorrelator #define MEM_LOCAL_CODE_WR_ENABLE 0x0C000000 // command to enable the ENA and WR pins of the internal memory of the multicorrelator
#define POW_2_2 4 // 2^2 (used for the conversion of floating point numbers to integers) #define POW_2_2 4 // 2^2 (used for the conversion of floating point numbers to integers)
#define POW_2_29 536870912 // 2^29 (used for the conversion of floating point numbers to integers) #define POW_2_29 536870912 // 2^29 (used for the conversion of floating point numbers to integers)
#define POW_2_31 2147483648 // 2^31 (used for the conversion of floating point numbers to integers)
//#define SELECT_LSBits 0x000003FF // Select the 10 LSbits out of a 20-bit word //#define SELECT_LSBits 0x000003FF // Select the 10 LSbits out of a 20-bit word
//#define SELECT_MSBbits 0x000FFC00 // Select the 10 MSbits out of a 20-bit word //#define SELECT_MSBbits 0x000FFC00 // Select the 10 MSbits out of a 20-bit word
//#define SELECT_ALL_CODE_BITS 0x000FFFFF // Select a 20 bit word //#define SELECT_ALL_CODE_BITS 0x000FFFFF // Select a 20 bit word
@ -103,12 +104,8 @@ Fpga_Acquisition::Fpga_Acquisition(std::string device_name,
d_fd = 0; // driver descriptor d_fd = 0; // driver descriptor
d_map_base = nullptr; // driver memory map d_map_base = nullptr; // driver memory map
d_all_fft_codes = all_fft_codes; d_all_fft_codes = all_fft_codes;
Fpga_Acquisition::reset_acquisition(); Fpga_Acquisition::reset_acquisition();
Fpga_Acquisition::open_device();
Fpga_Acquisition::fpga_acquisition_test_register(); Fpga_Acquisition::fpga_acquisition_test_register();
Fpga_Acquisition::close_device();
d_PRN = 0; d_PRN = 0;
DLOG(INFO) << "Acquisition FPGA class created"; DLOG(INFO) << "Acquisition FPGA class created";
//printf("d_excludelimit = %d\n", d_excludelimit); //printf("d_excludelimit = %d\n", d_excludelimit);
@ -134,8 +131,21 @@ bool Fpga_Acquisition::set_local_code(uint32_t PRN)
void Fpga_Acquisition::write_local_code() void Fpga_Acquisition::write_local_code()
{ {
Fpga_Acquisition::fpga_configure_acquisition_local_code( uint32_t local_code;
&d_all_fft_codes[d_nsamples_total * (d_PRN - 1)]); int32_t k, tmp, tmp2;
int32_t fft_data;
d_map_base[9] = LOCAL_CODE_CLEAR_MEM;
// write local code
for (k = 0; k < d_vector_length; k++)
{
tmp = d_all_fft_codes[d_nsamples_total * (d_PRN - 1) + k].real();
tmp2 = d_all_fft_codes[d_nsamples_total * (d_PRN - 1) + k].imag();
local_code = (tmp & SELECT_LSBits) | ((tmp2 * SHL_CODE_BITS) & SELECT_MSBbits); // put together the real part and the imaginary part
fft_data = local_code & SELECT_ALL_CODE_BITS;
d_map_base[6] = fft_data;
}
} }
@ -170,6 +180,8 @@ void Fpga_Acquisition::fpga_acquisition_test_register()
uint32_t writeval = TEST_REG_SANITY_CHECK; uint32_t writeval = TEST_REG_SANITY_CHECK;
uint32_t readval; uint32_t readval;
Fpga_Acquisition::open_device();
// write value to test register // write value to test register
d_map_base[15] = writeval; d_map_base[15] = writeval;
// read value from test register // read value from test register
@ -183,42 +195,8 @@ void Fpga_Acquisition::fpga_acquisition_test_register()
{ {
LOG(INFO) << "Acquisition test register sanity check success!"; LOG(INFO) << "Acquisition test register sanity check success!";
} }
}
Fpga_Acquisition::close_device();
void Fpga_Acquisition::fpga_configure_acquisition_local_code(lv_16sc_t fft_local_code[])
{
// uint32_t local_code;
// uint32_t k, tmp, tmp2;
// uint32_t fft_data;
//
// d_map_base[9] = LOCAL_CODE_CLEAR_MEM;
// // write local code
// for (k = 0; k < d_vector_length; k++)
// {
// tmp = fft_local_code[k].real();
// tmp2 = fft_local_code[k].imag();
//
// local_code = (tmp & SELECT_LSBits) | ((tmp2 * SHL_CODE_BITS) & SELECT_MSBbits); // put together the real part and the imaginary part
// fft_data = local_code & SELECT_ALL_CODE_BITS;
// d_map_base[6] = fft_data;
// }
uint32_t local_code;
int32_t k, tmp, tmp2;
int32_t fft_data;
d_map_base[9] = LOCAL_CODE_CLEAR_MEM;
// write local code
for (k = 0; k < d_vector_length; k++)
{
tmp = fft_local_code[k].real();
tmp2 = fft_local_code[k].imag();
local_code = (tmp & SELECT_LSBits) | ((tmp2 * SHL_CODE_BITS) & SELECT_MSBbits); // put together the real part and the imaginary part
fft_data = local_code & SELECT_ALL_CODE_BITS;
d_map_base[6] = fft_data;
}
} }
@ -264,35 +242,22 @@ void Fpga_Acquisition::set_doppler_sweep(uint32_t num_sweeps, uint32_t doppler_s
float phase_step_rad_real; float phase_step_rad_real;
float phase_step_rad_int_temp; float phase_step_rad_int_temp;
int32_t phase_step_rad_int; int32_t phase_step_rad_int;
auto doppler = static_cast<int32_t>(doppler_min);
float phase_step_rad = GPS_TWO_PI * (doppler) / static_cast<float>(d_fs_in);
// The doppler step can never be outside the range -pi to +pi, otherwise there would be aliasing // The doppler step can never be outside the range -pi to +pi, otherwise there would be aliasing
// The FPGA expects phase_step_rad between -1 (-pi) to +1 (+pi) // The FPGA expects phase_step_rad between -1 (-pi) to +1 (+pi)
// The FPGA also expects the phase to be negative since it produces cos(x) -j*sin(x) // The FPGA also expects the phase to be negative since it produces cos(x) -j*sin(x)
phase_step_rad_real = phase_step_rad / (GPS_TWO_PI / 2); phase_step_rad_real = 2.0 * (doppler_min) / static_cast<float>(d_fs_in);
// // avoid saturation of the fixed point representation in the fpga
// // (only the positive value can saturate due to the 2's complement representation)
// if (phase_step_rad_real >= 1.0)
// {
// phase_step_rad_real = MAX_PHASE_STEP_RAD;
// }
phase_step_rad_int_temp = phase_step_rad_real * POW_2_2; // * 2^2 phase_step_rad_int_temp = phase_step_rad_real * POW_2_2; // * 2^2
phase_step_rad_int = static_cast<int32_t>(phase_step_rad_int_temp * (POW_2_29)); // * 2^29 (in total it makes x2^31 in two steps to avoid the warnings phase_step_rad_int = static_cast<int32_t>(phase_step_rad_int_temp * (POW_2_29)); // * 2^29 (in total it makes x2^31 in two steps to avoid the warnings
d_map_base[3] = phase_step_rad_int; d_map_base[3] = phase_step_rad_int;
// repeat the calculation with the doppler step // repeat the calculation with the doppler step
//doppler = static_cast<int32_t>(d_doppler_step); phase_step_rad_real = 2.0 * (doppler_step) / static_cast<float>(d_fs_in);
doppler = static_cast<int32_t>(doppler_step);
phase_step_rad = GPS_TWO_PI * (doppler) / static_cast<float>(d_fs_in);
phase_step_rad_real = phase_step_rad / (GPS_TWO_PI / 2);
// if (phase_step_rad_real >= 1.0)
// {
// phase_step_rad_real = MAX_PHASE_STEP_RAD;
// }
phase_step_rad_int_temp = phase_step_rad_real * POW_2_2; // * 2^2 phase_step_rad_int_temp = phase_step_rad_real * POW_2_2; // * 2^2
phase_step_rad_int = static_cast<int32_t>(phase_step_rad_int_temp * (POW_2_29)); // * 2^29 (in total it makes x2^31 in two steps to avoid the warnings phase_step_rad_int = static_cast<int32_t>(phase_step_rad_int_temp * (POW_2_29)); // * 2^29 (in total it makes x2^31 in two steps to avoid the warnings
d_map_base[4] = phase_step_rad_int; d_map_base[4] = phase_step_rad_int;
// write number of doppler sweeps
d_map_base[5] = num_sweeps; d_map_base[5] = num_sweeps;
} }

1
src/algorithms/acquisition/libs/fpga_acquisition.h
View File

@ -122,7 +122,6 @@ private:
uint32_t d_PRN; // PRN uint32_t d_PRN; // PRN
// FPGA private functions // FPGA private functions
void fpga_acquisition_test_register(void); void fpga_acquisition_test_register(void);
void fpga_configure_acquisition_local_code(lv_16sc_t fft_local_code[]);
void read_result_valid(uint32_t *result_valid); void read_result_valid(uint32_t *result_valid);
}; };

1
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc
View File

@ -422,6 +422,7 @@ dll_pll_veml_tracking_fpga::dll_pll_veml_tracking_fpga(const Dll_Pll_Conf_Fpga &
uint32_t multicorr_type = trk_parameters.multicorr_type; uint32_t multicorr_type = trk_parameters.multicorr_type;
multicorrelator_fpga = std::make_shared<Fpga_Multicorrelator_8sc>(d_n_correlator_taps, device_name, device_base, ca_codes, data_codes, d_code_length_chips, trk_parameters.track_pilot, multicorr_type, d_code_samples_per_chip); multicorrelator_fpga = std::make_shared<Fpga_Multicorrelator_8sc>(d_n_correlator_taps, device_name, device_base, ca_codes, data_codes, d_code_length_chips, trk_parameters.track_pilot, multicorr_type, d_code_samples_per_chip);
multicorrelator_fpga->set_output_vectors(d_correlator_outs, d_Prompt_Data); multicorrelator_fpga->set_output_vectors(d_correlator_outs, d_Prompt_Data);
//multicorrelator_fpga->fpga_compute_signal_parameters_in_fpga();
d_sample_counter_next = 0ULL; d_sample_counter_next = 0ULL;
} }

85
src/algorithms/tracking/libs/fpga_multicorrelator.cc
View File

@ -51,9 +51,9 @@
#define CODE_PHASE_STEP_CHIPS_NUM_NBITS CODE_RESAMPLER_NUM_BITS_PRECISION #define CODE_PHASE_STEP_CHIPS_NUM_NBITS CODE_RESAMPLER_NUM_BITS_PRECISION
#define pwrtwo(x) (1 << (x)) #define pwrtwo(x) (1 << (x))
#define MAX_CODE_RESAMPLER_COUNTER pwrtwo(CODE_PHASE_STEP_CHIPS_NUM_NBITS) // 2^CODE_PHASE_STEP_CHIPS_NUM_NBITS #define MAX_CODE_RESAMPLER_COUNTER pwrtwo(CODE_PHASE_STEP_CHIPS_NUM_NBITS) // 2^CODE_PHASE_STEP_CHIPS_NUM_NBITS
#define PHASE_CARR_NBITS 32 #define PHASE_CARR_MAX 2147483648 // 2^(31) The phase is represented as a 32-bit vector in 1.31 format
#define PHASE_CARR_NBITS_INT 1 #define PHASE_CARR_MAX_div_PI 683565275.5764316 // 2^(31)/pi
#define PHASE_CARR_NBITS_FRAC PHASE_CARR_NBITS - PHASE_CARR_NBITS_INT #define TWO_PI 6.283185307179586
#define LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT 0x20000000 #define LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT 0x20000000
#define LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER 0x10000000 #define LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER 0x10000000
#define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000 #define LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY 0x0C000000
@ -118,6 +118,7 @@ Fpga_Multicorrelator_8sc::Fpga_Multicorrelator_8sc(int32_t n_correlators,
d_data_codes = data_codes; d_data_codes = data_codes;
d_multicorr_type = multicorr_type; d_multicorr_type = multicorr_type;
d_code_samples_per_chip = code_samples_per_chip; d_code_samples_per_chip = code_samples_per_chip;
d_code_length_samples = d_code_length_chips * d_code_samples_per_chip;
DLOG(INFO) << "TRACKING FPGA CLASS CREATED"; DLOG(INFO) << "TRACKING FPGA CLASS CREATED";
} }
@ -171,9 +172,9 @@ void Fpga_Multicorrelator_8sc::set_output_vectors(gr_complex *corr_out, gr_compl
} }
void Fpga_Multicorrelator_8sc::update_local_code(float rem_code_phase_chips) void Fpga_Multicorrelator_8sc::update_local_code()
{ {
d_rem_code_phase_chips = rem_code_phase_chips; //d_rem_code_phase_chips = rem_code_phase_chips;
Fpga_Multicorrelator_8sc::fpga_compute_code_shift_parameters(); Fpga_Multicorrelator_8sc::fpga_compute_code_shift_parameters();
Fpga_Multicorrelator_8sc::fpga_configure_code_parameters_in_fpga(); Fpga_Multicorrelator_8sc::fpga_configure_code_parameters_in_fpga();
} }
@ -188,11 +189,12 @@ void Fpga_Multicorrelator_8sc::Carrier_wipeoff_multicorrelator_resampler(
float code_phase_rate_step_chips __attribute__((unused)), float code_phase_rate_step_chips __attribute__((unused)),
int32_t signal_length_samples) int32_t signal_length_samples)
{ {
update_local_code(rem_code_phase_chips); d_rem_code_phase_chips = rem_code_phase_chips;
d_rem_carrier_phase_in_rad = rem_carrier_phase_in_rad; d_rem_carrier_phase_in_rad = rem_carrier_phase_in_rad;
d_code_phase_step_chips = code_phase_step_chips; d_code_phase_step_chips = code_phase_step_chips;
d_phase_step_rad = phase_step_rad; d_phase_step_rad = phase_step_rad;
d_correlator_length_samples = signal_length_samples; d_correlator_length_samples = signal_length_samples;
Fpga_Multicorrelator_8sc::update_local_code();
Fpga_Multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(); Fpga_Multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga();
Fpga_Multicorrelator_8sc::fpga_configure_signal_parameters_in_fpga(); Fpga_Multicorrelator_8sc::fpga_configure_signal_parameters_in_fpga();
Fpga_Multicorrelator_8sc::fpga_launch_multicorrelator_fpga(); Fpga_Multicorrelator_8sc::fpga_launch_multicorrelator_fpga();
@ -302,9 +304,9 @@ void Fpga_Multicorrelator_8sc::fpga_configure_tracking_gps_local_code(int32_t PR
uint32_t select_pilot_corelator = LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT; uint32_t select_pilot_corelator = LOCAL_CODE_FPGA_CORRELATOR_SELECT_COUNT;
d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER; d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER;
for (k = 0; k < d_code_length_chips * d_code_samples_per_chip; k++) for (k = 0; k < d_code_length_samples; k++)
{ {
if (d_ca_codes[((int32_t(d_code_length_chips)) * d_code_samples_per_chip * (PRN - 1)) + k] == 1) if (d_ca_codes[(d_code_length_samples * (PRN - 1)) + k] == 1)
{ {
code_chip = 1; code_chip = 1;
} }
@ -319,9 +321,9 @@ void Fpga_Multicorrelator_8sc::fpga_configure_tracking_gps_local_code(int32_t PR
if (d_track_pilot) if (d_track_pilot)
{ {
d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER; d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_CLEAR_ADDRESS_COUNTER;
for (k = 0; k < d_code_length_chips * d_code_samples_per_chip; k++) for (k = 0; k < d_code_length_samples; k++)
{ {
if (d_data_codes[((int32_t(d_code_length_chips)) * d_code_samples_per_chip * (PRN - 1)) + k] == 1) if (d_data_codes[(d_code_length_samples * (PRN - 1)) + k] == 1)
{ {
code_chip = 1; code_chip = 1;
} }
@ -332,46 +334,53 @@ void Fpga_Multicorrelator_8sc::fpga_configure_tracking_gps_local_code(int32_t PR
d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY | code_chip | select_pilot_corelator; d_map_base[PROG_MEMS_ADDR] = LOCAL_CODE_FPGA_ENABLE_WRITE_MEMORY | code_chip | select_pilot_corelator;
} }
} }
d_map_base[CODE_LENGTH_MINUS_1_REG_ADDR] = (d_code_length_samples)-1; // number of samples - 1
} }
void Fpga_Multicorrelator_8sc::fpga_compute_code_shift_parameters(void) void Fpga_Multicorrelator_8sc::fpga_compute_code_shift_parameters(void)
{ {
float temp_calculation; float frac_part; // decimal part
int32_t dec_part; // fractional part
int32_t i; int32_t i;
for (i = 0; i < d_n_correlators; i++) for (i = 0; i < d_n_correlators; i++)
{ {
temp_calculation = floor(d_shifts_chips[i] - d_rem_code_phase_chips); dec_part = floor(d_shifts_chips[i] - d_rem_code_phase_chips);
if (temp_calculation < 0) if (dec_part < 0)
{ {
temp_calculation = temp_calculation + (d_code_length_chips * d_code_samples_per_chip); // % operator does not work as in Matlab with negative numbers dec_part = dec_part + d_code_length_samples; // % operator does not work as in Matlab with negative numbers
} }
d_initial_index[i] = static_cast<uint32_t>((static_cast<int32_t>(temp_calculation)) % (d_code_length_chips * d_code_samples_per_chip)); d_initial_index[i] = dec_part;
temp_calculation = fmod(d_shifts_chips[i] - d_rem_code_phase_chips, 1.0);
if (temp_calculation < 0)
frac_part = fmod(d_shifts_chips[i] - d_rem_code_phase_chips, 1.0);
if (frac_part < 0)
{ {
temp_calculation = temp_calculation + 1.0; // fmod operator does not work as in Matlab with negative numbers frac_part = frac_part + 1.0; // fmod operator does not work as in Matlab with negative numbers
} }
d_initial_interp_counter[i] = static_cast<uint32_t>(floor(MAX_CODE_RESAMPLER_COUNTER * temp_calculation)); d_initial_interp_counter[i] = static_cast<uint32_t>(floor(MAX_CODE_RESAMPLER_COUNTER * frac_part));
} }
if (d_track_pilot) if (d_track_pilot)
{ {
temp_calculation = floor(d_prompt_data_shift[0] - d_rem_code_phase_chips); dec_part = floor(d_prompt_data_shift[0] - d_rem_code_phase_chips);
if (temp_calculation < 0) if (dec_part < 0)
{ {
temp_calculation = temp_calculation + (d_code_length_chips * d_code_samples_per_chip); // % operator does not work as in Matlab with negative numbers dec_part = dec_part + d_code_length_samples; // % operator does not work as in Matlab with negative numbers
} }
d_initial_index[d_n_correlators] = static_cast<uint32_t>((static_cast<int32_t>(temp_calculation)) % (d_code_length_chips * d_code_samples_per_chip)); d_initial_index[d_n_correlators] = dec_part;
temp_calculation = fmod(d_prompt_data_shift[0] - d_rem_code_phase_chips, 1.0);
if (temp_calculation < 0) frac_part = fmod(d_prompt_data_shift[0] - d_rem_code_phase_chips, 1.0);
if (frac_part < 0)
{ {
temp_calculation = temp_calculation + 1.0; // fmod operator does not work as in Matlab with negative numbers frac_part = frac_part + 1.0; // fmod operator does not work as in Matlab with negative numbers
} }
d_initial_interp_counter[d_n_correlators] = static_cast<uint32_t>(floor(MAX_CODE_RESAMPLER_COUNTER * temp_calculation)); d_initial_interp_counter[d_n_correlators] = static_cast<uint32_t>(floor(MAX_CODE_RESAMPLER_COUNTER * frac_part));
} }
} }
@ -390,7 +399,7 @@ void Fpga_Multicorrelator_8sc::fpga_configure_code_parameters_in_fpga(void)
d_map_base[INITIAL_INTERP_COUNTER_REG_BASE_ADDR + d_n_correlators] = d_initial_interp_counter[d_n_correlators]; d_map_base[INITIAL_INTERP_COUNTER_REG_BASE_ADDR + d_n_correlators] = d_initial_interp_counter[d_n_correlators];
} }
d_map_base[CODE_LENGTH_MINUS_1_REG_ADDR] = (d_code_length_chips * d_code_samples_per_chip) - 1; // number of samples - 1 //d_map_base[CODE_LENGTH_MINUS_1_REG_ADDR] = (d_code_length_samples)-1; // number of samples - 1
} }
@ -399,34 +408,22 @@ void Fpga_Multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga(void)
float d_rem_carrier_phase_in_rad_temp; float d_rem_carrier_phase_in_rad_temp;
d_code_phase_step_chips_num = static_cast<uint32_t>(roundf(MAX_CODE_RESAMPLER_COUNTER * d_code_phase_step_chips)); d_code_phase_step_chips_num = static_cast<uint32_t>(roundf(MAX_CODE_RESAMPLER_COUNTER * d_code_phase_step_chips));
if (d_code_phase_step_chips > 1.0)
{
std::cout << "Warning : d_code_phase_step_chips = " << d_code_phase_step_chips << " cannot be bigger than one" << std::endl;
}
if (d_rem_carrier_phase_in_rad > M_PI) if (d_rem_carrier_phase_in_rad > M_PI)
{ {
d_rem_carrier_phase_in_rad_temp = -2 * M_PI + d_rem_carrier_phase_in_rad; d_rem_carrier_phase_in_rad_temp = -TWO_PI + d_rem_carrier_phase_in_rad;
} }
else if (d_rem_carrier_phase_in_rad < -M_PI) else if (d_rem_carrier_phase_in_rad < -M_PI)
{ {
d_rem_carrier_phase_in_rad_temp = 2 * M_PI + d_rem_carrier_phase_in_rad; d_rem_carrier_phase_in_rad_temp = TWO_PI + d_rem_carrier_phase_in_rad;
} }
else else
{ {
d_rem_carrier_phase_in_rad_temp = d_rem_carrier_phase_in_rad; d_rem_carrier_phase_in_rad_temp = d_rem_carrier_phase_in_rad;
} }
d_rem_carr_phase_rad_int = static_cast<int32_t>(roundf((fabs(d_rem_carrier_phase_in_rad_temp) / M_PI) * pow(2, PHASE_CARR_NBITS_FRAC)));
if (d_rem_carrier_phase_in_rad_temp < 0)
{
d_rem_carr_phase_rad_int = -d_rem_carr_phase_rad_int;
}
d_phase_step_rad_int = static_cast<int32_t>(roundf((fabs(d_phase_step_rad) / M_PI) * pow(2, PHASE_CARR_NBITS_FRAC))); // the FPGA accepts a range for the phase step between -pi and +pi
if (d_phase_step_rad < 0) d_rem_carr_phase_rad_int = static_cast<int32_t>(roundf((d_rem_carrier_phase_in_rad_temp)*PHASE_CARR_MAX_div_PI));
{ d_phase_step_rad_int = static_cast<int32_t>(roundf((d_phase_step_rad)*PHASE_CARR_MAX_div_PI)); // the FPGA accepts a range for the phase step between -pi and +pi
d_phase_step_rad_int = -d_phase_step_rad_int;
}
} }

10
src/algorithms/tracking/libs/fpga_multicorrelator.h
View File

@ -75,9 +75,10 @@ public:
uint32_t device_base, int32_t *ca_codes, int32_t *data_codes, uint32_t code_length_chips, bool track_pilot, uint32_t multicorr_type, uint32_t code_samples_per_chip); uint32_t device_base, int32_t *ca_codes, int32_t *data_codes, uint32_t code_length_chips, bool track_pilot, uint32_t multicorr_type, uint32_t code_samples_per_chip);
~Fpga_Multicorrelator_8sc(); ~Fpga_Multicorrelator_8sc();
void set_output_vectors(gr_complex *corr_out, gr_complex *Prompt_Data); void set_output_vectors(gr_complex *corr_out, gr_complex *Prompt_Data);
//void fpga_compute_signal_parameters_in_fpga(void);
void set_local_code_and_taps( void set_local_code_and_taps(
float *shifts_chips, float *prompt_data_shift, int32_t PRN); float *shifts_chips, float *prompt_data_shift, int32_t PRN);
void update_local_code(float rem_code_phase_chips); void update_local_code();
void Carrier_wipeoff_multicorrelator_resampler( void Carrier_wipeoff_multicorrelator_resampler(
float rem_carrier_phase_in_rad, float phase_step_rad, float rem_carrier_phase_in_rad, float phase_step_rad,
float carrier_phase_rate_step_rad, float carrier_phase_rate_step_rad,
@ -96,15 +97,16 @@ private:
gr_complex *d_Prompt_Data; gr_complex *d_Prompt_Data;
float *d_shifts_chips; float *d_shifts_chips;
float *d_prompt_data_shift; float *d_prompt_data_shift;
int32_t d_code_length_chips; uint32_t d_code_length_chips;
int32_t d_n_correlators; // number of correlators uint32_t d_code_length_samples;
uint32_t d_n_correlators; // number of correlators
// data related to the hardware module and the driver // data related to the hardware module and the driver
int32_t d_device_descriptor; // driver descriptor int32_t d_device_descriptor; // driver descriptor
volatile uint32_t *d_map_base; // driver memory map volatile uint32_t *d_map_base; // driver memory map
// configuration data received from the interface // configuration data received from the interface
uint32_t d_channel; // channel number uint32_t d_channel; // channel number
uint32_t d_correlator_length_samples; uint32_t d_correlator_length_samples;
float d_rem_code_phase_chips; float d_rem_code_phase_chips;
float d_code_phase_step_chips; float d_code_phase_step_chips;