tracking.c

此程序为GPS接收机的源码

                                 // in this application anyway?
#endif
}


/******************************************************************************
 FUNCTION acquire( unsigned long ch)
 RETURNS  None.

 PARAMETERS
 ch  char // Which correlator channel to use

 PURPOSE  to perform initial acquire by searching code and frequency space
 looking for a high correlation

 WRITTEN BY
 Clifford Kelley

 ******************************************************************************/
static void
acquire( unsigned long ch)
{
    volatile union _gp4020_channel_accumulators *accumulators =
        (volatile union _gp4020_channel_accumulators *)
        GP4020_CORR_CHANNEL_ACCUM;

    /* Search carrier frequency bins */
    if( abs( CH[ch].n_freq) <= CarrSrchWidth)
    {
        CH[ch].prompt_mag = smag( CH[ch].i_prompt, CH[ch].q_prompt);
        CH[ch].dither_mag = smag( CH[ch].i_dither, CH[ch].q_dither);

        // "dither" is set to LATE, this "&&" seems funny
        // I think the deal is, if this is ||, then there are a fairly large
        // number of false hits, and the search process spends a lot more time
        // in the confirm() function, which doesn't scan, so the overall search
        // rate drops and finding all the satellites takes longer.
        // Probably this illustrates a flaw in the overall search process.
        if( (CH[ch].prompt_mag > AcqThresh) && (CH[ch].dither_mag > AcqThresh))
        {
            CH[ch].state = CHANNEL_CONFIRM;
            CH[ch].i_confirm = 0;
            CH[ch].n_thresh = 0;
            return;
        }

        /* No satellite yet; try delaying the code DCO 1/2 chip */
        accumulators[ch].write.code_slew_counter = 1;
        /* WAIT 300NS UNTIL NEXT ACCESS */
        CH[ch].codephase++; // count how many code phases we've searched

        // This code was commented out... why?
        // Supposedly, we can use tracking and prompt channels in parallel.
        // Are we setting the mode right? See TRACK_SEL in the CHx_SATCNTL
        // register.
        /*
        accumulators[ch].write.code_slew_counter = 2;
        CH[ch].codephase += 2;
        */

        if( CH[ch].codephase >= 2044) // PRN code length in 1/2 chips -2
        /* I'm not convinced this number is right. 2045 sounds better
         * since slew increment is (1) */
        // All code offsets have been searched; try another frequency bin
        {
            CH[ch].codephase = 0; // reset code phase count

            /* Move to another frequency bin */
            // Note the use of carrier_corr, this is meant to be a correction
            // for estimated TCXO frequency error, currently set to zero.
            // See the comment in cold_allocate_channel()
            /* Dither around the reference frequency in linearly increasing
             * steps (Why is this better than a sweep?) */
            // Generate a search sequence: 0, 1, -1, 2, -2, ...
            // This can be re-written to avoid the multiply.
            if( CH[ch].n_freq & 1) // Odd search bins map to the "right"
            {
                CH[ch].carrier_freq = CARRIER_REF + CH[ch].carrier_corr +
                    CarrSrchStep * (1 + (CH[ch].n_freq >> 1));
            }
            else // Even search bins are to the "left" of CARRIER_REF
            {
                CH[ch].carrier_freq = CARRIER_REF + CH[ch].carrier_corr -
                    CarrSrchStep * (CH[ch].n_freq >> 1);
            }

    //CH[ch].carrier_freq = CARRIER_REF; /* DELETE ME!!! */
            /* Set carrier DCO */
            set_carrier_dco_rate( ch, CH[ch].carrier_freq);
            /* WAIT 300NS UNTIL NEXT ACCESS */

            CH[ch].n_freq++; // next time try the next search bin
        }
    }
    else
    {
        /* End of frequency search: release the channel. A mainline thread will
         * eventually allocate  another satellite PRN to this channel */
        CH[ch].state = CHANNEL_OFF;
    }
}

