mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-16 05:00:35 +00:00
Code Issues Releases Wiki Activity

debug 3

Browse Source
This commit is contained in:
Antonio Ramos 2018-03-02 13:30:21 +01:00
parent a216d22509
commit 3ce49ada96
1 changed files with 83 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

143
src/algorithms/libs/rtklib/rtklib_pntpos.cc
View File

@ -54,7 +54,6 @@
#include "rtklib_ephemeris.h" #include "rtklib_ephemeris.h"
#include "rtklib_ionex.h" #include "rtklib_ionex.h"
#include "rtklib_sbas.h" #include "rtklib_sbas.h"
#include <iostream>
/* pseudorange measurement error variance ------------------------------------*/ /* pseudorange measurement error variance ------------------------------------*/
double varerr(const prcopt_t *opt, double el, int sys) double varerr(const prcopt_t *opt, double el, int sys)
@ -91,47 +90,52 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
*var = 0.0; *var = 0.0;
if (!(sys = satsys(obs->sat, NULL))) if (!(sys = satsys(obs->sat, NULL)))
{ {
trace(4, "prange: satsys NULL\n"); trace(4, "prange: satsys NULL\n");
return 0.0; return 0.0;
} }
/* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */
if (sys & (SYS_GAL | SYS_SBS)) {j = 2;} if (sys & (SYS_GAL | SYS_SBS)) {j = 2;}
if (sys == SYS_GPS) if (sys == SYS_GPS)
{ {
if(obs->code[1] != CODE_NONE) {j = 1;} if(obs->code[1] != CODE_NONE) {j = 1;}
else if(obs->code[2] != CODE_NONE) {j = 2;} else if(obs->code[2] != CODE_NONE) {j = 2;}
} }
if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0) if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0)
{ {
trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n");
printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]); printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]);
return 0.0; return 0.0;
} }
/* test snr mask */ /* test snr mask */
if (iter > 0) if (iter > 0)
{
if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask))
{ {
if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n",
{ time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25);
trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n", return 0.0;
time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25);
return 0.0;
}
if (opt->ionoopt == IONOOPT_IFLC)
{
if (testsnr(0, j, azel[1], obs->SNR[j] * 0.25, &opt->snrmask))
{
trace(4, "prange: testsnr error\n");
return 0.0;
}
}
} }
gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); /* f1^2/f2^2 */ if (opt->ionoopt == IONOOPT_IFLC)
{
if (testsnr(0, j, azel[1], obs->SNR[j] * 0.25, &opt->snrmask))
{
trace(4, "prange: testsnr error\n");
return 0.0;
}
}
}
/* fL1^2 / fL2(orL5)^2 . See IS-GPS-200, p. 103 and Galileo ICD p. 48 */
if(sys == SYS_GPS or sys == SYS_GAL)
{
gamma_ = std::pow(lam[i], 2.0) / std::pow(lam[j], 2.0);
}
P1 = obs->P[i]; P1 = obs->P[i];
P2 = obs->P[j]; P2 = obs->P[j];
P1_P2 = nav->cbias[obs->sat-1][0]; P1_P2 = nav->cbias[obs->sat-1][0];
@ -139,42 +143,61 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
P2_C2 = nav->cbias[obs->sat-1][2]; P2_C2 = nav->cbias[obs->sat-1][2];
/* if no P1-P2 DCB, use TGD instead */ /* if no P1-P2 DCB, use TGD instead */
if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) //CHECK! if(P1_P2 == 0.0 and sys == SYS_GPS)
{ {
P1_P2 = (1.0 - gamma_) * gettgd(obs->sat, nav); P1_P2 = (gamma_ - 1.0) * gettgd(obs->sat, nav);
} }
else if(P1_P2 == 0.0 and sys == SYS_GAL)
{
//TODO
}
if (opt->ionoopt == IONOOPT_IFLC) if (opt->ionoopt == IONOOPT_IFLC)
{ /* dual-frequency */ { /* dual-frequency */
if (P1 == 0.0 || P2 == 0.0) { return 0.0; } if (P1 == 0.0 || P2 == 0.0) { return 0.0; }
if (obs->code[i] == CODE_L1C) { P1 += P1_C1; } /* C1->P1 */ if (obs->code[i] == CODE_L1C) { P1 += P1_C1; } /* C1->P1 */
if (obs->code[j] == CODE_L2C) { P2 += P2_C2; } /* C2->P2 */ if (obs->code[j] == CODE_L2C) { P2 += P2_C2; } /* C2->P2 */
/* iono-free combination */ /* iono-free combination */
PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); PC = (gamma_ * P1 - P2) / (gamma_ - 1.0);
} }
else else
{ /* single-frequency */ { /* single-frequency */
if((obs->code[i] == CODE_NONE) && (obs->code[j] == CODE_NONE)) { return 0.0; } if((obs->code[i] == CODE_NONE) && (obs->code[j] == CODE_NONE)) { return 0.0; }
else if((obs->code[i] != CODE_NONE) && (obs->code[j] == CODE_NONE)) else if((obs->code[i] != CODE_NONE) && (obs->code[j] == CODE_NONE))
{ {//CHECK!!
P1 += P1_C1; /* C1->P1 */ P1 += P1_C1; /* C1->P1 */
PC = P1 - P1_P2 / (1.0 - gamma_); PC = P1 - P1_P2 / (1.0 - gamma_);
} }
else if((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE)) else if((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE))
{ {
P2 += P2_C2; /* C2->P2 */ if(sys == SYS_GPS)
PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); {//CHECK!!
} P2 += P2_C2; /* C2->P2 */
/* dual-frequency */ PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_);
else }
{ else if(sys == SYS_GAL)
P1 += P1_C1; {
P2 += P2_C2; //TODO
PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); }
} }
} /* dual-frequency */
else
{
if(sys == SYS_GPS) /* See IS-GPS-200 p. 179 */
{
P1 += P1_C1;
P2 += P2_C2;
PC = (P2 - gamma_ * P1) / (1 - gamma_) - P1_P2 / (gamma_ - 1);
}
else if(sys == SYS_GAL)
{
//TODO
}
}
}
if (opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; } /* sbas clock based C1 */ if (opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; } /* sbas clock based C1 */
*var = std::pow(ERR_CBIAS, 2.0); *var = std::pow(ERR_CBIAS, 2.0);