示例#1
0
/** Using available almanac and ephemeris information, determine
 * whether a satellite is in view and the range of doppler frequencies
 * in which we expect to find it.
 *
 * \param prn 0-indexed PRN
 * \param t Time at which to evaluate ephemeris and almanac (typically system's
 *  estimate of current time)
 * \param dopp_hint_low, dopp_hint_high Pointers to store doppler search range
 *  from ephemeris or almanac, if available and elevation > mask
 * \return Score (higher is better)
 */
static u16 manage_warm_start(u8 prn, gps_time_t t,
                             float *dopp_hint_low, float *dopp_hint_high)
{
    /* Do we have any idea where/when we are?  If not, no score. */
    /* TODO: Stricter requirement on time and position uncertainty?
       We ought to keep track of a quantitative uncertainty estimate. */
    if (time_quality < TIME_GUESS &&
        position_quality < POSITION_GUESS)
      return SCORE_COLDSTART;

    float el = 0;
    double el_d, _, dopp_hint = 0, dopp_uncertainty = DOPP_UNCERT_ALMANAC;

    /* Do we have a suitable ephemeris for this sat?  If so, use
       that in preference to the almanac. */
    if (ephemeris_good(&es[prn], t)) {
      double sat_pos[3], sat_vel[3], el_d;
      calc_sat_state(&es[prn], t, sat_pos, sat_vel, &_, &_);
      wgsecef2azel(sat_pos, position_solution.pos_ecef, &_, &el_d);
      el = (float)(el_d) * R2D;
      if (el < elevation_mask)
        return SCORE_BELOWMASK;
      vector_subtract(3, sat_pos, position_solution.pos_ecef, sat_pos);
      vector_normalize(3, sat_pos);
      /* sat_pos now holds unit vector from us to satellite */
      vector_subtract(3, sat_vel, position_solution.vel_ecef, sat_vel);
      /* sat_vel now holds velocity of sat relative to us */
      dopp_hint = -GPS_L1_HZ * (vector_dot(3, sat_pos, sat_vel) / GPS_C
                                + position_solution.clock_bias);
      /* TODO: Check sign of receiver frequency offset correction */
      if (time_quality >= TIME_FINE)
        dopp_uncertainty = DOPP_UNCERT_EPHEM;
    } else if (almanac[prn].valid) {
      calc_sat_az_el_almanac(&almanac[prn], t.tow, t.wn-1024,
                             position_solution.pos_ecef, &_, &el_d);
      el = (float)(el_d) * R2D;
      if (el < elevation_mask)
        return SCORE_BELOWMASK;
      dopp_hint = -calc_sat_doppler_almanac(&almanac[prn], t.tow, t.wn,
                                            position_solution.pos_ecef);
    } else {
      return SCORE_COLDSTART; /* Couldn't determine satellite state. */
    }
    /* Return the doppler hints and a score proportional to elevation */
    *dopp_hint_low = dopp_hint - dopp_uncertainty;
    *dopp_hint_high = dopp_hint + dopp_uncertainty;
    return SCORE_COLDSTART + SCORE_WARMSTART * el / 90.f;
}
示例#2
0
static void manage_calc_almanac_scores(void)
{
  double az, el;
  gps_time_t t = get_current_time();

  for (u8 prn=0; prn<32; prn++) {
    if (!almanac[prn].valid ||
        time_quality == TIME_UNKNOWN ||
        position_quality == POSITION_UNKNOWN) {
      /* No almanac or position/time information, give it the benefit of the
       * doubt. */
      acq_prn_param[prn].score[ACQ_HINT_ALMANAC] = 0;
    } else {
      calc_sat_az_el_almanac(&almanac[prn], t.tow, t.wn-1024, position_solution.pos_ecef, &az, &el);
      float dopp = -calc_sat_doppler_almanac(&almanac[prn], t.tow, t.wn,
                                             position_solution.pos_ecef);
      acq_prn_param[prn].score[ACQ_HINT_ALMANAC] =
            el > 0 ? (u16)(SCORE_ALMANAC * 2 * el / M_PI) : 0;
      acq_prn_param[prn].dopp_hint_low = dopp - ALMANAC_DOPPLER_WINDOW;
      acq_prn_param[prn].dopp_hint_high = dopp + ALMANAC_DOPPLER_WINDOW;
    }
  }
}
示例#3
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);
}
示例#4
0
/** Using available almanac and ephemeris information, determine
 * whether a satellite is in view and the range of doppler frequencies
 * in which we expect to find it.
 *
 * \param prn 0-indexed PRN
 * \param t Time at which to evaluate ephemeris and almanac (typically system's
 *  estimate of current time)
 * \param dopp_hint_low, dopp_hint_high Pointers to store doppler search range
 *  from ephemeris or almanac, if available and elevation > mask
 * \return Score (higher is better)
 */
