diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index c582301194695d91b4b794fed71b13681be00b76..dadb9384f8786ac3d2f135e5016eceec55cfd0d2 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -17,48 +17,49 @@ struct TdcpBatchParams
   int    max_iterations;
 };
 
-bool Tdcp(SnapshotPtr               snapshot_r,
-          SnapshotPtr               snapshot_k,
-          Eigen::Vector4d&          d,
-          Eigen::Matrix4d&          cov_d,
-          double&                   residual,
-          const std::set<int>&      discarded_sats,
-          std::set<int>&            raim_discarded_sats,
-          const TdcpBatchParams&    tdcp_params,
-          const Options&            opt);
+struct TdcpOutput
+{
+    bool success = false;
+    std::string msg = "";
+    std::set<int> raim_discarded_sats;
+    std::set<int> used_sats;
+    Eigen::Vector4d d = Eigen::Vector4d::Zero();
+    Eigen::Matrix4d cov_d = Eigen::Matrix4d::Zero();
+    double dt = 0;
+    double residual = 0;
+};
+
+TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
+                SnapshotPtr               snapshot_k,
+                const Eigen::Vector4d&    d_0,
+                const std::set<int>&      discarded_sats,
+                const TdcpBatchParams&    tdcp_params,
+                const Options&            opt);
 
-bool Tdcp(SnapshotPtr               snapshot_r,
-          SnapshotPtr               snapshot_k,
-          const Eigen::Vector3d&    x_r,
-          Eigen::Vector4d&          d,
-          Eigen::Matrix4d&          cov_d,
-          double&                   residual,
-          const std::set<int>&      discarded_sats,
-          std::set<int>&            raim_discarded_sats,
-          const TdcpBatchParams&    tdcp_params,
-          const Options&            opt);
+TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
+                SnapshotPtr               snapshot_k,
+                const Eigen::Vector3d&    x_r,
+                const Eigen::Vector4d&    d_0,
+                const std::set<int>&      discarded_sats,
+                const TdcpBatchParams&    tdcp_params,
+                const Options&            opt);
 
-bool Tdcp(SnapshotPtr               snapshot_r,
-          SnapshotPtr               snapshot_k,
-          const Eigen::Vector3d&    x_r,
-          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);
+TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
+                SnapshotPtr               snapshot_k,
+                const Eigen::Vector3d&    x_r,
+                const std::set<int>&      common_sats,
+                const Eigen::Vector4d&    d_0,
+                const TdcpBatchParams&    tdcp_params);
 
-bool Tdcp(const Eigen::Vector3d&    x_r,
-          Eigen::Matrix<double, Eigen::Dynamic, 4>& A,
-          Eigen::VectorXd& r,
-          Eigen::VectorXd& drho_m,
-          Eigen::Matrix<double, 3, Eigen::Dynamic>& s_k,
-          Eigen::Matrix<double, 3, Eigen::Dynamic>& s_r,
-          Eigen::Vector4d&          d,
-          Eigen::Matrix4d&          cov_d,
-          double&                   residual,
-          std::set<int>&            raim_discarded_sats,
-          const TdcpBatchParams&    tdcp_params);
+TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
+                Eigen::MatrixXd&        A,
+                Eigen::VectorXd&        r,
+                Eigen::VectorXd&        drho_m,
+                Eigen::MatrixXd&        s_k,
+                Eigen::MatrixXd&        s_r,
+                const Eigen::Vector4d&        d_0,
+                std::set<int>&          raim_discarded_rows,
+                const TdcpBatchParams&  tdcp_params);
 
 }  // namespace GnssUtils
 
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 75788b09c7ba9abb8e59e645cca7b64fa5f9e307..3c91462cc8283dda59122bfc91060b3cb0bd07e8 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -6,38 +6,29 @@
 
 namespace GnssUtils
 {
-bool Tdcp(SnapshotPtr               snapshot_r,
-          SnapshotPtr               snapshot_k,
-          Eigen::Vector4d&          d,
-          Eigen::Matrix4d&          cov_d,
-          double&                   residual,
-          const std::set<int>&      discarded_sats,
-          std::set<int>&            raim_discarded_sats,
-          const TdcpBatchParams&    tdcp_params,
-          const Options&            opt)
+TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
+                SnapshotPtr               snapshot_k,
+                const Eigen::Vector4d&    d_0,
+                const std::set<int>&      discarded_sats,
+                const TdcpBatchParams&    tdcp_params,
+                const Options&            opt)
 {
     return Tdcp(snapshot_r,
                 snapshot_k,
                 computePos(*snapshot_r->getObservations(), *snapshot_r->getNavigation(), opt).pos,
-                d,
-                cov_d,
-                residual,
+                d_0,
                 discarded_sats,
-                raim_discarded_sats,
                 tdcp_params,
                 opt);
 }
 
