 Timestamp:
 Oct 7, 2014, 10:40:33 AM (8 years ago)
 Location:
 branches/BNC_2.11.0/src/RTCM3
 Files:

 2 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.05.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0]; 322 va(5) = (t1 + t2*(1.05.0*z2/(rho*rho)) + t3) * rr(2)  t4*vv(1) + acc[1]; 323 va(4) = (t1 + t2*(1.05.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0]; 324 va(5) = (t1 + t2*(1.05.0*z2/(rho*rho)) + t3) * rr(2)  t4*vv(1) + acc[1]; 323 325 va(6) = (t1 + t2*(3.05.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.442807633e10 * _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) 
branches/BNC_2.11.0/src/RTCM3/ephemeris.h
r5541 r6214 7 7 #include <string> 8 8 #include "bnctime.h" 9 #include "bncconst.h" 9 10 extern "C" { 10 11 #include "rtcm3torinex.h" … … 25 26 virtual e_type type() const = 0; 26 27 virtual QString toString(double version) const = 0; 27 virtual void position(int GPSweek, double GPSweeks,28 virtual t_irc position(int GPSweek, double GPSweeks, 28 29 double* xc, double* vv) const = 0; 29 30 virtual int IOD() const = 0; … … 38 39 const QDateTime& receptDateTime() const {return _receptDateTime;} 39 40 40 void position(int GPSweek, double GPSweeks,41 t_irc position(int GPSweek, double GPSweeks, 41 42 double& xx, double& yy, double& zz, double& cc) const { 42 43 double tmp_xx[4]; 43 44 double tmp_vv[4]; 44 position(GPSweek, GPSweeks, tmp_xx, tmp_vv); 45 45 if (position(GPSweek, GPSweeks, tmp_xx, tmp_vv) != success) { 46 return failure; 47 } 46 48 xx = tmp_xx[0]; 47 49 yy = tmp_xx[1]; 48 50 zz = tmp_xx[2]; 49 51 cc = tmp_xx[3]; 52 53 return success; 50 54 } 51 55 … … 53 57 double version); 54 58 55 protected: 59 protected: 56 60 QString _prn; 57 61 bncTime _TOC; … … 73 77 void set(const gpsephemeris* ee); 74 78 75 virtual void position(int GPSweek, double GPSweeks,79 virtual t_irc position(int GPSweek, double GPSweeks, 76 80 double* xc, 77 81 double* vv) const; … … 84 88 85 89 private: 86 double _clock_bias; // [s] 87 double _clock_drift; // [s/s] 90 double _clock_bias; // [s] 91 double _clock_drift; // [s/s] 88 92 double _clock_driftrate; // [s/s^2] 89 93 90 double _IODE; 91 double _Crs; // [m] 94 double _IODE; 95 double _Crs; // [m] 92 96 double _Delta_n; // [rad/s] 93 double _M0; // [rad] 94 95 double _Cuc; // [rad] 96 double _e; // 97 double _Cus; // [rad] 97 double _M0; // [rad] 98 99 double _Cuc; // [rad] 100 double _e; // 101 double _Cus; // [rad] 98 102 double _sqrt_A; // [m^0.5] 99 103 100 double _TOEsec; // [s] 101 double _Cic; // [rad] 102 double _OMEGA0; // [rad] 103 double _Cis; // [rad] 104 105 double _i0; // [rad] 106 double _Crc; // [m] 107 double _omega; // [rad] 104 double _TOEsec; // [s] 105 double _Cic; // [rad] 106 double _OMEGA0; // [rad] 107 double _Cis; // [rad] 108 109 double _i0; // [rad] 110 double _Crc; // [m] 111 double _omega; // [rad] 108 112 double _OMEGADOT; // [rad/s] 109 113 110 114 double _IDOT; // [rad/s] 111 double _L2Codes; // Codes on L2 channel 115 double _L2Codes; // Codes on L2 channel 112 116 double _TOEweek; 113 117 double _L2PFlag; // L2 P data flag … … 115 119 double _ura; // SV accuracy 116 120 double _health; // SV health 117 double _TGD; // [s] 118 double _IODC; 119 120 double _TOT; // Transmisstion time 121 double _TGD; // [s] 122 double _IODC; 123 124 double _TOT; // Transmisstion time 121 125 double _fitInterval; // Fit interval 122 126 }; … … 133 137 virtual QString toString(double version) const; 134 138 135 virtual void position(int GPSweek, double GPSweeks,139 virtual t_irc position(int GPSweek, double GPSweeks, 136 140 double* xc, 137 141 double* vv) const; … … 149 153 double* acc); 150 154 151 mutable bncTime _tt; // time 155 mutable bncTime _tt; // time 152 156 mutable ColumnVector _xv; // status vector (position, velocity) at time _tt 153 157 154 158 double _gps_utc; 155 double _tau; // [s] 156 double _gamma; // 159 double _tau; // [s] 160 double _gamma; // 157 161 double _tki; // message frame time 158 162 159 double _x_pos; // [km] 160 double _x_velocity; // [km/s] 161 double _x_acceleration; // [km/s^2] 162 double _health; // 0 = O.K. 163 164 double _y_pos; // [km] 165 double _y_velocity; // [km/s] 166 double _y_acceleration; // [km/s^2] 167 double _frequency_number; // ICDGLONASS data position 168 169 double _z_pos; // [km] 170 double _z_velocity; // [km/s] 171 double _z_acceleration; // [km/s^2] 172 double _E; // Age of Information [days] 163 double _x_pos; // [km] 164 double _x_velocity; // [km/s] 165 double _x_acceleration; // [km/s^2] 166 double _health; // 0 = O.K. 167 168 double _y_pos; // [km] 169 double _y_velocity; // [km/s] 170 double _y_acceleration; // [km/s^2] 171 double _frequency_number; // ICDGLONASS data position 172 173 double _z_pos; // [km] 174 double _z_velocity; // [km/s] 175 double _z_acceleration; // [km/s^2] 176 double _E; // Age of Information [days] 173 177 }; 174 178 … … 185 189 void set(const galileoephemeris* ee); 186 190 187 virtual void position(int GPSweek, double GPSweeks,191 virtual t_irc position(int GPSweek, double GPSweeks, 188 192 double* xc, 189 193 double* vv) const; … … 194 198 195 199 private: 196 double _clock_bias; // [s] 197 double _clock_drift; // [s/s] 200 double _clock_bias; // [s] 201 double _clock_drift; // [s/s] 198 202 double _clock_driftrate; // [s/s^2] 199 203 200 double _IODnav; 201 double _Crs; // [m] 204 double _IODnav; 205 double _Crs; // [m] 202 206 double _Delta_n; // [rad/s] 203 double _M0; // [rad] 204 205 double _Cuc; // [rad] 206 double _e; // 207 double _Cus; // [rad] 207 double _M0; // [rad] 208 209 double _Cuc; // [rad] 210 double _e; // 211 double _Cus; // [rad] 208 212 double _sqrt_A; // [m^0.5] 209 213 210 double _TOEsec; // [s] 211 double _Cic; // [rad] 212 double _OMEGA0; // [rad] 213 double _Cis; // [rad] 214 215 double _i0; // [rad] 216 double _Crc; // [m] 217 double _omega; // [rad] 214 double _TOEsec; // [s] 215 double _Cic; // [rad] 216 double _OMEGA0; // [rad] 217 double _Cis; // [rad] 218 219 double _i0; // [rad] 220 double _Crc; // [m] 221 double _omega; // [rad] 218 222 double _OMEGADOT; // [rad/s] 219 223 … … 226 230 double _E5aHS; // E5a Health Status 227 231 double _E5bHS; // E5a Health Status 228 double _BGD_1_5A; // group delay [s] 229 double _BGD_1_5B; // group delay [s] 232 double _BGD_1_5A; // group delay [s] 233 double _BGD_1_5B; // group delay [s] 230 234 231 235 double _TOT; // [s]
Note:
See TracChangeset
for help on using the changeset viewer.