compute Sagnac correction

include/gnss_utils/range.h

@@ -47,6 +47,8 @@ class Range
         static std::set<int> findCommonSatellites(const Ranges& ranges_1, const Ranges& ranges_2);
 };
 
+double computeSagnacCorrection(const Eigen::Vector3d& rcv_ECEF, const Eigen::Vector3d& sat_ECEF);
+
 } /* namespace GnssUtils */
 
 #endif /* INCLUDE_GNSS_UTILS_RANGE_H_ */

src/range.cpp

@@ -190,4 +190,9 @@ std::set<int> Range::findCommonSatellites(const Ranges& ranges_1, const Ranges&
   return common_sats;
 }
 
+double computeSagnacCorrection(const Eigen::Vector3d& rcv_ECEF, const Eigen::Vector3d& sat_ECEF)
+{
+    return OMGE*(sat_ECEF(0)*rcv_ECEF(1)-sat_ECEF(1)*rcv_ECEF(0))/CLIGHT;
+}
+
 } /* namespace GnssUtils */

src/tdcp.cpp

@@ -248,8 +248,8 @@ TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
         // sagnac corrections
         if (tdcp_params.sagnac_correction)
         {
-            double sagnac_corr_r = OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
-            double sagnac_corr_k = OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
+            double sagnac_corr_r = computeSagnacCorrection(x_r,s_r); //OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
+            double sagnac_corr_k = computeSagnacCorrection(x_r,s_k); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
             drho_m(row) += -sagnac_corr_k + sagnac_corr_r;
         }