nav.c

GPS导航定位程序

#include "includes.h"

/****************************************************************************
* Function: void UpdateNavigation(void)
*
* Updates the navigation solution when measurements are available.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void UpdateNavigation(void)
{
    int number_of_observations;    /* No. of observation blocks to process */

    TCBComment("Navigating");
    number_of_observations = NOBS;

    /* Process observation data while there is still some pending. */

    while(ObsPending) 
    { 
	/* Update the maximum obseravtions pending variable if it's current
	   value has been exceeded. */

	if(ObsPending > MaxObsPending)
	    MaxObsPending = ObsPending;

	/* Attempt to produce a position fix or update integrated carrier
	   phase data. */

	Navigate(obsbuff+pobs);

	/* Increment the observations buffer pointer, modulo NOBS. */

	pobs = (pobs+1) % NOBS;
	ObsPending--;                  /* One less observation to process. */

	Keyboard();                        /* Perform keyboard processing. */

	/* This is an escape from the loop that is needed if the computer
	   falls too far behind and never empties the observation queue. */

	if(--number_of_observations==0) break;
    }
}

/****************************************************************************    
* Function: void Navigate(obsstruc *obs)
*
* Attempts to produce a navigation fix or updates the integrated carrier
* phase data using the current observation block.
*
* Input: obs - pointre to the observation block to be processed.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void Navigate(obsstruc *obs)
{
    /* Only do a full solution every 10th TIC else just update the
       integrated carrier phase data */

    if(obs->obstm%10==0)
	NavigateFull(obs);
    else 
	GetNavDataICP(obs);
}

