00001 /** 00002 \file gps.c 00003 \brief GNSS core 'c' function library: GPS specific functions. 00004 \author Glenn D. MacGougan (GDM) 00005 \date 2005-08-14 00006 \since 2005-07-31 00007 00008 \b "LICENSE INFORMATION" \n 00009 Copyright (c) 2007, refer to 'author' doxygen tags \n 00010 All rights reserved. \n 00011 00012 Redistribution and use in source and binary forms, with or without 00013 modification, are permitted provided the following conditions are met: \n 00014 00015 - Redistributions of source code must retain the above copyright 00016 notice, this list of conditions and the following disclaimer. \n 00017 - Redistributions in binary form must reproduce the above copyright 00018 notice, this list of conditions and the following disclaimer in the 00019 documentation and/or other materials provided with the distribution. \n 00020 - The name(s) of the contributor(s) may not be used to endorse or promote 00021 products derived from this software without specific prior written 00022 permission. \n 00023 00024 THIS SOFTWARE IS PROVIDED BY THE CONTRIBUTORS ``AS IS'' AND ANY EXPRESS 00025 OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00026 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00027 DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 00028 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00029 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00030 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 00033 OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 00034 SUCH DAMAGE. 00035 */ 00036 00037 00038 #include <math.h> 00039 #include "gps.h" 00040 #include "constants.h" 00041 #include "geodesy.h" 00042 00043 00044 /*************************************************************************************************/ 00045 // local preprocessor constant definitions, any related enumerations and descriptors 00046 00047 #ifndef GPS_CLOCK_CORRECTION_RELATIVISTIC_CONSTANT_F 00048 #define GPS_CLOCK_CORRECTION_RELATIVISTIC_CONSTANT_F (-4.442807633e-10) //!< combined constant defined in ICD-GPS-200C p. 88 [s]/[sqrt(m)] 00049 #endif 00050 #ifndef GPS_UNIVERSAL_GRAVITY_CONSTANT 00051 #define GPS_UNIVERSAL_GRAVITY_CONSTANT (3.986005e14) //!< gravity constant defined on ICD-GPS-200C p. 98 [m^3/s^2] 00052 #endif 00053 #ifndef GPS_RATIO_OF_SQUARED_FREQUENCIES_L1_OVER_L2 00054 #define GPS_RATIO_OF_SQUARED_FREQUENCIES_L1_OVER_L2 (1.6469444444444444444444444444444) //!< (f_L1/f_L2)^2 = (1575.42/1227.6)^2 = (77/60)^2 00055 #endif 00056 #ifndef GPS_WGS84_EARTH_ROTATION_RATE 00057 #define GPS_WGS84_EARTH_ROTATION_RATE (7.2921151467e-05) //!< constant defined on ICD-GPS-200C p. 98 [rad/s] 00058 #endif 00059 00060 #define TWO_TO_THE_POWER_OF_55 (36028797018963968.0) 00061 #define TWO_TO_THE_POWER_OF_43 (8796093022208.0) 00062 #define TWO_TO_THE_POWER_OF_33 (8589934592.0) 00063 #define TWO_TO_THE_POWER_OF_31 (2147483648.0) 00064 #define TWO_TO_THE_POWER_OF_29 (536870912.0) 00065 #define TWO_TO_THE_POWER_OF_19 (524288.0) 00066 00067 // 00068 /*************************************************************************************************/ 00069 00070 00071 00072 void GPS_ComputeSatelliteClockCorrectionAndDrift( 00073 const unsigned short transmission_gpsweek, //!< GPS week when signal was transmit (0-1024+) [weeks] 00074 const double transmission_gpstow, //!< GPS time of week when signal was transmit [s] 00075 const unsigned short ephem_week, //!< ephemeris: GPS week (0-1024+) [weeks] 00076 const unsigned toe, //!< ephemeris: time of week [s] 00077 const unsigned toc, //!< ephemeris: clock reference time of week [s] 00078 const double af0, //!< ephemeris: polynomial clock correction coefficient [s], Note: parameters from ephemeris preferred vs almanac (22 vs 11 bits) 00079 const double af1, //!< ephemeris: polynomial clock correction coefficient [s/s], Note: parameters from ephemeris preferred vs almanac (16 vs 11 bits) 00080 const double af2, //!< ephemeris: polynomial clock correction coefficient [s/s^2] 00081 const double ecc, //!< ephemeris: eccentricity of satellite orbit [] 00082 const double sqrta, //!< ephemeris: square root of the semi-major axis of orbit [m^(1/2)] 00083 const double delta_n, //!< ephemeris: mean motion difference from computed value [rad] 00084 const double m0, //!< ephemeris: mean anomaly at reference time [rad] 00085 const double tgd, //!< ephemeris: group delay differential between L1 and L2 [s] 00086 const unsigned char mode, //!< 0=L1 only, 1=L2 only (see p. 90, ICD-GPS-200C) 00087 double* clock_correction, //!< satellite clock correction [m] 00088 double* clock_drift //!< satellite clock drift correction [m/s] 00089 ) 00090 { 00091 unsigned char i; // counter 00092 00093 double tot; // time of transmission (including gps week) [s] 00094 double tk; // time from ephemeris reference epoch [s] 00095 double tc; // time from clock reference epoch [s] 00096 double d_tr; // relativistic correction term [s] 00097 double d_tsv; // SV PRN code phase time offset [s] 00098 double a; // semi-major axis of orbit [m] 00099 double n; // corrected mean motion [rad/s] 00100 double M; // mean anomaly, [rad] (Kepler's equation for eccentric anomaly, solved by iteration) 00101 double E; // eccentric anomaly [rad] 00102 00103 // compute the times from the reference epochs 00104 // By including the week in the calculation, week rollover and old ephmeris bugs are mitigated 00105 // The result should be between -302400 and 302400 if the ephemeris is within one week of transmission 00106 tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow; 00107 tk = tot - (ephem_week*SECONDS_IN_WEEK + toe); 00108 tc = tot - (ephem_week*SECONDS_IN_WEEK + toc); 00109 00110 // compute the corrected mean motion term 00111 a = sqrta*sqrta; 00112 n = sqrt( GPS_UNIVERSAL_GRAVITY_CONSTANT / (a*a*a) ); // computed mean motion 00113 n += delta_n; // corrected mean motion 00114 00115 // Kepler's equation for eccentric anomaly 00116 M = m0 + n*tk; // mean anomaly 00117 E = M; 00118 for( i = 0; i < 7; i++ ) 00119 { 00120 E = M + ecc * sin(E); 00121 } 00122 00123 // relativistic correction 00124 d_tr = GPS_CLOCK_CORRECTION_RELATIVISTIC_CONSTANT_F * ecc * sqrta * sin(E); // [s] 00125 d_tr *= LIGHTSPEED; 00126 00127 // clock correcton 00128 d_tsv = af0 + af1*tc + af2*tc*tc; // [s] 00129 00130 if( mode == 0 ) 00131 { 00132 // L1 only 00133 d_tsv -= tgd; // [s] 00134 } 00135 else if( mode == 1 ) 00136 { 00137 // L2 only 00138 d_tsv -= tgd*GPS_RATIO_OF_SQUARED_FREQUENCIES_L1_OVER_L2; // [s] 00139 } 00140 00141 // clock correction 00142 *clock_correction = d_tsv*LIGHTSPEED + d_tr; // [m] 00143 00144 // clock drift 00145 *clock_drift = (af1 + 2.0*af2*tc) * LIGHTSPEED; // [m/s] 00146 } 00147 00148 00149 00150 00151 void GPS_ComputeSatellitePositionAndVelocity( 00152 const unsigned short transmission_gpsweek, //!< GPS week when signal was transmit (0-1024+) [weeks] 00153 const double transmission_gpstow, //!< GPS time of week when signal was transmit [s] 00154 const unsigned short ephem_week, //!< ephemeris: GPS week (0-1024+) [weeks] 00155 const unsigned toe, //!< ephemeris: time of week [s] 00156 const double m0, //!< ephemeris: mean anomaly at reference time [rad] 00157 const double delta_n, //!< ephemeris: mean motion difference from computed value [rad] 00158 const double ecc, //!< ephemeris: eccentricity [] 00159 const double sqrta, //!< ephemeris: square root of the semi-major axis [m^(1/2)] 00160 const double omega0, //!< ephemeris: longitude of ascending node of orbit plane at weekly epoch [rad] 00161 const double i0, //!< ephemeris: inclination angle at reference time [rad] 00162 const double w, //!< ephemeris: argument of perigee [rad] 00163 const double omegadot, //!< ephemeris: rate of right ascension [rad/s] 00164 const double idot, //!< ephemeris: rate of inclination angle [rad/s] 00165 const double cuc, //!< ephemeris: amplitude of the cosine harmonic correction term to the argument of latitude [rad] 00166 const double cus, //!< ephemeris: amplitude of the sine harmonic correction term to the argument of latitude [rad] 00167 const double crc, //!< ephemeris: amplitude of the cosine harmonic correction term to the orbit radius [m] 00168 const double crs, //!< ephemeris: amplitude of the sine harmonic correction term to the orbit radius [m] 00169 const double cic, //!< ephemeris: amplitude of the cosine harmonic correction term to the angle of inclination [rad] 00170 const double cis, //!< ephemeris: amplitude of the sine harmonic correction term to the angle of inclination [rad] 00171 const double estimateOfTrueRange, //!< best estimate of the signal propagation time (in m) for Sagnac effect compensation [m] 00172 const double estimteOfRangeRate, //!< best estimate of the true signal Doppler (in m/s) for Sagnac effect compensation [m/s] 00173 double* x, //!< satellite x [m] 00174 double* y, //!< satellite y [m] 00175 double* z, //!< satellite z [m] 00176 double* vx, //!< satellite velocity x [m/s] 00177 double* vy, //!< satellite velocity y [m/s] 00178 double* vz //!< satellite velocity z [m/s] 00179 ) 00180 { 00181 unsigned char j; // counter 00182 00183 double tot; // time of transmission (including gps week) [s] 00184 double tk; // time from ephemeris reference epoch [s] 00185 double a; // semi-major axis of orbit [m] 00186 double n; // corrected mean motion [rad/s] 00187 double M; // mean anomaly, [rad] (Kepler's equation for eccentric anomaly, solved by iteration) 00188 double E; // eccentric anomaly [rad] 00189 double v; // true anomaly [rad] 00190 double u; // argument of latitude, corrected [rad] 00191 double r; // radius in the orbital plane [m] 00192 double i; // orbital inclination [rad] 00193 double cos2u; // cos(2*u) [] 00194 double sin2u; // sin(2*u) [] 00195 double d_u; // argument of latitude correction [rad] 00196 double d_r; // radius correction [m] 00197 double d_i; // inclination correction [rad] 00198 double x_op; // x position in the orbital plane [m] 00199 double y_op; // y position in the orbital plane [m] 00200 double omegak; // corrected longitude of the ascending node [rad] 00201 double cos_omegak; // cos(omegak) 00202 double sin_omegak; // sin(omegak) 00203 double cosu; // cos(u) 00204 double sinu; // sin(u) 00205 double cosi; // cos(i) 00206 double sini; // sin(i) 00207 double cosE; // cos(E) 00208 double sinE; // sin(E) 00209 double omegadotk; // corrected rate of right ascension [rad/s] 00210 double edot; // edot = n/(1.0 - ecc*cos(E)), [rad/s] 00211 double vdot; // d/dt of true anomaly [rad/s] 00212 double udot; // d/dt of argument of latitude [rad/s] 00213 double idotdot; // d/dt of the rate of the inclination angle [rad/s^2] 00214 double rdot; // d/dt of the radius in the orbital plane [m/s] 00215 double tmpa; // temp 00216 double tmpb; // temp 00217 double vx_op; // x velocity in the orbital plane [m/s] 00218 double vy_op; // y velocity in the orbital plane [m/s] 00219 00220 00221 // compute the times from the reference epochs 00222 // By including the week in the calculation, week rollover and older ephemeris bugs are mitigated 00223 // The result should be between -302400 and 302400 if the ephemeris is within one week of transmission 00224 tot = transmission_gpsweek*SECONDS_IN_WEEK + transmission_gpstow; 00225 tk = tot - (ephem_week*SECONDS_IN_WEEK + toe); 00226 00227 // compute the corrected mean motion term 00228 a = sqrta*sqrta; 00229 n = sqrt( GPS_UNIVERSAL_GRAVITY_CONSTANT / (a*a*a) ); // computed mean motion 00230 n += delta_n; // corrected mean motion 00231 00232 // Kepler's equation for eccentric anomaly 00233 M = m0 + n*tk; // mean anomaly 00234 E = M; 00235 for( j = 0; j < 7; j++ ) 00236 { 00237 E = M + ecc * sin(E); 00238 } 00239 00240 cosE = cos(E); 00241 sinE = sin(E); 00242 00243 // true anomaly 00244 v = atan2( (sqrt(1.0 - ecc*ecc)*sinE), (cosE - ecc) ); 00245 00246 // argument of latitude 00247 u = v + w; 00248 // radius in orbital plane 00249 r = a * (1.0 - ecc * cos(E)); 00250 // orbital inclination 00251 i = i0; 00252 00253 // second harmonic perturbations 00254 // 00255 cos2u = cos(2.0*u); 00256 sin2u = sin(2.0*u); 00257 // argument of latitude correction 00258 d_u = cuc * cos2u + cus * sin2u; 00259 // radius correction 00260 d_r = crc * cos2u + crs * sin2u; 00261 // correction to inclination 00262 d_i = cic * cos2u + cis * sin2u; 00263 00264 // corrected argument of latitude 00265 u += d_u; 00266 // corrected radius 00267 r += d_r; 00268 // corrected inclination 00269 i += d_i + idot * tk; 00270 00271 // positions in orbital plane 00272 cosu = cos(u); 00273 sinu = sin(u); 00274 x_op = r * cosu; 00275 y_op = r * sinu; 00276 00277 00278 // compute the corrected longitude of the ascending node 00279 // This equation deviates from that in Table 20-IV p. 100 GPSICD200C with the inclusion of the 00280 // signal propagation time (estimateOfTrueRange/LIGHTSPEED) term. This compensates for the Sagnac effect. 00281 // The omegak term is thus sensitive to the estimateOfTrueRange term which is usually unknown without 00282 // prior information. The average signal propagation time/range (70ms * c) can be used on first use 00283 // and this function must be called again to iterate this term. The sensitivity of the omegak term 00284 // typically requires N iterations - GDM_DEBUG{find out how many iterations are needed, how sensitive to the position?} 00285 omegak = omega0 + (omegadot - GPS_WGS84_EARTH_ROTATION_RATE)*tk - GPS_WGS84_EARTH_ROTATION_RATE*(toe + estimateOfTrueRange/LIGHTSPEED ); 00286 00287 // compute the WGS84 ECEF coordinates, 00288 // vector r with components x & y is now rotated using, R3(-omegak)*R1(-i) 00289 cos_omegak = cos(omegak); 00290 sin_omegak = sin(omegak); 00291 cosi = cos(i); 00292 sini = sin(i); 00293 *x = x_op * cos_omegak - y_op * sin_omegak * cosi; 00294 *y = x_op * sin_omegak + y_op * cos_omegak * cosi; 00295 *z = y_op * sini; 00296 00297 00298 // Satellite Velocity Computations are below 00299 // see Reference 00300 // Remodi, B. M (2004). GPS Tool Box: Computing satellite velocities using the broadcast ephemeris. 00301 // GPS Solutions. Volume 8(3), 2004. pp. 181-183 00302 // 00303 // example source code was available at [http://www.ngs.noaa.gov/gps-toolbox/bc_velo/bc_velo.c] 00304 00305 // recomputed the cos and sin of the corrected argument of latitude 00306 cos2u = cos(2.0*u); 00307 sin2u = sin(2.0*u); 00308 00309 edot = n / (1.0 - ecc*cosE); 00310 vdot = sinE*edot*(1.0 + ecc*cos(v)) / ( sin(v)*(1.0-ecc*cosE) ); 00311 udot = vdot + 2.0*(cus*cos2u - cuc*sin2u)*vdot; 00312 rdot = a*ecc*sinE*n/(1.0-ecc*cosE) + 2.0*(crs*cos2u - crc*sin2u)*vdot; 00313 idotdot = idot + (cis*cos2u - cic*sin2u)*2.0*vdot; 00314 00315 vx_op = rdot*cosu - y_op*udot; 00316 vy_op = rdot*sinu + x_op*udot; 00317 00318 // corrected rate of right ascension including similarily as above, for omegak, 00319 // compensation for the Sagnac effect 00320 omegadotk = omegadot - GPS_WGS84_EARTH_ROTATION_RATE*( 1.0 + estimteOfRangeRate/LIGHTSPEED ); 00321 00322 tmpa = vx_op - y_op*cosi*omegadotk; 00323 tmpb = x_op*omegadotk + vy_op*cosi - y_op*sini*idotdot; 00324 00325 *vx = tmpa * cos_omegak - tmpb * sin_omegak; 00326 *vy = tmpa * sin_omegak + tmpb * cos_omegak; 00327 *vz = vy_op*sini + y_op*cosi*idotdot; 00328 } 00329 00330 00331 void GPS_ComputeUserToSatelliteRange( 00332 const double userX, //!< user X position WGS84 ECEF [m] 00333 const double userY, //!< user Y position WGS84 ECEF [m] 00334 const double userZ, //!< user Z position WGS84 ECEF [m] 00335 const double satX, //!< satellite X position WGS84 ECEF [m] 00336 const double satY, //!< satellite Y positoin WGS84 ECEF [m] 00337 const double satZ, //!< satellite Z position WGS84 ECEF [m] 00338 double* range //!< user to satellite range [m] 00339 ) 00340 { 00341 double dx; 00342 double dy; 00343 double dz; 00344 00345 dx = satX - userX; 00346 dy = satY - userY; 00347 dz = satZ - userZ; 00348 00349 // compute the range 00350 *range = sqrt( dx*dx + dy*dy + dz*dz ); 00351 } 00352 00353 00354 void GPS_ComputeUserToSatelliteRangeAndRangeRate( 00355 const double userX, //!< user X position WGS84 ECEF [m] 00356 const double userY, //!< user Y position WGS84 ECEF [m] 00357 const double userZ, //!< user Z position WGS84 ECEF [m] 00358 const double userVx, //!< user X velocity WGS84 ECEF [m/s] 00359 const double userVy, //!< user Y velocity WGS84 ECEF [m/s] 00360 const double userVz, //!< user Z velocity WGS84 ECEF [m/s] 00361 const double satX, //!< satellite X position WGS84 ECEF [m] 00362 const double satY, //!< satellite Y positoin WGS84 ECEF [m] 00363 const double satZ, //!< satellite Z position WGS84 ECEF [m] 00364 const double satVx, //!< satellite X velocity WGS84 ECEF [m/s] 00365 const double satVy, //!< satellite Y velocity WGS84 ECEF [m/s] 00366 const double satVz, //!< satellite Z velocity WGS84 ECEF [m/s] 00367 double* range, //!< user to satellite range [m] 00368 double* range_rate //!< user to satellite range rate [m/s] 00369 ) 00370 { 00371 double dx; 00372 double dy; 00373 double dz; 00374 00375 dx = satX - userX; 00376 dy = satY - userY; 00377 dz = satZ - userZ; 00378 00379 // compute the range 00380 *range = sqrt( dx*dx + dy*dy + dz*dz ); 00381 00382 // compute the range rate 00383 // this method uses the NovAtel style sign convention! 00384 *range_rate = (userVx - satVx)*dx + (userVy - satVy)*dy + (userVz - satVz)*dz; 00385 *range_rate /= *range; 00386 } 00387 00388 00389 00390 void GPS_ComputeSatellitePositionVelocityAzimuthElevationDoppler_BasedOnAlmanacData( 00391 const double userX, //!< user X position WGS84 ECEF [m] 00392 const double userY, //!< user Y position WGS84 ECEF [m] 00393 const double userZ, //!< user Z position WGS84 ECEF [m] 00394 const unsigned short gpsweek, //!< user gps week (0-1024+) [week] 00395 const double gpstow, //!< user time of week [s] 00396 const double toa, //!< time of applicability [s] 00397 const unsigned short almanac_week, //!< gps week of almanac (0-1024+) [week] 00398 const unsigned short prn, //!< GPS prn number [] 00399 const double ecc, //!< eccentricity [] 00400 const double i0, //!< orbital inclination at reference time [rad] 00401 const double omegadot, //!< rate of right ascension [rad/s] 00402 const double sqrta, //!< square root of the semi-major axis [m^(1/2)] 00403 const double omega0, //!< longitude of ascending node of orbit plane at weekly epoch [rad] 00404 const double w, //!< argument of perigee [rad] 00405 const double m0, //!< mean anomaly at reference time [rad] 00406 const double af0, //!< polynomial clock correction coefficient (clock bias) [s], Note: parameters from ephemeris preferred vs almanac (22 vs 11 bits) 00407 const double af1, //!< polynomial clock correction coefficient (clock drift) [s/s], Note: parameters from ephemeris preferred vs almanac (16 vs 11 bits) 00408 double* clock_correction, //!< clock correction for this satellite for this epoch [m] 00409 double* clock_drift, //!< clock drift correction for this satellite for this epoch [m/s] 00410 double* satX, //!< satellite X position WGS84 ECEF [m] 00411 double* satY, //!< satellite Y position WGS84 ECEF [m] 00412 double* satZ, //!< satellite Z position WGS84 ECEF [m] 00413 double* satVx, //!< satellite X velocity WGS84 ECEF [m/s] 00414 double* satVy, //!< satellite Y velocity WGS84 ECEF [m/s] 00415 double* satVz, //!< satellite Z velocity WGS84 ECEF [m/s] 00416 double* azimuth, //!< satelilte azimuth [rad] 00417 double* elevation, //!< satelilte elevation [rad] 00418 double* doppler //!< satellite doppler with respect to the user position [m/s], Note: User must convert to Hz 00419 ) 00420 { 00421 double tow; // user time of week adjusted with the clock corrections [s] 00422 double range; // range estimate between user and satellite [m] 00423 double range_rate; // range_rate esimate between user and satellite [m/s] 00424 double x; // sat X position [m] 00425 double y; // sat Y position [m] 00426 double z; // sat Z position [m] 00427 double vx; // sat X velocity [m/s] 00428 double vy; // sat Y velocity [m/s] 00429 double vz; // sat Z velocity [m/s] 00430 00431 unsigned short week; // user week adjusted with the clock correction if needed [week] 00432 00433 unsigned char i; // counter 00434 00435 i = (unsigned char)prn; // get rid of a debug msg :) 00436 00437 // initialize to zero 00438 x = y = z = vx = vy = vz = 0.0; 00439 00440 GPS_ComputeSatelliteClockCorrectionAndDrift( 00441 gpsweek, 00442 gpstow, 00443 almanac_week, 00444 (unsigned)toa, 00445 (unsigned)toa, 00446 af0, 00447 af1, 00448 0.0, 00449 ecc, 00450 sqrta, 00451 0.0, 00452 m0, 00453 0.0, 00454 0, 00455 clock_correction, 00456 clock_drift ); 00457 00458 // adjust for week rollover 00459 week = gpsweek; 00460 tow = gpstow + (*clock_correction)/LIGHTSPEED; 00461 if( tow < 0.0 ) 00462 { 00463 tow += SECONDS_IN_WEEK; 00464 week--; 00465 } 00466 if( tow > 604800.0 ) 00467 { 00468 tow -= SECONDS_IN_WEEK; 00469 week++; 00470 } 00471 00472 // iterate to include the Sagnac correction 00473 // since the range is unknown, an approximate of 70 ms is good enough 00474 // to start the iterations so that 2 iterations are enough 00475 range = 0.070*LIGHTSPEED; 00476 range_rate = 0.0; 00477 for( i = 0; i < 2; i++ ) 00478 { 00479 GPS_ComputeSatellitePositionAndVelocity( 00480 week, 00481 tow, 00482 almanac_week, 00483 (unsigned)toa, 00484 m0, 00485 0.0, 00486 ecc, 00487 sqrta, 00488 omega0, 00489 i0, 00490 w, 00491 omegadot, 00492 0.0, 00493 0.0, 00494 0.0, 00495 0.0, 00496 0.0, 00497 0.0, 00498 0.0, 00499 range, 00500 range_rate, 00501 &x, 00502 &y, 00503 &z, 00504 &vx, 00505 &vy, 00506 &vz ); 00507 00508 GPS_ComputeUserToSatelliteRangeAndRangeRate( 00509 userX, 00510 userY, 00511 userZ, 00512 0.0, 00513 0.0, 00514 0.0, 00515 x, 00516 y, 00517 z, 00518 vx, 00519 vy, 00520 vz, 00521 &range, 00522 &range_rate ); 00523 } 00524 00525 GEODESY_ComputeAzimuthAndElevationAnglesBetweenToPointsInTheEarthFixedFrame( 00526 GEODESY_REFERENCE_ELLIPSE_WGS84, 00527 userX, 00528 userY, 00529 userZ, 00530 x, 00531 y, 00532 z, 00533 elevation, // sets the elevation 00534 azimuth ); // sets the azimuth 00535 00536 *satX = x; 00537 *satY = y; 00538 *satZ = z; 00539 *satVx = vx; 00540 *satVy = vy; 00541 *satVz = vz; 00542 00543 *doppler = range_rate; 00544 00545 } 00546 00547 00548 00549 void GPS_ComputeSatellitePositionVelocityAzimuthElevationDoppler_BasedOnEphmerisData( 00550 const double userX, //!< user X position WGS84 ECEF [m] 00551 const double userY, //!< user Y position WGS84 ECEF [m] 00552 const double userZ, //!< user Z position WGS84 ECEF [m] 00553 const unsigned short gpsweek, //!< gps week of signal transmission (0-1024+) [week] 00554 const double gpstow, //!< time of week of signal transmission (gpstow-psr/c) [s] 00555 const unsigned short ephem_week, //!< ephemeris: GPS week (0-1024+) [weeks] 00556 const unsigned toe, //!< ephemeris: time of week [s] 00557 const unsigned toc, //!< ephemeris: clock reference time of week [s] 00558 const double af0, //!< ephemeris: polynomial clock correction coefficient [s], Note: parameters from ephemeris preferred vs almanac (22 vs 11 bits) 00559 const double af1, //!< ephemeris: polynomial clock correction coefficient [s/s], Note: parameters from ephemeris preferred vs almanac (16 vs 11 bits) 00560 const double af2, //!< ephemeris: polynomial clock correction coefficient [s/s^2] 00561 const double tgd, //!< ephemeris: group delay differential between L1 and L2 [s] 00562 const double m0, //!< ephemeris: mean anomaly at reference time [rad] 00563 const double delta_n, //!< ephemeris: mean motion difference from computed value [rad/s] 00564 const double ecc, //!< ephemeris: eccentricity [] 00565 const double sqrta, //!< ephemeris: square root of the semi-major axis [m^(1/2)] 00566 const double omega0, //!< ephemeris: longitude of ascending node of orbit plane at weekly epoch [rad] 00567 const double i0, //!< ephemeris: inclination angle at reference time [rad] 00568 const double w, //!< ephemeris: argument of perigee [rad] 00569 const double omegadot, //!< ephemeris: rate of right ascension [rad/s] 00570 const double idot, //!< ephemeris: rate of inclination angle [rad/s] 00571 const double cuc, //!< ephemeris: amplitude of the cosine harmonic correction term to the argument of latitude [rad] 00572 const double cus, //!< ephemeris: amplitude of the sine harmonic correction term to the argument of latitude [rad] 00573 const double crc, //!< ephemeris: amplitude of the cosine harmonic correction term to the orbit radius [m] 00574 const double crs, //!< ephemeris: amplitude of the sine harmonic correction term to the orbit radius [m] 00575 const double cic, //!< ephemeris: amplitude of the cosine harmonic correction term to the angle of inclination [rad] 00576 const double cis, //!< ephemeris: amplitude of the sine harmonic correction term to the angle of inclination [rad] 00577 double* clock_correction, //!< clock correction for this satellite for this epoch [m] 00578 double* clock_drift, //!< clock drift correction for this satellite for this epoch [m/s] 00579 double* satX, //!< satellite X position WGS84 ECEF [m] 00580 double* satY, //!< satellite Y position WGS84 ECEF [m] 00581 double* satZ, //!< satellite Z position WGS84 ECEF [m] 00582 double* satVx, //!< satellite X velocity WGS84 ECEF [m/s] 00583 double* satVy, //!< satellite Y velocity WGS84 ECEF [m/s] 00584 double* satVz, //!< satellite Z velocity WGS84 ECEF [m/s] 00585 double* azimuth, //!< satelilte azimuth [rad] 00586 double* elevation, //!< satelilte elevation [rad] 00587 double* doppler //!< satellite doppler with respect to the user position [m/s], Note: User must convert to Hz 00588 ) 00589 { 00590 double tow; // user time of week adjusted with the clock corrections [s] 00591 double range; // range estimate between user and satellite [m] 00592 double range_rate; // range_rate esimate between user and satellite [m/s] 00593 double x; // sat X position [m] 00594 double y; // sat Y position [m] 00595 double z; // sat Z position [m] 00596 double vx; // sat X velocity [m/s] 00597 double vy; // sat Y velocity [m/s] 00598 double vz; // sat Z velocity [m/s] 00599 00600 unsigned short week; // user week adjusted with the clock correction if needed [week] 00601 00602 unsigned char i; // counter 00603 00604 // initialize to zero 00605 x = y = z = vx = vy = vz = 0.0; 00606 00607 GPS_ComputeSatelliteClockCorrectionAndDrift( 00608 gpsweek, 00609 gpstow, 00610 ephem_week, 00611 toe, 00612 toc, 00613 af0, 00614 af1, 00615 af2, 00616 ecc, 00617 sqrta, 00618 delta_n, 00619 m0, 00620 tgd, 00621 0, 00622 clock_correction, 00623 clock_drift ); 00624 00625 // adjust for week rollover 00626 week = gpsweek; 00627 tow = gpstow + (*clock_correction)/LIGHTSPEED; 00628 if( tow < 0.0 ) 00629 { 00630 tow += SECONDS_IN_WEEK; 00631 week--; 00632 } 00633 if( tow > 604800.0 ) 00634 { 00635 tow -= SECONDS_IN_WEEK; 00636 week++; 00637 } 00638 00639 // iterate to include the Sagnac correction 00640 // since the range is unknown, an approximate of 70 ms is good enough to start 00641 // the iterations so that 2 iterations are enough for sub mm accuracy 00642 range = 0.070*LIGHTSPEED; 00643 range_rate = 0.0; 00644 for( i = 0; i < 2; i++ ) 00645 { 00646 GPS_ComputeSatellitePositionAndVelocity( 00647 week, 00648 tow, 00649 ephem_week, 00650 toe, 00651 m0, 00652 delta_n, 00653 ecc, 00654 sqrta, 00655 omega0, 00656 i0, 00657 w, 00658 omegadot, 00659 idot, 00660 cuc, 00661 cus, 00662 crc, 00663 crs, 00664 cic, 00665 cis, 00666 range, 00667 range_rate, 00668 &x, 00669 &y, 00670 &z, 00671 &vx, 00672 &vy, 00673 &vz ); 00674 00675 GPS_ComputeUserToSatelliteRangeAndRangeRate( 00676 userX, 00677 userY, 00678 userZ, 00679 0.0, 00680 0.0, 00681 0.0, 00682 x, 00683 y, 00684 z, 00685 vx, 00686 vy, 00687 vz, 00688 &range, 00689 &range_rate ); 00690 } 00691 00692 GEODESY_ComputeAzimuthAndElevationAnglesBetweenToPointsInTheEarthFixedFrame( 00693 GEODESY_REFERENCE_ELLIPSE_WGS84, 00694 userX, 00695 userY, 00696 userZ, 00697 x, 00698 y, 00699 z, 00700 elevation, // sets the elevation 00701 azimuth ); // sets the azimuth 00702 00703 *satX = x; 00704 *satY = y; 00705 *satZ = z; 00706 *satVx = vx; 00707 *satVy = vy; 00708 *satVz = vz; 00709 00710 *doppler = range_rate; 00711 } 00712 00713 00714 BOOL GPS_DecodeRawGPSEphemeris( 00715 const unsigned char subframe1[30], //!< subframe 1 data, 30 bytes * 8bits/byte = 240 bits, thus parity bits have been removed 00716 const unsigned char subframe2[30], //!< subframe 2 data, 30 bytes * 8bits/byte = 240 bits, thus parity bits have been removed 00717 const unsigned char subframe3[30], //!< subframe 3 data, 30 bytes * 8bits/byte = 240 bits, thus parity bits have been removed 00718 unsigned short prn, //!< GPS PRN number (helps with debugging) 00719 unsigned* tow, //!< time of week in subframe1, the time of the leading bit edge of subframe 2 [s] 00720 unsigned short* iodc, //!< 10 bit issue of data (clock), 8 LSB bits will match the iode [] 00721 unsigned char* iode, //!< 8 bit issue of data (ephemeris) [] 00722 unsigned* toe, //!< reference time ephemeris (0-604800) [s] 00723 unsigned* toc, //!< reference time (clock) (0-604800) [s] 00724 unsigned short* week, //!< 10 bit gps week 0-1023 (user must account for week rollover ) [week] 00725 unsigned char* health, //!< 6 bit health parameter, 0 if healthy, unhealth othersize [0=healthy] 00726 unsigned char* alert_flag, //!< 1 = URA may be worse than indicated [0,1] 00727 unsigned char* anti_spoof, //!< anti-spoof flag from 0=off, 1=on [0,1] 00728 unsigned char* code_on_L2, //!< 0=reserved, 1=P code on L2, 2=C/A on L2 [0,1,2] 00729 unsigned char* ura, //!< User Range Accuracy lookup code, 0 is excellent, 15 is use at own risk [0-15], see p. 83 GPSICD200C 00730 unsigned char* L2_P_data_flag, //!< flag indicating if P is on L2 1=true [0,1] 00731 unsigned char* fit_interval_flag, //!< fit interval flag (four hour interval or longer) 0=4 fours, 1=greater [0,1] 00732 unsigned short* age_of_data_offset, //!< age of data offset [s] 00733 double* tgd, //!< group delay [s] 00734 double* af2, //!< polynomial clock correction coefficient (rate of clock drift) [s/s^2] 00735 double* af1, //!< polynomial clock correction coefficient (clock drift) [s/s] 00736 double* af0, //!< polynomial clock correction coefficient (clock bias) [s] 00737 double* m0, //!< mean anomaly at reference time [rad] 00738 double* delta_n, //!< mean motion difference from computed value [rad/s] 00739 double* ecc, //!< eccentricity [] 00740 double* sqrta, //!< square root of the semi-major axis [m^(1/2)] 00741 double* omega0, //!< longitude of ascending node of orbit plane at weekly epoch [rad] 00742 double* i0, //!< inclination angle at reference time [rad] 00743 double* w, //!< argument of perigee [rad] 00744 double* omegadot, //!< rate of right ascension [rad/s] 00745 double* idot, //!< rate of inclination angle [rad/s] 00746 double* cuc, //!< amplitude of the cosine harmonic correction term to the argument of latitude [rad] 00747 double* cus, //!< amplitude of the sine harmonic correction term to the argument of latitude [rad] 00748 double* crc, //!< amplitude of the cosine harmonic correction term to the orbit radius [m] 00749 double* crs, //!< amplitude of the sine harmonic correction term to the orbit radius [m] 00750 double* cic, //!< amplitude of the cosine harmonic correction term to the angle of inclination [rad] 00751 double* cis //!< amplitude of the sine harmonic correction term to the angle of inclination [rad] 00752 ) 00753 { 00754 /* 00755 ------------------------------------------------------------------ 00756 SUBFRAME1 00757 ------------------------------------------------------------------ 00758 TERM, NR BITS, BITS NO PARITY, BITS WITH PARITY 00759 preamble, 8, 1-8, 1-8, 00760 TLM, 14, 9-22, 9-22 00761 reserved, 2, 23-24, 23-24 00762 --PARITY-- 6, ----- 25-30 00763 TOW, 17, 25-41, 31-47 00764 alert_flag, 1, 42, 48 00765 anti_spoof_flag, 1, 43, 49 00766 subframeID, 3, 44-46, 50-52 00767 parity_related, 2, 47-48, 53-54 00768 --PARITY-- 6, ----- 25-30 00769 week, 10, 49-58, 61-70 00770 code_on_L2, 2, 59,60, 71-72 00771 ura, 4, 61-64, 73-76 00772 health, 6, 65-70, 77-82 00773 iodc_MSB, 2, 71-72, 83-84 00774 --PARITY-- 6, ----- 85-90 00775 L2_P_data_flag, 1, 73, 91 00776 reserved, 23, 74-96, 92-114 00777 --PARITY-- 6, ----- 115-120 00778 reserved, 24, 97-120, 121-144 00779 --PARITY-- 6, ----- 25-30 00780 reserved, 24, 121-144, 151-174 00781 --PARITY-- 6, ----- 25-30 00782 reserved, 16, 145-160, 181-196 00783 tgd, 8, 161-168, 197-204 00784 --PARITY-- 6, ----- 205-210 00785 iodc_LSB, 8, 169-176, 211-218 00786 toc, 16, 177-192, 219-234 00787 --PARITY-- 6, ----- 235-240 00788 af2, 8, 193-200, 241-248 00789 af1, 16, 201-216, 249-264 00790 --PARITY-- 6, ----- 265-270 00791 af0, 22, 217-238, 271-292 00792 parity_related, 2, 239-240, 293-294 00793 --PARITY-- 6, ----- 295-300 00794 ------------------------------------------------------------------ 00795 SUBFRAME2 00796 ------------------------------------------------------------------ 00797 TERM, NR BITS, BITS NO PARITY, BITS WITH PARITY 00798 preamble, 8, 1-8, 1-8, 00799 TLM, 14, 9-22, 9-22 00800 reserved, 2, 23-24, 23-24 00801 --PARITY-- 6, ----- 25-30 00802 TOW, 17, 25-41, 31-47 00803 alert_flag, 1, 42, 48 00804 anti_spoof_flag, 1, 43, 49 00805 subframeID, 3, 44-46, 50-52 00806 parity_related, 2, 47-48, 53-54 00807 --PARITY-- 6, ----- 25-30 00808 iode, 8, 49-56, 61-68 00809 crs, 16, 57-72, 69-84 00810 --PARITY-- 6, ----- 95-90 00811 delta_n, 16, 73-88, 91-106 00812 m0_MSB, 8, 89-96, 107-114 00813 --PARITY-- 6, ----- 115-120 00814 m0_LSB, 24, 97-120, 121-144 00815 --PARITY-- 6, ----- 145-150 00816 cuc, 16, 121-136, 151-166 00817 ecc_MSB, 8, 137-144, 167-174 00818 --PARITY-- 6, ----- 175-180 00819 ecc_LSB, 24, 145-168, 181-204 00820 --PARITY-- 6, ----- 205-210 00821 cus, 16, 169-184, 211-226 00822 sqrta_MSB, 8, 185-192, 227-234 00823 --PARITY-- 6, ----- 235-240 00824 sqrta_LSB, 24, 193-216, 241-264 00825 --PARITY-- 6, ----- 265-270 00826 toe, 16, 217-232, 271-286 00827 fit_interval_flag, 1, 233, 287 00828 age_of_data_offset, 5, 234-238, 288-292 00829 parity_related, 2, 239-240, 293-294 00830 --PARITY-- 6, ----- 295-300 00831 ------------------------------------------------------------------ 00832 SUBFRAME3 00833 ------------------------------------------------------------------ 00834 TERM, NR BITS, BITS NO PARITY, BITS WITH PARITY 00835 preamble, 8, 1-8, 1-8, 00836 TLM, 14, 9-22, 9-22 00837 reserved, 2, 23-24, 23-24 00838 --PARITY-- 6, ----- 25-30 00839 TOW, 17, 25-41, 31-47 00840 alert_flag, 1, 42, 48 00841 anti_spoof_flag, 1, 43, 49 00842 subframeID, 3, 44-46, 50-52 00843 parity_related, 2, 47-48, 53-54 00844 --PARITY-- 6, ----- 25-30 00845 cic, 16, 49-64, 61-76 00846 omega0_MSB, 8, 65-72, 77-84 00847 --PARITY-- 6, ----- 85-90 00848 omega0_LSB, 24, 73-96, 91-114 00849 --PARITY-- 6, ----- 115-120 00850 cis, 16, 97-112, 121-136 00851 i0_MSB, 8, 113-120, 137-144 00852 --PARITY-- 6, ----- 145-150 00853 i0_LSB, 24, 121-144, 151-174 00854 --PARITY-- 6, ----- 175-180 00855 crc, 16, 145-160, 181-196 00856 w_MSB, 8, 161-168, 197-204 00857 --PARITY-- 6, ----- 205-210 00858 w_LSB, 24, 169-192, 211-234 00859 --PARITY-- 6, ----- 235-240 00860 omegadot, 24, 193-216, 241-264 00861 --PARITY-- 6, ----- 265-270 00862 iode, 8, 217-224, 271-278 00863 idot, 14, 225-238, 279-292 00864 parity_related, 2, 239-240, 293-294 00865 ------------------------------------------------------------------ 00866 */ 00867 unsigned char subframe_id; // subrame id 00868 unsigned char iode_subframe1; // 8 LSB bits of the iodc in subframe 1 00869 unsigned char iode_subframe2; // subframe2 iode 00870 unsigned char iode_subframe3; // subframe3 iode 00871 00872 // temporary variables of different size 00873 char s8; 00874 short s16; 00875 int s32; 00876 unsigned short u16a, u16b; 00877 unsigned u32a, u32b, u32c, u32d; 00878 00879 u16a = prn; // gets rid of a debug msg :) 00880 00881 //------------------------------------------------------------------ 00882 // SUBFRAME1 00883 //------------------------------------------------------------------ 00884 00885 // time of week, actually a 19 bit value, 17 MSBs are available, 2 LSB bits are always zero 00886 u32a = subframe1[3] << 11; 00887 u32b = subframe1[4] << 3; 00888 u32c = (subframe1[5] & 0x80) >> 5; 00889 *tow = (u32a | u32b | u32c); // [z-count 1.5s intervals] 00890 *tow = (*tow * 3) / 2; // converted to [s] 00891 00892 // alert_flag 00893 *alert_flag = (unsigned char)( (subframe1[5] & 0x40) >> 6 ); 00894 00895 // anti-spoof 00896 *anti_spoof = (unsigned char)( (subframe1[5] & 0x20) >> 5 ); 00897 00898 // confirm that this is subframe 1 00899 subframe_id = (unsigned char)( (subframe1[5] & 0x1C) >> 2 ); 00900 if( subframe_id != 1 ) 00901 return FALSE; 00902 00903 // GPS Week 00904 u16a = (unsigned short)( subframe1[6] << 2 ); 00905 u16b = (unsigned short)( subframe1[7] >> 6 ); 00906 *week = (unsigned short)( u16a | u16b ); 00907 00908 /// code_on_L2 00909 *code_on_L2 = (unsigned char)( (subframe1[7] & 0x30) >> 4 ); 00910 00911 // ura 00912 *ura = (unsigned char)( (subframe1[7] & 0x0F) ); 00913 00914 // health 00915 *health = (unsigned char)( subframe1[8] >> 2 ); 00916 00917 // issue of data clock 00918 u16a = (unsigned short)( (subframe1[8] & 0x03) << 8 ); 00919 u16b = (unsigned short)( subframe1[21] ); 00920 *iodc = (unsigned short)( u16a | u16b ); // [] 00921 00922 // iode subframe1 for consistency checking 00923 iode_subframe1 = subframe1[21]; 00924 00925 // L2_P_data_flag 00926 *L2_P_data_flag = (unsigned char)( (subframe1[9] & 0x80) >> 7 ); 00927 00928 // tgd 00929 s8 = subframe1[20]; // signed 00930 *tgd = s8 / TWO_TO_THE_POWER_OF_31; 00931 00932 // toc 00933 u16a = (unsigned short)( subframe1[22] << 8 ); 00934 u16b = (unsigned short)( subframe1[23] ); 00935 *toc = (unsigned)( (u16a | u16b) ) * 16; 00936 00937 // af2 00938 s8 = subframe1[24]; // signed 00939 *af2 = s8; 00940 *af2 /= TWO_TO_THE_POWER_OF_55; 00941 00942 // af1 00943 u16a = (unsigned short)( subframe1[25] << 8 ); 00944 u16b = subframe1[26]; 00945 s16 = (unsigned short)( u16a | u16b ); // signed value 00946 *af1 = s16; 00947 *af1 /= TWO_TO_THE_POWER_OF_43; 00948 00949 // af0 00950 u32a = subframe1[27] << 24; 00951 u32b = subframe1[28] << 16; 00952 u32c = subframe1[29] & 0xFC; 00953 u32c <<= 8; // align to the sign bit (two's complement integer) 00954 u32d = (u32a | u32b | u32c); 00955 s32 = (int)(u32d); 00956 s32 >>= 10; // 22 bit value 00957 *af0 = s32; 00958 *af0 /= TWO_TO_THE_POWER_OF_31; 00959 00960 //------------------------------------------------------------------ 00961 // SUBFRAME2 00962 //------------------------------------------------------------------ 00963 00964 // confirm that this is subframe 2 00965 subframe_id = (unsigned char)( (subframe2[5] & 0x1C) >> 2 ); 00966 if( subframe_id != 2 ) 00967 return FALSE; 00968 00969 // iode subframe2 00970 iode_subframe2 = subframe2[6]; 00971 00972 // crs 00973 u16a = (unsigned short)( subframe2[7] << 8 ); 00974 u16b = subframe2[8]; 00975 s16 = (unsigned short)( u16a | u16b ); // signed value 00976 *crs = s16; 00977 *crs /= 32.0; // [m] 00978 00979 // delta_n 00980 u16a = (unsigned short)( subframe2[9] << 8 ); 00981 u16b = subframe2[10]; 00982 s16 = (short)( u16a | u16b ); // signed value 00983 *delta_n = s16; 00984 *delta_n *= PI / TWO_TO_THE_POWER_OF_43; // [rad/s] 00985 00986 // m0 00987 u32a = subframe2[11] << 24; 00988 u32b = subframe2[12] << 16; 00989 u32c = subframe2[13] << 8; 00990 u32d = subframe2[14]; 00991 s32 = (u32a | u32b | u32c | u32d); // signed value 00992 *m0 = s32; 00993 *m0 *= PI / TWO_TO_THE_POWER_OF_31; // [rad] 00994 00995 // cuc 00996 u16a = (unsigned short)( subframe2[15] << 8 ); 00997 u16b = subframe2[16]; 00998 s16 = (short)( u16a | u16b ); // signed value 00999 *cuc = s16; 01000 *cuc /= TWO_TO_THE_POWER_OF_29; // [rad] 01001 01002 // ecc 01003 u32a = subframe2[17] << 24; 01004 u32b = subframe2[18] << 16; 01005 u32c = subframe2[19] << 8; 01006 u32d = subframe2[20]; 01007 *ecc = u32a | u32b | u32c | u32d; 01008 *ecc /= TWO_TO_THE_POWER_OF_33; // [] 01009 01010 // cus 01011 u16a = (unsigned short)( subframe2[21] << 8 ); 01012 u16b = subframe2[22]; 01013 s16 = (short)( u16a | u16b ); 01014 *cus = s16; 01015 *cus /= TWO_TO_THE_POWER_OF_29; // [rad] 01016 01017 // sqrta 01018 u32a = subframe2[23] << 24; 01019 u32b = subframe2[24] << 16; 01020 u32c = subframe2[25] << 8; 01021 u32d = subframe2[26]; 01022 *sqrta = u32a | u32b | u32c | u32d; 01023 *sqrta /= TWO_TO_THE_POWER_OF_19; // [sqrt(m)] 01024 01025 // toe 01026 u16a = (unsigned short)( subframe2[27] << 8 ); 01027 u16b = subframe2[28]; 01028 *toe = (unsigned)( (u16a | u16b) ) * 16; // [s] 01029 01030 // fit_interval_flag 01031 *fit_interval_flag = (unsigned char)( subframe2[29] >> 7 ); 01032 01033 // age_of_data_offset 01034 *age_of_data_offset = (unsigned short)( (subframe2[29] & 0x74) >> 2 ); 01035 *age_of_data_offset *= 900; // [s] 01036 01037 //------------------------------------------------------------------ 01038 // SUBFRAME3 01039 //------------------------------------------------------------------ 01040 01041 // confirm that this is subframe 3 01042 subframe_id = (unsigned char)( (subframe3[5] & 0x1C) >> 2 ); 01043 if( subframe_id != 3 ) 01044 return FALSE; 01045 01046 // cic 01047 u16a = (unsigned short)( subframe3[6] << 8 ); 01048 u16b = subframe3[7]; 01049 s16 = (short)( u16a | u16b ); // signed value 01050 *cic = s16; 01051 *cic /= TWO_TO_THE_POWER_OF_29; // [rad] 01052 01053 // omego0 01054 u32a = subframe3[8] << 24; 01055 u32b = subframe3[9] << 16; 01056 u32c = subframe3[10] << 8; 01057 u32d = subframe3[11]; 01058 s32 = u32a | u32b | u32c | u32d; // signed value 01059 *omega0 = s32; 01060 *omega0 *= PI / TWO_TO_THE_POWER_OF_31; // [rad] 01061 01062 // cis 01063 u16a = (unsigned short)( subframe3[12] << 8 ); 01064 u16b = subframe3[13]; 01065 s16 = (short)( u16a | u16b ); // signed value 01066 *cis = s16; 01067 *cis /= TWO_TO_THE_POWER_OF_29; // [rad] 01068 01069 // i0 01070 u32a = subframe3[14] << 24; 01071 u32b = subframe3[15] << 16; 01072 u32c = subframe3[16] << 8; 01073 u32d = subframe3[17]; 01074 s32 = u32a | u32b | u32c | u32d; 01075 *i0 = s32; 01076 *i0 *= PI / TWO_TO_THE_POWER_OF_31; // [rad] 01077 01078 // crc 01079 u16a = (unsigned short)( subframe3[18] << 8 ); 01080 u16b = subframe3[19]; 01081 s16 = (short)( u16a | u16b ); // signed value 01082 *crc = s16; 01083 *crc /= 32.0; // [m] 01084 01085 // w 01086 u32a = subframe3[20] << 24; 01087 u32b = subframe3[21] << 16; 01088 u32c = subframe3[22] << 8; 01089 u32d = subframe3[23]; 01090 s32 = u32a | u32b | u32c | u32d; // signed value 01091 *w = s32; 01092 *w *= PI / TWO_TO_THE_POWER_OF_31; // [rad] 01093 01094 // omegadot 01095 u32a = subframe3[24] << 24; 01096 u32b = subframe3[25] << 16; 01097 u32c = subframe3[26] << 8; 01098 s32 = u32a | u32b | u32c; // signed value 01099 s32 = s32 >> 8; 01100 *omegadot = s32; 01101 *omegadot *= PI / TWO_TO_THE_POWER_OF_43; // [rad/s] 01102 01103 // iode subframe3 01104 iode_subframe3 = subframe3[27]; 01105 01106 // idot 01107 u16a = (unsigned short)( subframe3[28] << 8 ); 01108 u16b = (unsigned short)( subframe3[29] & 0xFC ); 01109 s16 = (short)( u16a | u16b ); // signed value 01110 s16 = (short)( s16 >> 2 ); 01111 *idot = s16; 01112 *idot *= PI / TWO_TO_THE_POWER_OF_43; // [rad/s] 01113 01114 // check that the IODE values match for all three subframes 01115 if( (iode_subframe1 == iode_subframe2) && (iode_subframe1 == iode_subframe3) ) 01116 { 01117 *iode = iode_subframe1; 01118 return TRUE; 01119 } 01120 else 01121 { 01122 *iode = 0; 01123 return FALSE; // inconsistent subframe dataset 01124 } 01125 } 01126 01127