📄 nav.c
字号:
*
* Return Value: None
****************************************************************************/
void GetNavDataFull(obsstruc *obs, navstatestruc *NewNavState,
ompstruc *omp, double solution_matrix[MAXCHANNELS][4],
unsigned long *svcurrent,
double pseudo_range[MAXCHANNELS],
double pseudo_range_rate[MAXCHANNELS])
{
int satellite; /* Satellite being used. */
int channel; /* Channel being processed. */
int gps_week; /* At the observation. */
/* The following variable indicates if this satellite has been used in
the current solution already. This is to stop redundant data being
used when the same satellite is being tracked on more than one
channel. */
int used_this_SV[MAXSATELLITES];
double icperr; /* ICP pseudo-range error (m). */
double code_time; /* Code time of the pseudo-range (s). */
double lmnr[4]; /* Direction cosines of the SV. */
double carrier_dco_freq; /* Carrier DCO frequency (Hz). */
double gps_second; /* At the observation. */
double ionospheric; /* PR correction due to ionosphere. */
double tropospheric; /* PR correction due to troposphere. */
double code_minus_icp; /* code minus icp pseudo-range. */
pseudostruc pseudo; /* Holds the pseudo-range data. */
/* First do some initialisation. */
NumSatsInSoln = 0;
*svcurrent = 0;
for(satellite=0;satellite<MAXSATELLITES;satellite++)
used_this_SV[satellite] = FALSE;
for(channel=0; channel<ActiveChannels; channel++)
{
if(obs->obspresent[channel]==YES)
{
satellite = obs->sv[channel];
if(used_this_SV[satellite]==FALSE)
{
used_this_SV[satellite] = TRUE;
/* Discard satellites without ephemeris data. */
if(ephs[satellite-1].vflg==0)
continue;
/* Discard satellites with bad health codes. */
if(ephs[satellite-1].s1hlth)
continue;
/* Discard satellites below the elevation mask except
when in Cold Start. */
if(ielvd[satellite-1] < ElvMask)
if(TrackMode != COLD_START)
continue;
/* Get the code time for the measurement block and make it
modulo 1 second. */
code_time = 20.0*(obs->epoch_count[channel]>>8)
+(obs->epoch_count[channel]&0x1F)
+(obs->code_phase[channel])/2046.0
+(obs->code_dco_phase[channel])/(2046.0*1024.0)
- 0.2443792766E-3;
code_time /= 1000.0; /* Convert from ms to s. */
if(code_time<0.0)
code_time += 1.0;
code_time = fmod(code_time,1.0);
UpdateICP(obs->LostLockDuringLastTIC[channel],
obs,channel,code_time,
&code_minus_icp,&pseudo.icp_range);
/* The observed code time is modulo 1 second. Compute the
pseudorange by assuming that the current GPS time estimate
is accurate to 1/2 second or better. */
TICToGpsTime(obs->obstm,&gps_week,&gps_second);
pseudo.range = fmod(gps_second,1.0) - code_time;
if(pseudo.range > 0.5)
pseudo.range -= 1.0;
else if(pseudo.range < -0.5)
pseudo.range += 1.0;
pseudo.range = pseudo.range*SPEED_OF_LIGHT;
pseudo_range[channel] = pseudo.range;
carrier_dco_freq = -(obs->carrdco[channel]/CARRIER_DCO_SCALE
- NOMINAL_CARRIER_DCO)*CARRIER_DCO_RESOLUTION;
pseudo.range_rate = -SPEED_OF_LIGHT*carrier_dco_freq/L1;
pseudo_range_rate[channel] = pseudo.range_rate;
/* Obtain observed-predicted values for pseudorange,
range rate, and integrated carrier phase. */
if(ObservedMinusPredicted(satellite,obs->obstm,*NewNavState,
&pseudo,&omp->range[NumSatsInSoln],
&omp->range_rate[NumSatsInSoln],
&icperr,lmnr)==FALSE)
continue;
ionospheric = sviono[satellite-1];
tropospheric = svtropo[satellite-1];
/* Apply either differential or atmospheric corrections. */
if(pseudo.differential_range_correction<1000.0)
{
NewNavState->diff = TRUE;
omp->range[NumSatsInSoln] +=
pseudo.differential_range_correction;
icperr += pseudo.differential_range_correction;
omp->range_rate[NumSatsInSoln] +=
pseudo.differential_range_rate_correction;
}
else
{
NewNavState->diff = FALSE;
omp->range[NumSatsInSoln] -= ionospheric + tropospheric;
icperr -= ionospheric + tropospheric;
}
/* Store the current errors for range, range rate, integrated
carrier phase (if being used) and differential corrections
(if received). */
NewNavState->Erngerr[channel] =
omp->range[NumSatsInSoln];
NewNavState->Erraterr[channel] =
omp->range_rate[NumSatsInSoln];
if(IntegratedCarrier==TRUE)
NewNavState->Eicperr[channel] = icperr;
NewNavState->EPRcorr[channel] =
pseudo.differential_range_correction;
/* Use re-biassed integrated carrier phase for navigation if
its long term average has settled to within 7.0 meters of
the measured pseudorange. */
if(fabs(code_minus_icp)<7.0 && IntegratedCarrier==TRUE)
omp->range[NumSatsInSoln] = icperr;
/* Mark this SV in the SV used bitmap. */
*svcurrent |= (1UL<<(satellite-1));
/* Store the direction cosines in the solution matrix. */
solution_matrix[NumSatsInSoln][0] = lmnr[0];
solution_matrix[NumSatsInSoln][1] = lmnr[1];
solution_matrix[NumSatsInSoln][2] = lmnr[2];
solution_matrix[NumSatsInSoln][3] = 1.0;
NumSatsInSoln++;
} /* Satellite used already. */
} /* Observation not present. */
} /* Channel count. */
}
/****************************************************************************
*
* Function: void GetNavDataICP(obsstruc *obs)
*
* Maintains the integrated carrier phase measurements.
*
* Input: obs - pointer to the observation block to be processed.
*
* Output: None.
*
* Return Value: None
****************************************************************************/
void GetNavDataICP(obsstruc *obs)
{
int channel; /* Channel being processed. */
double code_time; /* Code time of the pseudo-range (s). */
double code_minus_icp; /* Code minus icp pseudo-range. */
pseudostruc pseudo; /* Holds the pseudo-range data. */
/* Check for any missed observations which would mean that the
integrated carrier phase data is not continuous. */
if(MissedObservations())
return;
for(channel=0; channel<ActiveChannels; channel++)
{
if(obs->obspresent[channel]==YES)
{
/* Get the code time for the measurement block. */
code_time = 20.0*(obs->epoch_count[channel]>>8)
+(obs->epoch_count[channel]&0x1F)
+(obs->code_phase[channel])/2046.0
+(obs->code_dco_phase[channel])/(2046.0*1024.0)
- 0.2443792766E-3;
code_time /= 1000.0; /* Convert from milliseconds to seconds. */
if(code_time<0.0)
code_time += 1.0;
code_time = fmod(code_time,1.0);
UpdateICP(obs->LostLockDuringLastTIC[channel],obs,channel,
code_time,&code_minus_icp,&pseudo.icp_range);
}
}
}
/****************************************************************************
*
* Function: void UpdateICP(int lostlock, obsstruc *obs, int channel,
* double code_time, double *code_minus_icp
* double *icp_range)
*
* Updates the integrated carrier phase measurements with the new data.
*
* Input: lostlock - indicates signal lost during last TIC period.
* obs - pointer to the observation block to be processed.
* channel - the channel being processed.
* code_time - the code time of the pseudo-range.
*
* Output: code_minus_icp - code minus icp pseudo-range .
* icp_range - the integrated carrier phase as a pseudo-range.
*
* Return Value: None
****************************************************************************/
void UpdateICP(int lostlock, obsstruc *obs, int channel, double code_time,
double *code_minus_icp, double *icp_range)
{
int gps_week; /* GPS week number. */
double gps_second; /* GPS seconds. */
double delta_carrier_cycles; /* Carrier cycles between TICs. */
double new_icp_range; /* ICP range now. */
/* If signal lock was lost during the last measurement cycle then the
carrier phase data is nolonger continuous and so is reset. */
if(lostlock==TRUE)
{
ICP.Bias[channel] = code_time*SPEED_OF_LIGHT;
ICP.Current_Value[channel] = 0.0;
ICP.Beat_Phase[channel] = 0.0;
*code_minus_icp = 0.0;
}
else
{
/* Get the number of carrier cycles between last 2 TICs. */
delta_carrier_cycles = obs->CCycles[channel] +
(int)(obs->CPatThisTIC[channel] -
obs->CPatPrevTIC[channel])/CARRIER_DCO_PHASE_SCALE;
/* Update the icp beat phase. */
ICP.Beat_Phase[channel] -= (CARRIER_PHASE_ADVANCE_IN_TIC
- delta_carrier_cycles);
/* Update the current icp value with the nominal L1 cycles during
TIC period plus those due to frequency offset. */
ICP.Current_Value[channel] += TIC_PERIOD*L1
+ (CARRIER_PHASE_ADVANCE_IN_TIC
- delta_carrier_cycles);
new_icp_range = ICP.Bias[channel]
+ ICP.Current_Value[channel]*SPEED_OF_LIGHT/L1;
*code_minus_icp = code_time*SPEED_OF_LIGHT
- fmod(new_icp_range,SPEED_OF_LIGHT);
if(*code_minus_icp > SPEED_OF_LIGHT/2)
*code_minus_icp -= SPEED_OF_LIGHT;
else if(*code_minus_icp < -SPEED_OF_LIGHT/2)
*code_minus_icp += SPEED_OF_LIGHT;
/* The bias applied to the icp carrier cycle count to correct it
to a pseudo-range needs to be updated at each iteration by
applying a filtered version of the differnce between the code
and icp values of the pseudo-range. If the difference is greater
than 5m then a wideband filter is applied. */
ICP.Bias[channel] +=
(*code_minus_icp)/((fabs(*code_minus_icp)>5.0)? 8.0:128.0);
}
/* Express integrated carrier phase as an equivalent pseudorange. */
*icp_range = ICP.Current_Value[channel]/L1
+ ICP.Bias[channel]/SPEED_OF_LIGHT;
TICToGpsTime(obs->obstm,&gps_week,&gps_second);
*icp_range = fmod(gps_second,1.0) - fmod(*icp_range,1.0);
if(*icp_range>0.5)
*icp_range -= 1.0;
else if(*icp_range<-0.5)
*icp_range += 1.0;
*icp_range = *icp_range * SPEED_OF_LIGHT;
}
/****************************************************************************
*
* Function: int MissedObservations(void)
*
* Checks if any observations have been missed and if so decrements the
* maximum satellites tracked count.
*
* Input: None.
*
* Output: None.
*
* Return Value: TRUE - Observations have been missed.
* FALSE - Observations have not been missed.
****************************************************************************/
int MissedObservations(void)
{
if(misso!=lastmisso) /* Have missed observations. */
{
lastmisso = misso;
LostObservations = TRUE;
return(TRUE);
}
else
return(FALSE); /* No observations missed. */
}
/****************************************************************************
*
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -