source: ntrip/trunk/BNC/RTCM/rtcm_utils.cpp@ 1382

Last change on this file since 1382 was 1044, checked in by zdenek, 16 years ago

Zdenek Lukes: a) added logic for RTCM 2.3 messages 20/21 decoding

b) added logic for cycle slip flags (slip counters or lock time indicators) handling

File size: 3.7 KB
Line 
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
10using namespace std;
11
12void 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
26int 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}
Note: See TracBrowser for help on using the repository browser.