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

debug4

Browse Source
This commit is contained in:
Antonio Ramos 2018-03-05 11:05:58 +01:00
parent 3ce49ada96
commit 7646568314
1 changed files with 31 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -54,6 +54,8 @@
#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 <fstream>
#include <string>
/* 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)
@ -84,8 +86,16 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
int iter, const prcopt_t *opt, double *var) int iter, const prcopt_t *opt, double *var)
{ {
const double *lam = nav->lam[obs->sat - 1]; const double *lam = nav->lam[obs->sat - 1];
double PC, P1, P2, P1_P2, P1_C1, P2_C2, gamma_; double PC;
int i = 0, j = 1, sys; double P1;
double P2;
double P1_P2;
double P1_C1;
double P2_C2;
double gamma_ = 0.0;
int i = 0;
int j = 1;
int sys;
*var = 0.0; *var = 0.0;
@ -133,15 +143,22 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
/* fL1^2 / fL2(orL5)^2 . See IS-GPS-200, p. 103 and Galileo ICD p. 48 */ /* fL1^2 / fL2(orL5)^2 . See IS-GPS-200, p. 103 and Galileo ICD p. 48 */
if(sys == SYS_GPS or sys == SYS_GAL) if(sys == SYS_GPS or sys == SYS_GAL)
{ {
gamma_ = std::pow(lam[i], 2.0) / std::pow(lam[j], 2.0); gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 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];
P1_C1 = nav->cbias[obs->sat-1][1]; P1_C1 = nav->cbias[obs->sat-1][1];
P2_C2 = nav->cbias[obs->sat-1][2]; P2_C2 = nav->cbias[obs->sat-1][2];
std::string d_dump_filename = "/home/antonio/data/dump_prange.dat";
std::ofstream d_file;
d_file.exceptions (std::ifstream::failbit | std::ifstream::badbit );
d_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
d_file.write(reinterpret_cast<char*>(&P1), sizeof(double));
d_file.write(reinterpret_cast<char*>(&P2), sizeof(double));
d_file.write(reinterpret_cast<char*>(&P1_P2), sizeof(double));
d_file.write(reinterpret_cast<char*>(&P1_C1), sizeof(double));
d_file.write(reinterpret_cast<char*>(&P2_C2), sizeof(double));
/* if no P1-P2 DCB, use TGD instead */ /* if no P1-P2 DCB, use TGD instead */
if(P1_P2 == 0.0 and sys == SYS_GPS) if(P1_P2 == 0.0 and sys == SYS_GPS)
{ {
@ -151,7 +168,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
{ {
//TODO //TODO
} }
d_file.write(reinterpret_cast<char*>(&P1_P2), sizeof(double));
if (opt->ionoopt == IONOOPT_IFLC) if (opt->ionoopt == IONOOPT_IFLC)
{ /* dual-frequency */ { /* dual-frequency */
@ -166,17 +183,18 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
{ /* 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) and (obs->code[j] == CODE_NONE))
{//CHECK!! {//CHECK!!
P1 += P1_C1; /* C1->P1 */ P1 += P1_C1; /* C1->P1 */
PC = P1 - P1_P2 / (1.0 - gamma_); PC = P1 + P1_P2 / (gamma_ - 1.0);
} }
else if((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE)) else if((obs->code[i] == CODE_NONE) and (obs->code[j] != CODE_NONE))
{ {
if(sys == SYS_GPS) if(sys == SYS_GPS)
{//CHECK!! {//CHECK!!
P2 += P2_C2; /* C2->P2 */ P2 += P2_C2; /* C2->P2 */
PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); //PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_);
PC = P2 + gamma_ * P1_P2 / (gamma_ - 1.0);
} }
else if(sys == SYS_GAL) else if(sys == SYS_GAL)
{ {
@ -190,7 +208,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
{ {
P1 += P1_C1; P1 += P1_C1;
P2 += P2_C2; P2 += P2_C2;
PC = (P2 - gamma_ * P1) / (1 - gamma_) - P1_P2 / (gamma_ - 1); PC = (P2 - gamma_ * P1) / (1.0 - gamma_) - P1_P2 / (gamma_ - 1.0);
} }
else if(sys == SYS_GAL) else if(sys == SYS_GAL)
{ {
@ -198,10 +216,11 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
} }
} }
} }
d_file.write(reinterpret_cast<char*>(&PC), sizeof(double));
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);
d_file.close();
return PC; return PC;
} }