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

Last change on this file since 6139 was 6139, checked in by mervart, 5 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.