Changeset 6214 in ntrip for branches/BNC_2.11.0/src/RTCM3/ephemeris.cpp
- Timestamp:
- Oct 7, 2014, 10:40:33 AM (9 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
branches/BNC_2.11.0/src/RTCM3/ephemeris.cpp
r5542 r6214 88 88 // Compute GPS Satellite Position (virtual) 89 89 //////////////////////////////////////////////////////////////////////////// 90 void t_ephGPS::position(int GPSweek, double GPSweeks, 90 t_irc t_ephGPS::position(int GPSweek, double GPSweeks, 91 91 double* xc, 92 92 double* vv) const { … … 101 101 double a0 = _sqrt_A * _sqrt_A; 102 102 if (a0 == 0) { 103 return ;103 return failure; 104 104 } 105 105 … … 126 126 double xp = r*cos(u); 127 127 double yp = r*sin(u); 128 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - 128 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - 129 129 omegaEarth*_TOEsec; 130 130 131 131 double sinom = sin(OM); 132 132 double cosom = cos(OM); … … 135 135 xc[0] = xp*cosom - yp*cosi*sinom; 136 136 xc[1] = xp*sinom + yp*cosi*cosom; 137 xc[2] = yp*sini; 138 137 xc[2] = yp*sini; 138 139 139 double tc = tt - _TOC; 140 140 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc; … … 144 144 double tanv2 = tan(v/2); 145 145 double dEdM = 1 / (1 - _e*cos(E)); 146 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) 146 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) 147 147 * dEdM * n; 148 148 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv; 149 149 double dotom = _OMEGADOT - omegaEarth; 150 150 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv; 151 double dotr = a0 * _e*sin(E) * dEdM * n 151 double dotr = a0 * _e*sin(E) * dEdM * n 152 152 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv; 153 153 double dotx = dotr*cos(u) - r*sin(u)*dotu; … … 167 167 // ----------------------- 168 168 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c; 169 170 return success; 169 171 } 170 172 … … 274 276 /static_cast<double>(1<<13)) 275 277 GPSADDBITSFLOAT(8, _TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1)) 276 GPSADDBITS(6, _health) 278 GPSADDBITS(6, _health) 277 279 GPSADDBITS(1, _L2PFlag) 278 280 GPSADDBITS(1, 0) /* GPS fit interval */ … … 299 301 ColumnVector vv = xv.rows(4,6); 300 302 301 // Acceleration 303 // Acceleration 302 304 // ------------ 303 305 static const double gmWGS = 398.60044e12; … … 319 321 va(2) = vv(2); 320 322 va(3) = vv(3); 321 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0]; 322 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1]; 323 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0]; 324 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1]; 323 325 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2]; 324 326 … … 328 330 // Compute Glonass Satellite Position (virtual) 329 331 //////////////////////////////////////////////////////////////////////////// 330 void t_ephGlo::position(int GPSweek, double GPSweeks, 332 t_irc t_ephGlo::position(int GPSweek, double GPSweeks, 331 333 double* xc, double* vv) const { 332 334 … … 337 339 338 340 double dtPos = bncTime(GPSweek, GPSweeks) - _tt; 341 342 if (fabs(dtPos) > 24*3600.0) { 343 return failure; 344 } 339 345 340 346 int nSteps = int(fabs(dtPos) / nominalStep) + 1; … … 345 351 acc[1] = _y_acceleration * 1.e3; 346 352 acc[2] = _z_acceleration * 1.e3; 347 for (int ii = 1; ii <= nSteps; ii++) { 353 for (int ii = 1; ii <= nSteps; ii++) { 348 354 _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv); 349 355 _tt = _tt + step; … … 364 370 double dtClk = bncTime(GPSweek, GPSweeks) - _TOC; 365 371 xc[3] = -_tau + _gamma * dtClk; 372 373 return success; 366 374 } 367 375 … … 382 390 383 391 int ww = ee->GPSWeek; 384 int tow = ee->GPSTOW; 392 int tow = ee->GPSTOW; 385 393 updatetime(&ww, &tow, ee->tb*1000, 0); // Moscow -> GPS 386 394 387 // Check the day once more 395 // Check the day once more 388 396 // ----------------------- 389 397 bool timeChanged = false; … … 418 426 if (false && timeChanged && BNC_CORE->mode() == t_bncCore::batchPostProcessing) { 419 427 bncTime newHTime(ww, (double) tow); 420 cout << "GLONASS " << ee->almanac_number << " Time Changed at " 421 << currentTime.datestr() << " " << currentTime.timestr() 428 cout << "GLONASS " << ee->almanac_number << " Time Changed at " 429 << currentTime.datestr() << " " << currentTime.timestr() 422 430 << endl 423 << "old: " << hTime.datestr() << " " << hTime.timestr() 431 << "old: " << hTime.datestr() << " " << hTime.timestr() 424 432 << endl 425 << "new: " << newHTime.datestr() << " " << newHTime.timestr() 433 << "new: " << newHTime.datestr() << " " << newHTime.timestr() 426 434 << endl 427 << "eph: " << ee->GPSWeek << " " << ee->GPSTOW << " " << ee->tb 435 << "eph: " << ee->GPSWeek << " " << ee->GPSTOW << " " << ee->tb 428 436 << endl 429 << "ww, tow (old): " << ww_old << " " << tow_old 437 << "ww, tow (old): " << ww_old << " " << tow_old 430 438 << endl 431 << "ww, tow (new): " << ww << " " << tow 439 << "ww, tow (new): " << ww << " " << tow 432 440 << endl << endl; 433 441 } … … 444 452 _gamma = ee->gamma; 445 453 _x_pos = ee->x_pos; 446 _x_velocity = ee->x_velocity; 454 _x_velocity = ee->x_velocity; 447 455 _x_acceleration = ee->x_acceleration; 448 _y_pos = ee->y_pos; 449 _y_velocity = ee->y_velocity; 456 _y_pos = ee->y_pos; 457 _y_velocity = ee->y_velocity; 450 458 _y_acceleration = ee->y_acceleration; 451 _z_pos = ee->z_pos; 452 _z_velocity = ee->z_velocity; 459 _z_pos = ee->z_pos; 460 _z_velocity = ee->z_velocity; 453 461 _z_acceleration = ee->z_acceleration; 454 462 _health = 0; … … 460 468 _tt = _TOC; 461 469 462 _xv(1) = _x_pos * 1.e3; 463 _xv(2) = _y_pos * 1.e3; 464 _xv(3) = _z_pos * 1.e3; 465 _xv(4) = _x_velocity * 1.e3; 466 _xv(5) = _y_velocity * 1.e3; 467 _xv(6) = _z_velocity * 1.e3; 470 _xv(1) = _x_pos * 1.e3; 471 _xv(2) = _y_pos * 1.e3; 472 _xv(3) = _z_pos * 1.e3; 473 _xv(4) = _x_velocity * 1.e3; 474 _xv(5) = _y_velocity * 1.e3; 475 _xv(6) = _z_velocity * 1.e3; 468 476 469 477 _ok = true; … … 513 521 GLONASSADDBITS(6, (static_cast<int>(_tki)/60)%60) 514 522 GLONASSADDBITS(1, (static_cast<int>(_tki)/30)%30) 515 GLONASSADDBITS(1, _health) 523 GLONASSADDBITS(1, _health) 516 524 GLONASSADDBITS(1, 0) 517 525 unsigned long long timeofday = (static_cast<int>(_tt.gpssec()+3*60*60-_gps_utc)%86400); … … 581 589 582 590 _TOEsec = _TOC.gpssec(); 583 //// _TOEsec = ee->TOE; //// TODO: 591 //// _TOEsec = ee->TOE; //// TODO: 584 592 _Cic = ee->Cic; 585 593 _OMEGA0 = ee->OMEGA0; … … 609 617 // Compute Galileo Satellite Position (virtual) 610 618 //////////////////////////////////////////////////////////////////////////// 611 void t_ephGal::position(int GPSweek, double GPSweeks, 619 t_irc t_ephGal::position(int GPSweek, double GPSweeks, 612 620 double* xc, 613 621 double* vv) const { … … 621 629 double a0 = _sqrt_A * _sqrt_A; 622 630 if (a0 == 0) { 623 return ;631 return failure; 624 632 } 625 633 … … 646 654 double xp = r*cos(u); 647 655 double yp = r*sin(u); 648 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - 656 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - 649 657 omegaEarth*_TOEsec; 650 658 651 659 double sinom = sin(OM); 652 660 double cosom = cos(OM); … … 655 663 xc[0] = xp*cosom - yp*cosi*sinom; 656 664 xc[1] = xp*sinom + yp*cosi*cosom; 657 xc[2] = yp*sini; 658 665 xc[2] = yp*sini; 666 659 667 double tc = tt - _TOC; 660 668 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc; … … 664 672 double tanv2 = tan(v/2); 665 673 double dEdM = 1 / (1 - _e*cos(E)); 666 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) 674 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) 667 675 * dEdM * n; 668 676 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv; 669 677 double dotom = _OMEGADOT - omegaEarth; 670 678 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv; 671 double dotr = a0 * _e*sin(E) * dEdM * n 679 double dotr = a0 * _e*sin(E) * dEdM * n 672 680 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv; 673 681 double dotx = dotr*cos(u) - r*sin(u)*dotu; … … 688 696 // xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E); 689 697 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c; 698 699 return success; 690 700 } 691 701 … … 806 816 int year, month, day, hour, min; 807 817 double sec; 808 818 809 819 in >> _prn >> year >> month >> day >> hour >> min >> sec; 810 820 … … 812 822 _prn = QString("G%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0')); 813 823 } 814 824 815 825 if (year < 80) { 816 826 year += 2000; … … 926 936 int year, month, day, hour, min; 927 937 double sec; 928 938 929 939 in >> _prn >> year >> month >> day >> hour >> min >> sec; 930 940 … … 932 942 _prn = QString("R%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0')); 933 943 } 934 944 935 945 if (year < 80) { 936 946 year += 2000; … … 985 995 // ------------------------ 986 996 _tt = _TOC; 987 _xv.ReSize(6); 988 _xv(1) = _x_pos * 1.e3; 989 _xv(2) = _y_pos * 1.e3; 990 _xv(3) = _z_pos * 1.e3; 991 _xv(4) = _x_velocity * 1.e3; 992 _xv(5) = _y_velocity * 1.e3; 993 _xv(6) = _z_velocity * 1.e3; 997 _xv.ReSize(6); 998 _xv(1) = _x_pos * 1.e3; 999 _xv(2) = _y_pos * 1.e3; 1000 _xv(3) = _z_pos * 1.e3; 1001 _xv(4) = _x_velocity * 1.e3; 1002 _xv(5) = _y_velocity * 1.e3; 1003 _xv(6) = _z_velocity * 1.e3; 994 1004 995 1005 _ok = true; … … 1028 1038 int year, month, day, hour, min; 1029 1039 double sec; 1030 1040 1031 1041 in >> _prn >> year >> month >> day >> hour >> min >> sec; 1032 1042 … … 1034 1044 _prn = QString("E%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0')); 1035 1045 } 1036 1046 1037 1047 if (year < 80) { 1038 1048 year += 2000; … … 1113 1123 } 1114 1124 1115 // 1125 // 1116 1126 ////////////////////////////////////////////////////////////////////////////// 1117 1127 QString t_eph::rinexDateStr(const bncTime& tt, const QString& prn, … … 1119 1129 1120 1130 QString datStr; 1121 1131 1122 1132 unsigned year, month, day, hour, min; 1123 1133 double sec; 1124 1134 tt.civil_date(year, month, day); 1125 1135 tt.civil_time(hour, min, sec); 1126 1136 1127 1137 QTextStream out(&datStr); 1128 1138 … … 1155 1165 1156 1166 QString rnxStr = rinexDateStr(_TOC, _prn, version); 1157 1167 1158 1168 QTextStream out(&rnxStr); 1159 1169 1160 1170 out << QString("%1%2%3\n") 1161 1171 .arg(_clock_bias, 19, 'e', 12)
Note:
See TracChangeset
for help on using the changeset viewer.