PLL in Tracking for GNSS behaviour

Question

I'm doing work on SDR for GNSS and I'm tracking the signals I/Q using PLL. The calculations for the PLL is done like the following for error estimation:

double PLLAtan::compute(const CorrelationResult &correlation_result)
{
    sampleType I_P = correlation_result.getPromptCorrelator().real();
    sampleType Q_P = correlation_result.getPromptCorrelator().imag();

    double discriminator = static_cast<double>(std::atan(Q_P / I_P)) / (2 * M_PI);
    return discriminator;
}

in each epoch time frame:

pll_fll_filter_.setDiscriminator(pll_disc_->compute(correlation_result));

My problem is the PLL filter is oscillating between 0.98 to 0.96 and it never locks to 1

Should I do something like accumalting the error in the correllation result from the I/Q calculate the error between last I/Qs and current and use it as input for PLL phase error correction ?

    double LoopFilterPLLFLL::updateLoop()
    {
        if(filter_order_ == 2)
        {
            double out_velocity = velocity_integrator_.setInput(w0f_ * fll_discriminator_ + w0_ * w0_ * discriminator_);
            loop_output_        = a2_ * w0_ * discriminator_ + out_velocity;
        }
        if(filter_order_ == 3)
        {
            double out_acceleration
                = acceleration_integrator_.setInput(w0f_ * w0f_ * fll_discriminator_ + w0_ * w0_ * w0_ * discriminator_);
            double out_velocity = velocity_integrator_.setInput(a2_ * w0f_ * fll_discriminator_ + a3_ * w0_ * w0_ * discriminator_
                                                                + out_acceleration);
            loop_output_        = b3_ * w0_ * discriminator_ + out_velocity;
        }
        return (loop_output_);
    }


void LoopFilterPLLFLL::setLoopParameters()
{
    LoopFilter::setLoopParameters(fp_.filter_order_pllfll, fp_.update_rate, fp_.narrow_noise_bandwidth_pll);

    calcLoopParameter(0.0, fp_.wide_noise_bandwidth_fll);
}

void LoopFilterPLLFLL::calcLoopParameter(double pll_noise_bandwidth, double fll_noise_bandwidth)
{
    if(filter_order_ == 2)
    {
        w0f_ = fll_noise_bandwidth / 0.25;
        w0_  = pll_noise_bandwidth / 0.53;
    }

    if(filter_order_ == 3)
    {
        w0f_ = fll_noise_bandwidth / 0.53;
        w0_  = pll_noise_bandwidth / 0.7845;
    }
}

Gain Parameters:

 "pll": {
                "filter-order": 2,
                "discriminator": "PLL-ATAN",
                "wide-bandwidth": 20,
                "narrow-bandwidth": 5
            },
            "fll": {
                "discriminator": "FLL-DP",
                "wide-bandwidth": 10,
                "narrow-bandwidth": 2
            }