mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity
gnss-sdr/src/algorithms/tracking/libs/fpga_multicorrelator.cc

525 lines
19 KiB
C++
Raw Permalink Blame History

/*!
* \file fpga_multicorrelator.cc
* \brief FPGA vector correlator class
* \authors <ul>
* <li> Marc Majoral, 2019. mmajoral(at)cttc.cat
* <li> Javier Arribas, 2019. jarribas(at)cttc.es
* </ul>
*
* Class that controls and executes a highly optimized vector correlator
* class in the FPGA
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#include "fpga_multicorrelator.h"
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <cmath>
#include <fcntl.h> // for O_RDWR, O_RSYNC
#include <string>
#include <sys/mman.h> // for PROT_READ, PROT_WRITE, MAP_SHARED
#include <utility>
#if USE_GLOG_AND_GFLAGS
#include <glog/logging.h>
#else
#include <absl/log/log.h>
#endif
#ifndef TEMP_FAILURE_RETRY
#define TEMP_FAILURE_RETRY(exp) \
({ \
decltype(exp) _rc; \
do \
{ \
_rc = (exp); \
} \
while (_rc == -1 && errno == EINTR); \
_rc; \
})
#endif
// 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;
Fpga_Multicorrelator_8sc::Fpga_Multicorrelator_8sc(int32_t n_correlators,
int32_t *ca_codes,
int32_t *data_codes,
uint32_t code_length_chips,
bool track_pilot,
uint32_t code_samples_per_chip)
: d_initial_sample_counter(0),
d_corr_out(nullptr),
d_Prompt_Data(nullptr),
d_shifts_chips(nullptr),
d_prompt_data_shift(nullptr),
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),
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),
d_code_phase_rate_step_chips_num(0),
d_rem_carr_phase_rad_int(0),
d_phase_step_rad_int(0),
d_carrier_phase_rate_step_rad_int(0),
d_ca_codes(ca_codes),
d_data_codes(data_codes),
d_secondary_code_0_length(0),
d_secondary_code_1_length(0),
d_track_pilot(track_pilot),
d_secondary_code_enabled(false)
{
// instantiate variable length vectors
if (d_track_pilot)
{
d_initial_index = volk_gnsssdr::vector<uint32_t>(n_correlators + 1);
d_initial_interp_counter = volk_gnsssdr::vector<uint32_t>(n_correlators + 1);
}
else
{
d_initial_index = volk_gnsssdr::vector<uint32_t>(n_correlators);
d_initial_interp_counter = volk_gnsssdr::vector<uint32_t>(n_correlators);
}
DLOG(INFO) << "TRACKING FPGA CLASS CREATED";
}
Fpga_Multicorrelator_8sc::~Fpga_Multicorrelator_8sc()
{
close_device();
}
uint64_t Fpga_Multicorrelator_8sc::read_sample_counter()
{
uint64_t sample_counter_tmp;
uint64_t sample_counter_msw_tmp;
sample_counter_tmp = d_map_base[sample_counter_reg_addr_lsw];
sample_counter_msw_tmp = d_map_base[sample_counter_reg_addr_msw];
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;
}
void Fpga_Multicorrelator_8sc::set_initial_sample(uint64_t samples_offset)
{
d_initial_sample_counter = samples_offset;
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;
}
void Fpga_Multicorrelator_8sc::set_local_code_and_taps(float *shifts_chips, float *prompt_data_shift, int32_t PRN)
{
d_shifts_chips = shifts_chips;
d_prompt_data_shift = prompt_data_shift;
Fpga_Multicorrelator_8sc::fpga_configure_tracking_gps_local_code(PRN);
}
void Fpga_Multicorrelator_8sc::set_output_vectors(gr_complex *corr_out, gr_complex *Prompt_Data)
{
d_corr_out = corr_out;
d_Prompt_Data = Prompt_Data;
}
void Fpga_Multicorrelator_8sc::update_local_code()
{
Fpga_Multicorrelator_8sc::fpga_compute_code_shift_parameters();
Fpga_Multicorrelator_8sc::fpga_configure_code_parameters_in_fpga();
}
void Fpga_Multicorrelator_8sc::Carrier_wipeoff_multicorrelator_resampler(
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
{
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
Fpga_Multicorrelator_8sc::update_local_code();
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();
int32_t irq_count;
ssize_t nb;
nb = read(d_device_descriptor, &irq_count, sizeof(irq_count));
if (nb != sizeof(irq_count))
{
std::cout << "Tracking_module Read failed to retrieve 4 bytes!\n";
std::cout << "Tracking_module Interrupt number " << irq_count << '\n';
}
// release secondary code indices, keep channel locked
if (d_secondary_code_enabled == true)
{
d_map_base[drop_samples_reg_addr] = enable_secondary_code; // keep secondary code enabled
}
else
{
d_map_base[drop_samples_reg_addr] = 0; // block samples
}
Fpga_Multicorrelator_8sc::read_tracking_gps_results();
}
bool Fpga_Multicorrelator_8sc::free()
{
// unlock the channel
Fpga_Multicorrelator_8sc::unlock_channel();
return true;
}
void Fpga_Multicorrelator_8sc::open_channel(const std::string &device_io_name, uint32_t channel)
{
std::cout << "trk device_io_name = " << device_io_name << '\n';
if ((d_device_descriptor = open(device_io_name.c_str(), O_RDWR | O_SYNC)) == -1)
{
LOG(WARNING) << "Cannot open deviceio" << device_io_name;
std::cout << "Cannot open deviceio" << device_io_name << '\n';
}
d_map_base = reinterpret_cast<volatile uint32_t *>(mmap(nullptr, FPGA_PAGE_SIZE,
PROT_READ | PROT_WRITE, MAP_SHARED, d_device_descriptor, 0));
if (d_map_base == reinterpret_cast<void *>(-1))
{
LOG(WARNING) << "Cannot map the FPGA tracking module "
<< channel << "into user memory";
std::cout << "Cannot map deviceio" << device_io_name << '\n';
}
// sanity check: check test register
uint32_t writeval = test_register_track_writeval;
uint32_t readval;
readval = Fpga_Multicorrelator_8sc::fpga_acquisition_test_register(writeval);
if (writeval != readval)
{
LOG(WARNING) << "Test register sanity check failed";
std::cout << "Tracking test register sanity check failed\n";
}
else
{
LOG(INFO) << "Test register sanity check success !";
}
}
uint32_t Fpga_Multicorrelator_8sc::fpga_acquisition_test_register(uint32_t writeval)
{
uint32_t readval = 0;
// write value to test register
d_map_base[test_reg_addr] = writeval;
// read value from test register
readval = d_map_base[test_reg_addr];
// return read value
return readval;
}
void Fpga_Multicorrelator_8sc::fpga_configure_tracking_gps_local_code(int32_t PRN)
{
uint32_t k;
d_map_base[prog_mems_addr] = local_code_fpga_clear_address_counter;
for (k = 0; k < d_code_length_samples; k++)
{
d_map_base[prog_mems_addr] = d_ca_codes[(d_code_length_samples * (PRN - 1)) + k];
}
if (d_track_pilot)
{
d_map_base[prog_mems_addr] = local_code_fpga_clear_address_counter;
for (k = 0; k < d_code_length_samples; k++)
{
d_map_base[prog_mems_addr] = d_data_codes[(d_code_length_samples * (PRN - 1)) + k];
}
}
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()
{
float frac_part; // decimal part
int32_t dec_part; // fractional part
for (uint32_t i = 0; i < d_n_correlators; i++)
{
dec_part = std::floor(d_shifts_chips[i] - d_rem_code_phase_chips);
if (dec_part < 0)
{
dec_part = dec_part + d_code_length_samples; // % operator does not work as in Matlab with negative numbers
}
d_initial_index[i] = dec_part;
frac_part = fmod(d_shifts_chips[i] - d_rem_code_phase_chips, 1.0);
if (frac_part < 0)
{
frac_part = frac_part + 1.0F; // fmod operator does not work as in Matlab with negative numbers
}
d_initial_interp_counter[i] = static_cast<uint32_t>(std::floor(max_code_resampler_counter * frac_part));
}
if (d_track_pilot)
{
dec_part = std::floor(d_prompt_data_shift[0] - d_rem_code_phase_chips);
if (dec_part < 0)
{
dec_part = dec_part + d_code_length_samples; // % operator does not work as in Matlab with negative numbers
}
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)
{
frac_part = frac_part + 1.0F; // fmod operator does not work as in Matlab with negative numbers
}
d_initial_interp_counter[d_n_correlators] = static_cast<uint32_t>(std::floor(max_code_resampler_counter * frac_part));
}
}
void Fpga_Multicorrelator_8sc::fpga_configure_code_parameters_in_fpga()
{
for (uint32_t i = 0; i < d_n_correlators; i++)
{
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];
}
if (d_track_pilot)
{
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];
}
}
void Fpga_Multicorrelator_8sc::fpga_compute_signal_parameters_in_fpga()
{
float d_rem_carrier_phase_in_rad_temp;
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));
if (d_rem_carrier_phase_in_rad > M_PI)
{
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)
{
d_rem_carrier_phase_in_rad_temp = TWO_PI + d_rem_carrier_phase_in_rad;
}
else
{
d_rem_carrier_phase_in_rad_temp = d_rem_carrier_phase_in_rad;
}
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));
}
void Fpga_Multicorrelator_8sc::fpga_configure_signal_parameters_in_fpga()
{
d_map_base[code_phase_step_chips_num_reg_addr] = d_code_phase_step_chips_num; // code phase step
d_map_base[code_phase_step_chips_rate_reg_addr] = d_code_phase_rate_step_chips_num; // code phase step rate
d_map_base[nsamples_minus_1_reg_addr] = d_correlator_length_samples - 1; // number of samples
d_map_base[rem_carr_phase_rad_reg_addr] = d_rem_carr_phase_rad_int; // initial nco phase
d_map_base[phase_step_rad_reg_addr] = d_phase_step_rad_int; // nco phase step
d_map_base[phase_step_rate_reg_addr] = d_carrier_phase_rate_step_rad_int; // nco phase step rate
}
void Fpga_Multicorrelator_8sc::fpga_launch_multicorrelator_fpga()
{
// 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))
{
std::cerr << "Error launching the FPGA multicorrelator\n";
}
// writing 1 to reg 14 launches the tracking
d_map_base[start_flag_addr] = 1;
}
void Fpga_Multicorrelator_8sc::read_tracking_gps_results()
{
int32_t readval_real;
int32_t readval_imag;
for (uint32_t k = 0; k < d_n_correlators; k++)
{
readval_real = d_map_base[result_reg_real_base_addr + k];
readval_imag = d_map_base[result_reg_imag_base_addr + k];
d_corr_out[k] = gr_complex(readval_real, readval_imag);
}
if (d_track_pilot)
{
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];
d_Prompt_Data[0] = gr_complex(readval_real, readval_imag);
}
}
void Fpga_Multicorrelator_8sc::unlock_channel()
{
// unlock the channel to let the next samples go through
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
d_secondary_code_enabled = false;
}
void Fpga_Multicorrelator_8sc::close_device()
{
auto *aux = const_cast<uint32_t *>(d_map_base);
if (munmap(static_cast<void *>(aux), FPGA_PAGE_SIZE) == -1)
{
std::cout << "Failed to unmap memory uio\n";
}
close(d_device_descriptor);
}
void Fpga_Multicorrelator_8sc::lock_channel()
{
// lock the channel for processing
d_map_base[drop_samples_reg_addr] = 0; // lock the channel
}
void Fpga_Multicorrelator_8sc::set_secondary_code_lengths(uint32_t secondary_code_0_length, uint32_t secondary_code_1_length)
{
d_secondary_code_0_length = secondary_code_0_length;
d_secondary_code_1_length = secondary_code_1_length;
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;
d_map_base[secondary_code_lengths_reg_addr] = secondary_code_length_1_minus_1 * 256 + secondary_code_length_0_minus_1;
}
void Fpga_Multicorrelator_8sc::update_prn_code_length(uint32_t first_prn_length, uint32_t next_prn_length)
{
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;
}
void Fpga_Multicorrelator_8sc::initialize_secondary_code(uint32_t secondary_code, std::string *secondary_code_string)
{
uint32_t secondary_code_length;
uint32_t reg_addr;
if (secondary_code == 0)
{
secondary_code_length = d_secondary_code_0_length;
reg_addr = prog_secondary_code_0_data_reg_addr;
}
else
{
secondary_code_length = d_secondary_code_1_length;
reg_addr = prog_secondary_code_1_data_reg_addr;
}
Fpga_Multicorrelator_8sc::write_secondary_code(secondary_code_length, secondary_code_string, reg_addr);
}
void Fpga_Multicorrelator_8sc::write_secondary_code(uint32_t secondary_code_length, std::string *secondary_code_string, uint32_t reg_addr)
{
uint32_t num_words = std::ceil(static_cast<float>(secondary_code_length) / secondary_code_word_size);
uint32_t last_word_size = secondary_code_length % secondary_code_word_size;
if (last_word_size == 0)
{
last_word_size = secondary_code_word_size;
}
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;
for (unsigned int k = 0; k < secondary_code_word_size; k++)
{
std::string string_tmp(1, (*secondary_code_string)[mem_addr * secondary_code_word_size + k]);
write_val = write_val | std::stoi(string_tmp) * pow_k;
pow_k = pow_k * 2;
}
write_val = write_val | mem_addr * secondary_code_addr_bits | secondary_code_wr_strobe;
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++)
{
std::string string_tmp(1, (*secondary_code_string)[mem_addr * secondary_code_word_size + k]);
write_val = write_val | std::stoi(string_tmp) * pow_k;
pow_k = pow_k * 2;
}
write_val = write_val | (mem_addr * secondary_code_addr_bits) | (secondary_code_wr_strobe);
d_map_base[reg_addr] = write_val;
}
void Fpga_Multicorrelator_8sc::enable_secondary_codes()
{
d_map_base[drop_samples_reg_addr] = init_secondary_code_addresses | enable_secondary_code; // enable secondary codes and clear secondary code indices
d_secondary_code_enabled = true;
}
void Fpga_Multicorrelator_8sc::disable_secondary_codes()
{
// this function is to be called before starting the tracking process in order to disable the secondary codes by default
d_map_base[drop_samples_reg_addr] = drop_samples;
}
Reference in New Issue View Git Blame Copy Permalink