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