Example #1
0
/** Initialises a tracking channel.
 * Initialises a tracking channel on the Swift NAP. The start_sample_count
 * must be contrived to be at or close to a PRN edge (PROMPT code phase = 0).
 *
 * \param prn                PRN number - 1 (0-31).
 * \param channel            Tracking channel number on the Swift NAP.
 * \param carrier_freq       Carrier frequency (Doppler) at start of tracking in Hz.
 * \param start_sample_count Sample count on which to start tracking.
 */
void tracking_channel_init(u8 channel, u8 prn, float carrier_freq, u32 start_sample_count)
{
  /* Calculate code phase rate with carrier aiding. */
  float code_phase_rate = (1 + carrier_freq/L1_HZ) * NOMINAL_CODE_PHASE_RATE_HZ;

  /* Adjust the channel start time as the start_sample_count passed
   * in corresponds to a PROMPT code phase rollover but we want to
   * start the channel on an EARLY code phase rollover.
   */
  /* TODO : change hardcoded sample rate */
  start_sample_count -= 0.5*16;

  /* Setup tracking_channel struct. */
  tracking_channel[channel].state = TRACKING_RUNNING;
  tracking_channel[channel].prn = prn;
  tracking_channel[channel].update_count = 0;
  /* Use -1 to indicate an uninitialised value. */
  tracking_channel[channel].TOW_ms = -1;
  tracking_channel[channel].snr_threshold_count = 0;

  comp_tl_init(&(tracking_channel[channel].tl_state), 1e3,
               code_phase_rate-1.023e6, 1, 0.7, 1,
               carrier_freq, 15, 0.7, 1,
               0.005, 1540, 5000);

  tracking_channel[channel].I_filter = 0;
  tracking_channel[channel].Q_filter = 0;
  tracking_channel[channel].code_phase_early = 0;
  tracking_channel[channel].code_phase_rate_fp = code_phase_rate*NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ;
  tracking_channel[channel].code_phase_rate_fp_prev = tracking_channel[channel].code_phase_rate_fp;
  tracking_channel[channel].code_phase_rate = code_phase_rate;
  tracking_channel[channel].carrier_phase = 0;
  tracking_channel[channel].carrier_freq = carrier_freq;
  tracking_channel[channel].carrier_freq_fp = (s32)(carrier_freq * NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ);
  tracking_channel[channel].carrier_freq_fp_prev = tracking_channel[channel].carrier_freq_fp;
  tracking_channel[channel].sample_count = start_sample_count;

  nav_msg_init(&tracking_channel[channel].nav_msg);

  /* Starting carrier phase is set to zero as we don't
   * know the carrier freq well enough to calculate it.
   */
  /* Start with code phase of zero as we have conspired for the
   * channel to be initialised on an EARLY code phase rollover.
   */
  nap_track_code_wr_blocking(channel, prn);
  nap_track_init_wr_blocking(channel, prn, 0, 0);
  nap_track_update_wr_blocking(channel, \
                     carrier_freq*NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ, \
                     tracking_channel[channel].code_phase_rate_fp);

  /* Schedule the timing strobe for start_sample_count. */
  nap_timing_strobe(start_sample_count);
}
Example #2
0
/** Write to a NAP track channel's UPDATE register.
 * Write new carrier frequency and code phase rate to a NAP track channel's
 * UPDATE register, which will be used to accumulate the channel's carrier and
 * code phases during the next correlation period.
 *
 * \note This must be called in addition to nap_track_init_wr_blocking when a
 *       new track channel is being set up, before the NAP's internal timing
 *       strobe goes low.
 *
 * \note If two track channel IRQ's occur without a write to the tracking
 *       channel's UPDATE register between them, the error bit for the track
 *       channel in the NAP error register will go high.
 *
 * \param channel         NAP track channel whose UPDATE register to write.
 * \param carrier_freq    Next correlation period's carrier frequency.
 * \param code_phase_rate Next correlation period's code phase rate.
 */
void nap_track_update(u8 channel, double carrier_freq,
                      double code_phase_rate, u8 rollover_count,
                      u8 corr_spacing)
{
  struct nap_ch_state *s = &nap_ch_state[channel];
  s->carr_pinc_prev = s->carr_pinc;
  s->carr_pinc = carrier_freq * NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ;
  s->code_pinc_prev = s->code_pinc;
  s->code_pinc = code_phase_rate * NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ;

  nap_track_update_wr_blocking(channel, s->carr_pinc, s->code_pinc,
                               rollover_count, corr_spacing);
}
Example #3
0
/** Update tracking channels after the end of an integration period.
 * Update update_count, sample_count, TOW, run loop filters and update
 * SwiftNAP tracking channel frequencies.
 * \param channel Tracking channel to update.
 */
