| 1 | #include <iomanip>
|
|---|
| 2 | #include <iostream>
|
|---|
| 3 | #include <math.h>
|
|---|
| 4 | #include <stdio.h>
|
|---|
| 5 |
|
|---|
| 6 | #include "rtcm_utils.h"
|
|---|
| 7 | #include "ephemeris.h"
|
|---|
| 8 |
|
|---|
| 9 | using namespace std;
|
|---|
| 10 |
|
|---|
| 11 | void resolveEpoch (double secsHour,
|
|---|
| 12 | int refWeek, double refSecs,
|
|---|
| 13 | int& epochWeek, double& epochSecs) {
|
|---|
| 14 |
|
|---|
| 15 | const double secsPerWeek = 604800.0;
|
|---|
| 16 |
|
|---|
| 17 | epochWeek = refWeek;
|
|---|
| 18 | epochSecs = secsHour + 3600.0*(floor((refSecs-secsHour)/3600.0+0.5));
|
|---|
| 19 |
|
|---|
| 20 | if (epochSecs<0 ) { epochWeek--; epochSecs+=secsPerWeek; };
|
|---|
| 21 | if (epochSecs>secsPerWeek) { epochWeek++; epochSecs-=secsPerWeek; };
|
|---|
| 22 | };
|
|---|
| 23 |
|
|---|
| 24 |
|
|---|
| 25 | int cmpRho(const t_eph* eph,
|
|---|
| 26 | double stax, double stay, double staz,
|
|---|
| 27 | int GPSWeek, double GPSWeeks,
|
|---|
| 28 | double& rho, int& GPSWeek_tot, double& GPSWeeks_tot,
|
|---|
| 29 | double& xSat, double& ySat, double& zSat, double& clkSat) {
|
|---|
| 30 |
|
|---|
| 31 | const double omega_earth = 7292115.1467e-11;
|
|---|
| 32 | const double secsPerWeek = 604800.0;
|
|---|
| 33 |
|
|---|
| 34 | // Initial values
|
|---|
| 35 | // --------------
|
|---|
| 36 | rho = 0.0;
|
|---|
| 37 | ColumnVector xc(6);
|
|---|
| 38 | ColumnVector vv(3);
|
|---|
| 39 | eph->getCrd(bncTime(GPSWeek, GPSWeeks), xc, vv, false);
|
|---|
| 40 | xSat = xc(1);
|
|---|
| 41 | ySat = xc(2);
|
|---|
| 42 | zSat = xc(3);
|
|---|
| 43 | clkSat = xc(4);
|
|---|
| 44 |
|
|---|
| 45 | ////cout << "----- cmpRho -----\n";
|
|---|
| 46 | ////eph->print(cout);
|
|---|
| 47 | ////cout << " pos " << setw(4) << GPSWeek
|
|---|
| 48 | //// << " " << setw(14) << setprecision(6) << GPSWeeks
|
|---|
| 49 | //// << " " << setw(13) << setprecision(3) << xSat
|
|---|
| 50 | //// << " " << setw(13) << setprecision(3) << ySat
|
|---|
| 51 | //// << " " << setw(13) << setprecision(3) << zSat
|
|---|
| 52 | //// << endl;
|
|---|
| 53 |
|
|---|
| 54 | // Loop until the correct Time Of Transmission is found
|
|---|
| 55 | // ----------------------------------------------------
|
|---|
| 56 | double rhoLast = 0;
|
|---|
| 57 | do {
|
|---|
| 58 | rhoLast = rho;
|
|---|
| 59 |
|
|---|
| 60 | // Correction station position due to Earth Rotation
|
|---|
| 61 | // -------------------------------------------------
|
|---|
| 62 | double dPhi = omega_earth * rho / c_light;
|
|---|
| 63 | double xRec = stax * cos(dPhi) - stay * sin(dPhi);
|
|---|
| 64 | double yRec = stay * cos(dPhi) + stax * sin(dPhi);
|
|---|
| 65 | double zRec = staz;
|
|---|
| 66 |
|
|---|
| 67 | double dx = xRec - xSat;
|
|---|
| 68 | double dy = yRec - ySat;
|
|---|
| 69 | double dz = zRec - zSat;
|
|---|
| 70 |
|
|---|
| 71 | rho = sqrt(dx*dx + dy*dy + dz*dz);
|
|---|
| 72 |
|
|---|
| 73 | GPSWeek_tot = GPSWeek;
|
|---|
| 74 | GPSWeeks_tot = GPSWeeks - rho/c_light;
|
|---|
| 75 | while ( GPSWeeks_tot < 0 ) {
|
|---|
| 76 | GPSWeeks_tot += secsPerWeek;
|
|---|
| 77 | GPSWeek_tot -= 1;
|
|---|
| 78 | }
|
|---|
| 79 | while ( GPSWeeks_tot > secsPerWeek ) {
|
|---|
| 80 | GPSWeeks_tot -= secsPerWeek;
|
|---|
| 81 | GPSWeek_tot += 1;
|
|---|
| 82 | }
|
|---|
| 83 |
|
|---|
| 84 | eph->getCrd(bncTime(GPSWeek_tot, GPSWeeks_tot), xc, vv, false);
|
|---|
| 85 | xSat = xc(1);
|
|---|
| 86 | ySat = xc(2);
|
|---|
| 87 | zSat = xc(3);
|
|---|
| 88 | clkSat = xc(4);
|
|---|
| 89 |
|
|---|
| 90 | dx = xRec - xSat;
|
|---|
| 91 | dy = yRec - ySat;
|
|---|
| 92 | dz = zRec - zSat;
|
|---|
| 93 |
|
|---|
| 94 | rho = sqrt(dx*dx + dy*dy + dz*dz);
|
|---|
| 95 |
|
|---|
| 96 | ////cout << " scrd " << setw(4) << GPSWeek_tot
|
|---|
| 97 | //// << " " << setw(15) << setprecision(8) << GPSWeeks_tot
|
|---|
| 98 | //// << " " << setw(13) << setprecision(3) << xSat
|
|---|
| 99 | //// << " " << setw(13) << setprecision(3) << ySat
|
|---|
| 100 | //// << " " << setw(13) << setprecision(3) << zSat
|
|---|
| 101 | //// << " rcv0 " << setw(12) << setprecision(3) << stax
|
|---|
| 102 | //// << " " << setw(12) << setprecision(3) << stay
|
|---|
| 103 | //// << " " << setw(12) << setprecision(3) << staz
|
|---|
| 104 | //// << " rcv " << setw(12) << setprecision(3) << xRec
|
|---|
| 105 | //// << " " << setw(12) << setprecision(3) << yRec
|
|---|
| 106 | //// << " " << setw(12) << setprecision(3) << zRec
|
|---|
| 107 | //// << " dPhi " << scientific << setw(13) << setprecision(10) << dPhi << fixed
|
|---|
| 108 | //// << " rho " << setw(13) << setprecision(3) << rho
|
|---|
| 109 | //// << endl;
|
|---|
| 110 |
|
|---|
| 111 |
|
|---|
| 112 | ////cout.setf(ios::fixed);
|
|---|
| 113 | ////
|
|---|
| 114 | ////cout << "niter " << setw(3) << ++niter
|
|---|
| 115 | //// << " " << setw(14) << setprecision(3) << rhoLast
|
|---|
| 116 | //// << " " << setw(14) << setprecision(3) << rho
|
|---|
| 117 | //// << endl;
|
|---|
| 118 |
|
|---|
| 119 | } while ( fabs(rho - rhoLast) > 1e-4);
|
|---|
| 120 |
|
|---|
| 121 | clkSat *= c_light; // satellite clock correction in meters
|
|---|
| 122 |
|
|---|
| 123 | return 0;
|
|---|
| 124 | }
|
|---|
