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 | }
|
---|