From 664cff91b3fcd9052275f9d04923a8a932b0fc7f Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Tue, 17 Apr 2018 15:23:27 +0200
Subject: [PATCH] Improve the computation of the initial time.

---
 talos_roscontrol_sot/src/log.cpp | 10 ++++++----
 talos_roscontrol_sot/src/log.hh  |  3 ++-
 2 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/talos_roscontrol_sot/src/log.cpp b/talos_roscontrol_sot/src/log.cpp
index a30431b..7ec7abe 100644
--- a/talos_roscontrol_sot/src/log.cpp
+++ b/talos_roscontrol_sot/src/log.cpp
@@ -52,10 +52,11 @@ void Log::init(unsigned int nbDofs, unsigned int length)
   nbDofs_=nbDofs;
   length_=length;
   StoredData_.init(nbDofs,length);
-  struct timeval current;
-  gettimeofday(&current,0);
 
-  timeorigin_ = (double)current.tv_sec + 0.000001 * ((double)current.tv_usec);
+  gettimeofday(timeorigin_tv_,0);
+
+  timeorigin_ = (double)timeorigin_tv_.tv_sec +
+    0.000001 * ((double)timeorigin_tv_.tv_usec);
 
 }
 
@@ -97,7 +98,8 @@ void Log::record(DataToLog &aDataToLog)
   gettimeofday(&current,0);
 
   StoredData_.timestamp[lrefts_] = 
-    ((double)current.tv_sec + 0.000001 * (double)current.tv_usec) - timeorigin_;
+    ((double)(current.tv_sec - time_origin_tv_.tv_sec)
+     + 0.000001 * (double)(current.tv_usec - time_origin_tv_.tv_usec));
 
   StoredData_.duration[lrefts_] = time_stop_it_ - time_start_it_;
     
diff --git a/talos_roscontrol_sot/src/log.hh b/talos_roscontrol_sot/src/log.hh
index 66fc44d..8d3529f 100644
--- a/talos_roscontrol_sot/src/log.hh
+++ b/talos_roscontrol_sot/src/log.hh
@@ -9,6 +9,7 @@
 
 #include <vector>
 #include <string>
+#include <sys/time.h>
 
 namespace rc_sot_system {
 
@@ -62,8 +63,8 @@ namespace rc_sot_system {
 
     // Circular buffer for all the data.
     DataToLog StoredData_;
-
     double timeorigin_;
+    struct timeval timeorigin_tv_;
     double time_start_it_;
     double time_stop_it_;
 
-- 
GitLab