source: ntrip/trunk/BNC/src/RTCM/rtcm_utils.cpp@ 7908

Last change on this file since 7908 was 6139, checked in by mervart, 10 years ago
File size: 3.9 KB
Line 
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
9using namespace std;
10
11void 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
25int 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(4);
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}
Note: See TracBrowser for help on using the repository browser.