TimeStamp.cc
Go to the documentation of this file.
00001 
00042 #include "TimeStamp.hh"
00043 
00044 #ifndef WIN32
00045 #include <sys/time.h>
00046 #endif
00047 #include <time.h>
00048 
00049 namespace crl {
00050 namespace multisense {
00051 namespace details {
00052 namespace utility {
00053 
00054 
00055 // Initialize static routines.
00056 
00057 double TimeStamp::timeSynchronizationOffset = 0.0;
00058 
00059 #if defined (WIN32)
00060 
00061 //
00062 // The FILETIME structure in Windows measures time in 100-nanosecond intervals
00063 // since 1601-Jan-01. The timeval structure measures time in second intervals
00064 // since 1970-Jan-01. This function computes the number of seconds (as a double)
00065 // that we need to apply as an offset to ensure that clock times are all tracked
00066 // from the same epoch.
00067 static ULARGE_INTEGER initOffsetSecondsSince1970 ()
00068 {
00069     SYSTEMTIME epochTimeAsSystemTime = { 1970, 1, 0, 1, 0, 0, 0, 0 };
00070     FILETIME epochTimeAsFileTime;
00071     SystemTimeToFileTime (&epochTimeAsSystemTime, &epochTimeAsFileTime);
00072 
00073     ULARGE_INTEGER epochTime;
00074     epochTime.LowPart = epochTimeAsFileTime.dwLowDateTime;
00075     epochTime.HighPart = epochTimeAsFileTime.dwHighDateTime;
00076 
00077     return epochTime;
00078 }
00079 
00080 ULARGE_INTEGER TimeStamp::offsetSecondsSince1970 = initOffsetSecondsSince1970 ();
00081 
00082 #endif
00083 
00084 /*
00085  * Constructor. Empty. We rely on the getter methods to do
00086  * things that are more useful.
00087  */
00088 TimeStamp::TimeStamp()
00089 {
00090     this->time.tv_sec = 0;
00091     this->time.tv_usec = 0;
00092 }
00093 
00094 /*
00095  * Constructor. Initializes with the specified timestamp value.
00096  */
00097 TimeStamp::TimeStamp(struct timeval& value)
00098 {
00099     this->set(value);
00100 }
00101 
00102 /*
00103  * Cosntructor. Initializes with the specified timestamp value.
00104  */
00105 TimeStamp::TimeStamp(double value)
00106 {
00107     this->time.tv_sec = (int) value;
00108     this->time.tv_usec = (int) ((value - this->time.tv_sec) * 1000000);
00109 }
00110 
00111 /*
00112  * Sets this timestamp equal to the timestamp specified.
00113  */
00114 void TimeStamp::set(struct timeval& value)
00115 {
00116     this->time.tv_sec = value.tv_sec;
00117     this->time.tv_usec = value.tv_usec;
00118 }
00119 
00120 /*
00121  * Sets the time, for time synchronization. This will report the local clock when
00122  * the PPS event occurred, and the remote clock when the PPS event occurred.
00123  *
00124  * When you use the routine 'getCurrentTime()', it will then return a timestamp
00125  * that is time-synchronized with the remote system.
00126  */
00127 void TimeStamp::setTimeAtPps(TimeStamp& local, TimeStamp& remote)
00128 {
00129     setTimeAtPps(local.time, remote.time);
00130 }
00131 
00132 /*
00133  * Sets the time, for time synchronization. This will report the local clock when
00134  * the PPS event occurred, and the remote clock when the PPS event occurred.
00135  *
00136  * When you use the routine 'getCurrentTime()', it will then return a timestamp
00137  * that is time-synchronized with the remote system.
00138  */
00139 void TimeStamp::setTimeAtPps(struct timeval& local, struct timeval& remote)
00140 {
00141     //
00142     // To make things atomic, we are somewhat in trouble. To make things lock-free,
00143     // we are going to do all of the math as doubles. Convert the above timestamps
00144     // to doubles.
00145     //
00146 
00147     double localTimeAtPps = local.tv_sec + (local.tv_usec / 1000000.0);
00148     double remoteTimeAtPps = remote.tv_sec + (remote.tv_usec / 1000000.0);
00149 
00150     //
00151     // Store the offset between the two as a static variable.
00152     //
00153 
00154     timeSynchronizationOffset = remoteTimeAtPps - localTimeAtPps;
00155 }
00156 
00157 /*
00158  * Returns the offset from the remote clock to the local clock. Add this to the
00159  * time returned by a standard time call to have a synchronized time. Notice that
00160  * this is already applied to anything using TimeStamps normally.
00161  */
00162 double TimeStamp::getTimeSynchronizationOffset()
00163 {
00164     return timeSynchronizationOffset;
00165 }
00166 
00167 #ifndef SENSORPOD_FIRMWARE
00168 
00169 /*
00170  * This routine will get the current time (as gettimeofday()) and
00171  * store it off. It is the normal way of initializing time. Notice
00172  * that there may be large time skips when you call this routine, due
00173  * to time synchronization jumps.
00174  *
00175  * Notice that the timestamp returned by this object *is* atomic. It
00176  * will not change, even if time synchronization skews things.
00177  */
00178 TimeStamp TimeStamp::getCurrentTime()
00179 {
00180     //
00181     // Create a new timestamp object and fill it in with
00182     // the current time of day.
00183     //
00184 
00185     TimeStamp timeStamp;
00186 
00187 #if defined (WIN32)
00188 
00189     // gettimeofday does not exist on Windows
00190     FILETIME currentTimeAsFileTime;
00191     GetSystemTimeAsFileTime (&currentTimeAsFileTime);
00192 
00193     ULARGE_INTEGER currentTimeAsLargeInteger;
00194     currentTimeAsLargeInteger.LowPart = currentTimeAsFileTime.dwLowDateTime;
00195     currentTimeAsLargeInteger.HighPart = currentTimeAsFileTime.dwHighDateTime;
00196     currentTimeAsLargeInteger.QuadPart -= offsetSecondsSince1970.QuadPart;
00197 
00198     timeStamp.time.tv_sec = static_cast<long> (currentTimeAsLargeInteger.QuadPart / 10000000);
00199     timeStamp.time.tv_usec = static_cast<long> ((currentTimeAsLargeInteger.QuadPart - timeStamp.time.tv_sec * 10000000) / 10);
00200 
00201 #else
00202     gettimeofday(&timeStamp.time, 0);
00203 #endif
00204 
00205     //
00206     // Transform it into a double... Notice that this (quite handily)
00207     // removes all precision up to the 100-nanosecond mark, making carrying
00208     // times around as timevals fairly pointless. We do this to apply the
00209     // time synchronization offset, lockless-ly.
00210     //
00211     // It's probably that this is pointless, and we should add a lock
00212     // in the future.
00213     //
00214 
00215     double currentTime = (double) timeStamp;
00216 
00217     currentTime += timeSynchronizationOffset;
00218 
00219     timeStamp = currentTime;
00220 
00221     //
00222     // Return the final timestamp.
00223     //
00224 
00225     return timeStamp;
00226 }
00227 
00228 #endif // SENSORPOD_FIRMWARE
00229 
00230 /*
00231  * Returns the seconds portion of the timestamp.
00232  */
00233 uint32_t TimeStamp::getSeconds() const
00234 {
00235     return this->time.tv_sec;
00236 }
00237 
00238 /*
00239  * Returns the microseconds portion of the timestamp.
00240  */
00241 uint32_t TimeStamp::getMicroSeconds() const
00242 {
00243     return this->time.tv_usec;
00244 }
00245 
00246 /*
00247  * Returns the stored time as if it was a double. This will be seconds since 1970.
00248  * The time will be accurate to the (roughly) 100-nanosecond mark.
00249  */
00250 TimeStamp::operator double() const
00251 {
00252     return this->time.tv_sec + (this->time.tv_usec / 1000000.0);
00253 }
00254 
00255 /*
00256  * Sets the stored time from the seconds value given.
00257  */
00258 TimeStamp& TimeStamp::operator=(double timeStamp)
00259 {
00260     this->time.tv_sec = ((unsigned long) timeStamp);
00261     this->time.tv_usec = ((unsigned long) ((timeStamp - this->time.tv_sec) * 1000000));
00262 
00263     // This call avoids having negative microseconds if the input
00264     // argument is less than zero and non-integral.
00265     this->normalize();
00266     
00267     return *this;
00268 }
00269 
00270 
00271 TimeStamp& TimeStamp::operator+=(TimeStamp const& other)
00272 {
00273     this->time.tv_sec += other.time.tv_sec;
00274     this->time.tv_usec += other.time.tv_usec;
00275     this->normalize();
00276     return *this;
00277 }
00278   
00279 
00280 TimeStamp& TimeStamp::operator-=(TimeStamp const& other)
00281 {
00282     this->time.tv_sec -= other.time.tv_sec;
00283     this->time.tv_usec -= other.time.tv_usec;
00284     this->normalize();
00285     return *this;
00286 }
00287 
00288   
00289 void TimeStamp::normalize()
00290 {
00291     while(this->time.tv_usec < 0) {
00292         this->time.tv_usec += 1000000;
00293         this->time.tv_sec -= 1;
00294     }
00295 
00296     while(this->time.tv_usec >= 1000000) {
00297         this->time.tv_usec -= 1000000;
00298         this->time.tv_sec += 1;
00299     }
00300 }
00301 
00302 
00303 //
00304 // Arithmetic operators are mostly handled by implicit conversion to
00305 // and from double.
00306 //
00307 
00308 // TimeStamp operator +(TimeStamp const& arg0, TimeStamp const& arg1)
00309 // {
00310 //   TimeStamp result = arg0;
00311 //   result += arg1;
00312 //   return result;
00313 // }
00314 
00315   
00316 // TimeStamp operator -(TimeStamp const& arg0, TimeStamp const& arg1)
00317 // {
00318 //   TimeStamp result = arg0;
00319 //   result -= arg1;
00320 //   return result;
00321 // }
00322   
00323 }}}} // namespaces


multisense_lib
Author(s):
autogenerated on Fri Apr 5 2019 02:28:24