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 /// @file BDSEphemeris.cpp Encapsulates the GPS legacy broadcast ephemeris and clock. 40 /// Inherits OrbitEph, which does most of the work; this class adds health and 41 /// accuracy information, fit interval, ionospheric correction terms and data 42 /// flags. 43 44 #include <string> 45 #include "Exception.hpp" 46 #include "BDSWeekSecond.hpp" 47 48 #include "BDSEphemeris.hpp" 49 #include "GPSWeekSecond.hpp" 50 #include "WGS84Ellipsoid.hpp" 51 #include "TimeString.hpp" 52 #include "Matrix.hpp" 53 54 using namespace std; 55 56 namespace gpstk 57 { BDSEphemeris()58 BDSEphemeris :: BDSEphemeris() 59 : HOWtime(0), IODE(0), IODC(0), health(1), accuracy(0.0), Tgd13(0.0), 60 Tgd23(0.0), fitDuration(4) 61 { 62 beginValid.setTimeSystem(TimeSystem::BDT); 63 endValid.setTimeSystem(TimeSystem::BDT); 64 ctToe.setTimeSystem(TimeSystem::BDT); 65 ctToc.setTimeSystem(TimeSystem::BDT); 66 transmitTime.setTimeSystem(TimeSystem::BDT); 67 } 68 69 // Returns true if the time, ct, is within the period of validity of 70 // this OrbitEph object. 71 // @throw Invalid Request if the required data has not been stored. isValid(const CommonTime & ct) const72 bool BDSEphemeris::isValid(const CommonTime& ct) const 73 { 74 try { 75 if(ct >= beginValid && ct <= endValid) return true; 76 return false; 77 } 78 catch(Exception& e) { GPSTK_RETHROW(e); } 79 } 80 81 // This function returns the health status of the SV. isHealthy() const82 bool BDSEphemeris::isHealthy() const 83 { 84 try { 85 OrbitEph::isHealthy(); // ignore the return value; for dataLoaded check 86 if(health == 0) return true; 87 return false; 88 } 89 catch(Exception& e) { GPSTK_RETHROW(e); } 90 } 91 92 // adjustBeginningValidity determines the beginValid and endValid times. 93 // Note that this is currently a "best guess" based on observation of Beidou 94 // operation. The concept of a fit interval is mentioned in the ICD, but the 95 // fit interval is undefined. 96 // - It appears the Toe is aligned with the beginning of transmit. 97 // - It is assumed data should not be used prior to transmit. 98 // - The transmission period appears to be one hour. 99 // However, SVs set unhealthy have been observed to "coast" 100 // on a single message for more than a day. 101 // As a result of all this: 102 // beginValid is set to the ctToe or the actual time of receipt, 103 // whichever is later. 104 // endValid is set a week in the future. This is based on the 105 // fact that the ctToe is based on a a half-week assumption in the 106 // message, so there MUST be a new message within a half-week or 107 // a piece of user equipmeent collectin the message fo the first 108 // time will not derive a correct ctToe. 109 // @throw Invalid Request if the required data has not been stored. adjustValidity()110 void BDSEphemeris::adjustValidity() 111 { 112 try { 113 OrbitEph::adjustValidity(); // for dataLoaded check 114 115 beginValid = ctToe; // Default case 116 117 // If elements were updated during the hour, then 118 // we want to use the later time. 119 if (transmitTime>beginValid) beginValid = transmitTime; 120 endValid = ctToe + (SEC_PER_DAY * 7.0); 121 } 122 catch(Exception& e) { GPSTK_RETHROW(e); } 123 } 124 125 // Dump the orbit, etc information to the given output stream. 126 // @throw Invalid Request if the required data has not been stored. dumpBody(std::ostream & os) const127 void BDSEphemeris::dumpBody(std::ostream& os) const 128 { 129 try { 130 OrbitEph::dumpBody(os); 131 132 os << " BeiDou-SPECIFIC PARAMETERS\n" 133 << scientific << setprecision(8) 134 << "Tgd (B1/B3) : " << setw(16) << Tgd13 << " meters" << endl 135 << "Tgd (B2/B3) : " << setw(16) << Tgd23 << " meters" << endl 136 << "HOW time : " << setw(6) << HOWtime << " (sec of BDS week " 137 << setw(4) << static_cast<BDSWeekSecond>(ctToe).getWeek() << ")" 138 << " fitDuration: " << setw(2) << fitDuration << " hours" << endl 139 << "TransmitTime: " << OrbitEph::timeDisplay(transmitTime) << endl 140 << "Accuracy : " << fixed << setprecision(2) 141 << getAccuracy() << " meters" << endl 142 << "IODC: " << IODC << " IODE: " << IODE << " health: " << health 143 << endl; 144 } 145 catch(Exception& e) { GPSTK_RETHROW(e); } 146 } 147 dumpTerse(std::ostream & os) const148 void BDSEphemeris::dumpTerse(std::ostream& os) const 149 { 150 string tform = "%03j %02H:%02M:%02S"; 151 try 152 { 153 os << " " << setw(3) << satID.id << " ! "; 154 os << printTime(transmitTime,tform) << " ! " 155 << printTime(ctToe,tform) << " ! " 156 << printTime(endValid,tform) << " !" 157 << fixed << setprecision(2) 158 << setw(6) << getAccuracy() << "!" 159 << setw(4) << IODC << "!" 160 << setw(4) << IODE << "!" 161 << setw(6) << health << "!" << endl; 162 } 163 catch(Exception& e) { GPSTK_RETHROW(e); } 164 } 165 166 // BDS is different in that some satellites are in GEO orbits. 167 // According to the ICD, the 168 // SV position derivation for MEO and IGSO is identical to 169 // that for other kepler+perturbation systems (e.g. GPS); however, 170 // the position derivation for the GEO SVs is different. 171 // According to the ICD, the GEO SVs are those with PRNs 1-5. 172 // The following method overrides OrbitEph.svXvt( ). It uses 173 // OrbitEph::svXvt( ) for PRNs above 5, but implements a different 174 // algorithm for PRNs 1-5. svXvt(const CommonTime & t) const175 Xvt BDSEphemeris::svXvt(const CommonTime& t) const 176 { 177 if(!dataLoadedFlag) 178 GPSTK_THROW(InvalidRequest("Data not loaded")); 179 180 // If the inclination is more than 7 degrees, this 181 // is a MEO or IGSO SV and use the standard OrbitEph 182 // version of svXvt 183 if ((i0*180./gpstk::PI) > 7.0) 184 return(OrbitEph::svXvt(t)); 185 186 // If PRN ID is in the range 1-5, treat this as a GEO 187 // 188 // The initial calculations are identical to the standard 189 // Kepler+preturbation model 190 Xvt sv; 191 double ea; // eccentric anomaly 192 double delea; // delta eccentric anomaly during iteration 193 double elapte; // elapsed time since Toe 194 //double elaptc; // elapsed time since Toc 195 //double dtc,dtr; 196 double q,sinea,cosea; 197 double GSTA,GCTA; 198 double amm; 199 double meana; // mean anomaly 200 double F,G; // temporary real variables 201 double alat,talat,c2al,s2al,du,dr,di,U,R,truea,AINC; 202 double ANLON,cosu,sinu,xip,yip,can,san,cinc,sinc; 203 double dek,dlk,div,duv,drv; 204 double dxp,dyp; 205 double xGK,yGK,zGK; 206 207 WGS84Ellipsoid ell; 208 double sqrtgm = SQRT(ell.gm()); 209 double twoPI = 2.0e0 * PI; 210 double lecc; // eccentricity 211 double tdrinc; // dt inclination 212 double Ahalf = SQRT(A); // A is semi-major axis of orbit 213 double ToeSOW = GPSWeekSecond(ctToe).sow; // SOW is time-system-independent 214 215 lecc = ecc; 216 tdrinc = idot; 217 218 // Compute time since ephemeris & clock epochs 219 elapte = t - ctToe; 220 221 // Compute mean motion 222 amm = (sqrtgm / (A*Ahalf)) + dn; 223 224 // In-plane angles 225 // meana - Mean anomaly 226 // ea - Eccentric anomaly 227 // truea - True anomaly 228 meana = M0 + elapte * amm; 229 meana = fmod(meana, twoPI); 230 ea = meana + lecc * ::sin(meana); 231 232 int loop_cnt = 1; 233 do { 234 F = meana - (ea - lecc * ::sin(ea)); 235 G = 1.0 - lecc * ::cos(ea); 236 delea = F/G; 237 ea = ea + delea; 238 loop_cnt++; 239 } while ((fabs(delea) > 1.0e-11) && (loop_cnt <= 20)); 240 241 // Compute clock corrections 242 sv.relcorr = svRelativity(t); 243 sv.clkbias = svClockBias(t); 244 sv.clkdrift = svClockDrift(t); 245 sv.frame = ReferenceFrame::WGS84; 246 247 // Compute true anomaly 248 q = SQRT(1.0e0 - lecc*lecc); 249 sinea = ::sin(ea); 250 cosea = ::cos(ea); 251 G = 1.0e0 - lecc * cosea; 252 253 // G*SIN(TA) AND G*COS(TA) 254 GSTA = q * sinea; 255 GCTA = cosea - lecc; 256 257 // True anomaly 258 truea = atan2 (GSTA, GCTA); 259 260 // Argument of lat and correction terms (2nd harmonic) 261 alat = truea + w; 262 talat = 2.0e0 * alat; 263 c2al = ::cos(talat); 264 s2al = ::sin(talat); 265 266 du = c2al * Cuc + s2al * Cus; 267 dr = c2al * Crc + s2al * Crs; 268 di = c2al * Cic + s2al * Cis; 269 270 // U = updated argument of lat, R = radius, AINC = inclination 271 U = alat + du; 272 R = A*G + dr; 273 AINC = i0 + tdrinc * elapte + di; 274 275 // At this point, the ICD formulation diverges to something 276 // different. 277 // Longitude of ascending node (ANLON) 278 ANLON = OMEGA0 + OMEGAdot * elapte 279 - ell.angVelocity() * ToeSOW; 280 281 // In plane location 282 cosu = ::cos(U); 283 sinu = ::sin(U); 284 xip = R * cosu; 285 yip = R * sinu; 286 287 // Angles for rotation 288 can = ::cos(ANLON); 289 san = ::sin(ANLON); 290 cinc = ::cos(AINC); 291 sinc = ::sin(AINC); 292 293 // GEO satellite coordinates in user-defined inertial system 294 xGK = xip*can - yip*cinc*san; 295 yGK = xip*san + yip*cinc*can; 296 zGK = yip*sinc; 297 298 // Rz matrix 299 double angleZ = ell.angVelocity() * elapte; 300 double cosZ = ::cos(angleZ); 301 double sinZ = ::sin(angleZ); 302 303 // Initiailize 3X3 with all 0.0 304 gpstk::Matrix<double> matZ(3,3); 305 // Row,Col 306 matZ(0,0) = cosZ; 307 matZ(0,1) = sinZ; 308 matZ(0,2) = 0.0; 309 matZ(1,0) = -sinZ; 310 matZ(1,1) = cosZ; 311 matZ(1,2) = 0.0; 312 matZ(2,0) = 0.0; 313 matZ(2,1) = 0.0; 314 matZ(2,2) = 1.0; 315 316 // Rx matrix 317 double angleX = -5.0 * PI/180.0; /// This is a constant. Should set it once 318 double cosX = ::cos(angleX); 319 double sinX = ::sin(angleX); 320 gpstk::Matrix<double> matX(3,3); 321 matX(0,0) = 1.0; 322 matX(0,1) = 0.0; 323 matX(0,2) = 0.0; 324 matX(1,0) = 0.0; 325 matX(1,1) = cosX; 326 matX(1,2) = sinX; 327 matX(2,0) = 0.0; 328 matX(2,1) = -sinX; 329 matX(2,2) = cosX; 330 331 // Matrix (single column) of xGK, yGK, zGK 332 gpstk::Matrix<double> inertialPos(3,1); 333 inertialPos(0,0) = xGK; 334 inertialPos(1,0) = yGK; 335 inertialPos(2,0) = zGK; 336 337 gpstk::Matrix<double> result(3,1); 338 result = matZ * matX * inertialPos; 339 340 sv.x[0] = result(0,0); 341 sv.x[1] = result(1,0); 342 sv.x[2] = result(2,0); 343 344 // derivatives of true anamoly and arg of latitude 345 dek = amm / G; 346 dlk = amm * q / (G*G); 347 348 // in-plane, cross-plane, and radial derivatives 349 div = tdrinc - 2.0e0 * dlk * (Cic * s2al - Cis * c2al); 350 duv = dlk*(1.e0+ 2.e0 * (Cus*c2al - Cuc*s2al)); 351 drv = A * lecc * dek * sinea + 2.e0 * dlk * (Crs * c2al - Crc * s2al) + 352 Adot * G; 353 354 // 355 dxp = drv*cosu - R*sinu*duv; 356 dyp = drv*sinu + R*cosu*duv; 357 358 // Time-derivative of Rz matrix 359 gpstk::Matrix<double> dmatZ(3,3); 360 // Row,Col 361 dmatZ(0,0) = sinZ * -ell.angVelocity(); 362 dmatZ(0,1) = -cosZ * -ell.angVelocity(); 363 dmatZ(0,2) = 0.0; 364 dmatZ(1,0) = cosZ * -ell.angVelocity(); 365 dmatZ(1,1) = sinZ * -ell.angVelocity(); 366 dmatZ(1,2) = 0.0; 367 dmatZ(2,0) = 0.0; 368 dmatZ(2,1) = 0.0; 369 dmatZ(2,2) = 0.0; 370 371 // Time-derivative of X,Y,Z in interial form 372 gpstk::Matrix<double> dIntPos(3,1); 373 dIntPos(0,0) = - xip * san * OMEGAdot 374 + dxp * can 375 - yip * (cinc * can * OMEGAdot 376 -sinc * san * div ) 377 - dyp * cinc * san; 378 dIntPos(1,0) = xip * can * OMEGAdot 379 + dxp * san 380 - yip * (cinc * san * OMEGAdot 381 +sinc * can * div ) 382 + dyp * cinc * can; 383 dIntPos(2,0) = yip * cinc * div + dyp * sinc; 384 385 /* 386 cout << "dIntPos : " << dIntPos(0,0) 387 << ", " << dIntPos(1,0) 388 << ", " << dIntPos(2,0) << endl; 389 double vMag = ::sqrt(dIntPos(0,0)*dIntPos(0,0) + 390 dIntPos(1,0)*dIntPos(1,0) + 391 dIntPos(2,0)*dIntPos(2,0) ); 392 cout << " dIntPos Mag: " << vMag << endl; 393 cout << "dmatZ : " << dmatZ(0,0) 394 << ", " << dmatZ(0,1) 395 << ", " << dmatZ(0,2) << endl; 396 cout << "dmatZ : " << dmatZ(1,0) 397 << ", " << dmatZ(1,1) 398 << ", " << dmatZ(1,2) << endl; 399 cout << "dmatZ : " << dmatZ(2,0) 400 << ", " << dmatZ(2,1) 401 << ", " << dmatZ(2,2) << endl; 402 */ 403 gpstk::Matrix<double> vresult(3,1); 404 vresult = matZ * matX * dIntPos + 405 dmatZ * matX * inertialPos; 406 407 /* debug 408 gpstk::Matrix<double> firstHalf(3,1); 409 firstHalf = matZ * matX * dIntPos; 410 gpstk::Matrix<double> secondHalf(3,1); 411 secondHalf = dmatZ * matX * inertialPos; 412 413 cout << "firstHalf: " << firstHalf(0,0) 414 << ", " << firstHalf(1,0) 415 << ", " << firstHalf(2,0) << endl; 416 cout << "secondHalf: " << secondHalf(0,0) 417 << ", " << secondHalf(1,0) 418 << ", " << secondHalf(2,0) << endl; 419 end debug */ 420 421 // Move results into output variables 422 sv.v[0] = vresult(0,0); 423 sv.v[1] = vresult(1,0); 424 sv.v[2] = vresult(2,0); 425 sv.health = health == 0 ? Xvt::Healthy : Xvt::Unhealthy; 426 427 return sv; 428 } 429 430 } // end namespace 431