forked from pmonta/GNSS-DSP-tools
-
Notifications
You must be signed in to change notification settings - Fork 0
/
track-beidou-b2i.py
executable file
·168 lines (134 loc) · 4.66 KB
/
track-beidou-b2i.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
#!/usr/bin/env python
import optparse
import numpy as np
import gnsstools.beidou.b1i as b1i
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,prn,code_p,code_f,code_i,carrier_p,carrier_f,carrier_i,mode):
self.fs = fs
self.prn = prn
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):
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)
cf = (s.code_f+s.carrier_f/590.0)/fs
p_early = b1i.correlate(x, s.prn, 0, s.code_p-0.5, cf, b1i.b1i_code(prn))
p_prompt = b1i.correlate(x, s.prn, 0, s.code_p, cf, b1i.b1i_code(prn))
p_late = b1i.correlate(x, s.prn, 0, s.code_p+0.5, cf, b1i.b1i_code(prn))
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,b1i.code_length)
return p_prompt,s
#
# main program
#
parser = optparse.OptionParser(usage="""track-beidou-b2i.py [options] input_filename sample_rate carrier_offset PRN doppler code_offset
Track BeiDou B2I signal
Examples:
Track with default options:
track-beidou-b2i.py /dev/stdin 69984000 -20587126 31 1200.0 831.15
Track with pure PLL (no FLL intervals at the start) and with a specified carrier phase:
track-beidou-b2i.py --carrier-phase 0.214 /dev/stdin 69984000 -20587126 31 1200.0 831.15
Arguments:
input_filename input data file, i/q interleaved, 8 bit signed
sample_rate sampling rate in Hz
carrier_offset offset to L1 carrier in Hz (positive or negative)
PRN PRN
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])
prn = 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*((b1i.code_length-code_offset)/b1i.code_length)) # align with 1 ms code boundary
x = io.get_samples_complex(fp,n)
code_offset += n*1000.0*b1i.code_length/fs
s = tracking_state(fs=fs, prn=prn, # initialize tracking state
code_p=code_offset, code_f=b1i.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<b1i.code_length/2:
n = int(fs*0.001*(b1i.code_length-s.code_p)/b1i.code_length)
else:
n = int(fs*0.001*(2*b1i.code_length-s.code_p)/b1i.code_length)
x = io.get_samples_complex(fp,n)
if x is None:
break
nco.mix(x,-coffset/fs,coffset_phase)
coffset_phase = coffset_phase - n*coffset/fs
coffset_phase = np.mod(coffset_phase,1)
p_prompt,s = track(x,s)
vars = block, np.real(p_prompt), np.imag(p_prompt), s.carrier_f, s.code_f-b1i.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