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