📄 takemeas.c
字号:
#include "includes.h"
/****************************************************************************
* Function: void TakeMeasPhase(void)
*
* Gets the measurement blocks at each TIC and stores the data if valid. Also
* updates the currently searched frequency cell if appropriate.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void TakeMeasPhase(void)
{
GetCorrelatorMeasurements();
if(ObservationReady==TRUE) /* An observation(s) is ready. */
{
ObservationReady=FALSE;
/* If there is room in the circular observations buffer then update
the pointer to it. Else signal that the buffer is full and
decrement the number of active channels. */
if(ObsPending<(NOBS-1))
{
if(++cobs>=NOBS)
cobs=0;
ObsPending++; /* Another observation needs processing. */
}
else /* No room in the observations buffer. */
{
misso++;
cobs = (pobs+1) % NOBS;
ObsPending = 1;
LostObservations = TRUE;
}
TIC++;
EXECUTIC++;
}
}
/****************************************************************************
* Function: void GetCorrelatorMeasurements(void)
*
* If a TIC has occurred then collects measurement data from all active
* channels.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void GetCorrelatorMeasurements(void)
{
int meas_status_a; /* Contents of status register MEAS_STATUS_A. */
int channel; /* Channel counter. */
/* Check for missed measurement data on each channel. */
meas_status_a = inpw(TR(MEAS_STATUS_A));
/* Only check for measurement data when a TIC occurs. */
if(meas_status_a&0x2000)
{
/* Only do the active channels. The 'OR' with 0x2000 retains the TIC
bit of the status register. */
meas_status_a &= (ActiveChannelMask|0x2000);
/* First check for missed data. */
if(meas_status_a&0x0FFF)
{
if(meas_status_a&0x001)
{
missm++;
CH[0].LostCodeLockDuringLastTIC = TRUE;
CH[0].LostCarrierLockDuringLastTIC = TRUE;
}
if(meas_status_a&0x002)
{
missm++;
CH[1].LostCodeLockDuringLastTIC = TRUE;
CH[1].LostCarrierLockDuringLastTIC = TRUE;
}
if(meas_status_a&0x004)
{
missm++;
CH[2].LostCodeLockDuringLastTIC = TRUE;
CH[2].LostCarrierLockDuringLastTIC = TRUE;
}
if(meas_status_a&0x008)
{
missm++;
CH[3].LostCodeLockDuringLastTIC = TRUE;
CH[3].LostCarrierLockDuringLastTIC = TRUE;
}
if(meas_status_a&0x010)
{
missm++;
CH[4].LostCodeLockDuringLastTIC = TRUE;
CH[4].LostCarrierLockDuringLastTIC = TRUE;
}
if(meas_status_a&0x020)
{
missm++;
CH[5].LostCodeLockDuringLastTIC = TRUE;
CH[5].LostCarrierLockDuringLastTIC = TRUE;
}
if(meas_status_a&0x040)
{
missm++;
CH[6].LostCodeLockDuringLastTIC = TRUE;
CH[6].LostCarrierLockDuringLastTIC = TRUE;
}
if(meas_status_a&0x080)
{
missm++;
CH[7].LostCodeLockDuringLastTIC = TRUE;
CH[7].LostCarrierLockDuringLastTIC = TRUE;
}
if(meas_status_a&0x100)
{
missm++;
CH[8].LostCodeLockDuringLastTIC = TRUE;
CH[8].LostCarrierLockDuringLastTIC = TRUE;
}
if(meas_status_a&0x200)
{
missm++;
CH[9].LostCodeLockDuringLastTIC = TRUE;
CH[9].LostCarrierLockDuringLastTIC = TRUE;
}
if(meas_status_a&0x400)
{
missm++;
CH[10].LostCodeLockDuringLastTIC = TRUE;
CH[10].LostCarrierLockDuringLastTIC = TRUE;
}
if(meas_status_a&0x800)
{
missm++;
CH[11].LostCodeLockDuringLastTIC = TRUE;
CH[11].LostCarrierLockDuringLastTIC = TRUE;
}
}
/* Now get the current data. */
for(channel=0;channel<ActiveChannels;channel++)
{
if(CH[channel].SV!=FALSE)
{
NextFrequencyBin(&CH[channel]);
TakeMeas(&CH[channel]);
}
}
}
}
/****************************************************************************
* Function: void NextFrequencyBin(register chanstruc _ds *CHPTR)
*
* Determines if the current frequency bin has been searched for all code
* phase and selects the next frequency bin the signal has not been found.
*
* Input: *CHPTR - parameter block for the channel in question.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void NextFrequencyBin(register chanstruc _ds *CHPTR)
{
int freq_bins_searched; /* The number of frequency bins searched. */
int next_freq_bin; /* Bin number for the next search. */
unsigned code_phase; /* CODE_PHASE register contents. */
unsigned code_dco_phase; /* CODE_DCO_PHASE register contents. */
long delta_code_phase; /* The change in code phase between TICs. */
/* Get the contents of the CODE_PHASE and CODE_DCO_PHASE registers.
These form the code phase sampled at the TIC. */
code_phase = inpw(CHPTR->_CODE_PHASE);
code_dco_phase = inpw(CHPTR->_CODE_DCO_PHASE);
/* Store the old code phase, the units are 1/512 of a chip. */
CHPTR->old_codeph = CHPTR->codeph;
/* Get the new code phase. */
CHPTR->codeph = (((unsigned long)code_phase)<<10) + code_dco_phase;
/* Get the change in code phase between the current and last TIC. */
delta_code_phase = CHPTR->codeph - CHPTR->old_codeph;
if(delta_code_phase>HALF_CODE_LENGTH)
delta_code_phase -= CODE_LENGTH;
else if(delta_code_phase < -HALF_CODE_LENGTH)
delta_code_phase += CODE_LENGTH;
/* Get the total amount of code shift during this search. */
CHPTR->TotalCodeMovement += delta_code_phase;
/* Check if the whole code length has been searched. */
if(labs(CHPTR->TotalCodeMovement) > SEARCH_CODE_LENGTH)
{
/* Reset the the amount of code searched. */
CHPTR->TotalCodeMovement=0;
/* Go on to the next frequency bin. */
freq_bins_searched = CHPTR->CurrDoppBinIndex+1;
/* Check if all frequency bins have been searched. */
if(freq_bins_searched>=NDoppBin)
{
freq_bins_searched=0; /* Start again from 1st frequency bin. */
if(TrackMode==COLD_START) /* Cold start mode then abandon. */
CHPTR->SV=0; /* Search for this satellite. */
}
/* Update the index of the current frequency bin searched. */
CHPTR->CurrDoppBinIndex = freq_bins_searched;
/* Get the code and carrier DCO offsets from the start point for the
centre of the next frequency bin. A search of 10 bins means that 5
bins either side of the start point are searched. An equal number
of bins above and below the start point are always searched. */
if(freq_bins_searched&1)
next_freq_bin = (freq_bins_searched+1) / 2;
else
next_freq_bin = -freq_bins_searched/2;
CHPTR->CodeDoppBin = next_freq_bin*CodeDoppBinWidth;
CHPTR->CarrDoppBin = next_freq_bin*CarrDoppBinWidth;
/* Update the code and carrier DCO. This must not be interrupted
during the update so disable interrupts. */
disable();
/* The code DCO frequency is the sum of the predict code frequency,
which may or may not contain an adjustment for the Doppler offset,
the offset due to the receiver clock frequency error,
an offset for the current new frequency bin and an additional
fixed offset (CodeSrchIncr) which causes the generated and received
codes to drift past each other at the required code search rate. */
CHPTR->CODEDCO = CHPTR->pCODEDCO + CodeSrchIncr + CodeDoppFromClk
+ CHPTR->CodeDoppBin;
/* The carrier DCO frequency is the sum of the predict carrier
frequency, which may or may not contain an adjustment for the
Doppler offset, the offset due to the receiver clock frequency
error and an offset for the current new frequency bin. */
CHPTR->CARRDCO = CHPTR->pCARRDCO + CarrDoppFromClk
+ CHPTR->CarrDoppBin;
/* Write out the new DCO settings and re-enable the interrupts. */
outpw(CHPTR->_CARRIER_DCO_INCR_HIGH,(unsigned)CHPTR->CARRDCO>>20);
outpw(CHPTR->_CARRIER_DCO_INCR_LOW,(unsigned)CHPTR->CARRDCO>>4);
outpw(CHPTR->_CODE_DCO_INCR_HIGH,(unsigned)CHPTR->CODEDCO>>21);
outpw(CHPTR->_CODE_DCO_INCR_LOW,(unsigned)CHPTR->CODEDCO>>5);
enable();
}
}
/****************************************************************************
* Function: void TakeMeas(register chanstruc _ds *CHPTR)
*
* Gets the measurement data for the current channel and stores it in the
* observation buffer.
*
* Input: *CHPTR - parameter block for the channel in question.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void TakeMeas(register chanstruc _ds *CHPTR)
{
unsigned epoch_count; /* EPOCH_COUNT register contents. */
unsigned code_phase; /* CODE_PHASE register contents. */
unsigned code_dco_phase; /* CODE_DCO_PHASE register contents. */
unsigned carrier_dco_phase; /* CARRIER_DCO_PHASE register contents. */
unsigned carrier_cycle_counter_low; /* Low bits of the cycle counter. */
unsigned carrier_cycle_counter_high; /* High bits of the cycle cntr. */
unsigned long carrcycles; /* The number of carrier cycles btwn TICs. */
obsstruc *OB; /* Obs buffer must be in DGROUP. */
ObservationReady = TRUE; /* There is at least on observation ready. */
/* Get a pointer to where the observation should be stored i.e. the base
pointer, obsbuff, plus the offset, cobs. */
OB = obsbuff+cobs;
/* Get the contents of the EPOCH_COUNT register. This contains the 1 and
20ms epoch counts sampled at the TIC. */
epoch_count = inpw(CHPTR->_EPOCH_COUNT);
/* Get the contents of the CODE_PHASE and CODE_DCO_PHASE registers.
These form the code phase sampled at the TIC. */
code_phase = inpw(CHPTR->_CODE_PHASE);
code_dco_phase = inpw(CHPTR->_CODE_DCO_PHASE);
/* Get the contents of the CARRIER_DCO_PHASE, CARRIER_CYCLE_COUNTER_HIGH,
and CARRIER_CYCLE_COUNTER_LOW registers. These form the carrier
cycle count sampled at the TIC. */
carrier_dco_phase = inpw(CHPTR->_CARRIER_DCO_PHASE);
carrier_cycle_counter_low = inpw(CHPTR->_CARRIER_CYCLE_COUNTER_LOW);
carrier_cycle_counter_high = inpw(CHPTR->_CARRIER_CYCLE_COUNTER_HIGH);
/* Store the previous fractional carrier dco phase. */
CHPTR->prevcarrdcophase = CHPTR->carrdcophase;
/* Get the new fractional carrier dco phase. */
CHPTR->carrdcophase = carrier_dco_phase;
/* Get the number of complete carrier cycles between the last and current
TIC. */
carrcycles = ((unsigned long)carrier_cycle_counter_high<<16) +
carrier_cycle_counter_low;
/* If we have code, carrier, and data bit lock as well as frame sync,
add the measurements to the observation which is currently being
assembled. */
if(CHPTR->corlk && CHPTR->carfrlk && CHPTR->bitlk &&
CHPTR->FrameSyncAndTIC)
{
/* An observation exists for this channel. */
OB->obspresent[CHPTR->channel] = TRUE;
/* The satellite assigned to this channel. */
OB->sv[CHPTR->channel] = CHPTR->SV;
/* The EPOCH_COUNT register contents. */
OB->epoch_count[CHPTR->channel] = epoch_count;
/* The CODE_PHASE register contents. */
OB->code_phase[CHPTR->channel] = code_phase;
/* The CODE_DCO_PHASE register contents. */
OB->code_dco_phase[CHPTR->channel] = code_dco_phase;
/* The fractional carrier phase at this TIC. */
OB->CPatThisTIC[CHPTR->channel] = CHPTR->carrdcophase;
/* The fractional carrier phase at the previous TIC. */
OB->CPatPrevTIC[CHPTR->channel] = CHPTR->prevcarrdcophase;
/* The number of carrier cycles between the current and last TIC. */
OB->CCycles[CHPTR->channel] = carrcycles;
/* The current carrier DCO setting. */
OB->carrdco[CHPTR->channel] = CHPTR->CARRDCO;
/* The channel lost lock indicator. */
OB->LostLockDuringLastTIC[CHPTR->channel] =
CHPTR->LostLockDuringLastTIC;
OB->LostCodeLockDuringLastTIC[CHPTR->channel] =
CHPTR->LostCodeLockDuringLastTIC;
OB->LostCarrierLockDuringLastTIC[CHPTR->channel] =
CHPTR->LostCarrierLockDuringLastTIC;
/* The TIC for this observation block. */
OB->obstm = TIC;
/* Reset the amount of code searched and the code and carrier lock
indicators. */
CHPTR->TotalCodeMovement = 0;
CHPTR->LostLockDuringLastTIC = FALSE;
CHPTR->LostCodeLockDuringLastTIC = FALSE;
CHPTR->LostCarrierLockDuringLastTIC = FALSE;
}
else
OB->obspresent[CHPTR->channel] = FALSE;
if(CHPTR->corlk && CHPTR->carfrlk && CHPTR->bitlk && CHPTR->FrameSync)
CHPTR->FrameSyncAndTIC = TRUE;
else
CHPTR->FrameSyncAndTIC = FALSE;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -