예제 #1
0
/** Do a blocking acquisition search in two stages : coarse and fine.
 * Do a coarse acqusition to find the approximate code phase and carrier
 * frequency, and then a more fine grained acquisition to find the code phase
 * and carrier frequency more precisely.
 *
 * \param prn PRN to search (nap_acq_code_wr_blocking must be called prior)
 * \param cp  Code phase of the acquisition result
 * \param cf  Carrier frequency of the acquisition result
 * \param snr SNR of the acquisition result
 */
u32 acq_full_two_stage(u8 prn, float* cp, float* cf, float* snr)
{
  /* Initial coarse acq. */
  float coarse_code_phase;
  float coarse_carrier_freq;
  float coarse_snr;

  u32 coarse_count = nap_timing_count() + 1000;
  acq_schedule_load(coarse_count);
  while(!acq_get_load_done());

  acq_start(prn, 0, 1023, -7000, 7000, 300);
  while(!acq_get_done());
  acq_get_results(&coarse_code_phase, &coarse_carrier_freq, &coarse_snr);

  /* Fine acq. */
  u32 fine_count = nap_timing_count() + 2000;
  acq_schedule_load(fine_count);
  while(!acq_get_load_done());

  float fine_cp = propagate_code_phase(coarse_code_phase, coarse_carrier_freq, fine_count - coarse_count);

  acq_start(prn, fine_cp-20, fine_cp+20, coarse_carrier_freq-300, coarse_carrier_freq+300, 100);
  while(!acq_get_done());
  acq_get_results(cp, cf, snr);

  return fine_count;
}
예제 #2
0
bool acq_search(gnss_signal_t sid, float cf_min, float cf_max,
                float cf_bin_width, acq_result_t *acq_result)
{
  acq_set_sid(sid);

  /* We have our SID chosen, now load some fresh data
   * into the acquisition ram on the Swift NAP for
   * an initial coarse acquisition.
   */
  u32 sample_count;
  do {
    sample_count = nap_timing_count() + 20000;
    /* acq_load could timeout if we're preempted and miss the timing strobe */
  } while (!acq_load(sample_count));

  acq_search_begin(cf_min, cf_max, cf_bin_width);

  /* Done with the coarse acquisition, check if we have found a
   * satellite, if so save the results and start the loading
   * for the fine acquisition. If not, start again choosing a
   * different PRN.
   */
  acq_result->sample_count = sample_count;
  acq_get_results(&acq_result->cp, &acq_result->cf, &acq_result->cn0);
  return true;
}
예제 #3
0
/** Service an external event interrupt
 *
 * When an event occurs (i.e. pin edge) that matches the NAP's trigger
 * condition, the NAP will latch the time, pin number and trigger direction.
 * It will also set an IRQ bit which will lead to an EXTI.  The firmware
 * EXTI handling routine handle_nap_exti() will call this function, which
 * reads out the details and spits them out as an SBP message to our host.
 *
 */
void ext_event_service(void)
{
  u8 event_pin;
  ext_event_trigger_t event_trig;

  /* Read the details, and also clear IRQ + set up for next time */
  u32 event_nap_time = nap_rw_ext_event(&event_pin, &event_trig, trigger);
  
  /* We have to infer the most sig word (i.e. # of 262-second rollovers) */
  union {
    u32 half[2];
    u64 full;
  } tc;
  tc.full = nap_timing_count();
  if (tc.half[0] < event_nap_time)  /* Rollover occurred since event */
    tc.half[1]--;
  tc.half[0] = event_nap_time;

  /* Prepare the MSG_EXT_EVENT */
  msg_ext_event_t msg;
  msg.flags = (event_trig == TRIG_RISING) ? (1<<0) : (0<<0);
  if (time_quality == TIME_FINE)
    msg.flags |= (1 << 1);
  msg.pin = event_pin;

  /* Convert to the SBP convention of rounded ms + signed ns residual */
  gps_time_t gpst = rx2gpstime(tc.full);
  msg_gps_time_t mgt;
  sbp_make_gps_time(&mgt, &gpst, 0);
  msg.wn = mgt.wn;
  msg.tow = mgt.tow;
  msg.ns = mgt.ns;

  sbp_send_msg(SBP_MSG_EXT_EVENT, sizeof(msg), (u8 *)&msg);
}
예제 #4
0
/** Write to a NAP track channel's INIT register.
 * Sets PRN (deprecated), initial carrier phase, and initial code phase of a
 * NAP track channel. The tracking channel will start correlating with these
 * parameters at the falling edge of the next NAP internal timing strobe.
 * Also write CA code to track channel's code ram.
 *
 * \note The track channel's UPDATE register, which sets the carrier and
 *       code phase rates, must also be written to before the internal timing
 *       strobe goes low.
 *
 * \param channel       NAP track channel whose INIT register to write.
 * \param prn           CA code PRN (0-31) to track. (deprecated)
 * \param carrier_phase Initial code phase.
 * \param code_phase    Initial carrier phase.
 */
void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count,
                    float carrier_freq, float code_phase)
{
  struct nap_ch_state *s = &nap_ch_state[channel];
  memset(s, 0, sizeof(*s));

  u32 track_count = nap_timing_count() + 20000;
  float cp = propagate_code_phase(code_phase, carrier_freq,
                                  track_count - ref_timing_count);

  /* Contrive for the timing strobe to occur at or close to a
   * PRN edge (code phase = 0) */
  track_count += (NAP_FRONTEND_SAMPLE_RATE_Hz / GPS_CA_CHIPPING_RATE) * (1023.0-cp) *
                 (1.0 + carrier_freq / GPS_L1_HZ);

  nap_track_code_wr_blocking(channel, sid);
  nap_track_init_wr_blocking(channel, 0, 0, 0);

  double cp_rate = (1.0 + carrier_freq / GPS_L1_HZ) * GPS_CA_CHIPPING_RATE;
  nap_track_update(channel, carrier_freq, cp_rate, 0, 0);

  /* Schedule the timing strobe for start_sample_count. */
  track_count -= NAP_FRONTEND_SAMPLE_RATE_Hz / (2 * GPS_CA_CHIPPING_RATE);
  
  s->count_snapshot = track_count;
  s->carrier_phase = -s->carr_pinc;
  s->carr_pinc_prev = s->carr_pinc;
  s->code_pinc_prev = s->code_pinc;

  COMPILER_BARRIER();

  nap_timing_strobe(track_count);
  nap_timing_strobe_wait(100);
}
예제 #5
0
/** Get current GPS time.
 *
 * \note The GPS time may only be a guess or completely unknown. time_quality
 *       should be checked first to determine the quality of the GPS time
 *       estimate.
 *
 * This function should be used only for approximate timing purposes as simply
 * calling this function does not give a well defined instant at which the GPS
 * time is queried.
 *
 * \return Current GPS time.
 */
gps_time_t get_current_time(void)
{
  /* TODO: Return invalid when TIME_UNKNOWN. */
  /* TODO: Think about what happens when nap_timing_count overflows. */
  u64 tc = nap_timing_count();
  gps_time_t t = rx2gpstime(tc);

  return t;
}
예제 #6
0
/** Update GPS time estimate.
 *
 * This function may be called to update the GPS time estimate. If the time is
 * already known more precisely than the new estimate, the new estimate will be
 * ignored.
 *
 * This function should not be used to give an estimate with TIME_FINE quality
 * as this must be referenced to a particular value of the SwiftNAP timing
 * count.
 *
 * \param quality Quality of the time estimate.
 * \param t GPS time estimate.
 */