/*******************************************************************************
 FUNCTION confirm(unsigned long ch)
 RETURNS  None.

 PARAMETERS
 ch  char  channel number

 PURPOSE  to lock the presence of a high correlation peak using an n of m
 algorithm

 WRITTEN BY
 Clifford Kelley

*******************************************************************************/
static void
confirm( unsigned long ch)
{
    CH[ch].i_confirm++; // count number of confirm attempts

    CH[ch].prompt_mag = smag( CH[ch].i_prompt, CH[ch].q_prompt);
    CH[ch].dither_mag = smag( CH[ch].i_dither, CH[ch].q_dither);

    // In acquire, it's &&
    if( (CH[ch].prompt_mag > AcqThresh) || (CH[ch].dither_mag > AcqThresh))
        CH[ch].n_thresh++; // count number of good hits

    if( CH[ch].i_confirm > 10) // try "n" confirm attempts
    {
        if( CH[ch].n_thresh >= 8) // confirmed if good hits >= "m"
        {
            CH[ch].state = CHANNEL_PULL_IN;

            CH[ch].ch_time = 0;
            CH[ch].sum = 0;
            CH[ch].th_rms = 0;
            CH[ch].bit_sync = 0;

            CH[ch].delta_code_freq_old = 0;
            CH[ch].dcarr1 = 0;
            CH[ch].old_theta = 0;

            CH[ch].ms_sign = 0x12345; /* Some garbage data */
            CH[ch].ms_count = 0;
        }
        else
        {
            /* Keep searching - assumes search parameters are still ok */
            CH[ch].state = CHANNEL_ACQUISITION;

            /* Clear sync flags */
            CH[ch].bit_sync = 0;
        }
    }
}


