Example #1
0
/** Assemble a NMEA GPGSV message and send it out NMEA USARTs.
 * NMEA GPGSV message contains GPS satellites in view.
 *
 * \param n_used   Number of satellites currently being tracked.
 * \param nav_meas Array of navigation_measurement structs.
 * \param soln     Pointer to gnss_solution struct.
 */
void nmea_gpgsv(u8 n_used, const navigation_measurement_t *nav_meas,
                const gnss_solution *soln)
{
  if (n_used == 0)
    return;

  u8 n_mess = (n_used + 3) / 4;


  u8 n = 0;
  double az, el;

  for (u8 i = 0; i < n_mess; i++) {
    NMEA_SENTENCE_START(120);
    NMEA_SENTENCE_PRINTF("$GPGSV,%d,%d,%d", n_mess, i+1, n_used);

    for (u8 j = 0; j < 4; j++) {
      if (n < n_used) {
        wgsecef2azel(nav_meas[n].sat_pos, soln->pos_ecef, &az, &el);
        NMEA_SENTENCE_PRINTF(",%02d,%02d,%03d,%02d",
          nav_meas[n].prn + 1,
          (u8)round(el * R2D),
          (u16)round(az * R2D),
          (u8)round(nav_meas[n].snr)
          );
      } else {
        NMEA_SENTENCE_PRINTF(",,,,");
      }
      n++;
    }

    NMEA_SENTENCE_DONE();
  }

}
Example #2
0
/** Calculate the azimuth and elevation of a satellite from a reference
 * position given the satellite almanac.
 *
 * \param alm  Pointer to an almanac structure for the satellite of interest.
 * \param t    GPS time of week at which to calculate the az/el.
 * \param week GPS week number modulo 1024 or pass -1 to assume within one
 *             half-week of the almanac time of applicability.
 * \param ref  ECEF coordinates of the reference point from which the azimuth
 *             and elevation is to be determined, passed as [X, Y, Z], all in
 *             meters.
 * \param az   Pointer to where to store the calculated azimuth output.
 * \param el   Pointer to where to store the calculated elevation output.
 */
void calc_sat_az_el_almanac(almanac_t* alm, double t, s16 week,
                            double ref[3], double* az, double* el)
{
  double sat_pos[3];
  calc_sat_state_almanac(alm, t, week, sat_pos, 0);
  wgsecef2azel(sat_pos, ref, az, el);
}
Example #3
0
/** Assemble an NMEA GPRMC message and send it out NMEA USARTs.
 * NMEA RMC contains minimum GPS data 
 *
 * \param nav_meas Pointer to navigation_measurement struct.
 * \param soln Pointer to gnss_solution struct
 * \param gps_t Pointer to the current GPS Time
 */
void nmea_gprmc(const navigation_measurement_t *nav_meas,
                const gnss_solution *soln, const gps_time_t *gps_t)
{
  
  /* NMEA Parameters
   * Ex.
   * $GPRMC,220516,A,5133.82,N,00042.24,W,173.8,231.8,130694,004.2,W*70
   *   |      |    |    |    |    |     |   |      |      |     |  |  |
   * Command  |    |   Lat  N/S   |     |   |      | Date Stamp | W/E |
   *    Time (UTC) |            Long   W/E  |  True Course      |   Cksum
   *            Validity (A-OK)           Speed            Variation 
   * Variation is ignored as we have no way to maintain that information
   * currently
   */
  time_t unix_t;
  struct tm t;

  unix_t = gps2time(*gps_t);
  gmtime_r(&unix_t, &t);
  double frac_s = fmod(gps_t->tow, 1.0);

  s16 lat_deg = R2D * (soln->pos_llh[0]);
  double lat_min = MINUTES(soln->pos_llh[0]);
  s16 lon_deg = R2D * (soln->pos_llh[1]);
  double lon_min = MINUTES(soln->pos_llh[1]);
  lat_deg = abs(lat_deg);
  lon_deg = abs(lon_deg);

  char lat_dir = soln->pos_llh[0] < 0 ? 'S' : 'N';
  char lon_dir = soln->pos_llh[1] < 0 ? 'W' : 'E';

  float velocity;
  float x,y,z;
  x = soln->vel_ned[0];
  y = soln->vel_ned[1];
  z = soln->vel_ned[2];
  float course = atan2(y,x);

  /* Conversion to magnitue knots */
  velocity = MS2KNOTTS(x,y,z);

  double az, el;
  wgsecef2azel(nav_meas[0].sat_pos, soln->pos_ecef, &az, &el);

  char buf[100];
  u8 n = sprintf(buf,
                "$GPRMC,%02d%02d%06.3f,A," /* Command, Time (UTC), Valid */
                "%02d%010.7f,%c,%03d%010.7f,%c," /* Lat/Lon */
                "%06.2f,%05.1f," /* Speed, Course */
                "%02d%02d%02d," /* Date Stamp */
                ",", /* Variation */
                t.tm_hour, t.tm_min, t.tm_sec + frac_s,
                lat_deg, lat_min, lat_dir, lon_deg, lon_min, lon_dir,
                velocity, course * R2D, 
                t.tm_mday, t.tm_mon, t.tm_year-100);

  u8 sum = nmea_checksum(buf);
  sprintf(buf + n, "*%02X\r\n", sum);
  nmea_output(buf);
}
Example #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(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;
}
Example #5
0
/** Assemble a NMEA GPGSV message and send it out NMEA USARTs.
 * NMEA GPGSV message contains GPS satellites in view.
 *
 * \param n_used   Number of satellites currently being tracked.
 * \param nav_meas Pointer to navigation_measurement struct.
 * \param soln     Pointer to gnss_solution struct.
 */
