WIP

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();

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_;

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)

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)