void tracking_channel_update(u8 channel)
{
  tracking_channel_t* chan = &tracking_channel[channel];

  switch(chan->state)
  {
    case TRACKING_RUNNING:
    {
      chan->update_count++;
      chan->sample_count += chan->corr_sample_count;
      /* TODO: check TOW_ms = 0 case is correct, 0 is a valid TOW. */
      if (chan->TOW_ms > 0) {
        chan->TOW_ms++;
        if (chan->TOW_ms == 7*24*60*60*1000)
          chan->TOW_ms = 0;
      }

      chan->code_phase_early = (u64)chan->code_phase_early + (u64)chan->corr_sample_count*chan->code_phase_rate_fp_prev;
      chan->carrier_phase += chan->carrier_freq_fp_prev * chan->corr_sample_count;
      /* TODO: Fix this in the FPGA - first integration is one sample short. */
      if (chan->update_count == 1)
        chan->carrier_phase -= chan->carrier_freq_fp_prev;

#if 0
      u64 cp;
      s32 cf;
      nap_track_phase_rd_blocking(channel, &cf, &cp);
      /*if ((cp&0xFFFFFFFF) != chan->code_phase_early) {*/
        DO_EVERY_TICKS(TICK_FREQ,
          struct { s32 xtend : 24; } sign;
          sign.xtend = chan->carrier_phase & 0xFFFFFF;
          s32 x = sign.xtend;
          /*printf("%d %u CPR: 0x%08X, count: %d, NAP: 0x%011llX, STM: 0x%08X\n", chan->prn+1, (unsigned int)chan->update_count, (unsigned int)chan->code_phase_rate_fp_prev, (unsigned int)chan->corr_sample_count, (unsigned long long)cp, (unsigned int)chan->code_phase_early);*/
          printf("CF: %f 0x%08X, NAP: %d (%f), STM: %f (%d %f) - %d\n", chan->carrier_freq, (int)(chan->carrier_freq*NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ), (int)cf, (double)cf / (double)(1<<24), (double)chan->carrier_phase / (double)(1<<24), (int)x, (double)x / (double)(1<<24), (int)(cf - x));
        );
      /*}*/
#endif

      /* Correlations should already be in chan->cs thanks to
       * tracking_channel_get_corrs.
       */
      corr_t* cs = chan->cs;

      /* Update I and Q magnitude filters for SNR calculation.
       * filter = (1 - 2^-FILTER_COEFF)*filter + correlation_magnitude
       * If filters are uninitialised (=0) then initialise them with the
       * first set of correlations.
       */
      if (chan->I_filter == 0 && chan->Q_filter == 0) {
        chan->I_filter = abs(cs[1].I) << I_FILTER_COEFF;
        chan->Q_filter = abs(cs[1].Q) << Q_FILTER_COEFF;
      } else {
        chan->I_filter -= chan->I_filter >> I_FILTER_COEFF;
        chan->I_filter += abs(cs[1].I);
        chan->Q_filter -= chan->Q_filter >> Q_FILTER_COEFF;
        chan->Q_filter += abs(cs[1].Q);
      }

      /* Run the loop filters. */

      /* TODO: Make this more elegant. */
      correlation_t cs2[3];
      for (u32 i = 0; i < 3; i++) {
        cs2[i].I = cs[2-i].I;
        cs2[i].Q = cs[2-i].Q;
      }
      comp_tl_update(&(chan->tl_state), cs2);
      chan->carrier_freq = chan->tl_state.carr_freq;
      chan->code_phase_rate = chan->tl_state.code_freq + 1.023e6;

      /* TODO: check TOW_ms = 0 case is correct, 0 is a valid TOW. */
      s32 TOW_ms = nav_msg_update(&chan->nav_msg, cs[1].I);

      if (TOW_ms > 0 && chan->TOW_ms != TOW_ms) {
        printf("PRN %d TOW mismatch: %u, %u\n",(int)chan->prn + 1, (unsigned int)chan->TOW_ms, (unsigned int)TOW_ms);
        chan->TOW_ms = TOW_ms;
      }

      chan->code_phase_rate_fp_prev = chan->code_phase_rate_fp;
      chan->code_phase_rate_fp = chan->code_phase_rate*NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ;

      chan->carrier_freq_fp_prev = chan->carrier_freq_fp;
      chan->carrier_freq_fp = chan->carrier_freq*NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ;

      nap_track_update_wr_blocking(channel, \
                         chan->carrier_freq_fp, \
                         chan->code_phase_rate_fp);
      break;
    }
    case TRACKING_DISABLED:
    default:
      /* TODO: WTF? */
      tracking_channel_disable(channel);
      break;
  }
Example #4
0
void nap_track_disable(u8 channel)
{
  nap_track_update_wr_blocking(channel, 0, 0, 0, 0);
}