def makeCaTable(settings):
# Number of samples per code period
samplesPerCode = int(round(settings.samplingFreq / (settings.codeFreqBasis / settings.codeLength)))
#Find time constants
ts = 1/settings.samplingFreq
tc = 1/settings.codeFreqBasis
#Make array of code value indexes to sample code up to our sampling frequency
codeValueIndex = np.array([int(math.ceil(ts*i/tc)-1) for i in range(1,samplesPerCode+1)])
codeValueIndex[len(codeValueIndex)-1] = 1022
#Upsample each PRN and return as a 32 x len(codeValueIndex) list
caCodesTable = [[0 for i in range(0,1023)] for j in range(0,32)]
for PRN in range(0,32):
caCode = generateCAcode(PRN)
caCodesTable[PRN] = [caCode[i] for i in codeValueIndex]
return caCodesTable
def makeCaTable(settings):
# Number of samples per code period
samplesPerCode = int(
round(settings.samplingFreq /
(settings.codeFreqBasis / settings.codeLength)))
#Find time constants
ts = 1 / settings.samplingFreq
tc = 1 / settings.codeFreqBasis
#Make array of code value indexes to sample code up to our sampling frequency
codeValueIndex = np.array([
int(math.ceil(ts * i / tc) - 1) for i in range(1, samplesPerCode + 1)
])
codeValueIndex[len(codeValueIndex) - 1] = 1022
#Upsample each PRN and return as a 32 x len(codeValueIndex) list
caCodesTable = [[0 for i in range(0, 1023)] for j in range(0, 32)]
for PRN in range(0, 32):
caCode = generateCAcode(PRN)
caCodesTable[PRN] = [caCode[i] for i in codeValueIndex]
return caCodesTable
def track(samples, channel, settings):
#Create list of tracking channels results (correlations, freqs, etc)
trackResults = [trackResults_class(settings) for i in range(len(channel))]
#Initialize tracking variables
codePeriods = settings.msToProcess
##DLL Variables##
#Define early-late offset
earlyLateSpc = settings.dllCorrelatorSpacing
#Summation interval
PDIcode = 0.001
#Filter coefficient values
(tau1code, tau2code) = calcLoopCoef(settings.dllNoiseBandwidth,settings.dllDampingRatio,1.0)
##PLL Variables##
PDIcarr = 0.001
(tau1carr,tau2carr) = calcLoopCoef(settings.pllNoiseBandwidth,settings.pllDampingRatio,0.25)
progbar = Waitbar(True)
#Do tracking for each channel
for channelNr in range(len(channel)):
trackResults[channelNr].PRN = channel[channelNr].PRN
#Get a vector with the C/A code sampled 1x/chip
caCode = np.array(generateCAcode(channel[channelNr].PRN))
#Add wrapping to either end to be able to do early/late
caCode = np.concatenate(([caCode[1022]],caCode,[caCode[0]]))
#Initialize phases and frequencies
codeFreq = settings.codeFreqBasis
remCodePhase = 0.0 #residual code phase
carrFreq = channel[channelNr].acquiredFreq
carrFreqBasis = channel[channelNr].acquiredFreq
remCarrPhase = 0.0 #residual carrier phase
#code tracking loop parameters
oldCodeNco = 0.0
oldCodeError = 0.0
#carrier/Costas loop parameters
oldCarrNco = 0.0
oldCarrError = 0.0
#number of samples to seek ahead in file
numSamplesToSkip = settings.skipNumberOfBytes + channel[channelNr].codePhase
#Process the specified number of ms
for loopCnt in range(settings.msToProcess):
#Update progress every 50 loops
if (np.remainder(loopCnt,50)==0):
progbar.updated(float(loopCnt + channelNr*settings.msToProcess)\
/ float(len(channel)*settings.msToProcess))
# print "Channel %d/%d, %d/%d ms" % (channelNr+1,len(channel),loopCnt, settings.msToProcess)
#Update the code phase rate based on code freq and sampling freq
codePhaseStep = codeFreq/settings.samplingFreq
codePhaseStep = codePhaseStep*(10**12) #round it in the same way we are in octave
codePhaseStep = round(codePhaseStep)
codePhaseStep = codePhaseStep*(10**(-12))
blksize = int(np.ceil((settings.codeLength - remCodePhase)/codePhaseStep))
#Read samples for this integration period
rawSignal = np.array(getSamples.int8(settings.fileName,blksize,numSamplesToSkip))
numSamplesToSkip = numSamplesToSkip + blksize
#Define index into early code vector
tcode = np.r_[(remCodePhase-earlyLateSpc) : \
(blksize*codePhaseStep+remCodePhase-earlyLateSpc) : \
codePhaseStep]
earlyCode = caCode[np.int_(np.ceil(tcode))]
#Define index into late code vector
tcode = np.r_[(remCodePhase+earlyLateSpc) : \
(blksize*codePhaseStep+remCodePhase+earlyLateSpc) : \
codePhaseStep]
lateCode = caCode[np.int_(np.ceil(tcode))]
#Define index into prompt code vector
tcode = np.r_[(remCodePhase) : \
(blksize*codePhaseStep+remCodePhase) : \
codePhaseStep]
promptCode = caCode[np.int_(np.ceil(tcode))]
remCodePhase = (tcode[blksize-1] + codePhaseStep) - 1023
#Generate the carrier frequency to mix the signal to baseband
time = np.r_[0:blksize+1] / settings.samplingFreq #(seconds)
#Get the argument to sin/cos functions
trigarg = (carrFreq * 2.0 * math.pi)*time + remCarrPhase
remCarrPhase = np.remainder(trigarg[blksize],(2*math.pi))
#Finally compute the signal to mix the collected data to baseband
carrCos = np.cos(trigarg[0:blksize])
carrSin = np.sin(trigarg[0:blksize])
#Mix signals to baseband
qBasebandSignal = carrCos*rawSignal
iBasebandSignal = carrSin*rawSignal
#Get early, prompt, and late I/Q correlations
I_E = np.sum(earlyCode * iBasebandSignal)
Q_E = np.sum(earlyCode * qBasebandSignal)
I_P = np.sum(promptCode * iBasebandSignal)
Q_P = np.sum(promptCode * qBasebandSignal)
I_L = np.sum(lateCode * iBasebandSignal)
Q_L = np.sum(lateCode * qBasebandSignal)
#Find PLL error and update carrier NCO
#Carrier loop discriminator (phase detector)
carrError = math.atan(Q_P/I_P) / (2.0 * math.pi)
#Carrier loop filter and NCO
carrNco = oldCarrNco + (tau2carr/tau1carr) * \
(carrError-oldCarrError) + carrError*(PDIcarr/tau1carr)
oldCarrNco = carrNco
oldCarrError = carrError
#Modify carrier freq based on NCO
carrFreq = carrFreqBasis + carrNco
trackResults[channelNr].carrFreq[loopCnt] = carrFreq
#Find DLL error and update code NCO
codeError = (math.sqrt(I_E*I_E + Q_E*Q_E) - math.sqrt(I_L*I_L + Q_L*Q_L)) / \
(math.sqrt(I_E*I_E + Q_E*Q_E) + math.sqrt(I_L*I_L + Q_L*Q_L))
codeNco = oldCodeNco + (tau2code/tau1code)*(codeError-oldCodeError) \
+ codeError*(PDIcode/tau1code)
oldCodeNco = codeNco
oldCodeError = codeError
#Code freq based on NCO
codeFreq = settings.codeFreqBasis - codeNco
trackResults[channelNr].codeFreq[loopCnt] = codeFreq
#Record stuff for postprocessing
trackResults[channelNr].absoluteSample[loopCnt] = numSamplesToSkip
trackResults[channelNr].dllDiscr[loopCnt] = codeError
trackResults[channelNr].dllDiscrFilt[loopCnt] = codeNco
trackResults[channelNr].pllDiscr[loopCnt] = carrError
trackResults[channelNr].pllDiscrFilt[loopCnt] = carrNco
trackResults[channelNr].I_E[loopCnt] = I_E
trackResults[channelNr].I_P[loopCnt] = I_P
trackResults[channelNr].I_L[loopCnt] = I_L
trackResults[channelNr].Q_E[loopCnt] = Q_E
trackResults[channelNr].Q_P[loopCnt] = Q_P
trackResults[channelNr].Q_L[loopCnt] = Q_L
# print ("tR[%d].absoluteSample[%d] = %d" % (channelNr,loopCnt,trackResults[channelNr].absoluteSample[loopCnt]))
#
# print ("tR[%d].dllDiscr[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].dllDiscr[loopCnt]))
# print ("tR[%d].dllDiscrFilt[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].dllDiscrFilt[loopCnt]))
# print ("tR[%d].codeFreq[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].codeFreq[loopCnt]))
# print ("tR[%d].pllDiscr[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].pllDiscr[loopCnt]))
# print ("tR[%d].pllDiscrFilt[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].pllDiscrFilt[loopCnt]))
# print ("tR[%d].carrFreq[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].carrFreq[loopCnt]))
#
# print ("tR[%d].I_E[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].I_E[loopCnt]))
# print ("tR[%d].I_P[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].I_P[loopCnt]))
# print ("tR[%d].I_L[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].I_L[loopCnt]))
# print ("tR[%d].Q_E[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].Q_E[loopCnt]))
# print ("tR[%d].Q_P[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].Q_P[loopCnt]))
# print ("tR[%d].Q_L[%d] = %f" % (channelNr,loopCnt,trackResults[channelNr].Q_L[loopCnt]))
# print ""
#Possibility for lock-detection later
trackResults[channelNr].status = 'T'
print ""
return (trackResults,channel)
def acquisition(longSignal,settings):
# Number of samples per code period
samplesPerCode = int(round(settings.samplingFreq / (settings.codeFreqBasis / settings.codeLength)))
# Create two 1msec vectors of data to correlate with and one with zero DC
signal1 = np.array(longSignal[0:samplesPerCode])
signal2 = np.array(longSignal[samplesPerCode:2*samplesPerCode])
signal0DC = np.array(longSignal - np.mean(longSignal))
# Find sampling period
ts = 1/settings.samplingFreq
# Find phases for the local carrier
phasePoints = np.array([2*math.pi*i*ts for i in range(0,samplesPerCode)])
# Number of frequency bins for the given acquisition band (500 Hz steps)
numberOfFrqBins = int(math.floor(settings.acqSearchBand*1e3/500 + 1))
# Generate all C/A codes and sample them according to the sampling freq
caCodesTable = np.array(makeCaTable(settings))
# Initialize arrays
results = np.zeros((numberOfFrqBins, samplesPerCode))
# Carrier frequencies of the frequency bins
frqBins = np.zeros((numberOfFrqBins))
# Initialize acqResults
acqResults = [[0.0,0.0,0.0] for i in range(32)]
print "(",
sys.stdout.flush()
for PRN in settings.acqSatelliteList:
caCodeFreqDom = np.conj(np.fft.fft(caCodesTable[PRN]))
for frqBinIndex in range(numberOfFrqBins):
#--- Generate carrier wave frequency grid (0.5kHz step) -----------
frqBins[frqBinIndex] = settings.IF \
- settings.acqSearchBand/2*1000 \
+ 0.5e3*frqBinIndex
#--- Generate local sine and cosine -------------------------------
sinCarr = np.sin(frqBins[frqBinIndex]*phasePoints)
cosCarr = np.cos(frqBins[frqBinIndex]*phasePoints)
#--- "Remove carrier" from the signal -----------------------------
I1 = sinCarr*signal1
Q1 = cosCarr*signal1
I2 = sinCarr*signal2
Q2 = cosCarr*signal2
#--- Convert the baseband signal to frequency domain --------------
IQfreqDom1 = np.fft.fft(I1 + 1j*Q1);
IQfreqDom2 = np.fft.fft(I2 + 1j*Q2);
#--- Multiplication in frequency <--> correlation in time ---------
convCodeIQ1 = IQfreqDom1*caCodeFreqDom
convCodeIQ2 = IQfreqDom2*caCodeFreqDom
#--- Perform IFFT and store correlation results -------------------
acqRes1 = abs(np.fft.ifft(convCodeIQ1))**2
acqRes2 = abs(np.fft.ifft(convCodeIQ2))**2
#--- Check which msec had the greater power and save that, wil
#blend 1st and 2nd msec but corrects for nav bit
if (max(acqRes1) > max(acqRes1)):
results[frqBinIndex] = acqRes1
else:
results[frqBinIndex] = acqRes2
#--- Find the correlation peak and the carrier frequency ----------
peakSize = 0
for i in range(len(results)):
if (max(results[i]) > peakSize):
peakSize = max(results[i])
frequencyBinIndex = i
#--- Find the code phase of the same correlation peak -------------
peakSize = 0
for i in range(len(results.T)):
if (max(results.T[i]) > peakSize):
peakSize = max(results.T[i])
codePhase = i
#--- Find 1 chip wide C/A code phase exclude range around the peak
samplesPerCodeChip = int(round(settings.samplingFreq \
/ settings.codeFreqBasis))
excludeRangeIndex1 = codePhase - samplesPerCodeChip
excludeRangeIndex2 = codePhase + samplesPerCodeChip
#--- Correct C/A code phase exclude range if the range includes
#--- array boundaries
if (excludeRangeIndex1 < 1):
codePhaseRange = range(excludeRangeIndex2,samplesPerCode+excludeRangeIndex1+1)
elif (excludeRangeIndex2 >= (samplesPerCode-1)):
codePhaseRange = range(excludeRangeIndex2-samplesPerCode,excludeRangeIndex1+1)
else:
codePhaseRange = np.concatenate((range(0,excludeRangeIndex1+1),\
range(excludeRangeIndex2,samplesPerCode)))
#Find the second highest correlation peak in the same freq bin
secondPeakSize = 0
for i in codePhaseRange:
if (secondPeakSize < results[frequencyBinIndex][i]):
secondPeakSize = results[frequencyBinIndex][i]
#Store result
acqResults[PRN][0] = peakSize/secondPeakSize
#If the result is above the threshold, then we have acquired the satellite
if (acqResults[PRN][0] > settings.acqThreshold):
#Fine resolution frequency search
print (PRN+1),
sys.stdout.flush()
#Generate 8msc long C/A codes sequence for given PRN
caCode = generateCAcode(PRN)
codeValueIndex = np.array([int(math.floor(ts*i*settings.codeFreqBasis)) for i in \
range(1,8*samplesPerCode+1)])
longCaCode = np.array([caCode[i] for i in np.remainder(codeValueIndex,1023)])
#Remove CA code modulation from the original signal
xCarrier = np.array([signal0DC[codePhase+i]*longCaCode[i] for i in range(0,8*samplesPerCode)])
#Find next highest power of 2 and increase by 8x
fftNumPts = 8*(2**int(math.ceil(math.log(len(xCarrier),2))))
#Compute the magnitude of the FFT, find the maximum, and the associated carrrier frequency
#for some reason the output of this fft is different than Octave's, but they seem to
#preeeeetty much reach the same conclusion for the best carrier frequency
fftxc = np.abs(np.fft.fft(xCarrier,n=fftNumPts))
uniqFftPts = int(math.ceil((fftNumPts+1)/2))
fftMax = 0
for i in range(4,uniqFftPts-5):
if (fftMax < fftxc[i]):
fftMax = fftxc[i]
fftMaxIndex = i
fftFreqBins = np.array([i*settings.samplingFreq/fftNumPts for i in range(uniqFftPts)])
#Save properties of the detected satellite signal
# acqResults[PRN].carrFreq = fftFreqBins[fftMaxIndex]
# acqResults[PRN].codePhase = codePhase
acqResults[PRN][1] = fftFreqBins[fftMaxIndex]
acqResults[PRN][2] = codePhase
#If the result is NOT above the threshold, we haven't acquired the satellite
else:
print ".",
sys.stdout.flush()
for i in range(32): #Add PRN number to each result
acqResults[i].append(i)
#Acquisition is over
print ")"
return acqResults
def acquisition(longSignal, settings):
# Number of samples per code period
samplesPerCode = int(
round(settings.samplingFreq /
(settings.codeFreqBasis / settings.codeLength)))
# Create two 1msec vectors of data to correlate with and one with zero DC
signal1 = np.array(longSignal[0:samplesPerCode])
signal2 = np.array(longSignal[samplesPerCode:2 * samplesPerCode])
signal0DC = np.array(longSignal - np.mean(longSignal))
# Find sampling period
ts = 1 / settings.samplingFreq
# Find phases for the local carrier
phasePoints = np.array(
[2 * math.pi * i * ts for i in range(0, samplesPerCode)])
# Number of frequency bins for the given acquisition band (500 Hz steps)
numberOfFrqBins = int(math.floor(settings.acqSearchBand * 1e3 / 500 + 1))
# Generate all C/A codes and sample them according to the sampling freq
caCodesTable = np.array(makeCaTable(settings))
# Initialize arrays
results = np.zeros((numberOfFrqBins, samplesPerCode))
# Carrier frequencies of the frequency bins
frqBins = np.zeros((numberOfFrqBins))
# Initialize acqResults
acqResults = [[0.0, 0.0, 0.0] for i in range(32)]
print "(",
sys.stdout.flush()
for PRN in settings.acqSatelliteList:
caCodeFreqDom = np.conj(np.fft.fft(caCodesTable[PRN]))
for frqBinIndex in range(numberOfFrqBins):
#--- Generate carrier wave frequency grid (0.5kHz step) -----------
frqBins[frqBinIndex] = settings.IF \
- settings.acqSearchBand/2*1000 \
+ 0.5e3*frqBinIndex
#--- Generate local sine and cosine -------------------------------
sinCarr = np.sin(frqBins[frqBinIndex] * phasePoints)
cosCarr = np.cos(frqBins[frqBinIndex] * phasePoints)
#--- "Remove carrier" from the signal -----------------------------
I1 = sinCarr * signal1
Q1 = cosCarr * signal1
I2 = sinCarr * signal2
Q2 = cosCarr * signal2
#--- Convert the baseband signal to frequency domain --------------
IQfreqDom1 = np.fft.fft(I1 + 1j * Q1)
IQfreqDom2 = np.fft.fft(I2 + 1j * Q2)
#--- Multiplication in frequency <--> correlation in time ---------
convCodeIQ1 = IQfreqDom1 * caCodeFreqDom
convCodeIQ2 = IQfreqDom2 * caCodeFreqDom
#--- Perform IFFT and store correlation results -------------------
acqRes1 = abs(np.fft.ifft(convCodeIQ1))**2
acqRes2 = abs(np.fft.ifft(convCodeIQ2))**2
#--- Check which msec had the greater power and save that, wil
#blend 1st and 2nd msec but corrects for nav bit
if (max(acqRes1) > max(acqRes1)):
results[frqBinIndex] = acqRes1
else:
results[frqBinIndex] = acqRes2
#--- Find the correlation peak and the carrier frequency ----------
peakSize = 0
for i in range(len(results)):
if (max(results[i]) > peakSize):
peakSize = max(results[i])
frequencyBinIndex = i
#--- Find the code phase of the same correlation peak -------------
peakSize = 0
for i in range(len(results.T)):
if (max(results.T[i]) > peakSize):
peakSize = max(results.T[i])
codePhase = i
#--- Find 1 chip wide C/A code phase exclude range around the peak
samplesPerCodeChip = int(round(settings.samplingFreq \
/ settings.codeFreqBasis))
excludeRangeIndex1 = codePhase - samplesPerCodeChip
excludeRangeIndex2 = codePhase + samplesPerCodeChip
#--- Correct C/A code phase exclude range if the range includes
#--- array boundaries
if (excludeRangeIndex1 < 1):
codePhaseRange = range(excludeRangeIndex2,
samplesPerCode + excludeRangeIndex1 + 1)
elif (excludeRangeIndex2 >= (samplesPerCode - 1)):
codePhaseRange = range(excludeRangeIndex2 - samplesPerCode,
excludeRangeIndex1 + 1)
else:
codePhaseRange = np.concatenate((range(0,excludeRangeIndex1+1),\
range(excludeRangeIndex2,samplesPerCode)))
#Find the second highest correlation peak in the same freq bin
secondPeakSize = 0
for i in codePhaseRange:
if (secondPeakSize < results[frequencyBinIndex][i]):
secondPeakSize = results[frequencyBinIndex][i]
#Store result
acqResults[PRN][0] = peakSize / secondPeakSize
#If the result is above the threshold, then we have acquired the satellite
if (acqResults[PRN][0] > settings.acqThreshold):
#Fine resolution frequency search
print(PRN + 1),
sys.stdout.flush()
#Generate 8msc long C/A codes sequence for given PRN
caCode = generateCAcode(PRN)
codeValueIndex = np.array([int(math.floor(ts*i*settings.codeFreqBasis)) for i in \
range(1,8*samplesPerCode+1)])
longCaCode = np.array(
[caCode[i] for i in np.remainder(codeValueIndex, 1023)])
#Remove CA code modulation from the original signal
xCarrier = np.array([
signal0DC[codePhase + i] * longCaCode[i]
for i in range(0, 8 * samplesPerCode)
])
#Find next highest power of 2 and increase by 8x
fftNumPts = 8 * (2**int(math.ceil(math.log(len(xCarrier), 2))))
#Compute the magnitude of the FFT, find the maximum, and the associated carrrier frequency
#for some reason the output of this fft is different than Octave's, but they seem to
#preeeeetty much reach the same conclusion for the best carrier frequency
fftxc = np.abs(np.fft.fft(xCarrier, n=fftNumPts))
uniqFftPts = int(math.ceil((fftNumPts + 1) / 2))
fftMax = 0
for i in range(4, uniqFftPts - 5):
if (fftMax < fftxc[i]):
fftMax = fftxc[i]
fftMaxIndex = i
fftFreqBins = np.array([
i * settings.samplingFreq / fftNumPts
for i in range(uniqFftPts)
])
#Save properties of the detected satellite signal
# acqResults[PRN].carrFreq = fftFreqBins[fftMaxIndex]
# acqResults[PRN].codePhase = codePhase
acqResults[PRN][1] = fftFreqBins[fftMaxIndex]
acqResults[PRN][2] = codePhase
#If the result is NOT above the threshold, we haven't acquired the satellite
else:
print ".",
sys.stdout.flush()
for i in range(32): #Add PRN number to each result
acqResults[i].append(i)
#Acquisition is over
print ")"
return acqResults
