From a31a306f89426e500a34b2fe52e76930023b8763 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 21 Jun 2021 15:07:11 +0200
Subject: [PATCH] sagnac add/substract option

---
 include/gnss_utils/tdcp.h | 2 +-
 src/tdcp.cpp              | 8 ++++++--
 2 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index eb32399..de5ef4e 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -17,7 +17,7 @@ struct TdcpBatchParams
   bool   relinearize_jacobian;
   int    max_iterations;
   int    residual_opt; // 0: Normalized RMS of residual vector. 1: Max residual in Mahalanobis squared distance
-  bool   sagnac_correction;
+  int   sagnac_correction; // 0 deactivated, 1/2 substraction/addition
 };
 
 struct TdcpOutput
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index a270b8c..110c0f2 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -246,11 +246,15 @@ TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
         }
 
         // sagnac corrections
-        if (tdcp_params.sagnac_correction)
+        if (tdcp_params.sagnac_correction != 0)
         {
             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;
+
+            if (tdcp_params.sagnac_correction == 1)
+                drho_m(row) += -sagnac_corr_k + sagnac_corr_r;
+            else
+                drho_m(row) -= -sagnac_corr_k + sagnac_corr_r;
         }
 
         #if GNSS_UTILS_TDCP_DEBUG == 1
-- 
GitLab