Skip to content
Browse files

Add almanac code.

  • Loading branch information...
1 parent d89266b commit 459d4d965fc6a23fee724b5f10de9a1a041fd1db @fnoble fnoble committed Mar 13, 2013
Showing with 221 additions and 0 deletions.
  1. +48 −0 include/almanac.h
  2. +1 −0 src/CMakeLists.txt
  3. +172 −0 src/almanac.c
View
48 include/almanac.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2010 Swift Navigation Inc.
+ * Contact: Fergus Noble <fergus@swift-nav.com>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#ifndef LIBSWIFTNAV_ALMANAC_H
+#define LIBSWIFTNAV_ALMANAC_H
+
+#include "common.h"
+#include "almanac.h"
+
+/** \addtogroup almanac
+ * \{ */
+
+/** Structure containing the GPS almanac for one satellite. */
+typedef struct {
+ double ecc; /**< Eccentricity in radians. */
+ 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. */
+ u8 prn; /**< PRN number of the satellite. */
+ u8 healthy; /**< Satellite health status. */
+} almanac_t;
+
+/** \} */
+
+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]);
+
+#endif /* LIBSWIFTNAV_ALMANAC_H */
View
1 src/CMakeLists.txt
@@ -16,6 +16,7 @@ set(libswiftnav_SRCS
coord_system.c
linear_algebra.c
prns.c
+ almanac.c
)
if (CMAKE_CROSSCOMPILING)
View
172 src/almanac.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) 2010 Swift Navigation Inc.
+ * Contact: Fergus Noble <fergus@swift-nav.com>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#include <math.h>
+
+#include "pvt.h"
+#include "linear_algebra.h"
+#include "coord_system.h"
+#include "almanac.h"
+
+/** \defgroup almanac Almanac
+ * Functions and calculations related to the GPS almanac.
+ *
+ * \note All positions are referenced to the WGS84 coordinate system.
+ * \see coord_system
+ * \{ */
+
+/** Calculate the position / velocity state of a satellite from the almanac.
+ *
+ * \param alm Pointer to an almanac structure for the satellite of interest.
+ * \param t GPS time of week at which to calculate the satellite state in
+ * seconds since Sunday.
+ * \param week GPS week number or pass -1 to assume within one half-week of the
+ * almanac time of applicability.
+ * \param pos The satellite position in ECEF coordinates is returned in this
+ * vector.
+ * \param vel The satellite velocity in ECEF coordinates is returned in this
+ * vector. Ignored if NULL.
+ */
+void calc_sat_state_almanac(almanac_t* alm, double t, s16 week,
+ double pos[3], double vel[3])
+{
+ /* Seconds since the almanac reference epoch. */
+ double dt = t - alm->toa;
+
+ if (week < 0) {
+ /* Week number unknown, correct time for beginning or end of week
+ * crossovers and limit to +/- 302400 (i.e. assume dt is within a
+ * half-week). */
+ if (dt > 302400)
+ dt -= 604800;
+ else if (dt < -302400)
+ dt += 604800;
+ } else {
+ /* Week number specified, correct time using week number difference. */
+ s32 dweeks = week - alm->week;
+ dt += dweeks * 604800;
+ }
+
+ /* Calculate position and velocity per ICD-GPS-200D Table 20-IV. */
+
+ /* Calculate mean motion in radians/sec. */
+ double ma_dot = sqrt (NAV_GM / (alm->a * alm->a * alm->a));
+ /* Calculate corrected mean anomaly in radians. */
+ double ma = alm->ma + ma_dot * dt;
+
+ /* Iteratively solve for the Eccentric Anomaly
+ * (from Keith Alter and David Johnston). */
+ double ea = ma; /* Starting value for E. */
+ double ea_old = ea + 1;
+ double temp;
+
+ while (fabs(ea - ea_old) > 1.0e-14)
+ {
+ ea_old = ea;
+ temp = 1.0 - alm->ecc * cos(ea_old);
+ ea = ea + (ma - ea_old + alm->ecc * sin(ea_old)) / temp;
+ }
+ double ea_dot = ma_dot / temp;
+
+ /* Begin calculation for True Anomaly and Argument of Latitude. */
+ double temp2 = sqrt(1.0 - alm->ecc * alm->ecc);
+ /* Argument of Latitude = True Anomaly + Argument of Perigee. */
+ double al = atan2(temp2 * sin(ea), cos(ea) - alm->ecc) + alm->argp;
+ double al_dot = temp2 * ea_dot / temp;
+
+ /* Calculate corrected radius based on argument of latitude. */
+ double r = alm->a * temp;
+ double r_dot = alm->a * alm->ecc * sin(ea) * ea_dot;
+
+ /* Calculate position and velocity in orbital plane. */
+ double x = r * cos(al);
+ double y = r * sin(al);
+ double x_dot = r_dot * cos(al) - y * al_dot;
+ double y_dot = r_dot * sin(al) + x * al_dot;
+
+ /* Corrected longitude of ascending node. */
+ double om_dot = alm->rora - NAV_OMEGAE_DOT;
+ double om = alm->raaw + dt * om_dot - NAV_OMEGAE_DOT * alm->toa;
+
+ /* Compute the satellite's position in Earth-Centered Earth-Fixed
+ * coordinates. */
+ pos[0] = x * cos(om) - y * cos(alm->inc) * sin(om);
+ pos[1] = x * sin(om) + y * cos(alm->inc) * cos(om);
+ pos[2] = y * sin(alm->inc);
+
+ /* Compute the satellite's velocity in Earth-Centered Earth-Fixed
+ * coordinates. */
+ if (vel) {
+ temp = y_dot * cos(alm->inc);
+ vel[0] = -om_dot * pos[1] + x_dot * cos(om) - temp * sin(om);
+ vel[1] = om_dot * pos[0] + x_dot * sin(om) + temp * cos(om);
+ vel[2] = y_dot * sin(alm->inc);
+ }
+
+}
+
+/** 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 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);
+}
+
+/** Calculate the Doppler shift of a satellite as observed at 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 Doppler shift.
+ * \param week GPS week number 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.
+ * \return The Doppler shift in Hz.
+ */
+double calc_sat_doppler_almanac(almanac_t* alm, double t, s16 week,
+ double ref[3])
+{
+ double sat_pos[3];
+ double sat_vel[3];
+ double vec_ref_sat[3];
+
+ calc_sat_state_almanac(alm, t, week, sat_pos, sat_vel);
+
+ /* Find the vector from the reference position to the satellite. */
+ vector_subtract(3, sat_pos, ref, vec_ref_sat);
+
+ /* Find the satellite velocity projected on the line of sight vector from the
+ * reference position to the satellite. */
+ double radial_velocity = vector_dot(3, vec_ref_sat, sat_vel) / \
+ vector_norm(3, vec_ref_sat);
+
+ /* Return the Doppler shift. */
+ return GPS_L1_HZ * radial_velocity / NAV_C;
+}
+
+/** \} */
+

0 comments on commit 459d4d9

Please sign in to comment.
Something went wrong with that request. Please try again.