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

Last change on this file since 8483 was 8483, checked in by stuerze, 11 months ago

SSR parameter clock rate, clock drift and URA are added within RTNET format

File size: 3.8 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(7);
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.