mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-15 20:50:33 +00:00
Code Issues Releases Wiki Activity

debug 6

Browse Source
This commit is contained in:
Antonio Ramos 2018-03-05 18:34:20 +01:00
parent 8d37013e96
commit a25a6fb556
2 changed files with 43 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -83,8 +83,7 @@ double gettgd(int sat, const nav_t *nav)
/* get isc parameter (m) -----------------------------------------------------*/ /* get isc parameter (m) -----------------------------------------------------*/
double getiscl1(int sat, const nav_t *nav) double getiscl1(int sat, const nav_t *nav)
{ {
int i; for (int i = 0; i < nav->n; i++)
for (i = 0; i < nav->n; i++)
{ {
if (nav->eph[i].sat != sat) continue; if (nav->eph[i].sat != sat) continue;
return SPEED_OF_LIGHT * nav->eph[i].isc[0]; return SPEED_OF_LIGHT * nav->eph[i].isc[0];
@ -94,8 +93,7 @@ double getiscl1(int sat, const nav_t *nav)
double getiscl2(int sat, const nav_t *nav) double getiscl2(int sat, const nav_t *nav)
{ {
int i; for (int i = 0; i < nav->n; i++)
for (i = 0; i < nav->n; i++)
{ {
if (nav->eph[i].sat != sat) continue; if (nav->eph[i].sat != sat) continue;
return SPEED_OF_LIGHT * nav->eph[i].isc[1]; return SPEED_OF_LIGHT * nav->eph[i].isc[1];
@ -105,8 +103,7 @@ double getiscl2(int sat, const nav_t *nav)
double getiscl5i(int sat, const nav_t *nav) double getiscl5i(int sat, const nav_t *nav)
{ {
int i; for (int i = 0; i < nav->n; i++)
for (i = 0; i < nav->n; i++)
{ {
if (nav->eph[i].sat != sat) continue; if (nav->eph[i].sat != sat) continue;
return SPEED_OF_LIGHT * nav->eph[i].isc[2]; return SPEED_OF_LIGHT * nav->eph[i].isc[2];
@ -116,8 +113,7 @@ double getiscl5i(int sat, const nav_t *nav)
double getiscl5q(int sat, const nav_t *nav) double getiscl5q(int sat, const nav_t *nav)
{ {
int i; for (int i = 0; i < nav->n; i++)
for (i = 0; i < nav->n; i++)
{ {
if (nav->eph[i].sat != sat) continue; if (nav->eph[i].sat != sat) continue;
return SPEED_OF_LIGHT * nav->eph[i].isc[3]; return SPEED_OF_LIGHT * nav->eph[i].isc[3];
@ -129,13 +125,13 @@ double getiscl5q(int sat, const nav_t *nav)
double prange(const obsd_t *obs, const nav_t *nav, const double *azel, 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; double PC = 0.0;
double P1; double P1 = 0.0;
double P2; double P2 = 0.0;
double P1_P2; double P1_P2 = 0.0;
double P1_C1; double P1_C1 = 0.0;
double P2_C2; double P2_C2 = 0.0;
double ISCl1 = 0.0; double ISCl1 = 0.0;
double ISCl2 = 0.0; double ISCl2 = 0.0;
double ISCl5i = 0.0; double ISCl5i = 0.0;
@ -143,11 +139,10 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
double gamma_ = 0.0; double gamma_ = 0.0;
int i = 0; int i = 0;
int j = 1; int j = 1;
int sys; int sys = satsys(obs->sat, NULL);
*var = 0.0; *var = 0.0;
if (!(sys = satsys(obs->sat, NULL))) if(sys == SYS_NONE)
{ {
trace(4, "prange: satsys NULL\n"); trace(4, "prange: satsys NULL\n");
return 0.0; return 0.0;
@ -155,15 +150,15 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
/* 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 or sys == 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(lam[i] == 0.0 or 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]);
@ -171,7 +166,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
} }
/* 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))
{ {
@ -226,9 +221,15 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
ISCl2 = getiscl2(obs->sat, nav); ISCl2 = getiscl2(obs->sat, nav);
ISCl5i = getiscl5i(obs->sat, nav); ISCl5i = getiscl5i(obs->sat, nav);
ISCl5q = getiscl5q(obs->sat, nav); ISCl5q = getiscl5q(obs->sat, nav);
d_file.write(reinterpret_cast<char*>(&ISCl1), sizeof(double));
d_file.write(reinterpret_cast<char*>(&ISCl2), sizeof(double));
d_file.write(reinterpret_cast<char*>(&ISCl5i), sizeof(double));
d_file.write(reinterpret_cast<char*>(&ISCl5q), sizeof(double));
} }
d_file.write(reinterpret_cast<char*>(&P1_P2), sizeof(double)); d_file.write(reinterpret_cast<char*>(&P1_P2), sizeof(double));
if (opt->ionoopt == IONOOPT_IFLC)
//CHECK IF IT IS STILL NEEDED
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; }
@ -238,22 +239,23 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
/* 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 and obs->code[j] == CODE_NONE) { return 0.0; }
else if((obs->code[i] != CODE_NONE) and (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 / (gamma_ - 1.0); PC = P1 + P1_P2 / (gamma_ - 1.0);
} }
else if((obs->code[i] == CODE_NONE) and (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); PC = P2 + P1_P2 / (gamma_ - 1.0) - ISCl2;
} }
else if(sys == SYS_GAL) else if(sys == SYS_GAL)
{ {
@ -261,28 +263,27 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
} }
} }
/* dual-frequency */ /* dual-frequency */
else else if(sys == SYS_GPS)
{ {
if(sys == SYS_GPS) /* See IS-GPS-200 p. 179 */ if(obs->code[j] == CODE_L2S) /* L1 + L2 */
{ {
P1 += P1_C1; //PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2 / (gamma_ - 1.0);
P2 += P2_C2; PC = (P1 + P2) / 2.0;
if(obs->code[j] == CODE_L2S)
{
PC = (P2 + ISCl2 - gamma_ * (P1 + ISCl1)) / (1.0 - gamma_) - P1_P2 / (gamma_ - 1.0);
}
} }
else if(sys == SYS_GAL) if(obs->code[j] == CODE_L5X) /* L1 + L5 */
{ {
//TODO
} }
} }
else if(sys == SYS_GAL) /* E1 + E5a */
{
}
} }
d_file.write(reinterpret_cast<char*>(&PC), sizeof(double)); d_file.write(reinterpret_cast<char*>(&PC), sizeof(double));
if (opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; } /* sbas clock based C1 */
*var = std::pow(ERR_CBIAS, 2.0);
d_file.close(); d_file.close();
if(opt->sateph == EPHOPT_SBAS) { PC -= P1_C1; } /* sbas clock based C1 */
*var = std::pow(ERR_CBIAS, 2.0);
return PC; return PC;
} }

2
src/core/system_parameters/gps_cnav_navigation_message.cc
View File

@ -32,6 +32,7 @@
#include "gps_cnav_navigation_message.h" #include "gps_cnav_navigation_message.h"
#include "gnss_satellite.h" #include "gnss_satellite.h"
#include <iostream>
void Gps_CNAV_Navigation_Message::reset() void Gps_CNAV_Navigation_Message::reset()
@ -266,6 +267,7 @@ void Gps_CNAV_Navigation_Message::decode_page(std::bitset<GPS_CNAV_DATA_PAGE_BIT
ephemeris_record.d_ISCL1 = ephemeris_record.d_ISCL1 * CNAV_ISCL1_LSB; ephemeris_record.d_ISCL1 = ephemeris_record.d_ISCL1 * CNAV_ISCL1_LSB;
ephemeris_record.d_ISCL2 = static_cast<double>(read_navigation_signed(data_bits, CNAV_ISCL2)); ephemeris_record.d_ISCL2 = static_cast<double>(read_navigation_signed(data_bits, CNAV_ISCL2));
ephemeris_record.d_ISCL2 = ephemeris_record.d_ISCL2 * CNAV_ISCL2_LSB; ephemeris_record.d_ISCL2 = ephemeris_record.d_ISCL2 * CNAV_ISCL2_LSB;
std::cout << "ISCL2 * c = " << ephemeris_record.d_ISCL2 * 3e8 << " [m]" << std::endl;
ephemeris_record.d_ISCL5I = static_cast<double>(read_navigation_signed(data_bits, CNAV_ISCL5I)); ephemeris_record.d_ISCL5I = static_cast<double>(read_navigation_signed(data_bits, CNAV_ISCL5I));
ephemeris_record.d_ISCL5I = ephemeris_record.d_ISCL5I * CNAV_ISCL5I_LSB; ephemeris_record.d_ISCL5I = ephemeris_record.d_ISCL5I * CNAV_ISCL5I_LSB;
ephemeris_record.d_ISCL5Q = static_cast<double>(read_navigation_signed(data_bits, CNAV_ISCL5Q)); ephemeris_record.d_ISCL5Q = static_cast<double>(read_navigation_signed(data_bits, CNAV_ISCL5Q));