/*!
* \file gps_l1_ca_dll_fll_pll_tracking_cc.cc
* \brief Implementation of a code DLL + carrier FLL/PLL tracking block
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* This file implements the code Delay Locked Loop (DLL) + carrier
* Phase Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL)
* according to the algorithms described in:
* E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and
* Applications, Second Edition, Artech House Publishers, 2005.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* 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 .
*
* -------------------------------------------------------------------------
*/
#include "gps_l1_ca_dll_fll_pll_tracking_cc.h"
#include "gps_sdr_signal_processing.h"
#include "GPS_L1_CA.h"
#include "tracking_discriminators.h"
#include "CN_estimators.h"
#include "tracking_FLL_PLL_filter.h"
#include "control_message_factory.h"
#include "gnss_flowgraph.h"
#include
#include
#include
#include
#include "math.h"
#include
#include
#include
/*!
* \todo Include in definition header file
*/
#define CN0_ESTIMATION_SAMPLES 10
#define MINIMUM_VALID_CN0 25
#define MAXIMUM_LOCK_FAIL_COUNTER 200
using google::LogMessage;
gps_l1_ca_dll_fll_pll_tracking_cc_sptr
gps_l1_ca_dll_fll_pll_make_tracking_cc(Gnss_Satellite satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int order,
float fll_bw_hz, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips)
{
return gps_l1_ca_dll_fll_pll_tracking_cc_sptr(new Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(satellite, if_freq,
fs_in, vector_length, queue, dump, dump_filename, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,
early_late_space_chips));
}
void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{
ninput_items_required[0] = d_vector_length*2; //set the required available samples in each call
}
Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(Gnss_Satellite satellite,
long if_freq, long fs_in, unsigned int vector_length, gr_msg_queue_sptr queue, bool dump,
std::string dump_filename, int order, float fll_bw_hz, float pll_bw_hz, float dll_bw_hz,
float early_late_space_chips) :
gr_block ("Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
gr_make_io_signature(5, 5, sizeof(double)))
{
//gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
// gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
// initialize internal vars
d_queue = queue;
d_dump = dump;
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
d_if_freq = if_freq;
d_fs_in = fs_in;
d_vector_length = vector_length;
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
d_dump_filename = dump_filename;
// Initialize tracking variables ==========================================
d_carrier_loop_filter.set_params(fll_bw_hz,pll_bw_hz,order);
// Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+2];
// Get space for the resampled early / prompt / late local replicas
//d_early_code = new gr_complex[d_vector_length*2];
//d_prompt_code = new gr_complex[d_vector_length*2];
//d_late_code = new gr_complex[d_vector_length*2];
// space for carrier wipeoff LO vector
//d_carr_sign = new gr_complex[d_vector_length*2];
posix_memalign((void**)&d_early_code, 16, d_vector_length*sizeof(gr_complex)*2);
posix_memalign((void**)&d_late_code, 16, d_vector_length*sizeof(gr_complex)*2);
posix_memalign((void**)&d_prompt_code, 16, d_vector_length*sizeof(gr_complex)*2);
posix_memalign((void**)&d_carr_sign,16, d_vector_length*sizeof(gr_complex)*2);
// sample synchronization
d_sample_counter = 0;
d_sample_counter_seconds = 0;
d_acq_sample_stamp = 0;
d_last_seg = 0;// this is for debug output only
d_enable_tracking = false;
d_current_prn_length_samples = (int)d_vector_length;
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES];
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
d_carrier_lock_threshold = 5;
}
void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking()
{
/*
* correct the code phase according to the delay between acq and trk
*/
unsigned long int acq_trk_diff_samples;
float acq_trk_diff_seconds;
acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length;
acq_trk_diff_seconds = (float)acq_trk_diff_samples / (float)d_fs_in;
//doppler effect
// Fd=(C/(C+Vr))*F
float radial_velocity;
radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
float T_chip_mod_seconds;
float T_prn_mod_seconds;
float T_prn_mod_samples;
d_code_freq_hz = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
T_chip_mod_seconds = 1 / d_code_freq_hz;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * (float)d_fs_in;
d_next_prn_length_samples = round(T_prn_mod_samples);
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in;
float T_prn_diff_seconds;
T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
float N_prn_diff;
N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
float corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * (float)d_fs_in), T_prn_true_samples);
if (corrected_acq_phase_samples < 0)
{
corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
}
delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples;
d_acq_code_phase_samples = corrected_acq_phase_samples;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz);
d_FLL_wait = 1;
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
code_gen_conplex(&d_ca_code[1], this->d_satellite.get_PRN(), 0);
d_ca_code[0] = d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS];
d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1];
d_carrier_lock_fail_counter = 0;
d_Prompt_prev = 0;
d_rem_code_phase_samples = 0;
d_rem_carr_phase = 0;
d_FLL_discriminator_hz = 0;
d_rem_code_phase_samples = 0;
d_next_rem_code_phase_samples = 0;
d_acc_carrier_phase_rad = 0;
d_code_phase_samples = d_acq_code_phase_samples;
// DEBUG OUTPUT
std::cout << "Tracking start on channel " << d_channel << " for satellite " << this->d_satellite << std::endl;
DLOG(INFO) << "Start tracking for satellite " << this->d_satellite << " received ";
// enable tracking
d_pull_in = true;
d_enable_tracking = true;
std::cout << "PULL-IN Doppler [Hz]= " << d_carrier_doppler_hz << " Code Phase correction [samples]=" << delay_correction_samples << " PULL-IN Code Phase [samples]= " << d_acq_code_phase_samples << std::endl;
}
void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_code()
{
float tcode_chips;
float rem_code_phase_chips;
float code_phase_step_chips;
int associated_chip_index;
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS;
code_phase_step_chips = d_code_freq_hz / ((float)d_fs_in);
rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in);
// unified loop for E, P, L code vectors
tcode_chips = -rem_code_phase_chips;
for (int i=0; i3)
{
d_FLL_discriminator_hz = 0; //disconnect the FLL after the initial lock
}
/*!
* DLL and FLL+PLL filter and get current carrier Doppler and code frequency
*/
carr_nco_hz = d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz, PLL_discriminator_hz, correlation_time_s);
d_carrier_doppler_hz = (float)d_if_freq + carr_nco_hz;
d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ - (((d_carrier_doppler_hz - (float)d_if_freq) * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ) - code_error_chips;
/*!
* \todo Improve the lock detection algorithm!
*/
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = d_Prompt;
d_cn0_estimation_counter++;
}
else
{
d_cn0_estimation_counter = 0;
d_CN0_SNV_dB_Hz = gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in);
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
// ###### TRACKING UNLOCK NOTIFICATION #####
int tracking_message;
if (d_carrier_lock_test < d_carrier_lock_threshold or d_carrier_lock_test > MINIMUM_VALID_CN0)
{
d_carrier_lock_fail_counter++;
}
else
{
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
{
std::cout << "Channel " << d_channel << " loss of lock!" << std::endl;
tracking_message = 3; //loss of lock
d_channel_internal_queue->push(tracking_message);
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
//std::cout<<"d_carrier_lock_fail_counter"<d_satellite << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
//std::cout<<"TRK CH "<d_satellite << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
//std::cout<<"TRK CH "<(d_channel));
d_dump_filename.append(".dat");
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl;
}
catch (std::ifstream::failure e)
{
std::cout << "channel " << d_channel << " Exception opening trk dump file " << e.what() << std::endl;
}
}
}
}
void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp)
{
d_acq_sample_stamp = sample_stamp;
}
void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::set_channel_queue(concurrent_queue *channel_internal_queue)
{
d_channel_internal_queue = channel_internal_queue;
}