Merge pull request #269 from swift-nav/improve-almanac

Improve almanac and ephemeris functions

Author: ljbade

include/libswiftnav/almanac.h

@@ -15,57 +15,60 @@
 
 #include <libswiftnav/common.h>
 #include <libswiftnav/signal.h>
+#include <libswiftnav/time.h>
 
 /** \addtogroup almanac
  * \{ */
 
 /** Structure containing the GPS almanac for one satellite. */
 typedef struct {
-  double ecc;   /**< Eccentricity (unitless) */
-  double toa;   /**< Time of Applicability in seconds since Sunday. */
-  double inc;   /**< Inclination in radians. */
-  double rora;  /**< Rate of Right Ascension in radians/sec. */
-  double a;     /**< Semi-major axis in meters. */
-  double raaw;  /**< Right Ascension at Week in radians. */
-  double argp;  /**< Argument of Perigee in radians. */
-  double ma;    /**< Mean Anomaly at Time of Applicability in radians. */
-  double af0;   /**< 0-order clock correction in seconds. */
-  double af1;   /**< 1-order clock correction in seconds/second. */
-  u16 week;     /**< GPS week number, modulo 1024. */
-} almanac_gps_t;
+  double m0;       /**< Mean anomaly at reference time [semi-circles] */
+  double ecc;      /**< Eccentricity. */
+  double sqrta;    /**< Square root of the semi-major axis [sqrt(m)] */
+  double omega0;   /**< Longitude of ascending node
+                        of orbit plane at weekly epoch [semi-circles] */
+  double omegadot; /**< Rate of right ascension [semi-circles/s] */
+  double w;        /**< Argument of perigee [semi-circles] */
+  double inc;      /**< Inclindation angle at reference time [semi-circles]
+                        This must include the 0.3 offset from NAV delta_i. */
+  double af0;      /**< Time offset of the sat clock [s] **/
+  double af1;      /**< Drift of the sat clock [s/s] **/
+} almanac_kepler_t;
 
 /** Structure containing the SBAS almanac for one satellite. */
 typedef struct {
-  u8 data_id;   /**< Data ID. */
-
-  u16 x;        /**< X coordinate ECEF. */
-  u16 y;        /**< Y coordinate ECEF. */
-  u16 z;        /**< Z coordinate ECEF. */
-
-  u8 x_rate;    /**< X coordinate rate of change [m/s]. */
-  u8 y_rate;    /**< Y coordinate rate of change [m/s]. */
-  u8 z_rate;    /**< Z coordinate rate of change [m/s]. */
-
-  u16 t0;       /**< Time of day [seconds]. */
-} almanac_sbas_t;
+  double pos[3]; /**< Position of the GEO at time toe [m] */
+  double vel[3]; /**< velocity of the GEO at time toe [m/s] */
+  double acc[3]; /**< velocity of the GEO at time toe [m/s^2] */
+} almanac_xyz_t;
 
+/** Structure containing the almanac for one satellite. */
 typedef struct {
   gnss_signal_t sid; /**< Signal ID. */
-  u8 healthy;   /**< Satellite health status. */
-  u8 valid;     /**< Almanac is valid. */
+  gps_time_t toa;    /**< Reference time of almanac. */
+  float ura;         /**< User range accuracy [m] */
+  u32 fit_interval;  /**< Curve fit interval [s] */
+  u8 valid;          /**< Almanac is valid. */
+  u8 healthy;        /**< Satellite health status. */
   union {
-    almanac_gps_t gps;
-    almanac_sbas_t sbas;
+    almanac_kepler_t kepler; /**< Parameters specific to GPS. */
+    almanac_xyz_t xyz;       /**< Parameters specific to SBAS. */
   };
 } almanac_t;
 
 /** \} */
 
-void calc_sat_state_almanac(const almanac_t* alm, double t, s16 week,
-                            double pos[3], double vel[3]);
-void calc_sat_az_el_almanac(const almanac_t* alm, double t, s16 week,
-                            const double ref[3], double* az, double* el);
-double calc_sat_doppler_almanac(const almanac_t* alm, double t, s16 week,
-                                const double ref[3]);
+s8 calc_sat_state_almanac(const almanac_t *a, const gps_time_t *t,
+                            double pos[3], double vel[3],
+                            double *clock_err, double *clock_rate_err);
+s8 calc_sat_az_el_almanac(const almanac_t *a, const gps_time_t *t,
+                          const double ref[3], double *az, double *el);
+s8 calc_sat_doppler_almanac(const almanac_t *a, const gps_time_t *t,
+                            const double ref[3], double *doppler);
+
+u8 almanac_valid(const almanac_t *a, const gps_time_t *t);
+u8 satellite_healthy_almanac(const almanac_t *a);
+
+bool almanac_equal(const almanac_t *a, const almanac_t *b);
 
 #endif /* LIBSWIFTNAV_ALMANAC_H */

