Changeset 7527 in ntrip
- Timestamp:
- Oct 20, 2015, 11:12:13 AM (9 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/BNC/src/bncutils.cpp
r7260 r7527 35 35 * Created: 30-Aug-2006 36 36 * 37 * Changes: 37 * Changes: 38 38 * 39 39 * -----------------------------------------------------------------------*/ … … 145 145 // *secOfWeek -= 24*60*60; 146 146 147 // new version 147 // new version 148 148 if(mSecOfWeek < 4*60*60*1000 && glo_timeofday > 20*60*60) 149 149 *secOfWeek += 24*60*60; … … 158 158 } 159 159 160 // 160 // 161 161 //////////////////////////////////////////////////////////////////////////// 162 162 void expandEnvVar(QString& str) { … … 188 188 } 189 189 190 // 190 // 191 191 //////////////////////////////////////////////////////////////////////////// 192 192 QDateTime dateAndTimeFromGPSweek(int GPSWeek, double GPSWeeks) { 193 193 194 194 static const QDate zeroEpoch(1980, 1, 6); 195 195 196 196 QDate date(zeroEpoch); 197 197 QTime time(0,0,0,0); … … 204 204 } 205 205 206 // 206 // 207 207 //////////////////////////////////////////////////////////////////////////// 208 208 void currentGPSWeeks(int& week, double& sec) { … … 216 216 currDateTimeGPS = QDateTime::currentDateTime().toUTC(); 217 217 QDate hlp = currDateTimeGPS.date(); 218 currDateTimeGPS = currDateTimeGPS.addSecs(gnumleap(hlp.year(), 218 currDateTimeGPS = currDateTimeGPS.addSecs(gnumleap(hlp.year(), 219 219 hlp.month(), hlp.day())); 220 220 } … … 225 225 week = int( (double(currDateGPS.toJulianDay()) - 2444244.5) / 7 ); 226 226 227 sec = (currDateGPS.dayOfWeek() % 7) * 24.0 * 3600.0 + 228 currTimeGPS.hour() * 3600.0 + 229 currTimeGPS.minute() * 60.0 + 227 sec = (currDateGPS.dayOfWeek() % 7) * 24.0 * 3600.0 + 228 currTimeGPS.hour() * 3600.0 + 229 currTimeGPS.minute() * 60.0 + 230 230 currTimeGPS.second() + 231 231 currTimeGPS.msec() / 1000.0; 232 232 } 233 233 234 // 234 // 235 235 //////////////////////////////////////////////////////////////////////////// 236 236 QDateTime currentDateAndTimeGPS() { … … 246 246 } 247 247 248 // 248 // 249 249 //////////////////////////////////////////////////////////////////////////// 250 250 QByteArray ggaString(const QByteArray& latitude, … … 268 268 if (lat < 0.) {lat=lat*(-1.); flagN="S";} 269 269 QTime ttime(QDateTime::currentDateTime().toUTC().time()); 270 int lat_deg = (int)lat; 270 int lat_deg = (int)lat; 271 271 double lat_min=(lat-lat_deg)*60.; 272 int lon_deg = (int)lon; 272 int lon_deg = (int)lon; 273 273 double lon_min=(lon-lon_deg)*60.; 274 274 int hh = 0 , mm = 0; … … 287 287 gga += QString("M,10.000,M,,"); 288 288 int xori; 289 289 290 char XOR = 0; 290 char *Buff =gga.toAscii().data(); 291 char Buff[gga.size()]; 292 strncpy(Buff, gga.toAscii().data(), gga.size()); 291 293 int iLen = strlen(Buff); 292 294 for (xori = 0; xori < iLen; xori++) { … … 298 300 } 299 301 300 // 302 // 301 303 //////////////////////////////////////////////////////////////////////////// 302 304 void RSW_to_XYZ(const ColumnVector& rr, const ColumnVector& vv, … … 337 339 const double e2 = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ; 338 340 const double e2c = (t_CST::aell*t_CST::aell-bell*bell)/(bell*bell) ; 339 341 340 342 double nn, ss, zps, hOld, phiOld, theta, sin3, cos3; 341 343 … … 347 349 348 350 // Closed formula 349 Ell[0] = atan( (XYZ[2] + e2c * bell * sin3) / (ss - e2 * t_CST::aell * cos3) ); 351 Ell[0] = atan( (XYZ[2] + e2c * bell * sin3) / (ss - e2 * t_CST::aell * cos3) ); 350 352 Ell[1] = atan2(XYZ[1],XYZ[0]) ; 351 353 nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ; … … 429 431 } 430 432 431 // 433 // 432 434 //////////////////////////////////////////////////////////////////////////// 433 435 double Frac (double x) { 434 return x-floor(x); 435 } 436 437 // 438 //////////////////////////////////////////////////////////////////////////// 439 double Modulo (double x, double y) { 440 return y*Frac(x/y); 436 return x-floor(x); 437 } 438 439 // 440 //////////////////////////////////////////////////////////////////////////// 441 double Modulo (double x, double y) { 442 return y*Frac(x/y); 441 443 } 442 444 … … 473 475 474 476 475 // Jacobian XYZ --> NEU 477 // Jacobian XYZ --> NEU 476 478 //////////////////////////////////////////////////////////////////////////// 477 479 void jacobiXYZ_NEU(const double* Ell, Matrix& jacobi) { … … 487 489 jacobi(1,2) = - sinPhi * sinLam; 488 490 jacobi(1,3) = cosPhi; 489 490 jacobi(2,1) = - sinLam; 491 492 jacobi(2,1) = - sinLam; 491 493 jacobi(2,2) = cosLam; 492 jacobi(2,3) = 0.0; 493 494 jacobi(3,1) = cosPhi * cosLam; 495 jacobi(3,2) = cosPhi * sinLam; 494 jacobi(2,3) = 0.0; 495 496 jacobi(3,1) = cosPhi * cosLam; 497 jacobi(3,2) = cosPhi * sinLam; 496 498 jacobi(3,3) = sinPhi; 497 499 } … … 524 526 jacobi(3,2) = 0.0; 525 527 jacobi(3,3) = sinPhi; 526 } 528 } 527 529 528 530 // Covariance Matrix in NEU 529 531 //////////////////////////////////////////////////////////////////////////// 530 void covariXYZ_NEU(const SymmetricMatrix& QQxyz, const double* Ell, 532 void covariXYZ_NEU(const SymmetricMatrix& QQxyz, const double* Ell, 531 533 SymmetricMatrix& Qneu) { 532 534 … … 540 542 // Covariance Matrix in XYZ 541 543 //////////////////////////////////////////////////////////////////////////// 542 void covariNEU_XYZ(const SymmetricMatrix& QQneu, const double* Ell, 544 void covariNEU_XYZ(const SymmetricMatrix& QQneu, const double* Ell, 543 545 SymmetricMatrix& Qxyz) { 544 546 … … 558 560 double* acc, // aditional acceleration 559 561 ColumnVector (*der)(double x, const ColumnVector& y, double* acc) 560 // A pointer to a function that computes the 562 // A pointer to a function that computes the 561 563 // derivative of a function at a point (x,y) 562 564 ) { … … 568 570 569 571 ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0; 570 572 571 573 return yf; 572 574 } 573 // 575 // 574 576 //////////////////////////////////////////////////////////////////////////// 575 577 double djul(long jj, long mm, double tt) { … … 579 581 jj = jj - 1; 580 582 mm = mm + 12; 581 } 583 } 582 584 ii = jj/100; 583 585 kk = 2 - ii + ii/4; … … 585 587 djul = djul + floor( 30.6001*(mm + 1) ) + tt + kk; 586 588 return djul; 587 } 588 589 // 589 } 590 591 // 590 592 //////////////////////////////////////////////////////////////////////////// 591 593 double gpjd(double second, int nweek) { … … 593 595 deltat = nweek*7.0 + second/86400.0 ; 594 596 return( 44244.0 + deltat) ; 595 } 596 597 // 597 } 598 599 // 598 600 //////////////////////////////////////////////////////////////////////////// 599 601 void jdgp(double tjul, double & second, long & nweek) { … … 604 606 } 605 607 606 // 608 // 607 609 //////////////////////////////////////////////////////////////////////////// 608 610 void jmt(double djul, long& jj, long& mm, double& dd) { … … 622 624 jj = ih1; 623 625 if ( mm <= 2 ) jj = jj + 1; 624 } 625 626 // 627 //////////////////////////////////////////////////////////////////////////// 628 void GPSweekFromDateAndTime(const QDateTime& dateTime, 626 } 627 628 // 629 //////////////////////////////////////////////////////////////////////////// 630 void GPSweekFromDateAndTime(const QDateTime& dateTime, 629 631 int& GPSWeek, double& GPSWeeks) { 630 632 631 633 static const QDateTime zeroEpoch(QDate(1980, 1, 6),QTime(),Qt::UTC); 632 634 633 635 GPSWeek = zeroEpoch.daysTo(dateTime) / 7; 634 636 … … 637 639 638 640 GPSWeeks = (weekDay - 1) * 86400.0 639 - dateTime.time().msecsTo(QTime()) / 1e3; 640 } 641 642 // 641 - dateTime.time().msecsTo(QTime()) / 1e3; 642 } 643 644 // 643 645 //////////////////////////////////////////////////////////////////////////// 644 646 void GPSweekFromYMDhms(int year, int month, int day, int hour, int min, … … 650 652 jdgp(mjd, GPSWeeks, GPSWeek_long); 651 653 GPSWeek = GPSWeek_long; 652 GPSWeeks += hour * 3600.0 + min * 60.0 + sec; 653 } 654 655 // 654 GPSWeeks += hour * 3600.0 + min * 60.0 + sec; 655 } 656 657 // 656 658 //////////////////////////////////////////////////////////////////////////// 657 659 void mjdFromDateAndTime(const QDateTime& dateTime, int& mjd, double& dayfrac) { … … 663 665 dayfrac = (dateTime.time().hour() + 664 666 (dateTime.time().minute() + 665 (dateTime.time().second() + 667 (dateTime.time().second() + 666 668 dateTime.time().msec() / 1000.0) / 60.0) / 60.0) / 24.0; 667 669 } 668 670 669 // 671 // 670 672 //////////////////////////////////////////////////////////////////////////// 671 673 bool findInVector(const vector<QString>& vv, const QString& str) { … … 679 681 } 680 682 681 // 683 // 682 684 //////////////////////////////////////////////////////////////////////////// 683 685 int readInt(const QString& str, int pos, int len, int& value) { … … 687 689 } 688 690 689 // 691 // 690 692 //////////////////////////////////////////////////////////////////////////// 691 693 int readDbl(const QString& str, int pos, int len, double& value) { … … 703 705 // Topocentrical Distance and Elevation 704 706 //////////////////////////////////////////////////////////////////////////// 705 void topos(double xRec, double yRec, double zRec, 706 double xSat, double ySat, double zSat, 707 void topos(double xRec, double yRec, double zRec, 708 double xSat, double ySat, double zSat, 707 709 double& rho, double& eleSat, double& azSat) { 708 710 … … 712 714 dx[2] = zSat-zRec; 713 715 714 rho = sqrt( dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2] ); 716 rho = sqrt( dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2] ); 715 717 716 718 double xyzRec[3]; … … 741 743 } 742 744 743 // 745 // 744 746 //////////////////////////////////////////////////////////////////////////// 745 747 QString fortranFormat(double value, int width, int prec) { … … 760 762 // 761 763 ////////////////////////////////////////////////////////////////////////////// 762 void kalman(const Matrix& AA, const ColumnVector& ll, const DiagonalMatrix& PP, 764 void kalman(const Matrix& AA, const ColumnVector& ll, const DiagonalMatrix& PP, 763 765 SymmetricMatrix& QQ, ColumnVector& xx) { 764 766 … … 777 779 SRF.SubMatrix (nObs+1, nObs+nPar, 1, nObs) = SA; 778 780 SRF.SymSubMatrix(nObs+1, nObs+nPar) = SS; 779 781 780 782 UpperTriangularMatrix UU; 781 783 QRZ(SRF, UU); 782 784 783 785 SS = UU.SymSubMatrix(nObs+1, nObs+nPar); 784 786 UpperTriangularMatrix SH_rt = UU.SymSubMatrix(1, nObs); 785 787 Matrix YY = UU.SubMatrix(1, nObs, nObs+1, nObs+nPar); 786 788 787 789 UpperTriangularMatrix SHi = SH_rt.i(); 788 789 Matrix KT = SHi * YY; 790 791 Matrix KT = SHi * YY; 790 792 SymmetricMatrix Hi; Hi << SHi * SHi.t(); 791 793
Note:
See TracChangeset
for help on using the changeset viewer.