allocate.c

GPS导航定位程序

#include "includes.h"

static int srchlist[32];

/****************************************************************************    
* Function: int NextSVToSearch(void)
*
* Returns the PRN number of the next satellite to be searched for in the Cold 
* Start mode. On the first call, and every time a search pass has been
* completed for all 32 SV's, it builds a table of SV's in descending order of 
* predicted elevation. As the tracking channels become available, they are 
* assigned satellites in order of descending elevation. If the user's initial 
* position estimate is at all reasonable, this will speed up the cold start.
*
* Input: None.
*
* Output: None.
*
* Return Value: PRN number of the satellite to serach for.
****************************************************************************/
int NextSVToSearch(void)
{
    int sv;

    if(cursrch++ > MAXSATELLITES)
    {
        /* Build a new search list. */

        for(sv=1;sv<MAXSATELLITES+1;sv++)
        {
            if(alms[sv-1].vflg==1 || ephs[sv-1].vflg==1)
                srchlist[sv-1] = ielvd[sv-1];
            else
                srchlist[sv-1] = -90;
            srchlist[sv-1] = (srchlist[sv-1]<<5)+(sv-1);
      }
      qsort(srchlist,32,sizeof(srchlist[0]),CompareInt);
      cursrch=0;                               /* Start a new search pass. */
    }
    return(srchlist[cursrch] & 0x1F) + 1;
}

/****************************************************************************    
* Function: void UpdateSatelliteAllocation(int highest[])
*
* Updates the satellite tracking according to the current mode of operation.
*
* Input: highest - the satellite numbers ordered by elevation.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void UpdateSatelliteAllocation(int highest[])
{
    int i,j,itemp,high,assigned[MAXCHANNELS],allocatedSV;

    switch(TrackMode)
    {
        case HIGHEST_ELEVATIONS:
            for(i=0;i<ActiveChannels;i++)
                assigned[i] = FALSE;
            for(i=0;i<ActiveChannels;i++)
            {
                high = 0;
                for(j=0;j<ActiveChannels;j++)
                {
                    if(svsel[i] == highest[j])
                    {
                        if(assigned[j]==TRUE)
                            break;                  /* Release duplicates. */
                        assigned[j] = TRUE;
                        high = 1;
                        break;
                    }
                }
                if(high==0)
                    svsel[i] = 0;
            }

            for(i=0;i<ActiveChannels;i++)
            {
                if(svsel[i])
                    continue;
                for(j=0;j<ActiveChannels;j++)
                {
                    if(assigned[j]==TRUE)
                        continue;
                    svsel[i] = highest[j];
                    assigned[j] = TRUE;
                    break;
                }
            }

            for(i=0;i<MAXCHANNELS;i++)
            {
                if(svsel[i])                         /* Predicted Doppler. */
                    SetUpChannel(i,svsel[i],idopp[svsel[i]-1]);
                else                                  /* Idle the channel. */
                    SetUpChannel(i,0,0);                       
            }
            break;

        case SELECT_SATELLITES:
            for(i=0;i<MAXCHANNELS;i++)
            {
                if(svsel[i])                         /* Predicted Doppler. */
                    SetUpChannel(i,svsel[i],idopp[svsel[i]-1]); 
                else                                  /* Idle the channel. */
                    SetUpChannel(i,0,0);                      
            }
            break;

        case COLD_START:
            for(i=0;i<ActiveChannels;i++)
            {
                if(CH[i].SV==0)
                {
                    allocatedSV = FALSE;
                    while(allocatedSV==FALSE)
                    {
                        itemp = NextSVToSearch();
                        allocatedSV = TRUE;
                        for(j=0;j<ActiveChannels;j++)
                            if(itemp==CH[j].SV)
                                allocatedSV = FALSE;
                    }
                    SetUpChannel(i,itemp,0);
                }
            }       

        default:
            break;
    }
}