include/libswiftnav/ephemeris.h

@@ -18,50 +18,81 @@
 #include <libswiftnav/time.h>
 #include <libswiftnav/common.h>
 
+/** \addtogroup ephemeris
+ * \{ */
+
+/** Structure containing the GPS ephemeris for one satellite. */
 typedef struct {
-  double tgd;
-  double crs, crc, cuc, cus, cic, cis;
-  double dn, m0, ecc, sqrta, omega0, omegadot, w, inc, inc_dot;
-  double af0, af1, af2;
-  gps_time_t toc;
-  u16 iodc;
-  u8 iode;
+  double tgd;      /**< Group delay between L1 and L2 [s] */
+  double crc;      /**< Amplitude of the cosine harmonic correction term
+                        to the orbit radius [m] */
+  double crs;      /**< Amplitude of the sine harmonic correction term
+                        to the orbit radius [m] */
+  double cuc;      /**< Amplitude of the cosine harmonic correction term
+                        to the argument of latitude [rad] */
+  double cus;      /**< Amplitude of the sine harmonic correction term
+                        to the argument of latitude [rad] */
+  double cic;      /**< Amplitude of the cosine harmonic correction term
+                        to the angle of inclination [rad] */
+  double cis;      /**< Amplitude of the sine harmonic correction term
+                        to the angle of inclination [rad] */
+  double dn;       /**< Mean motion difference from computed value
+                        [semi-circles/s] */
+  double m0;       /**< Mean anomaly at reference time [semi-circles] */
+  double ecc;      /**< Eccentricity. */
+  double sqrta;    /**< Square root of the semi-major axis [sqrt(m)] */
+  double omega0;   /**< Longitude of ascending node
+                        of orbit plane at weekly epoch [semi-circles] */
+  double omegadot; /**< Rate of right ascension [semi-circles/s] */
+  double w;        /**< Argument of perigee [semi-circles] */
+  double inc;      /**< Inclindation angle at reference time [semi-circles] */
+  double inc_dot;  /**< Rate of inclination angle [semi-circles/s] */
+  double af0;      /**< Time offset of the sat clock [s] **/
+  double af1;      /**< Drift of the sat clock [s/s] **/
+  double af2;      /**< Acceleration of the sat clock [s/s^2] **/
+  gps_time_t toc;  /**< Reference time of clock. */
+  u16 iodc;        /**< Issue of data clock. */
+  u8 iode;         /**< Issue of data ephemeris. */
 } ephemeris_kepler_t;
 
+/** Structure containing the SBAS ephemeris for one satellite. */
 typedef struct {
-  double pos[3];
-  double rate[3];
-  double acc[3];
-  u8 iod;
-  u16 toa;
-  double a_gf0;
-  double a_gf1;
+  double pos[3]; /**< Position of the GEO at time toe [m] */
+  double vel[3]; /**< velocity of the GEO at time toe [m/s] */
+  double acc[3]; /**< velocity of the GEO at time toe [m/s^2] */
+  double a_gf0;  /**< Time offset of the GEO clock w.r.t. SNT [s] */
+  double a_gf1;  /**< Drift of the GEO clock w.r.t. SNT [s/s] */
 } ephemeris_xyz_t;
 
+/** Structure containing the ephemeris for one satellite. */
 typedef struct {
-  gnss_signal_t sid;
-  gps_time_t toe;
-  float ura;
-  u8 fit_interval;
-  u8 valid;
-  u8 healthy;
+  gnss_signal_t sid; /**< Signal ID. */
+  gps_time_t toe;    /**< Reference time of ephemeris. */
+  float ura;         /**< User range accuracy [m] */
+  u32 fit_interval;  /**< Curve fit interval [s] */
+  u8 valid;          /**< Ephemeris is valid. */
+  u8 healthy;        /**< Satellite health status. */
   union {
-    ephemeris_kepler_t kepler;
-    ephemeris_xyz_t xyz;
+    ephemeris_kepler_t kepler; /**< Parameters specific to GPS. */
+    ephemeris_xyz_t xyz;       /**< Parameters specific to SBAS. */
   };
 } ephemeris_t;
 
