xsens_time.cpp

00001 // Note, this function requires compiler option "-lrt" to be set when compiling with gcc
00002 
00003 #include "xsens_time.h"
00004 #include <sys/timeb.h>
00005 
00006 #ifdef _WIN32
00007 #        include <windows.h>
00008 #else
00009 #        include <unistd.h>
00010 #   include <sys/time.h>
00011 #endif
00012 #include <math.h>
00013 
00014 namespace xsens {
00015 
00016 //////////////////////////////////////////////////////////////////////////////////////////
00017 //////////////////////////////////// Other  functions ////////////////////////////////////
00018 //////////////////////////////////////////////////////////////////////////////////////////
00019 #ifdef __APPLE__
00020 /* clock_gettime is not available on Apple/Darwin platforms. This function helps
00021  * maintaining compatibility with Linux code.
00022  */
00023 #define CLOCK_REALTIME 0
00024 static int clock_gettime(int clk_id, struct timespec *tp)
00025 {
00026         struct timeval now;
00027         
00028         int rv = gettimeofday(&now, NULL);
00029         if (rv != 0)
00030                 return rv;
00031         
00032         tp->tv_sec = now.tv_sec;
00033         tp->tv_nsec = now.tv_usec * 1000;
00034 
00035         return 0;
00036 }
00037 #endif
00038 //////////////////////////////////////////////////////////////////////////////////////////
00039 // A platform-independent clock.
00040 uint32_t getTimeOfDay(tm* date_, time_t* secs_)
00041 {
00042 #ifdef _WIN32
00043         static uint64_t startTimePc;
00044         static uint64_t lpf = 0;
00045         static __timeb64 startTimeB;
00046 
00047         LARGE_INTEGER pc;
00048         if (QueryPerformanceCounter(&pc))
00049         {
00050                 if (!lpf)
00051                 {
00052                         LARGE_INTEGER tmp;
00053                         QueryPerformanceFrequency(&tmp);
00054                         lpf = tmp.QuadPart;
00055                         startTimePc = pc.QuadPart;
00056 
00057                         _ftime64_s(&startTimeB);
00058                         // get rid of millitm part
00059                         startTimePc -= startTimeB.millitm*lpf/1000;
00060                         startTimeB.millitm = 0;
00061                 }
00062 
00063                 __timeb64 tp = startTimeB;
00064                 //_ftime32_s(&tp);
00065                 uint64_t dms = (pc.QuadPart-startTimePc)*1000/lpf;
00066                 tp.time += dms/1000;
00067                 tp.millitm = (unsigned short) (dms%1000);
00068 
00069                 if (date_ != NULL)
00070                 {
00071                         __time64_t tin = tp.time;
00072                         _localtime64_s(date_,&tin);
00073                 }
00074                 if (secs_ != NULL)
00075                         secs_[0] = tp.time+(tp.dstflag*3600)-(tp.timezone*60);
00076 
00077                 // 86400 = 24*60*60 = secs in a day, this gives us the seconds since midnight
00078                 return (1000 * ((uint32_t) tp.time % XSENS_SEC_PER_DAY)) + tp.millitm;
00079         }
00080         else
00081         {
00082                 __timeb32 tp;
00083                 _ftime32_s(&tp);
00084 
00085                 if (date_ != NULL)
00086                 {
00087                         __time32_t tin = tp.time;
00088                         _localtime32_s(date_,&tin);
00089                 }
00090                 if (secs_ != NULL)
00091                         secs_[0] = tp.time+(tp.dstflag*3600)-(tp.timezone*60);
00092 
00093                 // 86400 = 24*60*60 = secs in a day, this gives us the seconds since midnight
00094                 return (1000 * ((uint32_t) tp.time % XSENS_SEC_PER_DAY)) + tp.millitm;
00095         }
00096 #else
00097         timespec tp;
00098         clock_gettime(CLOCK_REALTIME, &tp); // compile with -lrt
00099 
00100         if (date_ != NULL)
00101                 localtime_r(&tp.tv_sec,date_);
00102 
00103         if (secs_ != NULL)
00104                 secs_[0] = tp.tv_sec;
00105 
00106         // 86400 = 24*60*60 = secs in a day, this gives us the seconds since midnight
00107         return (1000 * (tp.tv_sec % XSENS_SEC_PER_DAY)) + (tp.tv_nsec/1000000);
00108 #endif
00109 }
00110 
00111 //////////////////////////////////////////////////////////////////////////////////////////
00112 // A platform-independent sleep routine.
00113 void msleep(uint32_t ms)
00114 {
00115 #ifdef _WIN32
00116         Sleep(ms);
00117 #else
00118         clock_t end = clock() + (CLOCKS_PER_SEC/1000) * ms;
00119         clock_t diff;
00120 
00121         while ((diff = end - clock()) > 0)
00122         {
00123                 diff = (1000 * diff) / CLOCKS_PER_SEC;
00124                 if (diff > 1000)
00125                         sleep(diff / 1000);
00126                 else
00127                         usleep(diff * 1000);
00128         }
00129 #endif
00130 }
00131 
00132 TimeStamp timeStampNow(void)
00133 {
00134         TimeStamp ms;
00135         time_t s;
00136         ms = (TimeStamp) getTimeOfDay(NULL,&s);
00137         ms = (ms % 1000) + (((TimeStamp)s)*1000);
00138 
00139         return ms;
00140 }
00141 
00142 
00143 }        // end of xsens namespace
Generated on Sun May 8 08:41:38 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3