almanac.c
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2010 Swift Navigation Inc.
00003  * Contact: Fergus Noble <fergus@swift-nav.com>
00004  *
00005  * This source is subject to the license found in the file 'LICENSE' which must
00006  * be be distributed together with this source. All other rights reserved.
00007  *
00008  * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
00009  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
00010  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
00011  */
00012 
00013 #include <math.h>
00014 
00015 #include "pvt.h"
00016 #include "linear_algebra.h"
00017 #include "coord_system.h"
00018 #include "almanac.h"
00019 
00039 void calc_sat_state_almanac(almanac_t* alm, double t, s16 week,
00040                             double pos[3], double vel[3])
00041 {
00042   /* Seconds since the almanac reference epoch. */
00043   double dt = t - alm->toa;
00044 
00045   if (week < 0) {
00046     /* Week number unknown, correct time for beginning or end of week
00047      * crossovers and limit to +/- 302400 (i.e. assume dt is within a
00048      * half-week). */
00049     if (dt > 302400)
00050       dt -= 604800;
00051     else if (dt < -302400)
00052       dt += 604800;
00053   } else {
00054     /* Week number specified, correct time using week number difference. */
00055     s32 dweeks = week - alm->week;
00056     dt += dweeks * 604800;
00057   }
00058 
00059   /* Calculate position and velocity per ICD-GPS-200D Table 20-IV. */
00060 
00061   /* Calculate mean motion in radians/sec. */
00062   double ma_dot = sqrt (NAV_GM / (alm->a * alm->a * alm->a));
00063   /* Calculate corrected mean anomaly in radians. */
00064   double ma = alm->ma + ma_dot * dt;
00065 
00066   /* Iteratively solve for the Eccentric Anomaly
00067    * (from Keith Alter and David Johnston). */
00068   double ea = ma;  /* Starting value for E. */
00069   double ea_old;
00070   double temp;
00071 
00072   do {
00073     ea_old = ea;
00074     temp = 1.0 - alm->ecc * cos(ea_old);
00075     ea = ea + (ma - ea_old + alm->ecc * sin(ea_old)) / temp;
00076   } while (fabs(ea - ea_old) > 1.0e-14);
00077 
00078   double ea_dot = ma_dot / temp;
00079 
00080   /* Begin calculation for True Anomaly and Argument of Latitude. */
00081   double temp2 = sqrt(1.0 - alm->ecc * alm->ecc);
00082   /* Argument of Latitude = True Anomaly + Argument of Perigee. */
00083   double al = atan2(temp2 * sin(ea), cos(ea) - alm->ecc) + alm->argp;
00084   double al_dot = temp2 * ea_dot / temp;
00085 
00086   /* Calculate corrected radius based on argument of latitude. */
00087   double r = alm->a * temp;
00088   double r_dot = alm->a * alm->ecc * sin(ea) * ea_dot;
00089 
00090   /* Calculate position and velocity in orbital plane. */
00091   double x = r * cos(al);
00092   double y = r * sin(al);
00093   double x_dot = r_dot * cos(al) - y * al_dot;
00094   double y_dot = r_dot * sin(al) + x * al_dot;
00095 
00096   /* Corrected longitude of ascending node. */
00097   double om_dot = alm->rora - NAV_OMEGAE_DOT;
00098   double om = alm->raaw + dt * om_dot - NAV_OMEGAE_DOT * alm->toa;
00099 
00100   /* Compute the satellite's position in Earth-Centered Earth-Fixed
00101    * coordinates. */
00102   pos[0] = x * cos(om) - y * cos(alm->inc) * sin(om);
00103   pos[1] = x * sin(om) + y * cos(alm->inc) * cos(om);
00104   pos[2] = y * sin(alm->inc);
00105 
00106   /* Compute the satellite's velocity in Earth-Centered Earth-Fixed
00107    * coordinates. */
00108   if (vel) {
00109     temp = y_dot * cos(alm->inc);
00110     vel[0] = -om_dot * pos[1] + x_dot * cos(om) - temp * sin(om);
00111     vel[1] =  om_dot * pos[0] + x_dot * sin(om) + temp * cos(om);
00112     vel[2] = y_dot * sin(alm->inc);
00113   }
00114 
00115 }
00116 
00130 void calc_sat_az_el_almanac(almanac_t* alm, double t, s16 week,
00131                             double ref[3], double* az, double* el)
00132 {
00133   double sat_pos[3];
00134   calc_sat_state_almanac(alm, t, week, sat_pos, 0);
00135   wgsecef2azel(sat_pos, ref, az, el);
00136 }
00137 
00150 double calc_sat_doppler_almanac(almanac_t* alm, double t, s16 week,
00151                                 double ref[3])
00152 {
00153   double sat_pos[3];
00154   double sat_vel[3];
00155   double vec_ref_sat[3];
00156 
00157   calc_sat_state_almanac(alm, t, week, sat_pos, sat_vel);
00158 
00159   /* Find the vector from the reference position to the satellite. */
00160   vector_subtract(3, sat_pos, ref, vec_ref_sat);
00161 
00162   /* Find the satellite velocity projected on the line of sight vector from the
00163    * reference position to the satellite. */
00164   double radial_velocity = vector_dot(3, vec_ref_sat, sat_vel) / \
00165                            vector_norm(3, vec_ref_sat);
00166 
00167   /* Return the Doppler shift. */
00168   return GPS_L1_HZ * radial_velocity / NAV_C;
00169 }
00170 


enu
Author(s): Mike Purvis
autogenerated on Fri Jan 3 2014 11:21:07