-bool Tdcp(SnapshotPtr            snapshot_r,
-          SnapshotPtr            snapshot_k,
-          const Eigen::Vector3d& x_r,
-          Eigen::Vector4d&       d,
-          Eigen::Matrix4d&       cov_d,
-          double&                residual,
-          const std::set<int>&   discarded_sats,
-          std::set<int>&         raim_discarded_sats,
-          const TdcpBatchParams& tdcp_params,
-          const Options&         opt)
+TdcpOutput Tdcp(SnapshotPtr            snapshot_r,
+                SnapshotPtr            snapshot_k,
+                const Eigen::Vector3d& x_r,
+                const Eigen::Vector4d& d_0,
+                const std::set<int>&   discarded_sats,
+                const TdcpBatchParams& tdcp_params,
+                const Options&         opt)
 {
     // If use old nav temporary change navigation to (re)compute satellites positions
     auto nav_k = snapshot_k->getNavigation();
@@ -68,15 +59,12 @@ bool Tdcp(SnapshotPtr            snapshot_r,
                                                             snapshot_k->getRanges());
 
     // COMPUTE TDCP
-    bool tdcp_ok = Tdcp(snapshot_r,
-                        snapshot_k,
-                        x_r,
-                        common_sats,
-                        d,
-                        cov_d,
-                        residual,
-                        raim_discarded_sats,
-                        tdcp_params);
+    TdcpOutput output = Tdcp(snapshot_r,
+                             snapshot_k,
+                             x_r,
+                             common_sats,
+                             d_0,
+                             tdcp_params);
 
     // UNDO temporary change navigation
     if (tdcp_params.tdcp.use_old_nav)
@@ -85,25 +73,19 @@ bool Tdcp(SnapshotPtr            snapshot_r,
         snapshot_k->computeSatellites(opt.sateph);
     }
 
-    return tdcp_ok;
+    return output;
 }
 
-bool Tdcp(SnapshotPtr               snapshot_r,
-          SnapshotPtr               snapshot_k,
-          const Eigen::Vector3d&    x_r,
-          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)
+TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
+                SnapshotPtr               snapshot_k,
+                const Eigen::Vector3d&    x_r,
+                const std::set<int>&      common_sats,
+                const Eigen::Vector4d&    d_0,
+                const TdcpBatchParams&    tdcp_params)
 {
-    // CHECKS
-    if (not raim_discarded_sats.empty())
-    {
-        std::cout << "TDCP: 'raim_discarded_sats' is just an output param, it will be cleared.\n";
-        raim_discarded_sats.clear();
-    }
+
+    TdcpOutput output;
+
     assert(snapshot_r->satellitesComputed() && "this TDCP assumes satellites already computed");
     assert(snapshot_k->satellitesComputed() && "this TDCP assumes satellites already computed");
     assert(snapshot_r->rangesComputed() && "this TDCP assumes ranges already computed");
@@ -120,7 +102,6 @@ bool Tdcp(SnapshotPtr               snapshot_r,
         assert(snapshot_k->getRanges().count(sat) && "common sat not stored in ranges");
     }
 
-
     int n_common_sats = common_sats.size();
     int required_n_sats(std::max(tdcp_params.min_common_sats + tdcp_params.raim_n, 4 + tdcp_params.raim_n));
     if (n_common_sats < required_n_sats)
@@ -129,7 +110,9 @@ bool Tdcp(SnapshotPtr               snapshot_r,
         printf("Tdcp: NOT ENOUGH COMMON SATS");
         printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]", n_common_sats, required_n_sats);
         #endif
-        return false;
+        output.msg = "Not enough common sats";
+        output.success = false;
+        return output;
     }
 
     // Times
