tracking.c

此程序为GPS接收机的源码

            channel_locked( ch);

            // Officially switch modes
            CH[ch].state = CHANNEL_LOCK;
        }
        else
        {
            // We lost the pullin. Eventually, do a nice transition back to
            // confirm and/or acquire. For now, to be paranoid, just kill
            // the channel.
            CH[ch].state = CHANNEL_OFF;

#if 0       // Code that was here:
            CH[ch].state = CHANNEL_ACQUISITION;
            clear_message( ch);   // flag the message_thread that the
                                        // past subframes are no longer valid
            CH[ch].codes = 0;
            CH[ch].code_freq = CODE_REF + CodeCorr;
            set_code_dco_rate( ch, CH[ch].code_freq);

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

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

 PARAMETERS  char ch  , channel number

 PURPOSE track carrier and code, and partially decode the navigation message
 (to determine TOW, subframe etc.)

 WRITTEN BY
 Clifford Kelley
 added Carrier Aiding as suggested by Jenna Cheng, UCR
*******************************************************************************/
static void
lock( unsigned long ch)
{
    long ddf, ddcar;    //
    long q_sum, i_sum;  //

    //    unsigned short BackToPullIn = 0;

    /* 50 Hz (20ms) tracking loop */
    CH[ch].ms_count++;
    if( CH[ch].ms_count > 19) CH[ch].ms_count = 0; // Efficient modulo 20

    CH[ch].q_dither_20 += CH[ch].q_dither;
    CH[ch].q_prompt_20 += CH[ch].q_prompt;
    CH[ch].i_dither_20 += CH[ch].i_dither;
    CH[ch].i_prompt_20 += CH[ch].i_prompt;

    /* Carrier loop */
    q_sum = CH[ch].q_dither + CH[ch].q_prompt;
    i_sum = CH[ch].i_dither + CH[ch].i_prompt;

    if( (q_sum != 0) || (i_sum != 0))
    { // osgps fscanf's for the tracking gains, see GPSRCVR.CPP
        // TrackCarrK is currently set to -9 in main.c and never changed.
        // TrackCarrK is only used here
        CH[ch].dcarr =
            TrackCarrK * sgn( q_sum) * (i_sum << 14) / lmag( q_sum, i_sum);
        // == Gain * Sgn[q] * Cos[Phi]

        // TrackCarrD is currently set to 21 in main.c and never changed.
        // TrackCarrD is only used here
        ddcar = TrackCarrD * (CH[ch].dcarr - CH[ch].dcarr1);
        // == Gain * delta_ddcar

        CH[ch].carrier_freq += (CH[ch].dcarr + ddcar) >> 14; // feedback
        set_carrier_dco_rate( ch, CH[ch].carrier_freq);

        // the carrier frequency and the code frequency should be in
        // concert (+/-)... change it here, update it later
    }


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

    if( CH[ch].ms_count == 19)
    {
        // Work on sum of last 20ms of data
        CH[ch].prompt_mag = lmag( CH[ch].i_prompt_20, CH[ch].q_prompt_20);
        CH[ch].dither_mag = lmag( CH[ch].i_dither_20, CH[ch].q_dither_20);
        CH[ch].sum += CH[ch].prompt_mag + CH[ch].dither_mag;

        /* Code tracking loop */
        if( CH[ch].prompt_mag | CH[ch].dither_mag)
        {
            /* Without carrier aiding. */
            // Like before, TrackCodeK fixed @ 55 in main.c
            CH[ch].delta_code_freq =
                TrackCodeK * (CH[ch].prompt_mag - CH[ch].dither_mag);
            // Like before, TrackCodeD fixed @ 3 in main.c
            ddf = TrackCodeD * (CH[ch].delta_code_freq -
                    CH[ch].delta_code_freq_old);
            // Like before, TrackDiv fixed @ 19643 in main.c
            CH[ch].code_freq += (CH[ch].delta_code_freq + ddf) / TrackDiv;

            /* With carrier aiding. */
            /*
            ddf = (TrackCodeK*(
            (CH[ch].prompt_mag - CH[ch].dither_mag) /
            (CH[ch].prompt_mag + CH[ch].dither_mag)
                   ))<<11;

            CH[ch].delta_code_freq += ddf;

            CH[ch].code_freq = (ddf + CH[ch].delta_code_freq/TrackCodeD)>>8
            + (CARRIER_REF - CH[ch].carrier_freq)/CC_SCALE + CODE_REF;
            */

            set_code_dco_rate( ch, CH[ch].code_freq);
        }

        CH[ch].delta_code_freq_old = CH[ch].delta_code_freq;

        /* Data bit */
        CH[ch].bit = ((CH[ch].q_prompt_20 + CH[ch].q_dither_20) > 0);

        // Flag that this bit is ready to process (written to the message_flag
        // in the tracking() function after we've gone through all the channels
        nav_bits_ready |= (1 << ch);

        // Increment the time, in bits, since the week began. Used in
        // the measurement thread. Also set to the true time of
        // week when we get the TOW from a valid subframe in the
        // messages thread.
        CH[ch].time_in_bits++;
        if( CH[ch].time_in_bits >= BITS_IN_WEEK)
            CH[ch].time_in_bits -= BITS_IN_WEEK;

        // Check the satellite signal strength every 100ms
        CH[ch].check_average++;
        if( CH[ch].check_average > 4)
        {
            CH[ch].check_average = 0;

            CH[ch].avg = CH[ch].sum / 10; // FIXME stop computing this on the
                                         // fly. Pre-multiply the test threshold
                                        // Also, do this whole thing better.
            CH[ch].sum = 0;

            if( (CH[ch].bit_sync) &&
                (CH[ch].avg < SIGNAL_LOSS_AVG)) /* 33 dBHz */
            {
                /* Signal loss. Clear channel. */
                CH[ch].state = CHANNEL_OFF;
                // BackToPullIn = 1;
            }
        }

        /* Clear coherent accumulations */
        CH[ch].i_dither_20 = 0;
        CH[ch].i_prompt_20 = 0;
        CH[ch].q_dither_20 = 0;
        CH[ch].q_prompt_20 = 0;
    }

#if 0
    // eventually we should switch back to pullin if we lose lock;
    // for now we're just going to kill the channel.
    // NOTE: This is dead with the BackToPullIn above commented out.
    if( BackToPullIn == TRUE) /* Tracking failed, Go back to pull in. */
    {
        CH[ch].state = CHANNEL_PULL_IN;
        clear_message( ch);
        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 = 0x54321; /* Some garbage data */
        CH[ch].ms_count = 0;
    }
#endif
}


/*******************************************************************************
 FUNCTION tracking( void)
 RETURNS  None.

 PARAMETERS  None

 PURPOSE Main routine which runs on an accum_int.

 WRITTEN BY
 Clifford Kelley
 added Carrier Aiding as suggested by Jenna Cheng, UCR
*******************************************************************************/
void
tracking( void)
{
    unsigned short      ch;
    cyg_flag_value_t    channels_off;

    volatile union _gp4020_channel_accumulators *accumulators =
        (volatile union _gp4020_channel_accumulators *)GP4020_CORR_CHANNEL_ACCUM;
    volatile union _gp4020_channel_control *channel_control =
        (volatile union _gp4020_channel_control *)GP4020_CORR_CHANNEL_CONTROL;


    // "accum_status_a" and "accum_status_b" are set in the accum_isr() when
    // clearing the interrupt

    // Sequentially check each channel for new data. Do this FAST so that
    // we get the data before we have an overrun. Note that reading
    // GP3020_CORR_ACCUM_STATUS_A also clears the ACCUM_INT interrupt.
    for( ch = 0; ch < N_CHANNELS; ch++)
    {
        // Check the bits in the accumulator status register (which was read in
        // the accum_isr() function) - DON'T READ IT AGAIN HERE! - to see if
        // the channel has dumped and is thus ready to be read.

        if( accum_status_a & (1 << ch))
        {
            /* Collect channel data accumulation. */
            CH[ch].i_dither = accumulators[ch].read.i_track;
            CH[ch].q_dither = accumulators[ch].read.q_track;
            CH[ch].i_prompt = accumulators[ch].read.i_prompt;
            CH[ch].q_prompt = accumulators[ch].read.q_prompt;

            // If the last dump was the first dump in a new satellite
            // message data bit, then lock() sets the load_1ms_epoch_flag
            // so that we can set the 1m epoch counter here. Why here?
            // GP4020 Baseband Processor Design Manual, pg 60: "Ideally,
            // epoch counter accesses should occur following the reading of
            // the accumulation register at each DUMP." Great, thanks for
            // the tip, now how 'bout you tell us WHY?!
            if( CH[ch].load_1ms_epoch_count)
            {
                /* Load 1 ms epoch counter */
                channel_control[ch].write.epoch_count_load = 1;
                /* WAIT 300NS UNTIL NEXT ACCESS */

                CH[ch].load_1ms_epoch_count = 0;
            }

            // We expect the 1ms epoch counter to always stay sync'd until
            // we lose lock. To sync the 20ms epoch counter (the upper bits)
            // we wait until we get a signal from the message thread that
            // we just got the TLM+HOW words; this means we're 60 bits into
            // the message. Since the damn epoch counter counts to *50* (?!)
            // we mod it with 60 which gives us 10 (0xA00 when shifted 8).
            // --What???
            if( CH[ch].sync_20ms_epoch_count)
            {
                channel_control[ch].write.epoch_count_load =
                    (channel_control[ch].read.epoch_check & 0x1f) | 0xa00;
                /* WAIT 300NS UNTIL NEXT ACCESS */
                CH[ch].sync_20ms_epoch_count = 0;
            }

            if( accum_status_b & (1 << ch)) // Check for missed DUMP
            {
                CH[ch].missed++;
                //Explicit reset of ACCUM status reg.s
                accumulators[ch].write.accum_reset = 0;
            }
        }
    }

    // Clear the IPC shadows (see below).
    nav_bits_ready = 0;
    channels_off = 0;

    // Finally, in a second (slower) loop, track each channel that dumped. Note
    // that channels which are CHANNEL_OFF will be allocated satellites to
    // track in a mainline thread.
    for( ch = 0; ch < N_CHANNELS; ch++)
    {
        if( (accum_status_a & (1 << ch)) && (CH[ch].state != CHANNEL_OFF))
        // We already checked for dumped channels above, can  we somehow
        // avoid checking this again??
        {
            switch( CH[ch].state)
            {
                case CHANNEL_ACQUISITION:
                    acquire( ch);
                    break;
                case CHANNEL_CONFIRM:
                    confirm( ch);
                    break;
                case CHANNEL_PULL_IN:
                    pull_in( ch);
                    break;
                case CHANNEL_LOCK:
                    lock( ch);
                    break;
                default:
                    CH[ch].state = CHANNEL_OFF;
                    // TODO: assert an error here
                    break;
            }
        }
        // If the channel is off, set a flag saying so
        if( CH[ch].state == CHANNEL_OFF)
            channels_off |= (1 << ch);

    }

    // If any channels are off, signal the allocation thread that it should
    // try to use them.
    if( channels_off)
        cyg_flag_setbits( &allocate_flag, channels_off);

    // Signal the message_thread that there are new nav-bits available.
    // Besides convenience, calling setbits here only once instead of per-bit
    // is a speed optimization.
    if( nav_bits_ready)
        cyg_flag_setbits( &message_flag, nav_bits_ready);
}