void nmea_gpgsv(u8 n_used, navigation_measurement_t *nav_meas,
                gnss_solution *soln)
{
  if (n_used == 0)
    return;

  u8 n_mess = (n_used + 3) / 4;

  char buf[80];
  char *buf0 = buf + sprintf(buf, "$GPGSV,%d,", n_mess);
  char *bufp = buf0;

  u8 n = 0;
  double az, el;

  for (u8 i = 0; i < n_mess; i++) {
    bufp = buf0;
    bufp += sprintf(bufp, "%d,%d", i + 1, n_used);

    for (u8 j = 0; j < 4; j++) {
      if (n < n_used) {
        wgsecef2azel(nav_meas[n].sat_pos, soln->pos_ecef, &az, &el);
        bufp += sprintf(
          bufp, ",%02d,%02d,%03d,%02d",
          nav_meas[n].prn + 1,
          (u8)round(el * 180.0 / M_PI),
          (u16)round(az * 180.0 / M_PI),
          (u8)(10.0 * nav_meas[n].snr)
          );
      } else {
        bufp += sprintf(bufp, ",,,,");
      }
      n++;
    }

    sprintf(bufp, "*%02X\r\n", nmea_checksum(buf));
    nmea_output(buf);
  }

}
Example #6
0
/** Assemble an NMEA GPVTG message and send it out NMEA USARTs.
 * NMEA VTG contains course and speed
 *
 * \param nav_meas Pointer to navigation_measurement struct.
 * \param soln Pointer to gnss_solution struct
 */
void nmea_gpvtg(const navigation_measurement_t *nav_meas,
                const gnss_solution *soln)
{
  /* NMEA Parameters for GPVTG
   * Ex.
   * $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K
   *    |     |   |    |  |   |   |   |   |
   * Command  |  'T'   | 'M'  |  'N'  |  'K'
   *     True Course   |  Speed (K)   |
   *               Mag. course     Speed (km/hr)
   */

  double az, el;
  wgsecef2azel(nav_meas[0].sat_pos, soln->pos_ecef, &az, &el);

  float vknots, vkmhr;
  float x,y,z;
  x = soln->vel_ned[0];
  y = soln->vel_ned[1];
  z = soln->vel_ned[2];
  float course = atan2(y,x);

  /* Conversion to magnitue knots */
  vknots = MS2KNOTTS(x,y,z);
  /* Conversion to magnitue km/hr */
  vkmhr = MS2KMHR(x,y,z);

  char buf[80];
  u8 n = sprintf(buf, 
                  "$GPVTG,%05.1f,T," /* Command, course, */
                  ",M," /* Magnetic Course (omitted) */
                  "%06.2f,N,%06.2f,K", /* Speed (knots, km/hr) */
                  course* R2D,
                  vknots, vkmhr);
  
  u8 sum = nmea_checksum(buf);
  sprintf(buf + n, "*%02X\r\n", sum);
  nmea_output(buf);
}
Example #7
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;
}