diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index bcb6843faf81dfae31baf09963498000ad842dea..d9eca70d109c82a2d96890cf6df67d4dcd06693b 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -21,12 +21,15 @@ namespace GnssUtils
// Structs
struct PseudoRange
{
+ int sat = 0;
double p = -1;
- double var = 1;
+ double prange_var = 1;
double prange = -1;
double iono_corr = 0;
double tropo_corr = 0;
double sat_clock_corr = 0;
+ double L = -1;
+ double carrier_range = -1;
};
struct ComputePosOutput
@@ -48,6 +51,9 @@ struct ComputePosOutput
Eigen::Vector3d lat_lon; // latitude_longitude_altitude
std::map<int,Eigen::Vector2d> sat_azel; // azimuth and elevation of each satellite provided
+ std::set<int> used_sats; // used sats for computing fix (applying RAIM if the case and discarding wrong data)
+ std::set<int> discarded_sats; // discarded sats when computing fix (applying RAIM if the case and discarding wrong data)
+ std::map<int,double> prange_residuals; // residuals of used pseudoranges (applying RAIM if the case and discarding wrong data)
};
// forward declarations
@@ -78,6 +84,50 @@ enum Combination
CARRIER_IONO_FREE ///< iono-free combination for carrier phase
};
+/* defaults processing options */
+const prcopt_t opt_default = {
+ PMODE_SINGLE, /* mode: positioning mode (PMODE_???) */
+ 0, /* soltype: solution type (0:forward,1:backward,2:combined) */
+ 2, /* nf: number of frequencies (1:L1,2:L1+L2,3:L1+L2+L3,4:L1+L2+L3+L4) */
+ SYS_GPS|SYS_GLO|SYS_GAL, /* navsys */
+ 15.0*D2R,{{0,0}}, /* elmin (rad) ,snrmask */
+ 0, /* satellite ephemeris/clock (EPHOPT_???) */
+ 3,3,1,0,1, /* modear,glomodear,gpsmodear,bdsmodear,arfilter */
+ 20,0,4,5,10,20, /* maxout,minlock,minfixsats,minholdsats,mindropsats,minfix */
+ 0,1, /* rcvstds,armaxiter */
+ IONOOPT_BRDC, /* ionoopt: ionosphere option (IONOOPT_???) */
+ TROPOPT_SAAS, /* tropopt: troposphere option (TROPOPT_???) */
+ 1,0, /* dynamics,tidecorr */
+ 3, /* niter: number of filter iteration */
+ 0,0, /* codesmooth,intpref */
+ 0, /* sbascorr: SBAS correction options */
+ 0, /* sbasssatsel: SBAS satellite selection (0:all) */
+ 0,0, /* rovpos,refpos */
+ WEIGHTOPT_ELEVATION, /* weightmode */
+ {300.0,300.0,300.0}, /* eratio[]: measurement error factor [0]:reserved [1-3]:error factor a/b/c of phase (m) [4]:doppler frequency (hz) [5]: snr max value (dB.Hz) */
+ {100.0,0.003,0.003,0.0,1.0,52.0}, /* err[]: */
+ {30.0,0.03,0.3}, /* std[] initial-state std [0]bias,[1]iono [2]trop */
+ {1E-4,1E-3,1E-4,1E-1,1E-2,0.0}, /* prn[] */
+ 5E-12, /* sclkstab */
+ {3.0,0.25,0.0,1E-9,1E-5,0.0,0.0,0.0}, /* thresar */
+ 0.0,0.0,0.05,0.1,0.01, /* elmaskar,elmaskhold,thresslip,varholdamb,gainholdamb */
+ 30.0, /* maxtdiff: max difference of time (sec) */
+ 5.0, /* maxinno: reject threshold of innovation (m) */
+ 30.0, /* maxgdop: reject threshold of gdop */
+ {0},{0},{0}, /* baseline,ru,rb */
+ {"",""}, /* anttype */
+ {{0}},{{0}},{0}, /* antdel,pcvr,exsats */
+ 1,1, /* maxaveep,initrst */
+ 0, /* outsingle */
+ {{0}}, /* rnxopt */
+ {0,0,0,0,1,0}, /* posopt: positioning options [0-1]:PPP pcv [2]:PPP phase windup [3]:PPP exclude eclipsing [4]:PNTPOS RAIM, [5]:PPP clock jump */
+ 0, /* syncsol */
+ {{0}}, /* odisp */
+ {{0}}, /* exterr */
+ 0, /* freqopt */
+ {0} /* pppopt */
+};
+
}
#endif
diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h
index 69b27055a6794e438fb78c94f6f0d2fd2cf18e2e..ca8bf12345a8133bd2ac61368460c6271fda71e9 100644
--- a/include/gnss_utils/snapshot.h
+++ b/include/gnss_utils/snapshot.h
@@ -43,6 +43,11 @@ public:
const bool &check_code,
const bool &check_carrier_phase,
const prcopt_t &opt);
+ std::set<int> filterObservations(const std::set<int> &discarded_sats,
+ const std::map<int,Eigen::Vector2d>& azels,
+ const bool &check_code,
+ const bool &check_carrier_phase,
+ const prcopt_t &opt);
// Pseudo-ranges
PseudoRanges& getPseudoRanges();
@@ -119,6 +124,15 @@ inline std::set<int> Snapshot::filterObservations(const std::set<int> &discarded
return obs_->filter(sats_, discarded_sats, x_r, check_code, check_carrier_phase, opt);
}
+inline std::set<int> Snapshot::filterObservations(const std::set<int> &discarded_sats,
+ const std::map<int,Eigen::Vector2d>& azels,
+ const bool &check_code,
+ const bool &check_carrier_phase,
+ const prcopt_t &opt)
+{
+ return obs_->filter(sats_, discarded_sats, azels, check_code, check_carrier_phase, opt);
+}
+
inline const PseudoRanges& Snapshot::getPseudoRanges() const
{
return pranges_;
diff --git a/src/snapshot.cpp b/src/snapshot.cpp
index 994f9499e6ee20721c6b87139f378e66f0241258..355cfb4f4a959c00c71573e1febf221e89b4d346 100644
--- a/src/snapshot.cpp
+++ b/src/snapshot.cpp
@@ -57,7 +57,7 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel,
pranges_.emplace(sat,PseudoRange());
/* psudorange with code bias correction */
- std::cout << "prange...\n";
+ //std::cout << "prange...\n";
if ((P=prange(&obs_i,&nav_->getNavigation(),azel_i,1,&opt,&vmeas))==0.0)
{
//std::cout << " error in prange\n";
@@ -65,7 +65,7 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel,
}
/* ionospheric corrections */
- std::cout << "iono...\n";
+ //std::cout << "iono...\n";
//std::cout << "\ttime: " << obs_i.time.time << " + " << obs_i.time.sec << "\n";
//std::cout << "\tnav: \n";
//nav_->print();
@@ -77,27 +77,27 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel,
{
if (opt.ionoopt == IONOOPT_SBAS)
{
- std::cout << "error IONOOPT_SBAS ionocorr, trying with IONOOPT_BRDC...";
+ //std::cout << "error IONOOPT_SBAS ionocorr, trying with IONOOPT_BRDC...";
if (!ionocorr(obs_i.time,&nav_->getNavigation(),sat,latlonalt.data(),azel_i,IONOOPT_BRDC,&dion,&vion))
{
- std::cout << " error in ionocorr\n";
+ //std::cout << " error in ionocorr\n";
continue;
}
}
else
{
- std::cout << " error in ionocorr\n";
+ //std::cout << " error in ionocorr\n";
continue;
}
}
/* GPS-L1 -> L1/B1 */
- std::cout << "iono2...\n";
+ //std::cout << "iono2...\n";
if ((lam_L1=nav_->getNavigation().lam[sat-1][0])>0.0)
dion*=std::pow(lam_L1/lam_carr[0],2);
/* tropospheric corrections */
- std::cout << "tropo...\n";
+ //std::cout << "tropo...\n";
if (!tropcorr(obs_i.time,&nav_->getNavigation(),latlonalt.data(),azel_i,opt.tropopt,&dtrp,&vtrp))
{
//std::cout << " error in tropcorr\n";
@@ -105,7 +105,8 @@ void Snapshot::computePseudoRanges(const std::map<int,Eigen::Vector2d>& azel,
}
/* Store in PseudoRange struct */
- std::cout << "storing\n";
+ //std::cout << "storing\n";
+ pranges_[sat].sat = sat;
pranges_[sat].p = P;
pranges_[sat].iono_corr = -dion;
pranges_[sat].tropo_corr = -dtrp;
diff --git a/src/utils/rcv_position.cpp b/src/utils/rcv_position.cpp
index 8a2b450c1b9e8ca4d40cc0e19677f7821a2f5aa9..fb6931151118b1ef62bbef5530874f34374dcddb 100644
--- a/src/utils/rcv_position.cpp
+++ b/src/utils/rcv_position.cpp
@@ -22,6 +22,7 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
sol_t sol;
sol = { { 0 } };
+ ssat_t sats_status[MAXSAT];
std::vector<double> sat_elevations(2 * _observations.size());
output.pos_stat = pntpos(_observations.data(),
@@ -30,7 +31,7 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
&_prcopt,
&sol,
sat_elevations.data(),
- NULL,
+ sats_status,
msg);
if (output.pos_stat == 0)
@@ -59,6 +60,21 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
output.sat_azel.emplace(_observations.getObservationByIdx(i).sat,
Eigen::Vector2d(sat_elevations.at(2 * i), sat_elevations.at(2 * i + 1)));
+ for (auto i = 0; i < MAXSAT; i++)
+ {
+ int sat = i+1;
+ if (_observations.hasSatellite(sat))
+ {
+ if (sats_status[i].vs)
+ {
+ output.prange_residuals.emplace(sat,sats_status[i].resp[0]);
+ output.used_sats.insert(sat);
+ }
+ else
+ output.discarded_sats.insert(sat);
+ }
+ }
+
return output;
}