+/** \} */
+
 s8 calc_sat_state(const ephemeris_t *e, const gps_time_t *t,
                   double pos[3], double vel[3],
                   double *clock_err, double *clock_rate_err);
+s8 calc_sat_az_el(const ephemeris_t *e, const gps_time_t *t,
+                  const double ref[3], double *az, double *el);
+s8 calc_sat_doppler(const ephemeris_t *e, const gps_time_t *t,
+                    const double ref[3], double *doppler);
 
-u8 ephemeris_valid(const ephemeris_t *eph, const gps_time_t *t);
-u8 ephemeris_params_valid(const u8 v, const u8 fit_interval,
+u8 ephemeris_valid(const ephemeris_t *e, const gps_time_t *t);
+u8 ephemeris_params_valid(const u8 valid, const u32 fit_interval,
                       const gps_time_t* toe, const gps_time_t *t);
-u8 satellite_healthy(const ephemeris_t *eph);
+u8 satellite_healthy(const ephemeris_t *e);
 
-float decode_ura_index(const u8 index);
-u8 decode_fit_interval(u8 fit_interval_flag, u16 iodc);
 void decode_ephemeris(u32 frame_words[3][8], ephemeris_t *e);
 bool ephemeris_equal(const ephemeris_t *a, const ephemeris_t *b);
 

python/swiftnav/almanac.pxd

@@ -8,43 +8,40 @@
 # WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
 
 from common cimport *
-from signal cimport *
+from time cimport gps_time_t
+from signal cimport gnss_signal_t
 
 cdef extern from "libswiftnav/almanac.h":
-  ctypedef struct almanac_gps_t:
-    double ecc
-    double toa
-    double inc
-    double rora
-    double a
-    double raaw
-    double argp
-    double ma
-    double af0
-    double af1
-    u16 week
+  ctypedef struct almanac_kepler_t:
+    double m0, ecc, sqrta, omega0, omegadot, w, inc
+    double af0, af1
 
-  ctypedef struct almanac_sbas_t:
-    u8 data_id
-    u16 x
-    u16 y
-    u16 z
-    u8 x_rate
-    u8 y_rate
-    u8 z_rate
-    u16 t0
+  ctypedef struct almanac_xyz_t:
+    double pos[3]
+    double vel[3]
+    double acc[3]
 
   ctypedef struct almanac_t:
     gnss_signal_t sid
-    u8 healthy
+    gps_time_t toa
+    float ura
+    u32 fit_interval
     u8 valid
+    u8 healthy
     # HACK: Actually an anonymous union in libswiftnat!
-    almanac_gps_t gps
-    almanac_sbas_t sbas
+    almanac_kepler_t kepler
+    almanac_xyz_t xyz
 
-  void calc_sat_state_almanac(almanac_t* alm, double t, s16 week, double pos[3], double vel[3])
-  void calc_sat_az_el_almanac(almanac_t* alm, double t, s16 week, double ref[3], double* az, double* el)
-  double calc_sat_doppler_almanac(almanac_t* alm, double t, s16 week, double ref[3])
+  void calc_sat_state_almanac(const almanac_t *a, const gps_time_t *t,
+                              double pos[3], double vel[3],
+                              double *clock_err, double *clock_rate_err)
+  void calc_sat_az_el_almanac(const almanac_t *a, const gps_time_t *t,
+                              double ref[3], double *az, double *el)
+  double calc_sat_doppler_almanac(const almanac_t *a, const gps_time_t *t,
+                                  double ref[3], double *doppler)
+  u8 almanac_valid(const almanac_t *a, const gps_time_t *t)
+  u8 satellite_healthy_almanac(const almanac_t *a)
+  u8 almanac_equal(const almanac_t *a, const almanac_t *b)
 
 cdef class Almanac:
   cdef almanac_t _thisptr

python/swiftnav/almanac.pyx

@@ -17,6 +17,8 @@ Note: All positions are referenced to the WGS84 coordinate system.
 """
 
 from common cimport *
+from time cimport *
+from time import GpsTime
 from fmt_utils import fmt_repr
 from libc.string cimport memset
 cimport numpy as np
@@ -27,12 +29,15 @@ cdef class Almanac:
   def __init__(self, **kwargs):
     memset(&self._thisptr, 0, sizeof(almanac_t))
     self._thisptr.sid = kwargs.pop('sid')
-    self._thisptr.healthy = kwargs.pop('healthy')
+    self._thisptr.toa = kwargs.pop('toa')
+    self._thisptr.ura = kwargs.pop('ura')
+    self._thisptr.fit_interval = kwargs.pop('fit_interval')
     self._thisptr.valid = kwargs.pop('valid')
-    if 'gps' in kwargs:
-      self._thisptr.gps = kwargs.pop('gps')
-    elif 'sbas' in kwargs:
-      self._thisptr.sbas = kwargs.pop('sbas')
+    self._thisptr.healthy = kwargs.pop('healthy')
+    if 'kepler' in kwargs:
+      self._thisptr.kepler = kwargs.pop('kepler')
+    elif 'xyz' in kwargs:
+      self._thisptr.xyz = kwargs.pop('xyz')
 
   def __getattr__(self, k):
     return self._thisptr.get(k)
@@ -46,17 +51,14 @@ cdef class Almanac:
   def from_dict(self, d):
     self._thisptr = d
 
-  def calc_state(self, t, week=-1):
+  def calc_state(self, GpsTime t):
     """
     Wraps the function :libswiftnav:`calc_sat_state_almanac`.
 
     Parameters
     ----------
-    t : float
+    t : GpsTime
       The GPS time at which to calculate the azimuth and elevation.
-    week : int, optional
-      The GPS week number modulo 1024. If `None` then it is assumed that `t` is
-      within one half week of the almanac time of applicability.
 
     Returns
     -------
@@ -65,24 +67,22 @@ cdef class Almanac:
       coordinate system.
 
     """
-    cdef np.ndarray[np.double_t, ndim=1, mode="c"] pos = np.empty(3, dtype=np.double)
-    cdef np.ndarray[np.double_t, ndim=1, mode="c"] vel = np.empty(3, dtype=np.double)
-    calc_sat_state_almanac(&self._thisptr, t, week, &pos[0], &vel[0])
-    return (pos, vel)
+    cdef np.ndarray[np.double_t, ndim=1, mode="c"] pos = np.array([0,0,0], dtype=np.double)
+    cdef np.ndarray[np.double_t, ndim=1, mode="c"] vel = np.array([0,0,0], dtype=np.double)
+    cdef double clock_err, clock_rate_err
+    calc_sat_state_almanac(&self._thisptr, &t._thisptr, &pos[0], &vel[0], &clock_err, &clock_rate_err)
+    return (pos, vel, clock_err, clock_rate_err)
 
-  def calc_az_el(self, t, ref, week=None):
+  def calc_az_el(self, GpsTime t, ref):
     """
     Wraps the function :libswiftnav:`calc_sat_az_el_almanac`.
 
     Parameters
     ----------
-    t : float
+    t : GpsTime
       The GPS time at which to calculate the azimuth and elevation.
     ref : (float, float, float)
       The tuple of coordinates of the reference position, `(x, y, z)`
-    week : int, optional
-      The GPS week number modulo 1024. If `None` then it is assumed that `t` is
-      within one half week of the almanac time of applicability.
 
     Returns
     -------
@@ -93,22 +93,19 @@ cdef class Almanac:
     assert len(ref) == 3, "ECEF coordinates must have dimension 3."
     cdef np.ndarray[np.double_t, ndim=1, mode="c"] ref_ = np.array(ref, dtype=np.double)
     cdef double az, el
-    calc_sat_az_el_almanac(&self._thisptr, t, week, &ref_[0], &az, &el)
+    calc_sat_az_el_almanac(&self._thisptr, &t._thisptr, &ref_[0], &az, &el)
     return (az, el)
 
-  def calc_doppler(self, t, ref, week=-1):
+  def calc_doppler(self, GpsTime t, ref):
     """
     Wraps the function :libswiftnav:`calc_sat_doppler_almanac`.
 
     Parameters
     ----------
-    t : float
+    t : GpsTime
       The GPS time at which to calculate the Doppler shift.
     ref : (float, float, float)
       The tuple of coordinates of the reference position, `(x, y, z)`
-    week : int, optional
-      The GPS week number modulo 1024. If `None` then it is assumed that `t` is
-      within one half week of the almanac time of applicability.
 
     Returns
     -------
@@ -118,4 +115,6 @@ cdef class Almanac:
     """
     assert len(ref) == 3, "ECEF coordinates must have dimension 3."
     cdef np.ndarray[np.double_t, ndim=1, mode="c"] ref_ = np.array(ref, dtype=np.double)
-    return calc_sat_doppler_almanac(&self._thisptr, t, week, &ref_[0])
+    cdef double doppler
+    calc_sat_doppler_almanac(&self._thisptr, &t._thisptr, &ref_[0], &doppler)
+    return doppler