/****************************************************************************    
* Function: void SetUpChannel(int chan, int newsv, int dopp)
*
* The caller periodically performs a satellite selection process which
* decides which satellites should be tracked and which channels they should
* be assigned to.  This procedure updates the SV's Doppler prediction in the
* channel control block, then if necessary switches the channel to a new
* satellite.
*
* Input: chan - channel number.
*        newsv - the satellite PRN number.
*        dopp - the satellite Doppler.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void SetUpChannel(int chan, int newsv, int dopp)
{
    /* GP2021 tap combinations for the various C/A codes. */

    static unsigned prntaps[45] = 
                  {0x03F6,0x03EC,0x03D8,0x03B0,0x004B,0x0096,0x02CB,0x0196,
                   0x032C,0x03BA,0x0374,0x01D0,0x03A0,0x0340,0x0280,0x0100,
                   0x0113,0x0226,0x004C,0x0098,0x0130,0x0260,0x0267,0x0338,
                   0x0270,0x00E0,0x01C0,0x0380,0x022B,0x0056,0x00AC,0x0158,
                   0x02B0,0x0058,0x018B,0x0316,0x0058,0x02C4,0x010A,0x03E3,
                   0x00F8,0x025F,0x01E7,0x02B5,0x010E};

    unsigned long CodeDCOForPredictedDoppler,CarrDCOForPredictedDoppler;

    /* Just in case, map all unreal SV #'s to SV=0, which idles the channel.
       Set the Doppler prediction to zero for idle channels. */

    if( (newsv<1) || (newsv>32) )
    {
        newsv = 0;
        dopp=0.0;
    }

    /* Generate code and carrier DCO settings corresponding to the precicted
       Doppler shift for this satellite. */

    CodeDCOForPredictedDoppler = CODE_DCO_ZERO_DOPPLER + 
         (long)((double)dopp/L1_TO_CA/CARRIER_DCO_RESOLUTION*CODE_DCO_SCALE);
    CarrDCOForPredictedDoppler = CARR_DCO_ZERO_DOPPLER - 
         (long)((double)dopp/CARRIER_DCO_RESOLUTION*CARRIER_DCO_SCALE);

    disable();      /* While updating vars shared with the correlator ISR. */
    CH[chan].pCODEDCO = CodeDCOForPredictedDoppler;
    CH[chan].pCARRDCO = CarrDCOForPredictedDoppler;
    enable();

    if(CH[chan].SV==newsv)
    {
        /* Channel is already tracking the desired SV, do not reinitiate. */
        return;
    }
    else
    {
        /* Reinitiate tracking if the channel is switching to a new SV. */

        disable();
        if(newsv==0) 
            outpw(CH[chan]._SATCNTL,0x0800);
        else
            outpw(CH[chan]._SATCNTL,prntaps[newsv-1]|0x8000);
        CH[chan].CurrDoppBinIndex = 0;
        CH[chan].TotalCodeMovement = 0;
        CH[chan].CodeDoppBin = CH[chan].CarrDoppBin = 0L;
        CH[chan].CODEDCO = CH[chan].pCODEDCO + CodeDoppFromClk;
        CH[chan].CARRDCO = CH[chan].pCARRDCO + CarrDoppFromClk;
        CH[chan].coasting = CH[chan].carfrlk = CH[chan].corlk = 0;
        CH[chan].LostLockDuringLastTIC = TRUE;
        CH[chan].CdLI = CH[chan].CrfrLI = 0;
        CH[chan].TotalCodeMovement = 0;
        CH[chan].FrameSync = FALSE;
        CH[chan].FrameSyncAndTIC = FALSE;
        outpw(CH[chan]._CARRIER_DCO_INCR_HIGH,CH[chan].CARRDCO>>20);
        outpw(CH[chan]._CARRIER_DCO_INCR_LOW,CH[chan].CARRDCO>>4);
        outpw(CH[chan]._CODE_DCO_INCR_HIGH,CH[chan].CODEDCO>>21);
        outpw(CH[chan]._CODE_DCO_INCR_LOW,CH[chan].CODEDCO>>5);
        enable();
        CH[chan].SV = newsv;                                    /* Engage! */
    }
}

/****************************************************************************    
* Function: void NumberOfDopplerBins(void)
*
* Calculates the number of Doppler bins to search based on the oscillator
* error and the tracking mode.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void NumberOfDopplerBins(void)
{
    unsigned utemp;

    /* Adjust the number of Doppler bins for search according to the
       reference oscillator accuracy and the Doppler bin width. */

    if(TrackMode==COLD_START)
    {
        /* Cold Start: we have no Doppler predictions. Search an additional
           5000 Hz each side of zero Doppler to allow for SV Dopplers. The
           2.0 is because we must search the Doppler range on each side of
           the nominal. */

        utemp = (InitOscAcc*L1/1.0E6+5000.0)*2.0/DoppBinWidth + 0.5;
        if((utemp&1) == 0)
            utemp++;
        NDoppBin = utemp;
    }
    else
    {
        /* Normal Operation: Doppler predictions are presumably correct, so
           search only enough Doppler bins to accommodate the oscillator
           frequency error.  See above for the magic numbers. */

        utemp = OscAcc*L1/1.0E6*2.0/DoppBinWidth + 0.5;
        if((utemp&1) == 0)
            utemp++;
        NDoppBin = utemp;
    }
}