Пример #1
0
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
Пример #2
0
  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