[5743] | 1 |
|
---|
| 2 | #include <cmath>
|
---|
| 3 |
|
---|
| 4 | #include "bncconst.h"
|
---|
| 5 | #include "windup.h"
|
---|
| 6 | #include "bncutils.h"
|
---|
[5753] | 7 | #include "bnctides.h"
|
---|
[5743] | 8 |
|
---|
| 9 | using namespace std;
|
---|
| 10 | using namespace BNC;
|
---|
| 11 |
|
---|
| 12 | // Constructor
|
---|
| 13 | ///////////////////////////////////////////////////////////////////////////
|
---|
| 14 | t_windUp::t_windUp() {
|
---|
| 15 | for (unsigned ii = 0; ii <= t_prn::MAXPRN; ii++) {
|
---|
| 16 | sumWind[ii] = 0.0;
|
---|
| 17 | lastEtime[ii] = 0.0;
|
---|
| 18 | }
|
---|
| 19 | }
|
---|
| 20 |
|
---|
| 21 | // Phase Wind-Up Correction
|
---|
| 22 | ///////////////////////////////////////////////////////////////////////////
|
---|
| 23 | double t_windUp::value(const bncTime& etime, const ColumnVector& rRec,
|
---|
| 24 | t_prn prn, const ColumnVector& rSat) {
|
---|
| 25 |
|
---|
| 26 | if (etime.mjddec() != lastEtime[prn.toInt()]) {
|
---|
| 27 |
|
---|
| 28 | // Unit Vector GPS Satellite --> Receiver
|
---|
| 29 | // --------------------------------------
|
---|
| 30 | ColumnVector rho = rRec - rSat;
|
---|
| 31 | rho /= rho.norm_Frobenius();
|
---|
| 32 |
|
---|
| 33 | // GPS Satellite unit Vectors sz, sy, sx
|
---|
| 34 | // -------------------------------------
|
---|
| 35 | ColumnVector sz = -rSat / rSat.norm_Frobenius();
|
---|
| 36 |
|
---|
[5753] | 37 | ColumnVector xSun = Sun(etime.mjddec());
|
---|
[5743] | 38 | xSun /= xSun.norm_Frobenius();
|
---|
| 39 |
|
---|
| 40 | ColumnVector sy = crossproduct(sz, xSun);
|
---|
| 41 | ColumnVector sx = crossproduct(sy, sz);
|
---|
| 42 |
|
---|
| 43 | // Effective Dipole of the GPS Satellite Antenna
|
---|
| 44 | // ---------------------------------------------
|
---|
| 45 | ColumnVector dipSat = sx - rho * DotProduct(rho,sx)
|
---|
| 46 | - crossproduct(rho, sy);
|
---|
| 47 |
|
---|
| 48 | // Receiver unit Vectors rx, ry
|
---|
| 49 | // ----------------------------
|
---|
| 50 | ColumnVector rx(3);
|
---|
| 51 | ColumnVector ry(3);
|
---|
| 52 |
|
---|
| 53 | double recEll[3]; xyz2ell(rRec.data(), recEll) ;
|
---|
| 54 | double neu[3];
|
---|
| 55 |
|
---|
| 56 | neu[0] = 1.0;
|
---|
| 57 | neu[1] = 0.0;
|
---|
| 58 | neu[2] = 0.0;
|
---|
| 59 | neu2xyz(recEll, neu, rx.data());
|
---|
| 60 |
|
---|
| 61 | neu[0] = 0.0;
|
---|
| 62 | neu[1] = -1.0;
|
---|
| 63 | neu[2] = 0.0;
|
---|
| 64 | neu2xyz(recEll, neu, ry.data());
|
---|
| 65 |
|
---|
| 66 | // Effective Dipole of the Receiver Antenna
|
---|
| 67 | // ----------------------------------------
|
---|
| 68 | ColumnVector dipRec = rx - rho * DotProduct(rho,rx)
|
---|
| 69 | + crossproduct(rho, ry);
|
---|
| 70 |
|
---|
| 71 | // Resulting Effect
|
---|
| 72 | // ----------------
|
---|
| 73 | double alpha = DotProduct(dipSat,dipRec) /
|
---|
| 74 | (dipSat.norm_Frobenius() * dipRec.norm_Frobenius());
|
---|
| 75 |
|
---|
| 76 | if (alpha > 1.0) alpha = 1.0;
|
---|
| 77 | if (alpha < -1.0) alpha = -1.0;
|
---|
| 78 |
|
---|
[5753] | 79 | double dphi = acos(alpha) / 2.0 / M_PI; // in cycles
|
---|
[5743] | 80 |
|
---|
| 81 | if ( DotProduct(rho, crossproduct(dipSat, dipRec)) < 0.0 ) {
|
---|
| 82 | dphi = -dphi;
|
---|
| 83 | }
|
---|
| 84 |
|
---|
| 85 | if (lastEtime[prn.toInt()] == 0.0) {
|
---|
| 86 | sumWind[prn.toInt()] = dphi;
|
---|
| 87 | }
|
---|
| 88 | else {
|
---|
| 89 | sumWind[prn.toInt()] = nint(sumWind[prn.toInt()] - dphi) + dphi;
|
---|
| 90 | }
|
---|
| 91 |
|
---|
| 92 | lastEtime[prn.toInt()] = etime.mjddec();
|
---|
| 93 | }
|
---|
| 94 |
|
---|
| 95 | return sumWind[prn.toInt()];
|
---|
| 96 | }
|
---|