00001 00060 #ifdef __LINUX__ 00061 #include <schunk_powercube_chain/TimeStamp.h> 00062 #else 00063 #include <windows.h> 00064 #include <schunk_powercube_chain/TimeStamp.h> 00065 00066 namespace RTB { 00067 #endif 00068 00069 //----------------------------------------------------------------------------- 00070 #ifdef __LINUX__ 00071 TimeStamp::TimeStamp() 00072 { 00073 m_TimeStamp.tv_sec = 0; 00074 m_TimeStamp.tv_nsec = 0; 00075 } 00076 00077 void TimeStamp::SetNow() 00078 { 00079 ::clock_gettime(CLOCK_REALTIME, &m_TimeStamp); 00080 } 00081 00082 double TimeStamp::TimespecToDouble(const ::timespec& LargeInt) 00083 { 00084 return double(LargeInt.tv_sec) + double(LargeInt.tv_nsec) / 1e9; 00085 } 00086 00087 ::timespec TimeStamp::DoubleToTimespec(double TimeS) 00088 { 00089 ::timespec DeltaTime; 00090 if (! ( TimeS < 4e9 && TimeS > 0.0 )) 00091 { 00092 DeltaTime.tv_sec = 0; 00093 DeltaTime.tv_nsec = 0; 00094 return DeltaTime; 00095 } 00096 00097 DeltaTime.tv_sec = ::time_t(TimeS); 00098 DeltaTime.tv_nsec 00099 = static_cast<long int>((TimeS - double(DeltaTime.tv_sec)) * 1e9); 00100 00101 return DeltaTime; 00102 } 00103 00104 double TimeStamp::operator-(const TimeStamp& EarlierTime) const 00105 { 00106 ::timespec Res; 00107 00108 Res.tv_sec = m_TimeStamp.tv_sec - EarlierTime.m_TimeStamp.tv_sec; 00109 Res.tv_nsec = m_TimeStamp.tv_nsec - EarlierTime.m_TimeStamp.tv_nsec; 00110 00111 if (Res.tv_nsec < 0) { 00112 Res.tv_sec--; 00113 Res.tv_nsec += 1000000000; 00114 } 00115 00116 return TimespecToDouble(Res); 00117 } 00118 00119 void TimeStamp::operator+=(double TimeS) 00120 { 00121 ::timespec Dbl = DoubleToTimespec(TimeS); 00122 m_TimeStamp.tv_sec += Dbl.tv_sec; 00123 m_TimeStamp.tv_nsec += Dbl.tv_nsec; 00124 if (m_TimeStamp.tv_nsec > 1000000000) 00125 { 00126 m_TimeStamp.tv_sec++; 00127 m_TimeStamp.tv_nsec -= 1000000000; 00128 } 00129 } 00130 00131 void TimeStamp::operator-=(double TimeS) 00132 { 00133 ::timespec Dbl = DoubleToTimespec(TimeS); 00134 m_TimeStamp.tv_sec -= Dbl.tv_sec; 00135 m_TimeStamp.tv_nsec -= Dbl.tv_nsec; 00136 if (m_TimeStamp.tv_nsec < 0.0) 00137 { 00138 m_TimeStamp.tv_sec--; 00139 m_TimeStamp.tv_nsec += 1000000000; 00140 } 00141 } 00142 00143 bool TimeStamp::operator>(const TimeStamp& Time) 00144 { 00145 if (m_TimeStamp.tv_sec > Time.m_TimeStamp.tv_sec) return true; 00146 if ((m_TimeStamp.tv_sec == Time.m_TimeStamp.tv_sec) && 00147 (m_TimeStamp.tv_nsec > Time.m_TimeStamp.tv_nsec)) return true; 00148 return false; 00149 } 00150 00151 bool TimeStamp::operator<(const TimeStamp& Time) 00152 { 00153 if (m_TimeStamp.tv_sec < Time.m_TimeStamp.tv_sec) return true; 00154 if ((m_TimeStamp.tv_sec == Time.m_TimeStamp.tv_sec) && 00155 (m_TimeStamp.tv_nsec < Time.m_TimeStamp.tv_nsec)) return true; 00156 return false; 00157 } 00158 00159 void TimeStamp::getTimeStamp(long& lSeconds, long& lNanoSeconds) 00160 { 00161 lSeconds = m_TimeStamp.tv_sec; 00162 lNanoSeconds = m_TimeStamp.tv_nsec; 00163 }; 00164 00165 void TimeStamp::setTimeStamp(const long& lSeconds, const long& lNanoSeconds) 00166 { 00167 m_TimeStamp.tv_sec = lSeconds; 00168 m_TimeStamp.tv_nsec = lNanoSeconds; 00169 }; 00170 00171 #else 00172 //------------------------------------------------------------------- 00173 00174 LARGE_INTEGER ClockFrequency; 00175 BOOL CounterHW = QueryPerformanceFrequency( &ClockFrequency ); 00176 double TimeStamp::m_SecPerCount = 1.0 / TimeStamp::LargeIntToDouble(ClockFrequency); 00177 double TimeStamp::m_DwordSize = double(0xFFFFFFFF); 00178 00179 //----------------------------------------------------------------------------- 00180 00181 TimeStamp::TimeStamp() 00182 { 00183 m_TimeStamp.HighPart = 0; 00184 m_TimeStamp.LowPart = 0; 00185 } 00186 00187 void TimeStamp::SetNow() 00188 { 00189 QueryPerformanceCounter( &m_TimeStamp ); 00190 } 00191 00192 double TimeStamp::LargeIntToDouble(const LARGE_INTEGER& LargeInt) 00193 { 00194 return LargeInt.HighPart * m_DwordSize + LargeInt.LowPart; 00195 } 00196 00197 LARGE_INTEGER TimeStamp::DoubleToLargeInt(double TimeS) 00198 { 00199 LARGE_INTEGER DeltaTime; 00200 DeltaTime.LowPart = 0; 00201 DeltaTime.HighPart = 0; 00202 if (! ( TimeS < 4e9 && TimeS > 0.0 )) return DeltaTime; 00203 DeltaTime.LowPart = (unsigned long)(TimeS / m_SecPerCount); 00204 return DeltaTime; 00205 } 00206 00207 double TimeStamp::operator-(const TimeStamp& EarlierTime) const 00208 { 00209 LARGE_INTEGER Res; 00210 Res.HighPart = m_TimeStamp.HighPart - EarlierTime.m_TimeStamp.HighPart; 00211 Res.LowPart = m_TimeStamp.LowPart - EarlierTime.m_TimeStamp.LowPart; 00212 if (EarlierTime.m_TimeStamp.LowPart > m_TimeStamp.LowPart) 00213 Res.HighPart--; 00214 return LargeIntToDouble(Res) * m_SecPerCount; 00215 } 00216 00217 void TimeStamp::operator+=(double TimeS) 00218 { 00219 LARGE_INTEGER& Dbl = DoubleToLargeInt(TimeS); 00220 LARGE_INTEGER Res; 00221 Res.LowPart = m_TimeStamp.LowPart + Dbl.LowPart; 00222 if ((Res.LowPart < m_TimeStamp.LowPart) || (Res.LowPart < Dbl.LowPart)) 00223 m_TimeStamp.HighPart++; 00224 m_TimeStamp.LowPart = Res.LowPart; 00225 } 00226 00227 void TimeStamp::operator-=(double TimeS) 00228 { 00229 LARGE_INTEGER& Dbl = DoubleToLargeInt(TimeS); 00230 LARGE_INTEGER Res; 00231 Res.LowPart = m_TimeStamp.LowPart - Dbl.LowPart; 00232 Res.HighPart = m_TimeStamp.HighPart - Dbl.HighPart; 00233 if ((Res.LowPart > m_TimeStamp.LowPart)) 00234 Res.HighPart--; 00235 m_TimeStamp.LowPart = Res.LowPart; 00236 m_TimeStamp.HighPart = Res.HighPart; 00237 } 00238 00239 bool TimeStamp::operator>(const TimeStamp& Time) 00240 { 00241 if (m_TimeStamp.HighPart > Time.m_TimeStamp.HighPart) return true; 00242 if ((m_TimeStamp.HighPart == Time.m_TimeStamp.HighPart) && 00243 (m_TimeStamp.LowPart > Time.m_TimeStamp.LowPart)) return true; 00244 return false; 00245 } 00246 00247 bool TimeStamp::operator<(const TimeStamp& Time) 00248 { 00249 if (m_TimeStamp.HighPart < Time.m_TimeStamp.HighPart) return true; 00250 if ((m_TimeStamp.HighPart == Time.m_TimeStamp.HighPart) && 00251 (m_TimeStamp.LowPart < Time.m_TimeStamp.LowPart)) return true; 00252 return false; 00253 } 00254 } 00255 #endif 00256