1 //============================================================================== 2 // 3 // This file is part of GPSTk, the GPS Toolkit. 4 // 5 // The GPSTk is free software; you can redistribute it and/or modify 6 // it under the terms of the GNU Lesser General Public License as published 7 // by the Free Software Foundation; either version 3.0 of the License, or 8 // any later version. 9 // 10 // The GPSTk is distributed in the hope that it will be useful, 11 // but WITHOUT ANY WARRANTY; without even the implied warranty of 12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 // GNU Lesser General Public License for more details. 14 // 15 // You should have received a copy of the GNU Lesser General Public 16 // License along with GPSTk; if not, write to the Free Software Foundation, 17 // Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA 18 // 19 // This software was developed by Applied Research Laboratories at the 20 // University of Texas at Austin. 21 // Copyright 2004-2020, The Board of Regents of The University of Texas System 22 // 23 //============================================================================== 24 25 //============================================================================== 26 // 27 // This software was developed by Applied Research Laboratories at the 28 // University of Texas at Austin, under contract to an agency or agencies 29 // within the U.S. Department of Defense. The U.S. Government retains all 30 // rights to use, duplicate, distribute, disclose, or release this software. 31 // 32 // Pursuant to DoD Directive 523024 33 // 34 // DISTRIBUTION STATEMENT A: This software has been approved for public 35 // release, distribution is unlimited. 36 // 37 //============================================================================== 38 39 /** 40 * @file AlmOrbit.cpp 41 * Encapsulate almanac data, and compute satellite orbit, etc. 42 */ 43 44 #include "GNSSconstants.hpp" 45 #include "GPSEllipsoid.hpp" 46 #include "AlmOrbit.hpp" 47 #include "GPSWeekSecond.hpp" 48 #include <cmath> 49 50 namespace gpstk 51 { AlmOrbit()52 AlmOrbit :: AlmOrbit() throw() 53 { 54 ecc = i_offset = OMEGAdot = Ahalf = OMEGA0 = w = M0 = AF0 = AF1 = 0.0; 55 56 Toa = xmit_time = 0; 57 58 week = SV_health = 0; 59 PRN = 0; 60 } 61 AlmOrbit(short prn,double aEcc,double ai_offset,double aOMEGAdot,double aAhalf,double aOMEGA0,double aw,double aM0,double aAF0,double aAF1,long aToa,long axmit_time,short aweek,short aSV_health)62 AlmOrbit :: AlmOrbit(short prn, double aEcc, double ai_offset, 63 double aOMEGAdot, double aAhalf, double aOMEGA0, 64 double aw, double aM0, double aAF0, double aAF1, 65 long aToa, long axmit_time, short aweek, 66 short aSV_health) 67 : PRN(prn), ecc(aEcc), i_offset(ai_offset), OMEGAdot(aOMEGAdot), Ahalf(aAhalf), 68 OMEGA0(aOMEGA0), w(aw), M0(aM0), AF0(aAF0), AF1(aAF1), Toa(aToa), 69 xmit_time(axmit_time), week(aweek), SV_health(aSV_health) 70 { 71 } 72 svXvt(const CommonTime & t) const73 Xvt AlmOrbit :: svXvt(const CommonTime& t) const 74 { 75 Xvt sv; 76 GPSEllipsoid ell; 77 78 double elapt; /* elapsed time since Toa */ 79 double A; /* semi-major axis */ 80 double n; /* mean motion */ 81 double meana; /* mean anomoly */ 82 double ea; /* eccentric anomoly */ 83 short loop; /* counter */ 84 double f,g,delea,q,gsta,gcta; /* temp. variables */ 85 double dtc; /* corrected time */ 86 double ta; /* true anomoly */ 87 double sinea,cosea,sinu,cosu; 88 double alat; /* arguement of latitude */ 89 double ualat; /* corrected arguement of latitude */ 90 double r; /* radius */ 91 double i; /* inclination */ 92 double anlon; /* corrected longitue of ascending node */ 93 double xip,yip,can,san,cinc,sinc,xef,yef,zef,dek,dlk,div,domk,duv, 94 drv,dxp,dyp,vxef,vyef,vzef; 95 double sqrtgm = ::sqrt(ell.gm()); 96 97 /* Compute time since Almanac epoch (Toa) including week change */ 98 elapt = t - getToaTime(); 99 100 /* compute mean motion from semi-major axis */ 101 A = Ahalf * Ahalf; 102 n = sqrtgm / (Ahalf * A); 103 104 /* compute the mean anomaly */ 105 meana = M0 + elapt * n; 106 meana = ::fmod(meana, 2.0 * PI); 107 108 /* compute eccentric anomaly by iteration */ 109 110 ea = meana + ecc * ::sin(meana); 111 loop = 1; 112 113 do { 114 f = meana - (ea - ecc * ::sin(ea)); 115 g = 1.0 - ecc * ::cos(ea); 116 delea = f / g; 117 ea += delea; 118 loop++; 119 } while ( ::fabs(delea) > 1.0e-11 && (loop <= 20)); 120 121 /* compute clock corrections (no relativistic correction computed) */ 122 dtc = AF0 + elapt * AF1; 123 sv.clkbias = dtc; 124 125 /* compute the true anomaly */ 126 q = ::sqrt (1.0e0 - ecc * ecc); 127 sinea = ::sin(ea); 128 cosea = ::cos(ea); 129 gsta = q * sinea; 130 gcta = cosea - ecc; 131 ta = ::atan2(gsta,gcta); 132 g = 1.0 - ecc * cosea; 133 134 /* compute argument of latitude for orbit */ 135 alat = ta + w; 136 137 /* compute correction terms ( no pertubation ) */ 138 ualat = alat; 139 r = A * (1.0 - ecc * cosea); 140 i = i_offset + 0.3e0 * PI; 141 142 /* compute corrected longitude of ascending node */ 143 anlon = OMEGA0 + 144 (OMEGAdot - ell.angVelocity()) * elapt - 145 ell.angVelocity() * (double)Toa; 146 147 /* compute positions in orbital plane */ 148 cosu = ::cos(ualat); 149 sinu = ::sin(ualat); 150 xip = r * cosu; 151 yip = r * sinu; 152 153 /* compute earch fixed coordinates (in meters) */ 154 can = ::cos (anlon); 155 san = ::sin (anlon); 156 cinc = ::cos(i); 157 sinc = ::sin(i); 158 159 xef = xip * can - yip * cinc * san; 160 yef = xip * san + yip * cinc * can; 161 zef = yip * sinc; 162 163 sv.x[0] = xef; 164 sv.x[1] = yef; 165 sv.x[2] = zef; 166 167 /* compute velocity of rotation coordinates & velocity of sat. */ 168 dek = n / g; 169 dlk = n * q / (g*g); 170 div = 0.0e0; 171 domk = OMEGAdot - ell.angVelocity(); 172 duv = dlk; 173 drv = A * ecc * dek * sinea; 174 175 dxp = drv * cosu - r * sinu * duv; 176 dyp = drv * sinu + r * cosu * duv; 177 178 vxef = dxp * can - xip * san * domk - dyp * cinc * san 179 + yip * (sinc * san * div - cinc * can * domk); 180 vyef = dxp * san + xip * can * domk + dyp * cinc * can 181 - yip * (sinc * can * div + cinc * san * domk); 182 vzef = dyp * sinc + yip * cinc * div; 183 184 sv.v[0] = vxef; 185 sv.v[1] = vyef; 186 sv.v[2] = vzef; 187 sv.health = SV_health == 0 ? Xvt::Healthy : Xvt::Unhealthy; 188 189 return sv; 190 } 191 getTransmitTime() const192 CommonTime AlmOrbit::getTransmitTime() const throw() 193 { 194 return GPSWeekSecond(getFullWeek(), xmit_time); 195 } 196 getFullWeek() const197 short AlmOrbit::getFullWeek() const throw() 198 { 199 // return value of the transmit week for the given PRN 200 short xmit_week = week; 201 double sow_diff = (double)(Toa - xmit_time); 202 if (sow_diff < -HALFWEEK) 203 xmit_week--; 204 else if (sow_diff > HALFWEEK) 205 xmit_week++; 206 207 return xmit_week; 208 } 209 getToaTime() const210 CommonTime AlmOrbit::getToaTime() const throw() 211 { 212 return GPSWeekSecond(week, Toa); 213 } 214 dump(std::ostream & s,int verbosity) const215 void AlmOrbit::dump(std::ostream& s, int verbosity) const 216 { 217 using std::endl; 218 using std::setw; 219 220 s << std::setprecision(4); 221 s.setf(std::ios::scientific); 222 switch (verbosity) 223 { 224 case 0: 225 s << PRN << ", " 226 << Toa << ", " 227 << week << ", " 228 << std::hex 229 << SV_health << ", " 230 << std::dec 231 << AF0 << ", " 232 << AF1 << ", " 233 << ecc << ", " 234 << w << ", " 235 << Ahalf << ", " 236 << M0 << ", " 237 << OMEGA0 << ", " 238 << OMEGAdot << ", " 239 << i_offset 240 << endl; 241 break; 242 243 case 1: 244 s << "PRN:" << PRN 245 << " Toa:" << Toa 246 << " H:" << SV_health 247 << " AFO:" << AF0 248 << " AF1:" << AF1 249 << " Ecc:" << ecc 250 << endl 251 << " w:" << w 252 << " Ahalf:" << Ahalf 253 << " M0:" << M0 254 << endl 255 << " OMEGA0:" << OMEGA0 256 << " OMEGAdot:" << OMEGAdot 257 << " Ioff:" << i_offset 258 << endl; 259 break; 260 261 default: 262 s << "PRN: " << PRN << endl 263 << "Toa: " << Toa << endl 264 << "xmit_time: " << xmit_time << endl 265 << "week: " << week << endl 266 << "SV_health: " << SV_health << endl 267 << "AFO: " << setw(12) << AF0 << " sec" << endl 268 << "AF1: " << setw(12) << AF1 << " sec/sec" << endl 269 << "Sqrt A: " << setw(12) << Ahalf << " sqrt meters" << endl 270 << "Eccentricity: " << setw(12) << ecc << endl 271 << "Arg of perigee: " << setw(12) << w << " rad" << endl 272 << "Mean anomaly at epoch: " << setw(12) << M0 << " rad" << endl 273 << "Right ascension: " << setw(12) << OMEGA0 << " rad " << setw(16) << OMEGAdot << " rad/sec" << endl 274 << "Inclination offset: " << setw(12) << i_offset << " rad " << endl; 275 } 276 } 277 operator <<(std::ostream & s,const AlmOrbit & ao)278 std::ostream& operator<<(std::ostream& s, const AlmOrbit& ao) 279 { 280 ao.dump(s); 281 return s; 282 } 283 284 } // namespace 285