@@ -171,11 +154,11 @@ bool Tdcp(SnapshotPtr               snapshot_r,
     #endif
 
     // FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
-    Eigen::Matrix<double, Eigen::Dynamic, 4> A(Eigen::Matrix<double, Eigen::Dynamic, 4>::Zero(n_differences, 4));
-    Eigen::VectorXd                          r(Eigen::VectorXd::Zero(n_differences));
-    Eigen::VectorXd                          drho_m(Eigen::VectorXd::Zero(n_differences));
-    Eigen::Matrix<double, 3, Eigen::Dynamic> s_k(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
-    Eigen::Matrix<double, 3, Eigen::Dynamic> s_r(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
+    Eigen::MatrixXd A(Eigen::MatrixXd::Zero(n_differences, 4));
+    Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
+    Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
+    Eigen::MatrixXd s_k(Eigen::MatrixXd::Zero(3, n_differences));
+    Eigen::MatrixXd s_r(Eigen::MatrixXd::Zero(3, n_differences));
 
     for (auto row_sat_freq_it : row_2_sat_freq)
     {
@@ -209,7 +192,7 @@ bool Tdcp(SnapshotPtr               snapshot_r,
         if (!tdcp_params.relinearize_jacobian)
         {
             // Unit vectors from receivers to satellites
-            Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+            Eigen::Vector3d u_k = (s_k.col(row) - x_r - d_0.head(3)).normalized();
 
             // Fill Jacobian matrix
             A(row, 0) = u_k(0);
@@ -221,34 +204,47 @@ bool Tdcp(SnapshotPtr               snapshot_r,
 
     // LEAST SQUARES SOLUTION =======================================================================
     std::set<int> raim_discarded_rows;
-    bool result = Tdcp(x_r, A, r, drho_m, s_k, s_r, d, cov_d, residual, raim_discarded_rows, tdcp_params);
+    output = Tdcp(x_r, A, r, drho_m, s_k, s_r, d_0, raim_discarded_rows, tdcp_params);
 
+    // FILL OUTPUT
+    output.used_sats = common_sats;
     for (auto disc_row : raim_discarded_rows)
-        raim_discarded_sats.insert(row_2_sat_freq[disc_row].first);
+    {
+        output.raim_discarded_sats.insert(row_2_sat_freq[disc_row].first);
+        output.used_sats.erase(row_2_sat_freq[disc_row].first);
+    }
+    output.dt = tk - tr;
 
     // weight covariance with the measurement noise (proportional to time)
     double sq_sigma_Tdcp = (tk - tr) * tdcp_params.tdcp.sigma_atm * tdcp_params.tdcp.sigma_atm +
                            2 * tdcp_params.tdcp.sigma_carrier * tdcp_params.tdcp.sigma_carrier;
-    cov_d *= sq_sigma_Tdcp;
+    output.cov_d *= sq_sigma_Tdcp;
 
-    return result;
+    return output;
 }
 
-bool Tdcp(const Eigen::Vector3d&    x_r,
-          Eigen::Matrix<double, Eigen::Dynamic, 4>& A,
-          Eigen::VectorXd& r,
-          Eigen::VectorXd& drho_m,
-          Eigen::Matrix<double, 3, Eigen::Dynamic>& s_k,
-          Eigen::Matrix<double, 3, Eigen::Dynamic>& s_r,
-          Eigen::Vector4d&          d,
-          Eigen::Matrix4d&          cov_d,
-          double&                   residual,
-          std::set<int>&            raim_discarded_rows,
-          const TdcpBatchParams&    tdcp_params)
+TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
+                Eigen::MatrixXd&        A,
+                Eigen::VectorXd&        r,
+                Eigen::VectorXd&        drho_m,
+                Eigen::MatrixXd&        s_k,
+                Eigen::MatrixXd&        s_r,
+                const Eigen::Vector4d&  d_0,
+                std::set<int>&          raim_discarded_rows,
+                const TdcpBatchParams&  tdcp_params)
 {
+    assert(A.cols() == 4);
+    assert(s_k.rows() == 3);
+    assert(s_r.rows() == 3);
+
+    TdcpOutput output;
+    Eigen::Vector4d& d(output.d);
+    Eigen::Matrix4d& cov_d(output.cov_d);
+    double& residual(output.residual);
 
     // Initial guess
-    Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
+    Eigen::Vector4d delta_d(Eigen::Vector4d::Zero());
+    d = d_0;
 
     for (int j = 0; j < tdcp_params.max_iterations; j++)
     {
@@ -298,7 +294,9 @@ bool Tdcp(const Eigen::Vector3d&    x_r,
                 std::cout << "Tdcp: cov_d         = \n" << cov_d << std::endl;
                 printf("Tdcp: dT            = %8.3f secs\n", d(3));
             #endif
-            return false;
+            output.msg = "NaN values in NLS";
+            output.success = false;
+            return output;
         }
 
         // residual
@@ -387,7 +385,9 @@ bool Tdcp(const Eigen::Vector3d&    x_r,
                 if (worst_sat_row == -1)
                 {
                     printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
-                    return false;
+                    output.msg = "NaN values in NLS after RAIM";
+                    output.success = false;
+                    return output;
                 }
 
                 // store removed sat
@@ -466,7 +466,9 @@ bool Tdcp(const Eigen::Vector3d&    x_r,
     // residual = (r - A * d).norm() / A.rows();
     residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
 
-    return true;
+    output.success = true;
+
+    return output;
 }
 
 
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
index 7120539b8ab19fe5355afb5f1bd0156f9fb475b5..1338b7e09879b6d6ae93c551a52f6165e34f0112 100644
--- a/test/gtest_tdcp.cpp
+++ b/test/gtest_tdcp.cpp
@@ -65,7 +65,7 @@ TEST(Tdcp, Tdcp)
     Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
     Matrix3d R_ENU_ECEF;
     Vector2d azel, azel2;
-    Vector4d d, d_gt;
+    Vector4d d_gt;
     Matrix4d cov_d;
     double range, residual;
 
@@ -166,22 +166,18 @@ TEST(Tdcp, Tdcp)
         }
 
         // TDCP
-        d = Vector4d::Zero(); // initialize
-        bool tdcp_ok = Tdcp(snapshot_r,
-                            snapshot_k,
-                            x_r_ECEF,
-                            common_sats,
-                            d,
-                            cov_d,
-                            residual,
-                            raim_discarded_sats,
-                            tdcp_params);
+        auto tdcp_out = Tdcp(snapshot_r,
+                             snapshot_k,
+                             x_r_ECEF,
+                             common_sats,
+                             Vector4d::Zero(),
+                             tdcp_params);
 
         // CHECKS
         std::cout << "CHECKS iteration " << i << std::endl;
-        ASSERT_TRUE(tdcp_ok);
-        ASSERT_LE(residual, 1e-9);
-        ASSERT_MATRIX_APPROX(d, d_gt, 1e-6);
+        ASSERT_TRUE(tdcp_out.success);
+        ASSERT_LE(tdcp_out.residual, 1e-9);
+        ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
     }
 }
 
@@ -307,26 +303,23 @@ TEST(Tdcp, Tdcp_raim)
         int wrong_sat2 = (i + N_sats / 2) % N_sats;
         snapshot_r->getRanges().at(wrong_sat2).L_corrected -= 10;
 
+
         // TDCP
-        d = Vector4d::Zero(); // initialize
-        bool tdcp_ok = Tdcp(snapshot_r,
-                            snapshot_k,
-                            x_r_ECEF,
-                            common_sats,
-                            d,
-                            cov_d,
-                            residual,
-                            raim_discarded_sats,
-                            tdcp_params);
+        auto tdcp_out = Tdcp(snapshot_r,
+                             snapshot_k,
+                             x_r_ECEF,
+                             common_sats,
+                             Vector4d::Zero(),
+                             tdcp_params);
 
         // CHECKS
         std::cout << "CHECKS iteration " << i << std::endl;
-        ASSERT_EQ(raim_discarded_sats.size(), 2);
-        ASSERT_TRUE(raim_discarded_sats.count(wrong_sat1));
-        ASSERT_TRUE(raim_discarded_sats.count(wrong_sat2));
-        ASSERT_TRUE(tdcp_ok);
-        ASSERT_LE(residual, 1e-9);
-        ASSERT_MATRIX_APPROX(d, d_gt, 1e-6);
+        ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2);
+        ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1));
+        ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2));
+        ASSERT_TRUE(tdcp_out.success);
+        ASSERT_LE(tdcp_out.residual, 1e-9);
+        ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
     }
 }