diff --git a/include/gnss_utils/range.h b/include/gnss_utils/range.h index caf9e9db23c97692ba3e4bb1a438e8162b5c24ea..7c893a3ddb98031b3688ab856191c7a1c5f9ffff 100644 --- a/include/gnss_utils/range.h +++ b/include/gnss_utils/range.h @@ -18,6 +18,7 @@ namespace GnssUtils class Range { public: + int sys = SYS_GPS; int sat = 0; double P = -1; double P_var = 1; @@ -28,6 +29,7 @@ class Range double L = -1; double L_corrected = -1; double L_var = 1; + int freq = 0; public: Range(); diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h index e2ef0cefc5d3144f0e916e92619edd2b50e4fd37..f1ebc6338512fd0a2677ed235e78da3a2f9274ab 100644 --- a/include/gnss_utils/snapshot.h +++ b/include/gnss_utils/snapshot.h @@ -27,16 +27,15 @@ public: // Public methods void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = ""); + double getGPST() const; - ObservationsPtr getObservations() const; + // Navigation NavigationPtr getNavigation() const; - const Satellites& getSatellites() const; - Satellites& getSatellites(); - void setObservations(ObservationsPtr obs); void setNavigation(NavigationPtr nav); - void computeSatellites(const int& eph_opt); // see rtklib.h L396); - bool satellitesComputed() const; + // Observations + ObservationsPtr getObservations() const; + void setObservations(ObservationsPtr obs); std::set<int> filterObservations(const std::set<int>& discarded_sats, const Eigen::Vector3d& x_r, @@ -49,7 +48,13 @@ public: const bool& check_carrier_phase, const Options& opt); - // Pseudo-ranges + // Satellites + const Satellites& getSatellites() const; + Satellites& getSatellites(); + void computeSatellites(const int& eph_opt); // see rtklib.h L396); + bool satellitesComputed() const; + + // Ranges Ranges& getRanges(); const Ranges& getRanges() const; bool rangesComputed() const; @@ -81,6 +86,11 @@ private: namespace GnssUtils { +inline double Snapshot::getGPST() const +{ + return obs_->getObservations()[0].time.time + obs_->getObservations()[0].time.sec; +} + inline GnssUtils::ObservationsPtr Snapshot::getObservations() const { return obs_; diff --git a/src/tdcp.cpp b/src/tdcp.cpp index c67052f2f53d8f3d7f9a757f9afac4d9cbf86513..25a1726373a53805841e60b0ee9606407f803efc 100644 --- a/src/tdcp.cpp +++ b/src/tdcp.cpp @@ -102,6 +102,7 @@ bool Tdcp(SnapshotPtr snapshot_r, std::set<int> raim_discarded_sats, const TdcpBatchParams& tdcp_params) { + // TODO: change obs -> ranges // Checks assert(snapshot_r->satellitesComputed() && "this TDCP assumes satellites already computed"); assert(snapshot_k->satellitesComputed() && "this TDCP assumes satellites already computed"); @@ -131,7 +132,8 @@ bool Tdcp(SnapshotPtr snapshot_r, } // Times - double tr(-1), tk(-1); + double tr = snapshot_r->getGPST(); + double tk = snapshot_k->getGPST(); // MULTI-FREQUENCY std::map<int, std::pair<int, int>> row_2_sat_freq; @@ -144,15 +146,6 @@ bool Tdcp(SnapshotPtr snapshot_r, auto&& obs_r = snapshot_r->getObservations()->getObservationBySat(sat); auto&& obs_k = snapshot_k->getObservations()->getObservationBySat(sat); - // fill times - if (tr < 0) - { - tr = obs_r.time.time + obs_r.time.sec; - tk = obs_k.time.time + obs_k.time.sec; - std::cout << "tr: " << tr << std::endl; - std::cout << "tk: " << tk << std::endl; - } - // Carrier phase // L1/E1 if (std::abs(obs_r.L[0]) > 1e-12 and std::abs(obs_k.L[0]) > 1e-12) diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp index 447c273aab089959db8f905184516e5e4d4146e5..3b5d0dc20b93f1c6b060bf90c1f2d89e13321041 100644 --- a/test/gtest_tdcp.cpp +++ b/test/gtest_tdcp.cpp @@ -52,8 +52,8 @@ void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt, TEST(Tdcp, Tdcp) { - Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt, sat_ENU, sat_ECEF; - Eigen::Vector3d r1_LLA, r1_ECEF, r2_ECEF, d_ECEF; + Eigen::Vector3d sat_ENU, sat_ECEF; + Eigen::Vector3d r1_LLA, r1_ECEF, r2_LLA, r2_ECEF, d_ECEF; Eigen::Matrix3d R_ENU_ECEF; Eigen::Vector2d azel, azel2; double range; @@ -61,6 +61,9 @@ TEST(Tdcp, Tdcp) Satellite sat1({0,0,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),1.0,0,0,1}); Satellite sat2(sat1); + auto shapshot1 = std::make_shared<Snapshot>(); + auto shapshot2 = std::make_shared<Snapshot>(); + // Random receiver position for (auto i = 0; i<100; i++) { @@ -68,22 +71,36 @@ TEST(Tdcp, Tdcp) r1_ECEF = latLonAltToEcef(r1_LLA); d_ECEF = Eigen::Vector3d::Random() * 10; r2_ECEF = r1_ECEF + d_ECEF; - - Satellites sats1, sats2; + r2_LLA = ecefToLatLonAlt(r2_ECEF); // random visible satellites for (auto j = 0; j<10; j++) { - sat1.pos; + sat1.sat = j; + sat2.sat = j; + computeRandomVisibleSatellite(r1_LLA, sat_ENU, sat_ECEF, azel, range); + sat1.pos = sat_ECEF; computeRandomVisibleSatellite(r2_LLA, sat_ENU, sat_ECEF, azel, range); azel2 = computeAzel(sat_ECEF, t_ECEF_ENU); ASSERT_MATRIX_APPROX(azel,azel2,1e-6); } + + bool Tdcp(SnapshotPtr snapshot_r, + SnapshotPtr snapshot_k, + r1_ECEF, + const std::set<int> common_sats, + Eigen::Vector4d& d, + Eigen::Matrix4d& cov_d, + double& residual, + std::set<int> raim_discarded_sats, + const TdcpBatchParams& tdcp_params) } + + } int main(int argc, char **argv)