/*******************************************************************************
 FUNCTION pull_in( unsigned long ch)
 RETURNS  None.

 PARAMETERS
 ch  char  channel number

 PURPOSE
 pull in the frequency by trying to track the signal with a
 combination FLL and PLL
 it will attempt to track for xxx ms, the last yyy ms of data will be
 gathered to determine if we have both code and carrier lock
 if so we will transition to track

 WRITTEN BY
 Clifford Kelley

*******************************************************************************/
static void
pull_in( unsigned long ch)
{
    long ddf, ddcar;
    long q_sum,i_sum;
    long theta,theta_dot,theta_e;
    long wdot_gain;
    long prompt_mag,dither_mag;

    // Why lmag() when it's smag() elsewhere?
    prompt_mag = lmag( CH[ch].i_prompt, CH[ch].q_prompt);
    dither_mag = lmag( CH[ch].i_dither, CH[ch].q_dither);

    CH[ch].sum += (prompt_mag + dither_mag);

    /* pull_in Code tracking loop */ // "dither" set to LATE?
    if( (prompt_mag != 0) || (dither_mag != 0))
    // This branch is probably almost always taken, so maybe skip the test?
    // If both zero the correction is not zero (2nd order) except this test
    // makes it so, which is a kink in the transfer function. Is this right?
    {
        CH[ch].delta_code_freq = PullCodeK *
            ((prompt_mag - dither_mag) << 14) / (prompt_mag + dither_mag);
        ddf = (CH[ch].delta_code_freq - CH[ch].delta_code_freq_old) * PullCodeD;

        /* Don't close the loop for 2 ms to reduce transient effects? */
        if( CH[ch].ch_time > 2)
        {
            CH[ch].code_freq += (CH[ch].delta_code_freq + ddf) >> 14;
            set_code_dco_rate( ch, CH[ch].code_freq);
        }
    }
    CH[ch].delta_code_freq_old = CH[ch].delta_code_freq;

    q_sum = CH[ch].q_dither + CH[ch].q_prompt;
    i_sum = CH[ch].i_dither + CH[ch].i_prompt;

    if( i_sum || q_sum)
        theta = fix_atan2( q_sum, -i_sum);
    else
        theta = CH[ch].old_theta;

    theta_dot = theta - CH[ch].old_theta;
    // theta_dot is a measure of frequency error, used for the FLL

    /* Increase 1 ms epoch counter modulo 20 */
    CH[ch].ms_count++;
    if( CH[ch].ms_count > 19)
        CH[ch].ms_count = 0;

    // Check if the last 20 ms have the same sign and this dump
    // is different: if so, then we just had a bit edge transition
    if( !CH[ch].bit_sync &&
        (((q_sum < 0) && (CH[ch].ms_sign == 0x00000)) ||
         ((q_sum > 0) && (CH[ch].ms_sign == 0xfffff))))
    {
        // Test if last two sums were within 1/4 radian of pi/2
        // Why exactly are we testing for this here?
        if( (labs( labs( theta) - PI_OVER2_SHIFT14) < 4096) &&
              (labs( labs( CH[ch].old_theta) - PI_OVER2_SHIFT14) < 4096))
        {
            // Let the world know we're synced to the satellite message bits
            CH[ch].bit_sync = 1;
            // sync the ms count to the bit stream
            CH[ch].ms_count = 0;
            // set the flag that tells tracking() to set the 1ms epoch counter
            // after the accumulator registers are read: this will sync the
            // epoch counter with the bit stream (and the ms_count, too).
            CH[ch].load_1ms_epoch_count = 1;
        }
    }

    /* Shift sign buffer to left */
    CH[ch].ms_sign = ((CH[ch].ms_sign << 1) & 0x000fffff);

    if( q_sum < 0)
        CH[ch].ms_sign |= 1; /* Set the LSB bit if negative */

    CH[ch].old_theta = theta;

    if( theta > 0) // in lock, theta is near (+/-)pi/2
                  // this pushes theta_e toward zero
                 // Shouldn't we track near zero instead?
        theta_e = theta - PI_OVER2_SHIFT14;
    else
        theta_e = theta + PI_OVER2_SHIFT14;
    // theta_e is the discriminated phase error for the PLL

    // Near the end of pull in, start the phase test
    if( CH[ch].ch_time > (PullInTime - PhaseTest))
        CH[ch].th_rms += (theta_e * theta_e) >> 14;

    /* pull_in Carrier tracking loop */
    if( labs( theta_dot) < 32768) // less than 0.5 radian / ms
    {
        if( q_sum | i_sum) // This check was also done above, redundant?
        {
            wdot_gain = CH[ch].ch_time / 499;
            wdot_gain *= wdot_gain;
            wdot_gain *= wdot_gain; // Net: (ch_time/499)**4
            // As this factor increases the FLL is increasingly ignored
            // in favor of the PLL.
            // (This seems an expensive technique.)

            CH[ch].dcarr =
                PullCarrK * (theta_dot * 5 / (1 + wdot_gain) + theta_e);
            ddcar = PullCarrD * (CH[ch].dcarr - CH[ch].dcarr1);

            if( CH[ch].ch_time > 5) /* Don't close the loop for 5 ms */
            {
                CH[ch].carrier_freq += (CH[ch].dcarr+ddcar) >> 14;
                set_carrier_dco_rate( ch, CH[ch].carrier_freq);
            }
        }
    }

    CH[ch].dcarr1 = CH[ch].dcarr;
    CH[ch].ch_time++;

    /* Done with pull in. Wait until the end of a data bit. */
    // Are we sure we even think we're data-locked now?
    if( (CH[ch].ms_count == 19) && (CH[ch].ch_time >= PullInTime))
    /* A bit transition will happen at the next dump. */
    {
        // This is an odd average since PullInTime is a constant
        CH[ch].avg = CH[ch].sum / PullInTime / 2;
        // Since PhaseTest is a constant, the run-time division could be avoided
        // This probably can't be done now since PhaseTest is stored in a
        // variable.
        CH[ch].th_rms = fix_sqrt( CH[ch].th_rms / PhaseTest);
        // The above two calculations appear to be unnecessary for the if
        // statement on the next line.

        if( (CH[ch].avg > (14 * Rms / 10)) &&
              (CH[ch].th_rms < 12000) && CH[ch].bit_sync)
        /* Bit edge was detected. */
        {
            /* Sufficient signal, transition to tracking mode */
            CH[ch].avg *= 20;

            CH[ch].check_average = 0;
            CH[ch].sum = 0;
            CH[ch].i_prompt_20 = 0;
            CH[ch].q_prompt_20 = 0;
            CH[ch].i_dither_20 = 0;
            CH[ch].q_dither_20 = 0;
            CH[ch].delta_code_freq = 0;

            // Tell the allocate thread that we're locking