/****************************************************************************    
*
* Function: void NavigateFull(obsstruc *obs)
*
* Attempts to produce a navigation fix.
*
* Input: obs - pointer to the observation block to be processed.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void NavigateFull(obsstruc *obs)
{
    char buff[90];                         /* Message displayed on screen. */

    int channel;                                        /* Channel number. */

    unsigned long svcurrent;     /* Bitmap of satellites used in solution. */

    double north_velocity;                    /* North velocity component. */
    double east_velocity;                      /* East velocity component. */
    double solution_matrix[MAXCHANNELS][4];   /* Direction cosines to SVs. */
    double pseudo_range[MAXCHANNELS];
    double pseudo_range_rate[MAXCHANNELS];

    ompstruc omp;                    /* Observed - predicted measurements. */
    navstatestruc NewNavState;        /* Copy of current navigation state. */
    clockmodelstruc NewClkModel;           /* Copy of current clock model. */

    PROTECT++;
    NewNavState = CurNavState;                /* Current navigation state. */
    PROTECT--;

    PROTECT++;
    NewClkModel = CurClkModel;                     /* Current clock model. */
    PROTECT--;

    /* Zero out the quantities reported to the display task. */
    
    for(channel=0; channel<MAXCHANNELS; channel++)
    {
	NewNavState.Erngerr[channel] = 0;
	NewNavState.Erraterr[channel] = 0;
	NewNavState.Eicperr[channel] = 0;
	NewNavState.EPRcorr[channel] = NO_DGPS; 
    }
    
    /* Extract observation data and process to observed-predicted. */

    GetNavDataFull(obs,&NewNavState,&omp,solution_matrix,&svcurrent,
		   pseudo_range,pseudo_range_rate);

    if(Rx2TIC)                        /* Update the RINEX data if enabled. */
    {
	int obswk;                     /* The observation GPS week number. */
	int y,m,d,hh,mm;        /* Year, month, day, hour & minute of obs. */
	int channel;                              /* Channel loop counter. */

	double obssec;                      /* The observation GPS second. */
	double second;                    /* The second (0-60) of the obs. */
	double delta_time;            /* Time between obs and clock model. */
	double RcvrClkOff;                       /* Receiver clock offset. */

	/* Is it time to send out a Rinex2 observation? */

	if(obs->obstm>=Rx2TIC)
	{
	    TICToGpsTime(obs->obstm,&obswk,&obssec);
	    GpsTimeToUTCDate(obswk,obssec,&y,&m,&d,&hh,&mm,&second);
	    for(channel=0; channel<ActiveChannels; channel++)
	    {
		if(obs->obspresent[channel]==YES)
		{
		    /* L1: Integrated carrier beat phase measurement,
		       cycles. */

		    Rx2Obsv[channel*3] = ICP.Beat_Phase[channel];
		    Rx2Lli[channel*3] = 
				 obs->LostCarrierLockDuringLastTIC[channel];
		    Rx2Sigstr[channel*3] = 0;

		    /* C1: C/A code-derived pseudorange, meters. */

		    Rx2Obsv[channel*3+1] = pseudo_range[channel];
		    Rx2Lli[channel*3+1] = 
				    obs->LostCodeLockDuringLastTIC[channel];
		    Rx2Sigstr[channel*3+1] = 0;

		    /* D1: Doppler (including clock-induced component), Hz. */

		    Rx2Obsv[channel*3+2] = 
			-pseudo_range_rate[channel]*1575.42E6/SPEED_OF_LIGHT;
		    Rx2Lli[channel*3+2] = 
				 obs->LostCarrierLockDuringLastTIC[channel];
		    Rx2Sigstr[channel*3+2] = 0;
		}
	    }

	    delta_time = (double)(obs->obstm - NewClkModel.RCOtic)*
					TIC_PERIOD*(1.0-NewClkModel.RCOrate);
	    RcvrClkOff = SECONDS_IN_WEEK*NewClkModel.RCOwk
			 + NewClkModel.RCOsec
			 + delta_time*NewClkModel.RCOrate;
	    Rx2OBS(y,m,d,hh,mm,second,0,NumSatsInSoln,obs->obspresent,
		   obs->sv,RcvrClkOff,3,Rx2Obsv,Rx2Lli,Rx2Sigstr);
	    Rx2TIC += Rx2Interval * 10ul;               /* 10 TICs/second. */
	}
    }
    
    /* Check if observations were missed and take the appropriate action. */

    if(MissedObservations())
    {
	PROTECT++;
	CurNavState = NewNavState;
	PROTECT--;
	return;                       /* Data was missed - bad navigation. */
    }

    /* Check if still waiting to set the clock from the data message. */ 

    if(NewClkModel.RCOtic==0)
    {
	sprintf(buff,"Waiting for satellite clock and ephemeris data.    ");
	OutputString(2,23,buff);
	NewNavState.navmode = FIX_NO;
	PROTECT++;
	CurNavState = NewNavState;
	PROTECT--;
	return;
    }

    NewNavState.nsats = NumSatsInSoln;

    /* If the number of satellites is less than 3 in normal mode or less than
       4 in Cold Start mode then cannot perform any sort of solution. */

    if(NumSatsInSoln<3 || (NumSatsInSoln<4 && TrackMode==COLD_START))
    {
	sprintf(buff,"INSUFFICIENT PSEUDORANGES FOR NAV (%d)                ",
		NumSatsInSoln);
	OutputString(2,23,buff);
	NewNavState.navmode = FIX_NO;
	PROTECT++;
	CurNavState = NewNavState;
	PROTECT--;
	return;  
    }

    /* Construct the navigation matrix from the covariance matrix and the 
       solution matrix. */

    NewNavState.toa = obs->obstm;                /* Time of applicability. */
    NewNavState.tofnavmat = obs->obstm;
    NewNavState.svnavmat = svcurrent;              /* SV used in solution. */
    NewNavState.altitude_aiding = NO;         /* No altitude aiding - yet. */

    if(BuildNavMat(solution_matrix,&NewNavState,&omp)==FALSE)
	return;

    /* Perform the solution update with the navigation matrix and the matrix
       of pseudo-ranges. */

    if(SolutionUpdate(&NewNavState,&NewClkModel,omp,obs->obstm)==FALSE)
	return;
    
    /* Convert the new xyz position to longitude, latitude and height. */

    XYZToLatLonHgt(NewNavState.x,NewNavState.y,NewNavState.z,
		   &NewNavState.lat,&NewNavState.lon,&NewNavState.hgt);

    /* Convert the llh co-ordinates to the topocentric North, East and Up. */

    LatLonToNorthEastUp(NewNavState.lat,NewNavState.lon,NewNavState.topo);

    /* Express velocity as speed (m/s) and true heading (degrees E of N). */

    north_velocity = NewNavState.topo[0][0]*NewNavState.velx
		     + NewNavState.topo[0][1]*NewNavState.vely
		     + NewNavState.topo[0][2]*NewNavState.velz;

    east_velocity = NewNavState.topo[1][0]*NewNavState.velx
		    + NewNavState.topo[1][1]*NewNavState.vely
		    + NewNavState.topo[1][2]*NewNavState.velz;

    /* Calculate the rate-of-climb, heading and speed. */

    NewNavState.roc = NewNavState.topo[2][0]*NewNavState.velx
		      + NewNavState.topo[2][1]*NewNavState.vely
		      + NewNavState.topo[2][2]*NewNavState.velz;

    NewNavState.hdg = atan2(east_velocity,north_velocity)*180.0/PI;
    NewNavState.hdg = fmod(NewNavState.hdg+360.0,360.0);

    NewNavState.speed = sqrt(DSquare(north_velocity)+DSquare(east_velocity));

    if(NewNavState.altitude_aiding==YES)   /* Display the navigation mode. */
    {
	sprintf(buff,
	    "2-D GPS NAVIGATION IN PROGRESS (%d Pseudoranges)          ",
	    NumSatsInSoln-1);
	OutputString(2,23,buff);
	NewNavState.navmode = FIX_2D;
    }
    else
    {
	if(NewNavState.hgt>HEIGHT_LIMIT)
	{
	    sprintf(buff,"NO NAV: HEIGHT LIMITATION EXCEEDED"
			 "                    ");
	    NewNavState.navmode = FIX_NO;
	}
	else if(NewNavState.speed>SPEED_LIMIT)
	{
	    sprintf(buff,"NO NAV: VELOCITY LIMITATION EXCEEDED"
			 "                  ");
	    NewNavState.navmode = FIX_NO;
	}
	else
	{
	    sprintf(buff,"3-D GPS NAVIGATION IN PROGRESS (%d Pseudoranges)"
			 "          ",NumSatsInSoln);
	    NewNavState.navmode = FIX_3D;
	}
	    
	OutputString(2,23,buff);
    }

    PROTECT++;                     /* Update the current navigation state. */
    CurNavState = NewNavState;
    PROTECT--;

    PROTECT++;                          /* Update the current clock model. */
    CurClkModel = NewClkModel;
    PROTECT--;

    /* If still in Cold Start mode and 10 fixes have been made so that
       convergence should have occurred then switch to Highest Elevations. */

    if(TrackMode==COLD_START) 
    {
	if(FixesInColdStart>9)
	{   
	    /* Don't lose tracked SVs. */

	    for(channel=0;channel<ActiveChannels;channel++)
		svsel[channel] = CH[channel].SV;

	    TrackMode = HIGHEST_ELEVATIONS;
	}
	else
	    FixesInColdStart++;
    }
}

/****************************************************************************    
*
* Function: void GetNavDataFull(obsstruc *obs, navstatestruc *NewNavState,
*                               ompstruc *omp, unsigned long *svcurrent,
*                               double pseudo_range,
*                               double pseudo_range_rate)
*
* Updates the navigation matrix with code and carrier phase measurements.
*
* Input: obs - pointer to the observation block to be processed.
*        NewNavState - pointer to the current navigation results structure.
*
* Output: omp - pointer to the  OMP data (range and range rate).
*         svcurrent - pointer to the bitmap of SVs used in the solution.
*         pseudo_range - the pseudo-range
*         pseudo_range_rate - the pseudo-range rate