-
Notifications
You must be signed in to change notification settings - Fork 55
/
Copy pathtrack-glonass-l1.py
executable file
·170 lines (136 loc) · 4.72 KB
/
track-glonass-l1.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
#!/usr/bin/env python
import optparse
import numpy as np
import gnsstools.glonass.ca as ca
import gnsstools.nco as nco
import gnsstools.io as io
import gnsstools.discriminator as discriminator
import gnsstools.util as util
class tracking_state:
def __init__(self,fs,code_p,code_f,code_i,carrier_p,carrier_f,carrier_i,mode):
self.fs = fs
self.code_p = code_p
self.code_f = code_f
self.code_i = code_i
self.carrier_p = carrier_p
self.carrier_f = carrier_f
self.carrier_i = carrier_i
self.mode = mode
self.prompt1 = 0 + 0*(1j)
self.carrier_e1 = 0
self.code_e1 = 0
self.eml = 0
# tracking loops
def track(x,s,chan):
n = len(x)
fs = s.fs
nco.mix(x,-s.carrier_f/fs, s.carrier_p)
s.carrier_p = s.carrier_p - n*s.carrier_f/fs
s.carrier_p = np.mod(s.carrier_p,1)
rf_carrier = 1602.0 + 0.5625*chan
scale_factor = rf_carrier/0.511
cf = (s.code_f+s.carrier_f/scale_factor)/fs
p_early = ca.correlate(x, 0, s.code_p-0.5, cf, ca.ca_code())
p_prompt = ca.correlate(x, 0, s.code_p, cf, ca.ca_code())
p_late = ca.correlate(x, 0, s.code_p+0.5, cf, ca.ca_code())
if s.mode=='FLL_WIDE':
fll_k = 3.0
a = p_prompt
b = s.prompt1
e = discriminator.fll_atan(a,b)
s.carrier_f = s.carrier_f + fll_k*e
s.prompt1 = p_prompt
elif s.mode=='FLL_NARROW':
fll_k = 0.8
a = p_prompt
b = s.prompt1
e = discriminator.fll_atan(a,b)
s.carrier_f = s.carrier_f + fll_k*e
s.prompt1 = p_prompt
elif s.mode=='PLL':
pll_k1 = 0.1
pll_k2 = 3.5
e = discriminator.pll_costas(p_prompt)
e1 = s.carrier_e1
s.carrier_f = s.carrier_f + pll_k1*e + pll_k2*(e-e1)
s.carrier_e1 = e
# code loop
dll_k1 = 0.00002
dll_k2 = 0.2
s.early = np.absolute(p_early)
s.prompt = np.absolute(p_prompt)
s.late = np.absolute(p_late)
if (s.late+s.early)==0:
e = 0
else:
e = (s.late-s.early)/(s.late+s.early)
s.eml = e
e1 = s.code_e1
s.code_f = s.code_f + dll_k1*e + dll_k2*(e-e1)
s.code_e1 = e
s.code_p = s.code_p + n*cf
s.code_p = np.mod(s.code_p,ca.code_length)
return p_prompt,s
#
# main program
#
parser = optparse.OptionParser(usage="""track-glonass-l1.py [options] input_filename sample_rate carrier_offset chan doppler code_offset
Track GLONASS L1 C/A signal
Examples:
Track with default options:
track-glonass-l1.py /dev/stdin 69984000 17245125 4 1200.0 381.1
Track with pure PLL (no FLL intervals at the start) and with a specified carrier phase:
track-glonass-l1.py --carrier-phase 0.214 /dev/stdin 69984000 17245125 4 1200.0 381.1
Arguments:
input_filename input data file, i/q interleaved, 8 bit signed
sample_rate sampling rate in Hz
carrier_offset offset to carrier for channel 0 in Hz (positive or negative)
chan RF channel index, -7 to 7
doppler Doppler estimate from acquisition
code_offset Code-offset estimate from acquisition""")
parser.disable_interspersed_args()
parser.add_option("--loop-dwells", default="500,500", help="initial time intervals for wide FLL, then narrow FLL, in milliseconds (default %default)")
parser.add_option("--carrier-phase", help="initial carrier phase in cycles (disables FLL: uses PLL from the start)")
(options, args) = parser.parse_args()
filename = args[0]
fs = float(args[1])
coffset = float(args[2])
chan = int(args[3])
doppler = float(args[4])
code_offset = float(args[5])
loop_dwells = util.parse_list_floats(options.loop_dwells)
carrier_p = 0.0
if options.carrier_phase is not None:
carrier_p = float(options.carrier_phase)
loop_dwells = 0,0
fll_wide_time,fll_narrow_time = loop_dwells
fp = open(filename,"rb")
n = int(fs*0.001*((ca.code_length-code_offset)/ca.code_length)) # align with 1 ms code boundary
x = io.get_samples_complex(fp,n)
code_offset += n*1000.0*ca.code_length/fs
s = tracking_state(fs=fs, # initialize tracking state
code_p=code_offset, code_f=ca.chip_rate, code_i=0,
carrier_p=carrier_p, carrier_f=doppler, carrier_i=0,
mode='FLL_WIDE')
block = 0
coffset_phase = 0.0
while True:
if block>=fll_wide_time:
s.mode = 'FLL_NARROW'
if block>=fll_wide_time+fll_narrow_time:
s.mode = 'PLL'
if s.code_p<ca.code_length/2:
n = int(fs*0.001*(ca.code_length-s.code_p)/ca.code_length)
else:
n = int(fs*0.001*(2*ca.code_length-s.code_p)/ca.code_length)
x = io.get_samples_complex(fp,n)
if x is None:
break
fm = -(coffset+562500*chan)/fs
nco.mix(x,fm,coffset_phase)
coffset_phase = coffset_phase + n*fm
coffset_phase = np.mod(coffset_phase,1)
p_prompt,s = track(x,s,chan)
vars = block, np.real(p_prompt), np.imag(p_prompt), s.carrier_f, s.code_f-ca.chip_rate, (180/np.pi)*np.angle(p_prompt), s.early, s.prompt, s.late
print('%d %f %f %f %f %f %f %f %f' % vars)
block = block + 1