mirror of https://github.com/gnss-sdr/gnss-sdr
100 lines
3.3 KiB
C++
100 lines
3.3 KiB
C++
/*!
|
|
* \file tracking_FLL_PLL_filter.cc
|
|
* \brief Implementation of a hybrid FLL and PLL filter for tracking carrier loop
|
|
* \author Javier Arribas, 2011. jarribas(at)cttc.es
|
|
*
|
|
* Class that implements hybrid FLL and PLL filter for tracking carrier loop
|
|
* Filter design (Kaplan 2nd ed., Pag. 181 Fig. 181)
|
|
*
|
|
* -----------------------------------------------------------------------------
|
|
*
|
|
* 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 "tracking_FLL_PLL_filter.h"
|
|
|
|
|
|
void Tracking_FLL_PLL_filter::set_params(float fll_bw_hz, float pll_bw_hz, int order)
|
|
{
|
|
/*
|
|
* Filter design (Kaplan 2nd ed., Pag. 181 Fig. 181)
|
|
*/
|
|
d_order = order;
|
|
if (d_order == 3)
|
|
{
|
|
/*
|
|
* 3rd order PLL with 2nd order FLL assist
|
|
*/
|
|
d_pll_b3 = 2.400;
|
|
d_pll_a3 = 1.100;
|
|
d_pll_a2 = 1.414;
|
|
d_pll_w0p = pll_bw_hz / 0.7845F;
|
|
d_pll_w0p2 = d_pll_w0p * d_pll_w0p;
|
|
d_pll_w0p3 = d_pll_w0p2 * d_pll_w0p;
|
|
|
|
d_pll_w0f = fll_bw_hz / 0.53F;
|
|
d_pll_w0f2 = d_pll_w0f * d_pll_w0f;
|
|
}
|
|
else
|
|
{
|
|
/*
|
|
* 2nd order PLL with 1st order FLL assist
|
|
*/
|
|
d_pll_a2 = 1.414;
|
|
d_pll_w0p = pll_bw_hz / 0.53F;
|
|
d_pll_w0p2 = d_pll_w0p * d_pll_w0p;
|
|
d_pll_w0f = fll_bw_hz / 0.25F;
|
|
}
|
|
}
|
|
|
|
|
|
void Tracking_FLL_PLL_filter::initialize(float d_acq_carrier_doppler_hz)
|
|
{
|
|
if (d_order == 3)
|
|
{
|
|
d_pll_x = 2.0F * d_acq_carrier_doppler_hz;
|
|
d_pll_w = 0;
|
|
}
|
|
else
|
|
{
|
|
d_pll_w = d_acq_carrier_doppler_hz;
|
|
d_pll_x = 0;
|
|
}
|
|
}
|
|
|
|
|
|
float Tracking_FLL_PLL_filter::get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s)
|
|
{
|
|
float carrier_error_hz;
|
|
if (d_order == 3)
|
|
{
|
|
/*
|
|
* 3rd order PLL with 2nd order FLL assist
|
|
*/
|
|
d_pll_w = d_pll_w + correlation_time_s * (d_pll_w0p3 * PLL_discriminator + d_pll_w0f2 * FLL_discriminator);
|
|
d_pll_x = d_pll_x + correlation_time_s * (0.5F * d_pll_w + d_pll_a2 * d_pll_w0f * FLL_discriminator + d_pll_a3 * d_pll_w0p2 * PLL_discriminator);
|
|
carrier_error_hz = 0.5F * d_pll_x + d_pll_b3 * d_pll_w0p * PLL_discriminator;
|
|
}
|
|
else
|
|
{
|
|
/*
|
|
* 2nd order PLL with 1st order FLL assist
|
|
*/
|
|
const float pll_w_new = d_pll_w + PLL_discriminator * d_pll_w0p2 * correlation_time_s + FLL_discriminator * d_pll_w0f * correlation_time_s;
|
|
carrier_error_hz = 0.5F * (pll_w_new + d_pll_w) + d_pll_a2 * d_pll_w0p * PLL_discriminator;
|
|
d_pll_w = pll_w_new;
|
|
/* std::cout << " d_pll_w = " << carrier_error_hz << ", pll_w_new = " << pll_w_new
|
|
<< ", PLL_discriminator=" << PLL_discriminator
|
|
<< " FLL_discriminator =" << FLL_discriminator
|
|
<< " correlation_time_s = " << correlation_time_s << "\r\n"; */
|
|
}
|
|
|
|
return carrier_error_hz;
|
|
}
|