void set_time(time_quality_t quality, gps_time_t t)
{
  if (quality > time_quality) {
    clock_state.t0_gps = t;
    clock_state.t0_gps.tow -= nap_timing_count() * RX_DT_NOMINAL;
    clock_state.t0_gps = normalize_gps_time(clock_state.t0_gps);

    time_quality = quality;
    time_t unix_t = gps2time(t);
    log_info("Time set to: %s (quality=%d)", ctime(&unix_t), quality);
  }
}
예제 #7
0
/** Manages acquisition searches and starts tracking channels after successful acquisitions. */
static void manage_acq()
{
  /* Decide which PRN to try and then start it acquiring. */
  u8 prn = choose_prn();
  if (prn == (u8)-1)
    return;

  u32 timer_count;
  float snr, cp, cf;

  acq_set_prn(prn);

  /* We have our PRN chosen, now load some fresh data
   * into the acquisition ram on the Swift NAP for
   * an initial coarse acquisition.
   */
  do {
    timer_count = nap_timing_count() + 20000;
    /* acq_load could timeout if we're preempted and miss the timing strobe */
  } while (!acq_load(timer_count));

  acq_search(acq_prn_param[prn].dopp_hint_low,
             acq_prn_param[prn].dopp_hint_high,
             ACQ_FULL_CF_STEP);

  /* Done with the coarse acquisition, check if we have found a
   * satellite, if so save the results and start the loading
   * for the fine acquisition. If not, start again choosing a
   * different PRN.
   */
  acq_get_results(&cp, &cf, &snr);
  /* Send result of an acquisition to the host. */
  acq_send_result(prn, snr, cp, cf);
  if (snr < ACQ_THRESHOLD) {
    /* Didn't find the satellite :( */
    /* Double the size of the doppler search space for next time. */
    float dilute = (acq_prn_param[prn].dopp_hint_high -
                    acq_prn_param[prn].dopp_hint_low) / 2;
    acq_prn_param[prn].dopp_hint_high =
        MIN(acq_prn_param[prn].dopp_hint_high + dilute, ACQ_FULL_CF_MAX);
    acq_prn_param[prn].dopp_hint_low =
        MAX(acq_prn_param[prn].dopp_hint_low - dilute, ACQ_FULL_CF_MIN);
    /* Decay hint scores */
    for (u8 i = 0; i < ACQ_HINT_NUM; i++)
      acq_prn_param[prn].score[i] = (acq_prn_param[prn].score[i] * 3) / 4;
    /* Reset hint score for acuisition. */
    acq_prn_param[prn].score[ACQ_HINT_ACQ] = 0;
    return;
  }

  log_info("acq: PRN %d found @ %d Hz, %d SNR\n", prn + 1, (int)cf, (int)snr);

  u8 chan = manage_track_new_acq();
  if (chan == MANAGE_NO_CHANNELS_FREE) {
    /* No channels are free to accept our new satellite :( */
    /* TODO: Perhaps we can try to warm start this one
     * later using another fine acq.
     */
    log_info("No channels free :(\n");
    if (snr > ACQ_RETRY_THRESHOLD) {
      acq_prn_param[prn].score[ACQ_HINT_ACQ] = SCORE_ACQ + (snr - 20) / 20;
      acq_prn_param[prn].dopp_hint_low = cf - ACQ_FULL_CF_STEP;
      acq_prn_param[prn].dopp_hint_high = cf + ACQ_FULL_CF_STEP;
    }
    return;
  }
  /* Transition to tracking. */
  u32 track_count = nap_timing_count() + 20000;
  cp = propagate_code_phase(cp, cf, track_count - timer_count);

  // Contrive for the timing strobe to occur at or close to a PRN edge (code phase = 0)
  track_count += 16*(1023.0-cp)*(1.0 + cf / GPS_L1_HZ);

  tracking_channel_init(chan, prn, cf, track_count, snr);
  acq_prn_param[prn].state = ACQ_PRN_TRACKING;
  nap_timing_strobe_wait(100);
}
예제 #8
0
/** Manages acquisition searches and starts tracking channels after successful acquisitions. */
static void manage_acq()
{
  /* Decide which PRN to try and then start it acquiring. */
  u8 prn = choose_prn();
  if (prn == (u8)-1)
    return;
  gnss_signal_t sid = {.sat = prn};

  u32 timer_count;
  float cn0, cp, cf;

  acq_set_prn(prn);

  /* We have our PRN chosen, now load some fresh data
   * into the acquisition ram on the Swift NAP for
   * an initial coarse acquisition.
   */
  do {
    timer_count = nap_timing_count() + 20000;
    /* acq_load could timeout if we're preempted and miss the timing strobe */
  } while (!acq_load(timer_count));

  /* Check for NaNs in dopp hints, or low > high */
  if (!(acq_prn_param[prn].dopp_hint_low
        <= acq_prn_param[prn].dopp_hint_high)) {
    log_error("Acq: caught bogus dopp_hints (%f, %f)",
              acq_prn_param[prn].dopp_hint_low,
              acq_prn_param[prn].dopp_hint_high);
    acq_prn_param[prn].dopp_hint_high = ACQ_FULL_CF_MAX;
    acq_prn_param[prn].dopp_hint_low = ACQ_FULL_CF_MIN;
  }
  acq_search(acq_prn_param[prn].dopp_hint_low,
             acq_prn_param[prn].dopp_hint_high,
             ACQ_FULL_CF_STEP);

  /* Done with the coarse acquisition, check if we have found a
   * satellite, if so save the results and start the loading
   * for the fine acquisition. If not, start again choosing a
   * different PRN.
   */
  acq_get_results(&cp, &cf, &cn0);
  /* Send result of an acquisition to the host. */
  acq_send_result(sid, cn0, cp, cf);
  if (cn0 < ACQ_THRESHOLD) {
    /* Didn't find the satellite :( */
    /* Double the size of the doppler search space for next time. */
    float dilute = (acq_prn_param[prn].dopp_hint_high -
                    acq_prn_param[prn].dopp_hint_low) / 2;
    acq_prn_param[prn].dopp_hint_high =
        MIN(acq_prn_param[prn].dopp_hint_high + dilute, ACQ_FULL_CF_MAX);
    acq_prn_param[prn].dopp_hint_low =
        MAX(acq_prn_param[prn].dopp_hint_low - dilute, ACQ_FULL_CF_MIN);
    /* Decay hint scores */
    for (u8 i = 0; i < ACQ_HINT_NUM; i++)
      acq_prn_param[prn].score[i] = (acq_prn_param[prn].score[i] * 3) / 4;
    /* Reset hint score for acquisition. */
    acq_prn_param[prn].score[ACQ_HINT_PREV_ACQ] = 0;
    return;
  }

  u8 chan = manage_track_new_acq();
  if (chan == MANAGE_NO_CHANNELS_FREE) {
    /* No channels are free to accept our new satellite :( */
    /* TODO: Perhaps we can try to warm start this one
     * later using another fine acq.
     */
    if (cn0 > ACQ_RETRY_THRESHOLD) {
      acq_prn_param[prn].score[ACQ_HINT_PREV_ACQ] =
        SCORE_ACQ + (cn0 - ACQ_THRESHOLD);
      acq_prn_param[prn].dopp_hint_low = cf - ACQ_FULL_CF_STEP;
      acq_prn_param[prn].dopp_hint_high = cf + ACQ_FULL_CF_STEP;
    }
    return;
  }
  /* Transition to tracking. */
  u32 track_count = nap_timing_count() + 20000;
  cp = propagate_code_phase(cp, cf, track_count - timer_count);

  // Contrive for the timing strobe to occur at or close to a PRN edge (code phase = 0)
  track_count += 16*(1023.0-cp)*(1.0 + cf / GPS_L1_HZ);

  /* Start the tracking channel */
  tracking_channel_init(chan, sid, cf, track_count, cn0,
                        TRACKING_ELEVATION_UNKNOWN);
  /* TODO: Initialize elevation from ephemeris if we know it precisely */

  acq_prn_param[prn].state = ACQ_PRN_TRACKING;
  nap_timing_strobe_wait(100);
}
예제 #9
0
/** Manages acquisition searches and starts tracking channels after successful acquisitions. */
void manage_acq()
{
  /* Decide which PRN to try and then start it acquiring. */
  u8 prn = best_prn();
  if (prn == (u8)-1)
    return;

  u32 timer_count;
  float snr, cp, cf;

  acq_set_prn(prn);

  /* We have our PRN chosen, now load some fresh data
   * into the acquisition ram on the Swift NAP for
   * an initial coarse acquisition.
   */
  acq_prn_param[prn].state = ACQ_PRN_ACQUIRING;
  do {
    timer_count = nap_timing_count() + 20000;
    /* acq_load could timeout if we're preempted and miss the timing strobe */
  } while (!acq_load(timer_count));

  /* Done loading, now lets set that coarse acquisition going. */
  if (almanac[prn].valid && time_quality == TIME_COARSE) {
    gps_time_t t = rx2gpstime(timer_count);

    double dopp = -calc_sat_doppler_almanac(&almanac[prn], t.tow, t.wn, position_solution.pos_ecef);
    /* TODO: look into accuracy of prediction and possibilities for
     * improvement, e.g. use clock bias estimated by PVT solution. */
    /*log_info("Expecting PRN %02d @ %.1f\n", prn+1, dopp);*/
    acq_search(dopp - 4000, dopp + 4000, ACQ_FULL_CF_STEP);
  } else {
    acq_search(ACQ_FULL_CF_MIN, ACQ_FULL_CF_MAX, ACQ_FULL_CF_STEP);
  }

  /* Done with the coarse acquisition, check if we have found a
   * satellite, if so save the results and start the loading
   * for the fine acquisition. If not, start again choosing a
   * different PRN.
   */
  acq_get_results(&cp, &cf, &snr);
  /* Send result of an acquisition to the host. */
  acq_send_result(prn, snr, cp, cf);
  if (snr < ACQ_THRESHOLD) {
    /* Didn't find the satellite :( */
    acq_prn_param[prn].state = ACQ_PRN_TRIED;
    return;
  }

  log_info("acq: PRN %d found @ %d Hz, %d SNR\n", prn + 1, (int)cf, (int)snr);

  u8 chan = manage_track_new_acq(snr);
  if (chan == MANAGE_NO_CHANNELS_FREE) {
    /* No channels are free to accept our new satellite :( */
    /* TODO: Perhaps we can try to warm start this one
     * later using another fine acq.
     */
    log_info("No channels free :(\n");
    acq_prn_param[prn].state = ACQ_PRN_TRIED;
    return;
  }
  /* Transition to tracking. */
  u32 track_count = nap_timing_count() + 20000;
  cp = propagate_code_phase(cp, cf, track_count - timer_count);

  // Contrive for the timing strobe to occur at or close to a PRN edge (code phase = 0)
  track_count += 16*(1023.0-cp)*(1.0 + cf / GPS_L1_HZ);

  tracking_channel_init(chan, prn, cf, track_count, snr);
  acq_prn_param[prn].state = ACQ_PRN_TRACKING;
  nap_timing_strobe_wait(100);
}
예제 #10
0
int main(void)
{
  init();

  led_toggle(LED_RED);

  printf("\n\nFirmware info - git: " GIT_VERSION ", built: " __DATE__ " " __TIME__ "\n");
  u8 nap_git_hash[20];
  nap_conf_rd_git_hash(nap_git_hash);
  printf("SwiftNAP git: ");
  for (u8 i=0; i<20; i++)
    printf("%02x", nap_git_hash[i]);
  if (nap_conf_rd_git_unclean())
    printf(" (unclean)");
  printf("\n");
  printf("SwiftNAP configured with %d tracking channels\n\n", nap_track_n_channels);

  cw_setup();
  manage_acq_setup();
  tick_timer_setup();
  timing_setup();
  position_setup();

  channel_measurement_t meas[nap_track_n_channels];
  navigation_measurement_t nav_meas[nap_track_n_channels];

  /* TODO: Think about thread safety when updating ephemerides. */
  static ephemeris_t es[32];
  static ephemeris_t es_old[32];
  while(1)
  {
    for (u32 i = 0; i < 3000; i++)
      __asm__("nop");
    sbp_process_messages();
    manage_track();
    manage_acq();

    /* Check if there is a new nav msg subframe to process.
     * TODO: move this into a function */

    memcpy(es_old, es, sizeof(es));
    for (u8 i=0; i<nap_track_n_channels; i++)
      if (tracking_channel[i].state == TRACKING_RUNNING && tracking_channel[i].nav_msg.subframe_start_index) {
        s8 ret = process_subframe(&tracking_channel[i].nav_msg, &es[tracking_channel[i].prn]);
        if (ret < 0)
          printf("PRN %02d ret %d\n", tracking_channel[i].prn+1, ret);

        if (ret == 1 && !es[tracking_channel[i].prn].healthy)
          printf("PRN %02d unhealthy\n", tracking_channel[i].prn+1);
        if (memcmp(&es[tracking_channel[i].prn], &es_old[tracking_channel[i].prn], sizeof(ephemeris_t))) {
          printf("New ephemeris for PRN %02d\n", tracking_channel[i].prn+1);
          /* TODO: This is a janky way to set the time... */
          gps_time_t t;
          t.wn = es[tracking_channel[i].prn].toe.wn;
          t.tow = tracking_channel[i].TOW_ms / 1000.0;
          if (gpsdifftime(t, es[tracking_channel[i].prn].toe) > 2*24*3600)
            t.wn--;
          else if (gpsdifftime(t, es[tracking_channel[i].prn].toe) < 2*24*3600)
            t.wn++;
          set_time(TIME_COARSE, t);
      }
    }

    DO_EVERY_TICKS(TICK_FREQ/10,

      u8 n_ready = 0;
      for (u8 i=0; i<nap_track_n_channels; i++) {
        if (es[tracking_channel[i].prn].valid == 1 && \
            es[tracking_channel[i].prn].healthy == 1 && \
            tracking_channel[i].state == TRACKING_RUNNING && \
            tracking_channel[i].TOW_ms > 0) {
          __asm__("CPSID i;");
          tracking_update_measurement(i, &meas[n_ready]);
          __asm__("CPSIE i;");

          n_ready++;
        }
      }

      if (n_ready >= 4) {
        /* Got enough sats/ephemerides, do a solution. */
        /* TODO: Instead of passing 32 LSBs of nap_timing_count do something
         * more intelligent with the solution time.
         */
        calc_navigation_measurement(n_ready, meas, nav_meas, (double)((u32)nap_timing_count())/SAMPLE_FREQ, es);

        dops_t dops;
        if (calc_PVT(n_ready, nav_meas, &position_solution, &dops) == 0) {
          position_updated();

          sbp_send_msg(MSG_SOLUTION, sizeof(gnss_solution), (u8 *) &position_solution);
          nmea_gpgga(&position_solution, &dops);

          DO_EVERY(10,
            sbp_send_msg(MSG_DOPS, sizeof(dops_t), (u8 *) &dops);
            nmea_gpgsv(n_ready, nav_meas, &position_solution);
          );
        }