static u16 manage_warm_start(gnss_signal_t sid, const gps_time_t* t,
                             float *dopp_hint_low, float *dopp_hint_high)
{
    /* Do we have any idea where/when we are?  If not, no score. */
    /* TODO: Stricter requirement on time and position uncertainty?
       We ought to keep track of a quantitative uncertainty estimate. */
    if (time_quality < TIME_GUESS &&
        position_quality < POSITION_GUESS)
      return SCORE_COLDSTART;

    float el = 0;
    double _, dopp_hint = 0, dopp_uncertainty = DOPP_UNCERT_ALMANAC;
    bool ready = false;
    /* Do we have a suitable ephemeris for this sat?  If so, use
       that in preference to the almanac. */
    const ephemeris_t *e = ephemeris_get(sid);
    u8 eph_valid;
    s8 ss_ret;
    double sat_pos[3], sat_vel[3], el_d;

    ephemeris_lock();
    eph_valid = ephemeris_valid(e, t);
    if (eph_valid) {
      ss_ret = calc_sat_state(e, t, sat_pos, sat_vel, &_, &_);
    }
    ephemeris_unlock();

    if (eph_valid && (ss_ret == 0)) {
      wgsecef2azel(sat_pos, position_solution.pos_ecef, &_, &el_d);
      el = (float)(el_d) * R2D;
      if (el < elevation_mask)
        return SCORE_BELOWMASK;
      vector_subtract(3, sat_pos, position_solution.pos_ecef, sat_pos);
      vector_normalize(3, sat_pos);
      /* sat_pos now holds unit vector from us to satellite */
      vector_subtract(3, sat_vel, position_solution.vel_ecef, sat_vel);
      /* sat_vel now holds velocity of sat relative to us */
      dopp_hint = -GPS_L1_HZ * (vector_dot(3, sat_pos, sat_vel) / GPS_C
                                + position_solution.clock_bias);
      /* TODO: Check sign of receiver frequency offset correction */
      if (time_quality >= TIME_FINE)
        dopp_uncertainty = DOPP_UNCERT_EPHEM;
      ready = true;
    }

    if(!ready) {
      const almanac_t *a = &almanac[sid_to_global_index(sid)];
      if (a->valid &&
          calc_sat_az_el_almanac(a, t, position_solution.pos_ecef,
                                 &_, &el_d) == 0) {
          el = (float)(el_d) * R2D;
          if (el < elevation_mask)
            return SCORE_BELOWMASK;
          if (calc_sat_doppler_almanac(a, t, position_solution.pos_ecef,
                                       &dopp_hint) != 0) {
            return SCORE_COLDSTART;
          }
          dopp_hint = -dopp_hint;
      } else {
        return SCORE_COLDSTART; /* Couldn't determine satellite state. */
      }
    }

    /* Return the doppler hints and a score proportional to elevation */
    *dopp_hint_low = dopp_hint - dopp_uncertainty;
    *dopp_hint_high = dopp_hint + dopp_uncertainty;
    return SCORE_COLDSTART + SCORE_WARMSTART * el / 90.f;
}