/** \file gps.c \brief GNSS core 'c' function library: GPS specific functions. \author Glenn D. MacGougan (GDM) \date 2005-08-14 \since 2005-07-31 \b "LICENSE INFORMATION" \n Copyright (c) 2007, refer to 'author' doxygen tags \n All rights reserved. \n Redistribution and use in source and binary forms, with or without modification, are permitted provided the following conditions are met: \n - Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. \n - Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. \n - The name(s) of the contributor(s) may not be used to endorse or promote products derived from this software without specific prior written permission. \n THIS SOFTWARE IS PROVIDED BY THE CONTRIBUTORS ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include #include "gnss_error.h" #include "gps.h" #include "constants.h" #include "geodesy.h" /*************************************************************************************************/ // local preprocessor constant definitions, any related enumerations and descriptors #ifndef GPS_CLOCK_CORRECTION_RELATIVISTIC_CONSTANT_F #define GPS_CLOCK_CORRECTION_RELATIVISTIC_CONSTANT_F (-4.442807633e-10) //!< combined constant defined in ICD-GPS-200C p. 88 [s]/[sqrt(m)] #endif #ifndef GPS_UNIVERSAL_GRAVITY_CONSTANT #define GPS_UNIVERSAL_GRAVITY_CONSTANT (3.986005e14) //!< gravity constant defined on ICD-GPS-200C p. 98 [m^3/s^2] #endif #ifndef GPS_RATIO_OF_SQUARED_FREQUENCIES_L1_OVER_L2 #define GPS_RATIO_OF_SQUARED_FREQUENCIES_L1_OVER_L2 (1.6469444444444444444444444444444) //!< (f_L1/f_L2)^2 = (1575.42/1227.6)^2 = (77/60)^2 #endif #ifndef GPS_WGS84_EARTH_ROTATION_RATE #define GPS_WGS84_EARTH_ROTATION_RATE (7.2921151467e-05) //!< constant defined on ICD-GPS-200C p. 98 [rad/s] #endif #define TWO_TO_THE_POWER_OF_55 (36028797018963968.0) #define TWO_TO_THE_POWER_OF_43 (8796093022208.0) #define TWO_TO_THE_POWER_OF_33 (8589934592.0) #define TWO_TO_THE_POWER_OF_31 (2147483648.0) #define TWO_TO_THE_POWER_OF_29 (536870912.0) #define TWO_TO_THE_POWER_OF_19 (524288.0) // /*************************************************************************************************/ void GPS_ComputeSatelliteClockCorrectionAndDrift( const unsigned short transmission_gpsweek, //!< GPS week when signal was transmit (0-1024+) [weeks] const double transmission_gpstow, //!< GPS time of week when signal was transmit [s] const unsigned short ephem_week, //!< ephemeris: GPS week (0-1024+) [weeks] const unsigned toe, //!< ephemeris: time of week [s] const unsigned toc, //!< ephemeris: clock reference time of week [s] const double af0, //!< ephemeris: polynomial clock correction coefficient [s], Note: parameters from ephemeris preferred vs almanac (22 vs 11 bits) const double af1, //!< ephemeris: polynomial clock correction coefficient [s/s], Note: parameters from ephemeris preferred vs almanac (16 vs 11 bits) const double af2, //!< ephemeris: polynomial clock correction coefficient [s/s^2] const double ecc, //!< ephemeris: eccentricity of satellite orbit [] const double sqrta, //!< ephemeris: square root of the semi-major axis of orbit [m^(1/2)] const double delta_n, //!< ephemeris: mean motion difference from computed value [rad] const double m0, //!< ephemeris: mean anomaly at reference time [rad] const double tgd, //!< ephemeris: group delay differential between L1 and L2 [s] const unsigned char mode, //!< 0=L1 only, 1=L2 only (see p. 90, ICD-GPS-200C) double* clock_correction, //!< satellite clock correction [m] double* clock_drift //!< satellite clock drift correction [m/s] ) { unsigned char i; // counter double tot; // time of transmission (including gps week) [s] double tk; // time from ephemeris reference epoch [s] double tc; // time from clock reference epoch [s] double d_tr; // relativistic correction term [s] double d_tsv; // SV PRN code phase time offset [s] double a; // semi-major axis of orbit [m] double n; // corrected mean motion [rad/s] double M; // mean anomaly, [rad] (Kepler's equation for eccentric anomaly, solved by iteration) double E; // eccentric anomaly [rad] // compute the times from the reference epochs // By including the week in the calculation, week rollover and old ephmeris bugs are mitigated // The result should be between -302400 and 302400 if the ephemeris is within one week of transmission tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow; tk = tot - (ephem_week*SECONDS_IN_WEEK + toe); tc = tot - (ephem_week*SECONDS_IN_WEEK + toc); // compute the corrected mean motion term a = sqrta*sqrta; n = sqrt( GPS_UNIVERSAL_GRAVITY_CONSTANT / (a*a*a) ); // computed mean motion n += delta_n; // corrected mean motion // Kepler's equation for eccentric anomaly M = m0 + n*tk; // mean anomaly E = M; for( i = 0; i < 7; i++ ) { E = M + ecc * sin(E); } // relativistic correction d_tr = GPS_CLOCK_CORRECTION_RELATIVISTIC_CONSTANT_F * ecc * sqrta * sin(E); // [s] d_tr *= LIGHTSPEED; // clock correcton d_tsv = af0 + af1*tc + af2*tc*tc; // [s] if( mode == 0 ) { // L1 only d_tsv -= tgd; // [s] } else if( mode == 1 ) { // L2 only d_tsv -= tgd*GPS_RATIO_OF_SQUARED_FREQUENCIES_L1_OVER_L2; // [s] } // clock correction *clock_correction = d_tsv*LIGHTSPEED + d_tr; // [m] // clock drift *clock_drift = (af1 + 2.0*af2*tc) * LIGHTSPEED; // [m/s] } void GPS_ComputeSatellitePositionAndVelocity( const unsigned short transmission_gpsweek, //!< GPS week when signal was transmit (0-1024+) [weeks] const double transmission_gpstow, //!< GPS time of week when signal was transmit [s] const unsigned short ephem_week, //!< ephemeris: GPS week (0-1024+) [weeks] const unsigned toe, //!< ephemeris: time of week [s] const double m0, //!< ephemeris: mean anomaly at reference time [rad] const double delta_n, //!< ephemeris: mean motion difference from computed value [rad] const double ecc, //!< ephemeris: eccentricity [] const double sqrta, //!< ephemeris: square root of the semi-major axis [m^(1/2)] const double omega0, //!< ephemeris: longitude of ascending node of orbit plane at weekly epoch [rad] const double i0, //!< ephemeris: inclination angle at reference time [rad] const double w, //!< ephemeris: argument of perigee [rad] const double omegadot, //!< ephemeris: rate of right ascension [rad/s] const double idot, //!< ephemeris: rate of inclination angle [rad/s] const double cuc, //!< ephemeris: amplitude of the cosine harmonic correction term to the argument of latitude [rad] const double cus, //!< ephemeris: amplitude of the sine harmonic correction term to the argument of latitude [rad] const double crc, //!< ephemeris: amplitude of the cosine harmonic correction term to the orbit radius [m] const double crs, //!< ephemeris: amplitude of the sine harmonic correction term to the orbit radius [m] const double cic, //!< ephemeris: amplitude of the cosine harmonic correction term to the angle of inclination [rad] const double cis, //!< ephemeris: amplitude of the sine harmonic correction term to the angle of inclination [rad] const double estimateOfTrueRange, //!< best estimate of the signal propagation time (in m) for Sagnac effect compensation [m] const double estimteOfRangeRate, //!< best estimate of the true signal Doppler (in m/s) for Sagnac effect compensation [m/s] double* x, //!< satellite x [m] double* y, //!< satellite y [m] double* z, //!< satellite z [m] double* vx, //!< satellite velocity x [m/s] double* vy, //!< satellite velocity y [m/s] double* vz //!< satellite velocity z [m/s] ) { unsigned char j; // counter double tot; // time of transmission (including gps week) [s] double tk; // time from ephemeris reference epoch [s] double a; // semi-major axis of orbit [m] double n; // corrected mean motion [rad/s] double M; // mean anomaly, [rad] (Kepler's equation for eccentric anomaly, solved by iteration) double E; // eccentric anomaly [rad] double v; // true anomaly [rad] double u; // argument of latitude, corrected [rad] double r; // radius in the orbital plane [m] double i; // orbital inclination [rad] double cos2u; // cos(2*u) [] double sin2u; // sin(2*u) [] double d_u; // argument of latitude correction [rad] double d_r; // radius correction [m] double d_i; // inclination correction [rad] double x_op; // x position in the orbital plane [m] double y_op; // y position in the orbital plane [m] double omegak; // corrected longitude of the ascending node [rad] double cos_omegak; // cos(omegak) double sin_omegak; // sin(omegak) double cosu; // cos(u) double sinu; // sin(u) double cosi; // cos(i) double sini; // sin(i) double cosE; // cos(E) double sinE; // sin(E) double omegadotk; // corrected rate of right ascension [rad/s] double edot; // edot = n/(1.0 - ecc*cos(E)), [rad/s] double vdot; // d/dt of true anomaly [rad/s] double udot; // d/dt of argument of latitude [rad/s] double idotdot; // d/dt of the rate of the inclination angle [rad/s^2] double rdot; // d/dt of the radius in the orbital plane [m/s] double tmpa; // temp double tmpb; // temp double vx_op; // x velocity in the orbital plane [m/s] double vy_op; // y velocity in the orbital plane [m/s] // compute the times from the reference epochs // By including the week in the calculation, week rollover and older ephemeris bugs are mitigated // The result should be between -302400 and 302400 if the ephemeris is within one week of transmission tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow; tk = tot - (ephem_week*SECONDS_IN_WEEK + toe); // compute the corrected mean motion term a = sqrta*sqrta; n = sqrt( GPS_UNIVERSAL_GRAVITY_CONSTANT / (a*a*a) ); // computed mean motion n += delta_n; // corrected mean motion // Kepler's equation for eccentric anomaly M = m0 + n*tk; // mean anomaly E = M; for( j = 0; j < 7; j++ ) { E = M + ecc * sin(E); } cosE = cos(E); sinE = sin(E); // true anomaly v = atan2( (sqrt(1.0 - ecc*ecc)*sinE), (cosE - ecc) ); // argument of latitude u = v + w; // radius in orbital plane r = a * (1.0 - ecc * cos(E)); // orbital inclination i = i0; // second harmonic perturbations // cos2u = cos(2.0*u); sin2u = sin(2.0*u); // argument of latitude correction d_u = cuc * cos2u + cus * sin2u; // radius correction d_r = crc * cos2u + crs * sin2u; // correction to inclination d_i = cic * cos2u + cis * sin2u; // corrected argument of latitude u += d_u; // corrected radius r += d_r; // corrected inclination i += d_i + idot * tk; // positions in orbital plane cosu = cos(u); sinu = sin(u); x_op = r * cosu; y_op = r * sinu; // compute the corrected longitude of the ascending node // This equation deviates from that in Table 20-IV p. 100 GPSICD200C with the inclusion of the // signal propagation time (estimateOfTrueRange/LIGHTSPEED) term. This compensates for the Sagnac effect. // The omegak term is thus sensitive to the estimateOfTrueRange term which is usually unknown without // prior information. The average signal propagation time/range (70ms * c) can be used on first use // and this function must be called again to iterate this term. The sensitivity of the omegak term // typically requires N iterations - GDM_DEBUG{find out how many iterations are needed, how sensitive to the position?} omegak = omega0 + (omegadot - GPS_WGS84_EARTH_ROTATION_RATE)*tk - GPS_WGS84_EARTH_ROTATION_RATE*(toe + estimateOfTrueRange/LIGHTSPEED ); // compute the WGS84 ECEF coordinates, // vector r with components x & y is now rotated using, R3(-omegak)*R1(-i) cos_omegak = cos(omegak); sin_omegak = sin(omegak); cosi = cos(i); sini = sin(i); *x = x_op * cos_omegak - y_op * sin_omegak * cosi; *y = x_op * sin_omegak + y_op * cos_omegak * cosi; *z = y_op * sini; // Satellite Velocity Computations are below // see Reference // Remodi, B. M (2004). GPS Tool Box: Computing satellite velocities using the broadcast ephemeris. // GPS Solutions. Volume 8(3), 2004. pp. 181-183 // // example source code was available at [http://www.ngs.noaa.gov/gps-toolbox/bc_velo/bc_velo.c] // recomputed the cos and sin of the corrected argument of latitude cos2u = cos(2.0*u); sin2u = sin(2.0*u); edot = n / (1.0 - ecc*cosE); vdot = sinE*edot*(1.0 + ecc*cos(v)) / ( sin(v)*(1.0-ecc*cosE) ); udot = vdot + 2.0*(cus*cos2u - cuc*sin2u)*vdot; rdot = a*ecc*sinE*n/(1.0-ecc*cosE) + 2.0*(crs*cos2u - crc*sin2u)*vdot; idotdot = idot + (cis*cos2u - cic*sin2u)*2.0*vdot; vx_op = rdot*cosu - y_op*udot; vy_op = rdot*sinu + x_op*udot; // corrected rate of right ascension including similarily as above, for omegak, // compensation for the Sagnac effect omegadotk = omegadot - GPS_WGS84_EARTH_ROTATION_RATE*( 1.0 + estimteOfRangeRate/LIGHTSPEED ); tmpa = vx_op - y_op*cosi*omegadotk; tmpb = x_op*omegadotk + vy_op*cosi - y_op*sini*idotdot; *vx = tmpa * cos_omegak - tmpb * sin_omegak; *vy = tmpa * sin_omegak + tmpb * cos_omegak; *vz = vy_op*sini + y_op*cosi*idotdot; } void GPS_ComputeUserToSatelliteRange( const double userX, //!< user X position WGS84 ECEF [m] const double userY, //!< user Y position WGS84 ECEF [m] const double userZ, //!< user Z position WGS84 ECEF [m] const double satX, //!< satellite X position WGS84 ECEF [m] const double satY, //!< satellite Y positoin WGS84 ECEF [m] const double satZ, //!< satellite Z position WGS84 ECEF [m] double* range //!< user to satellite range [m] ) { double dx; double dy; double dz; dx = satX - userX; dy = satY - userY; dz = satZ - userZ; // compute the range *range = sqrt( dx*dx + dy*dy + dz*dz ); } void GPS_ComputeUserToSatelliteRangeAndRangeRate( const double userX, //!< user X position WGS84 ECEF [m] const double userY, //!< user Y position WGS84 ECEF [m] const double userZ, //!< user Z position WGS84 ECEF [m] const double userVx, //!< user X velocity WGS84 ECEF [m/s] const double userVy, //!< user Y velocity WGS84 ECEF [m/s] const double userVz, //!< user Z velocity WGS84 ECEF [m/s] const double satX, //!< satellite X position WGS84 ECEF [m] const double satY, //!< satellite Y positoin WGS84 ECEF [m] const double satZ, //!< satellite Z position WGS84 ECEF [m] const double satVx, //!< satellite X velocity WGS84 ECEF [m/s] const double satVy, //!< satellite Y velocity WGS84 ECEF [m/s] const double satVz, //!< satellite Z velocity WGS84 ECEF [m/s] double* range, //!< user to satellite range [m] double* range_rate //!< user to satellite range rate [m/s] ) { double dx; double dy; double dz; dx = satX - userX; dy = satY - userY; dz = satZ - userZ; // compute the range *range = sqrt( dx*dx + dy*dy + dz*dz ); // compute the range rate // this method uses the NovAtel style sign convention! *range_rate = (userVx - satVx)*dx + (userVy - satVy)*dy + (userVz - satVz)*dz; *range_rate /= *range; } void GPS_ComputeSatellitePositionVelocityAzimuthElevationDoppler_BasedOnAlmanacData( const double userX, //!< user X position WGS84 ECEF [m] const double userY, //!< user Y position WGS84 ECEF [m] const double userZ, //!< user Z position WGS84 ECEF [m] const unsigned short gpsweek, //!< user gps week (0-1024+) [week] const double gpstow, //!< user time of week [s] const double toa, //!< time of applicability [s] const unsigned short almanac_week, //!< gps week of almanac (0-1024+) [week] const unsigned short prn, //!< GPS prn number [] const double ecc, //!< eccentricity [] const double i0, //!< orbital inclination at reference time [rad] const double omegadot, //!< rate of right ascension [rad/s] const double sqrta, //!< square root of the semi-major axis [m^(1/2)] const double omega0, //!< longitude of ascending node of orbit plane at weekly epoch [rad] const double w, //!< argument of perigee [rad] const double m0, //!< mean anomaly at reference time [rad] const double af0, //!< polynomial clock correction coefficient (clock bias) [s], Note: parameters from ephemeris preferred vs almanac (22 vs 11 bits) const double af1, //!< polynomial clock correction coefficient (clock drift) [s/s], Note: parameters from ephemeris preferred vs almanac (16 vs 11 bits) double* clock_correction, //!< clock correction for this satellite for this epoch [m] double* clock_drift, //!< clock drift correction for this satellite for this epoch [m/s] double* satX, //!< satellite X position WGS84 ECEF [m] double* satY, //!< satellite Y position WGS84 ECEF [m] double* satZ, //!< satellite Z position WGS84 ECEF [m] double* satVx, //!< satellite X velocity WGS84 ECEF [m/s] double* satVy, //!< satellite Y velocity WGS84 ECEF [m/s] double* satVz, //!< satellite Z velocity WGS84 ECEF [m/s] double* azimuth, //!< satelilte azimuth [rad] double* elevation, //!< satelilte elevation [rad] double* doppler //!< satellite doppler with respect to the user position [m/s], Note: User must convert to Hz ) { double tow; // user time of week adjusted with the clock corrections [s] double range; // range estimate between user and satellite [m] double range_rate; // range_rate esimate between user and satellite [m/s] double x; // sat X position [m] double y; // sat Y position [m] double z; // sat Z position [m] double vx; // sat X velocity [m/s] double vy; // sat Y velocity [m/s] double vz; // sat Z velocity [m/s] unsigned short week; // user week adjusted with the clock correction if needed [week] unsigned char i; // counter i = (unsigned char)prn; // get rid of a debug msg :) // initialize to zero x = y = z = vx = vy = vz = 0.0; GPS_ComputeSatelliteClockCorrectionAndDrift( gpsweek, gpstow, almanac_week, (unsigned)toa, (unsigned)toa, af0, af1, 0.0, ecc, sqrta, 0.0, m0, 0.0, 0, clock_correction, clock_drift ); // adjust for week rollover week = gpsweek; tow = gpstow + (*clock_correction)/LIGHTSPEED; if( tow < 0.0 ) { tow += SECONDS_IN_WEEK; week--; } if( tow > 604800.0 ) { tow -= SECONDS_IN_WEEK; week++; } // iterate to include the Sagnac correction // since the range is unknown, an approximate of 70 ms is good enough // to start the iterations so that 2 iterations are enough range = 0.070*LIGHTSPEED; range_rate = 0.0; for( i = 0; i < 2; i++ ) { GPS_ComputeSatellitePositionAndVelocity( week, tow, almanac_week, (unsigned)toa, m0, 0.0, ecc, sqrta, omega0, i0, w, omegadot, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, range, range_rate, &x, &y, &z, &vx, &vy, &vz ); GPS_ComputeUserToSatelliteRangeAndRangeRate( userX, userY, userZ, 0.0, 0.0, 0.0, x, y, z, vx, vy, vz, &range, &range_rate ); } GEODESY_ComputeAzimuthAndElevationAnglesBetweenToPointsInTheEarthFixedFrame( GEODESY_REFERENCE_ELLIPSE_WGS84, userX, userY, userZ, x, y, z, elevation, // sets the elevation azimuth ); // sets the azimuth *satX = x; *satY = y; *satZ = z; *satVx = vx; *satVy = vy; *satVz = vz; *doppler = range_rate; } void GPS_ComputeSatellitePositionVelocityAzimuthElevationDoppler_BasedOnEphmerisData( const double userX, //!< user X position WGS84 ECEF [m] const double userY, //!< user Y position WGS84 ECEF [m] const double userZ, //!< user Z position WGS84 ECEF [m] const unsigned short gpsweek, //!< gps week of signal transmission (0-1024+) [week] const double gpstow, //!< time of week of signal transmission (gpstow-psr/c) [s] const unsigned short ephem_week, //!< ephemeris: GPS week (0-1024+) [weeks] const unsigned toe, //!< ephemeris: time of week [s] const unsigned toc, //!< ephemeris: clock reference time of week [s] const double af0, //!< ephemeris: polynomial clock correction coefficient [s], Note: parameters from ephemeris preferred vs almanac (22 vs 11 bits) const double af1, //!< ephemeris: polynomial clock correction coefficient [s/s], Note: parameters from ephemeris preferred vs almanac (16 vs 11 bits) const double af2, //!< ephemeris: polynomial clock correction coefficient [s/s^2] const double tgd, //!< ephemeris: group delay differential between L1 and L2 [s] const double m0, //!< ephemeris: mean anomaly at reference time [rad] const double delta_n, //!< ephemeris: mean motion difference from computed value [rad/s] const double ecc, //!< ephemeris: eccentricity [] const double sqrta, //!< ephemeris: square root of the semi-major axis [m^(1/2)] const double omega0, //!< ephemeris: longitude of ascending node of orbit plane at weekly epoch [rad] const double i0, //!< ephemeris: inclination angle at reference time [rad] const double w, //!< ephemeris: argument of perigee [rad] const double omegadot, //!< ephemeris: rate of right ascension [rad/s] const double idot, //!< ephemeris: rate of inclination angle [rad/s] const double cuc, //!< ephemeris: amplitude of the cosine harmonic correction term to the argument of latitude [rad] const double cus, //!< ephemeris: amplitude of the sine harmonic correction term to the argument of latitude [rad] const double crc, //!< ephemeris: amplitude of the cosine harmonic correction term to the orbit radius [m] const double crs, //!< ephemeris: amplitude of the sine harmonic correction term to the orbit radius [m] const double cic, //!< ephemeris: amplitude of the cosine harmonic correction term to the angle of inclination [rad] const double cis, //!< ephemeris: amplitude of the sine harmonic correction term to the angle of inclination [rad] double* clock_correction, //!< clock correction for this satellite for this epoch [m] double* clock_drift, //!< clock drift correction for this satellite for this epoch [m/s] double* satX, //!< satellite X position WGS84 ECEF [m] double* satY, //!< satellite Y position WGS84 ECEF [m] double* satZ, //!< satellite Z position WGS84 ECEF [m] double* satVx, //!< satellite X velocity WGS84 ECEF [m/s] double* satVy, //!< satellite Y velocity WGS84 ECEF [m/s] double* satVz, //!< satellite Z velocity WGS84 ECEF [m/s] double* azimuth, //!< satelilte azimuth [rad] double* elevation, //!< satelilte elevation [rad] double* doppler //!< satellite doppler with respect to the user position [m/s], Note: User must convert to Hz ) { double tow; // user time of week adjusted with the clock corrections [s] double range; // range estimate between user and satellite [m] double range_rate; // range_rate esimate between user and satellite [m/s] double x; // sat X position [m] double y; // sat Y position [m] double z; // sat Z position [m] double vx; // sat X velocity [m/s] double vy; // sat Y velocity [m/s] double vz; // sat Z velocity [m/s] unsigned short week; // user week adjusted with the clock correction if needed [week] unsigned char i; // counter // initialize to zero x = y = z = vx = vy = vz = 0.0; GPS_ComputeSatelliteClockCorrectionAndDrift( gpsweek, gpstow, ephem_week, toe, toc, af0, af1, af2, ecc, sqrta, delta_n, m0, tgd, 0, clock_correction, clock_drift ); // adjust for week rollover week = gpsweek; tow = gpstow + (*clock_correction)/LIGHTSPEED; if( tow < 0.0 ) { tow += SECONDS_IN_WEEK; week--; } if( tow > 604800.0 ) { tow -= SECONDS_IN_WEEK; week++; } // iterate to include the Sagnac correction // since the range is unknown, an approximate of 70 ms is good enough to start // the iterations so that 2 iterations are enough for sub mm accuracy range = 0.070*LIGHTSPEED; range_rate = 0.0; for( i = 0; i < 2; i++ ) { GPS_ComputeSatellitePositionAndVelocity( week, tow, ephem_week, toe, m0, delta_n, ecc, sqrta, omega0, i0, w, omegadot, idot, cuc, cus, crc, crs, cic, cis, range, range_rate, &x, &y, &z, &vx, &vy, &vz ); GPS_ComputeUserToSatelliteRangeAndRangeRate( userX, userY, userZ, 0.0, 0.0, 0.0, x, y, z, vx, vy, vz, &range, &range_rate ); } GEODESY_ComputeAzimuthAndElevationAnglesBetweenToPointsInTheEarthFixedFrame( GEODESY_REFERENCE_ELLIPSE_WGS84, userX, userY, userZ, x, y, z, elevation, // sets the elevation azimuth ); // sets the azimuth *satX = x; *satY = y; *satZ = z; *satVx = vx; *satVy = vy; *satVz = vz; *doppler = range_rate; } BOOL GPS_DecodeRawGPSEphemeris( const unsigned char subframe1[30], //!< subframe 1 data, 30 bytes * 8bits/byte = 240 bits, thus parity bits have been removed const unsigned char subframe2[30], //!< subframe 2 data, 30 bytes * 8bits/byte = 240 bits, thus parity bits have been removed const unsigned char subframe3[30], //!< subframe 3 data, 30 bytes * 8bits/byte = 240 bits, thus parity bits have been removed unsigned short prn, //!< GPS PRN number (helps with debugging) unsigned* tow, //!< time of week in subframe1, the time of the leading bit edge of subframe 2 [s] unsigned short* iodc, //!< 10 bit issue of data (clock), 8 LSB bits will match the iode [] unsigned char* iode, //!< 8 bit issue of data (ephemeris) [] unsigned* toe, //!< reference time ephemeris (0-604800) [s] unsigned* toc, //!< reference time (clock) (0-604800) [s] unsigned short* week, //!< 10 bit gps week 0-1023 (user must account for week rollover ) [week] unsigned char* health, //!< 6 bit health parameter, 0 if healthy, unhealth othersize [0=healthy] unsigned char* alert_flag, //!< 1 = URA may be worse than indicated [0,1] unsigned char* anti_spoof, //!< anti-spoof flag from 0=off, 1=on [0,1] unsigned char* code_on_L2, //!< 0=reserved, 1=P code on L2, 2=C/A on L2 [0,1,2] unsigned char* ura, //!< User Range Accuracy lookup code, 0 is excellent, 15 is use at own risk [0-15], see p. 83 GPSICD200C unsigned char* L2_P_data_flag, //!< flag indicating if P is on L2 1=true [0,1] unsigned char* fit_interval_flag, //!< fit interval flag (four hour interval or longer) 0=4 fours, 1=greater [0,1] unsigned short* age_of_data_offset, //!< age of data offset [s] double* tgd, //!< group delay [s] double* af2, //!< polynomial clock correction coefficient (rate of clock drift) [s/s^2] double* af1, //!< polynomial clock correction coefficient (clock drift) [s/s] double* af0, //!< polynomial clock correction coefficient (clock bias) [s] double* m0, //!< mean anomaly at reference time [rad] double* delta_n, //!< mean motion difference from computed value [rad/s] double* ecc, //!< eccentricity [] double* sqrta, //!< square root of the semi-major axis [m^(1/2)] double* omega0, //!< longitude of ascending node of orbit plane at weekly epoch [rad] double* i0, //!< inclination angle at reference time [rad] double* w, //!< argument of perigee [rad] double* omegadot, //!< rate of right ascension [rad/s] double* idot, //!< rate of inclination angle [rad/s] 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* 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* 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] ) { /* ------------------------------------------------------------------ SUBFRAME1 ------------------------------------------------------------------ TERM, NR BITS, BITS NO PARITY, BITS WITH PARITY preamble, 8, 1-8, 1-8, TLM, 14, 9-22, 9-22 reserved, 2, 23-24, 23-24 --PARITY-- 6, ----- 25-30 TOW, 17, 25-41, 31-47 alert_flag, 1, 42, 48 anti_spoof_flag, 1, 43, 49 subframeID, 3, 44-46, 50-52 parity_related, 2, 47-48, 53-54 --PARITY-- 6, ----- 25-30 week, 10, 49-58, 61-70 code_on_L2, 2, 59,60, 71-72 ura, 4, 61-64, 73-76 health, 6, 65-70, 77-82 iodc_MSB, 2, 71-72, 83-84 --PARITY-- 6, ----- 85-90 L2_P_data_flag, 1, 73, 91 reserved, 23, 74-96, 92-114 --PARITY-- 6, ----- 115-120 reserved, 24, 97-120, 121-144 --PARITY-- 6, ----- 25-30 reserved, 24, 121-144, 151-174 --PARITY-- 6, ----- 25-30 reserved, 16, 145-160, 181-196 tgd, 8, 161-168, 197-204 --PARITY-- 6, ----- 205-210 iodc_LSB, 8, 169-176, 211-218 toc, 16, 177-192, 219-234 --PARITY-- 6, ----- 235-240 af2, 8, 193-200, 241-248 af1, 16, 201-216, 249-264 --PARITY-- 6, ----- 265-270 af0, 22, 217-238, 271-292 parity_related, 2, 239-240, 293-294 --PARITY-- 6, ----- 295-300 ------------------------------------------------------------------ SUBFRAME2 ------------------------------------------------------------------ TERM, NR BITS, BITS NO PARITY, BITS WITH PARITY preamble, 8, 1-8, 1-8, TLM, 14, 9-22, 9-22 reserved, 2, 23-24, 23-24 --PARITY-- 6, ----- 25-30 TOW, 17, 25-41, 31-47 alert_flag, 1, 42, 48 anti_spoof_flag, 1, 43, 49 subframeID, 3, 44-46, 50-52 parity_related, 2, 47-48, 53-54 --PARITY-- 6, ----- 25-30 iode, 8, 49-56, 61-68 crs, 16, 57-72, 69-84 --PARITY-- 6, ----- 95-90 delta_n, 16, 73-88, 91-106 m0_MSB, 8, 89-96, 107-114 --PARITY-- 6, ----- 115-120 m0_LSB, 24, 97-120, 121-144 --PARITY-- 6, ----- 145-150 cuc, 16, 121-136, 151-166 ecc_MSB, 8, 137-144, 167-174 --PARITY-- 6, ----- 175-180 ecc_LSB, 24, 145-168, 181-204 --PARITY-- 6, ----- 205-210 cus, 16, 169-184, 211-226 sqrta_MSB, 8, 185-192, 227-234 --PARITY-- 6, ----- 235-240 sqrta_LSB, 24, 193-216, 241-264 --PARITY-- 6, ----- 265-270 toe, 16, 217-232, 271-286 fit_interval_flag, 1, 233, 287 age_of_data_offset, 5, 234-238, 288-292 parity_related, 2, 239-240, 293-294 --PARITY-- 6, ----- 295-300 ------------------------------------------------------------------ SUBFRAME3 ------------------------------------------------------------------ TERM, NR BITS, BITS NO PARITY, BITS WITH PARITY preamble, 8, 1-8, 1-8, TLM, 14, 9-22, 9-22 reserved, 2, 23-24, 23-24 --PARITY-- 6, ----- 25-30 TOW, 17, 25-41, 31-47 alert_flag, 1, 42, 48 anti_spoof_flag, 1, 43, 49 subframeID, 3, 44-46, 50-52 parity_related, 2, 47-48, 53-54 --PARITY-- 6, ----- 25-30 cic, 16, 49-64, 61-76 omega0_MSB, 8, 65-72, 77-84 --PARITY-- 6, ----- 85-90 omega0_LSB, 24, 73-96, 91-114 --PARITY-- 6, ----- 115-120 cis, 16, 97-112, 121-136 i0_MSB, 8, 113-120, 137-144 --PARITY-- 6, ----- 145-150 i0_LSB, 24, 121-144, 151-174 --PARITY-- 6, ----- 175-180 crc, 16, 145-160, 181-196 w_MSB, 8, 161-168, 197-204 --PARITY-- 6, ----- 205-210 w_LSB, 24, 169-192, 211-234 --PARITY-- 6, ----- 235-240 omegadot, 24, 193-216, 241-264 --PARITY-- 6, ----- 265-270 iode, 8, 217-224, 271-278 idot, 14, 225-238, 279-292 parity_related, 2, 239-240, 293-294 ------------------------------------------------------------------ */ unsigned char subframe_id; // subrame id unsigned char iode_subframe1; // 8 LSB bits of the iodc in subframe 1 unsigned char iode_subframe2; // subframe2 iode unsigned char iode_subframe3; // subframe3 iode // temporary variables of different size char s8; short s16; int s32; unsigned short u16a, u16b; unsigned u32a, u32b, u32c, u32d; u16a = prn; // gets rid of a debug msg :) //------------------------------------------------------------------ // SUBFRAME1 //------------------------------------------------------------------ // time of week, actually a 19 bit value, 17 MSBs are available, 2 LSB bits are always zero u32a = subframe1[3] << 11; u32b = subframe1[4] << 3; u32c = (subframe1[5] & 0x80) >> 5; *tow = (u32a | u32b | u32c); // [z-count 1.5s intervals] *tow = (*tow * 3) / 2; // converted to [s] // alert_flag *alert_flag = (unsigned char)( (subframe1[5] & 0x40) >> 6 ); // anti-spoof *anti_spoof = (unsigned char)( (subframe1[5] & 0x20) >> 5 ); // confirm that this is subframe 1 subframe_id = (unsigned char)( (subframe1[5] & 0x1C) >> 2 ); if( subframe_id != 1 ) { GNSS_ERROR_MSG( "if( subframe_id != 1 )" ); return FALSE; } // GPS Week u16a = (unsigned short)( subframe1[6] << 2 ); u16b = (unsigned short)( subframe1[7] >> 6 ); *week = (unsigned short)( u16a | u16b ); /// code_on_L2 *code_on_L2 = (unsigned char)( (subframe1[7] & 0x30) >> 4 ); // ura *ura = (unsigned char)( (subframe1[7] & 0x0F) ); // health *health = (unsigned char)( subframe1[8] >> 2 ); // issue of data clock u16a = (unsigned short)( (subframe1[8] & 0x03) << 8 ); u16b = (unsigned short)( subframe1[21] ); *iodc = (unsigned short)( u16a | u16b ); // [] // iode subframe1 for consistency checking iode_subframe1 = subframe1[21]; // L2_P_data_flag *L2_P_data_flag = (unsigned char)( (subframe1[9] & 0x80) >> 7 ); // tgd s8 = subframe1[20]; // signed *tgd = s8 / TWO_TO_THE_POWER_OF_31; // toc u16a = (unsigned short)( subframe1[22] << 8 ); u16b = (unsigned short)( subframe1[23] ); *toc = (unsigned)( (u16a | u16b) ) * 16; // af2 s8 = subframe1[24]; // signed *af2 = s8; *af2 /= TWO_TO_THE_POWER_OF_55; // af1 u16a = (unsigned short)( subframe1[25] << 8 ); u16b = subframe1[26]; s16 = (unsigned short)( u16a | u16b ); // signed value *af1 = s16; *af1 /= TWO_TO_THE_POWER_OF_43; // af0 u32a = subframe1[27] << 24; u32b = subframe1[28] << 16; u32c = subframe1[29] & 0xFC; u32c <<= 8; // align to the sign bit (two's complement integer) u32d = (u32a | u32b | u32c); s32 = (int)(u32d); s32 >>= 10; // 22 bit value *af0 = s32; *af0 /= TWO_TO_THE_POWER_OF_31; //------------------------------------------------------------------ // SUBFRAME2 //------------------------------------------------------------------ // confirm that this is subframe 2 subframe_id = (unsigned char)( (subframe2[5] & 0x1C) >> 2 ); if( subframe_id != 2 ) { GNSS_ERROR_MSG( "if( subframe_id != 2 )" ); return FALSE; } // iode subframe2 iode_subframe2 = subframe2[6]; // crs u16a = (unsigned short)( subframe2[7] << 8 ); u16b = subframe2[8]; s16 = (unsigned short)( u16a | u16b ); // signed value *crs = s16; *crs /= 32.0; // [m] // delta_n u16a = (unsigned short)( subframe2[9] << 8 ); u16b = subframe2[10]; s16 = (short)( u16a | u16b ); // signed value *delta_n = s16; *delta_n *= PI / TWO_TO_THE_POWER_OF_43; // [rad/s] // m0 u32a = subframe2[11] << 24; u32b = subframe2[12] << 16; u32c = subframe2[13] << 8; u32d = subframe2[14]; s32 = (u32a | u32b | u32c | u32d); // signed value *m0 = s32; *m0 *= PI / TWO_TO_THE_POWER_OF_31; // [rad] // cuc u16a = (unsigned short)( subframe2[15] << 8 ); u16b = subframe2[16]; s16 = (short)( u16a | u16b ); // signed value *cuc = s16; *cuc /= TWO_TO_THE_POWER_OF_29; // [rad] // ecc u32a = subframe2[17] << 24; u32b = subframe2[18] << 16; u32c = subframe2[19] << 8; u32d = subframe2[20]; *ecc = u32a | u32b | u32c | u32d; *ecc /= TWO_TO_THE_POWER_OF_33; // [] // cus u16a = (unsigned short)( subframe2[21] << 8 ); u16b = subframe2[22]; s16 = (short)( u16a | u16b ); *cus = s16; *cus /= TWO_TO_THE_POWER_OF_29; // [rad] // sqrta u32a = subframe2[23] << 24; u32b = subframe2[24] << 16; u32c = subframe2[25] << 8; u32d = subframe2[26]; *sqrta = u32a | u32b | u32c | u32d; *sqrta /= TWO_TO_THE_POWER_OF_19; // [sqrt(m)] // toe u16a = (unsigned short)( subframe2[27] << 8 ); u16b = subframe2[28]; *toe = (unsigned)( (u16a | u16b) ) * 16; // [s] // fit_interval_flag *fit_interval_flag = (unsigned char)( subframe2[29] >> 7 ); // age_of_data_offset *age_of_data_offset = (unsigned short)( (subframe2[29] & 0x74) >> 2 ); *age_of_data_offset *= 900; // [s] //------------------------------------------------------------------ // SUBFRAME3 //------------------------------------------------------------------ // confirm that this is subframe 3 subframe_id = (unsigned char)( (subframe3[5] & 0x1C) >> 2 ); if( subframe_id != 3 ) { GNSS_ERROR_MSG( "if( subframe_id != 3 )" ); return FALSE; } // cic u16a = (unsigned short)( subframe3[6] << 8 ); u16b = subframe3[7]; s16 = (short)( u16a | u16b ); // signed value *cic = s16; *cic /= TWO_TO_THE_POWER_OF_29; // [rad] // omego0 u32a = subframe3[8] << 24; u32b = subframe3[9] << 16; u32c = subframe3[10] << 8; u32d = subframe3[11]; s32 = u32a | u32b | u32c | u32d; // signed value *omega0 = s32; *omega0 *= PI / TWO_TO_THE_POWER_OF_31; // [rad] // cis u16a = (unsigned short)( subframe3[12] << 8 ); u16b = subframe3[13]; s16 = (short)( u16a | u16b ); // signed value *cis = s16; *cis /= TWO_TO_THE_POWER_OF_29; // [rad] // i0 u32a = subframe3[14] << 24; u32b = subframe3[15] << 16; u32c = subframe3[16] << 8; u32d = subframe3[17]; s32 = u32a | u32b | u32c | u32d; *i0 = s32; *i0 *= PI / TWO_TO_THE_POWER_OF_31; // [rad] // crc u16a = (unsigned short)( subframe3[18] << 8 ); u16b = subframe3[19]; s16 = (short)( u16a | u16b ); // signed value *crc = s16; *crc /= 32.0; // [m] // w u32a = subframe3[20] << 24; u32b = subframe3[21] << 16; u32c = subframe3[22] << 8; u32d = subframe3[23]; s32 = u32a | u32b | u32c | u32d; // signed value *w = s32; *w *= PI / TWO_TO_THE_POWER_OF_31; // [rad] // omegadot u32a = subframe3[24] << 24; u32b = subframe3[25] << 16; u32c = subframe3[26] << 8; s32 = u32a | u32b | u32c; // signed value s32 = s32 >> 8; *omegadot = s32; *omegadot *= PI / TWO_TO_THE_POWER_OF_43; // [rad/s] // iode subframe3 iode_subframe3 = subframe3[27]; // idot u16a = (unsigned short)( subframe3[28] << 8 ); u16b = (unsigned short)( subframe3[29] & 0xFC ); s16 = (short)( u16a | u16b ); // signed value s16 = (short)( s16 >> 2 ); *idot = s16; *idot *= PI / TWO_TO_THE_POWER_OF_43; // [rad/s] // check that the IODE values match for all three subframes if( (iode_subframe1 == iode_subframe2) && (iode_subframe1 == iode_subframe3) ) { *iode = iode_subframe1; return TRUE; } else { *iode = 0; GNSS_ERROR_MSG( "inconsistent subframe dataset" ); return FALSE; // inconsistent subframe dataset } }