def track(
samples,
channels,
ms_to_track=None,
sampling_freq=defaults.sampling_freq,
chipping_rate=defaults.chipping_rate,
IF=defaults.IF,
show_progress=True,
loop_filter_class=swiftnav.track.AidedTrackingLoop,
stage1_loop_filter_params=(
(1, 0.7, 1), # Code loop NBW, zeta, k
(25, 0.7, 1), # Carrier loop NBW, zeta, k
1e3, # Loop frequency
5, # Carrier loop aiding_igain
1540,
),
correlator=swiftnav.correlate.track_correlate,
stage2_coherent_ms=None,
stage2_loop_filter_params=None,
multi=True,
):
n_channels = len(channels)
# Add 22ms for safety, the corellator might try to access data a bit past
# just the number of milliseconds specified.
# TODO: Fix the correlator so this isn't an issue.
samples_length_ms = int(1e3 * len(samples) / sampling_freq - 22)
if ms_to_track is None:
ms_to_track = samples_length_ms
if samples_length_ms < ms_to_track:
logger.warning("Samples set too short for requested tracking length (%.4fs)" % (ms_to_track * 1e-3))
ms_to_track = samples_length_ms
logger.info("Tracking %.4fs of data (%d samples)" % (ms_to_track * 1e-3, ms_to_track * 1e-3 * sampling_freq))
# Make sure we have an integer number of points
num_points = int(math.floor(ms_to_track))
logger.info("Tracking starting")
logger.debug("Tracking %d channels, PRNs %s" % (n_channels, [chan.prn + 1 for chan in channels]))
# If progressbar is not available, disable show_progress.
if show_progress and not _progressbar_available:
show_progress = False
logger.warning("show_progress = True but progressbar module not found.")
# Setup our progress bar if we need it
if show_progress and not multi:
widgets = [
" Tracking ",
progressbar.Attribute(["chan", "nchan"], "(CH: %d/%d)", "(CH: -/-)"),
" ",
progressbar.Percentage(),
" ",
progressbar.ETA(),
" ",
progressbar.Bar(),
]
pbar = progressbar.ProgressBar(widgets=widgets, maxval=n_channels * num_points, attr={"nchan": n_channels})
pbar.start()
else:
pbar = None
# Run tracking for each channel
def do_channel(chan, n=None, q_progress=None):
loop_filter = loop_filter_class(*stage1_loop_filter_params)
track_result = TrackResults(num_points, chan.prn)
# Convert acquisition SNR to C/N0
cn0_0 = 10 * np.log10(chan.snr)
cn0_0 += 10 * np.log10(1000) # Channel bandwidth
cn0_est = swiftnav.track.CN0Estimator(1e3, cn0_0, 10, 1e3)
# Estimate initial code freq via aiding from acq carrier freq
code_freq_init = (chan.carr_freq - IF) * gps_constants.chip_rate / gps_constants.l1
code_freq_init = 0
loop_filter.start(code_freq_init, chan.carr_freq - IF)
code_phase = 0.0
carr_phase = 0.0
# Get a vector with the C/A code sampled 1x/chip
ca_code = caCodes[chan.prn]
# Add wrapping to either end to be able to do early/late
ca_code = np.concatenate(([ca_code[1022]], ca_code, [ca_code[0]]))
# Number of samples to seek ahead in file
samples_per_chip = int(round(sampling_freq / chipping_rate))
# Set sample_index to start on a code rollover
sample_index = chan.code_phase * samples_per_chip
# Start in 1ms integration until we know the nav bit phase
stage1 = True
carr_phase_acc = 0.0
code_phase_acc = 0.0
progress = 0
ms_tracked = 0
i = 0
# Process the specified number of ms
while ms_tracked < ms_to_track:
if pbar:
pbar.update(ms_tracked + n * num_points, attr={"chan": n + 1})
E = 0 + 0.0j
P = 0 + 0.0j
L = 0 + 0.0j
if stage1 and stage2_coherent_ms and track_result.nav_msg.bit_phase == track_result.nav_msg.bit_phase_ref:
# print "PRN %02d transition to stage 2 at %d ms" % (chan.prn+1, ms_tracked)
stage1 = False
loop_filter.retune(*stage2_loop_filter_params)
cn0_est = swiftnav.track.CN0Estimator(
1e3 / stage2_coherent_ms, track_result.cn0[i - 1], 10, 1e3 / stage2_coherent_ms
)
coherent_ms = 1 if stage1 else stage2_coherent_ms
for j in range(coherent_ms):
samples_ = samples[sample_index:]
E_, P_, L_, blksize, code_phase, carr_phase = correlator(
samples_,
loop_filter.code_freq + chipping_rate,
code_phase,
loop_filter.carr_freq + IF,
carr_phase,
ca_code,
sampling_freq,
)
sample_index += blksize
carr_phase_acc += loop_filter.carr_freq * blksize / sampling_freq
code_phase_acc += loop_filter.code_freq * blksize / sampling_freq
E += E_
P += P_
L += L_
loop_filter.update(E, P, L)
track_result.coherent_ms[i] = coherent_ms
track_result.nav_bit_sync.update(np.real(P), coherent_ms)
tow = track_result.nav_msg.update(np.real(P), coherent_ms)
track_result.nav_msg_bit_phase_ref[i] = track_result.nav_msg.bit_phase_ref
track_result.tow[i] = tow or (track_result.tow[i - 1] + coherent_ms)
track_result.carr_phase[i] = carr_phase
track_result.carr_phase_acc[i] = carr_phase_acc
track_result.carr_freq[i] = loop_filter.carr_freq + IF
track_result.code_phase[i] = code_phase
track_result.code_phase_acc[i] = code_phase_acc
track_result.code_freq[i] = loop_filter.code_freq + chipping_rate
# Record stuff for postprocessing
track_result.absolute_sample[i] = sample_index
track_result.E[i] = E
track_result.P[i] = P
track_result.L[i] = L
track_result.cn0[i] = cn0_est.update(P.real, P.imag)
i += 1
ms_tracked += coherent_ms
if q_progress and (i % 200 == 0):
p = 1.0 * ms_tracked / ms_to_track
q_progress.put(p - progress)
progress = p
# Possibility for lock-detection later
track_result.status = "T"
track_result.resize(i)
if q_progress:
q_progress.put(1.0 - progress)
return track_result
if multi:
track_results = pp.parmap(do_channel, channels, show_progress=show_progress, func_progress=show_progress)
else:
track_results = map(lambda (n, chan): do_channel(chan, n=n), enumerate(channels))
if pbar:
pbar.finish()
logger.info("Tracking finished")
return track_results
def run_channels(self, samples):
"""
Run tracking channels.
Parameters
----------
samples : dictionary
Sample data together with description data
Return
------
out : int
The smallest data sample index across all tracking channels.
The index tells the offset, from which the next sample data batch
is to be read from the input data file.
"""
channels = self.tracking_channels
self.tracking_channels = []
def _run_parallel(i, samples):
"""
Run a tracking channel.
Expected to be run in a child process.
Parameters
----------
i : int
Channel index within self.parallel_channels list
Return
out : TrackingChannel, AcquisitionResult
Tracking channel state and handover result
"""
handover = self.parallel_channels[i].run(samples)
return self.parallel_channels[i], handover
while channels and not all(v is None for v in channels):
if self.multi:
self.parallel_channels = filter(lambda x: x.is_pickleable(), channels)
else:
self.parallel_channels = []
serial_channels = list(set(channels) - set(self.parallel_channels))
channels = []
handover = []
if self.parallel_channels:
res = pp.parmap(lambda i: _run_parallel(i, samples),
range(len(self.parallel_channels)),
nprocs=len(self.parallel_channels),
show_progress=False,
func_progress=False)
channels = map(lambda x: x[0], res)
handover += map(lambda x: x[1], res)
if serial_channels:
handover += map(lambda x: x.run(samples), serial_channels)
self.tracking_channels += channels + serial_channels
handover = [h for h in handover if h is not None]
if handover:
channels = map(self._create_channel, handover)
else:
channels = None
indicies = map(lambda x: x.get_index(), self.tracking_channels)
min_index = min(indicies)
if self.pbar:
self.pbar.update(min_index - self.init_sample_index,
attr={'sample': min_index})
return min_index
