00001 /* 00002 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 00018 #ifdef __LINUX__ 00019 #include <schunk_powercube_chain/TimeStamp.h> 00020 #else 00021 #include <windows.h> 00022 #include <schunk_powercube_chain/TimeStamp.h> 00023 00024 namespace RTB 00025 { 00026 #endif 00027 00028 //----------------------------------------------------------------------------- 00029 #ifdef __LINUX__ 00030 TimeStamp::TimeStamp() 00031 { 00032 m_TimeStamp.tv_sec = 0; 00033 m_TimeStamp.tv_nsec = 0; 00034 } 00035 00036 void TimeStamp::SetNow() 00037 { 00038 ::clock_gettime(CLOCK_REALTIME, &m_TimeStamp); 00039 } 00040 00041 double TimeStamp::TimespecToDouble(const ::timespec& LargeInt) 00042 { 00043 return double(LargeInt.tv_sec) + double(LargeInt.tv_nsec) / 1e9; 00044 } 00045 00046 ::timespec TimeStamp::DoubleToTimespec(double TimeS) 00047 { 00048 ::timespec DeltaTime; 00049 if (!(TimeS < 4e9 && TimeS > 0.0)) 00050 { 00051 DeltaTime.tv_sec = 0; 00052 DeltaTime.tv_nsec = 0; 00053 return DeltaTime; 00054 } 00055 00056 DeltaTime.tv_sec = ::time_t(TimeS); 00057 DeltaTime.tv_nsec = static_cast<long int>((TimeS - double(DeltaTime.tv_sec)) * 1e9); 00058 00059 return DeltaTime; 00060 } 00061 00062 double TimeStamp::operator-(const TimeStamp& EarlierTime) const 00063 { 00064 ::timespec Res; 00065 00066 Res.tv_sec = m_TimeStamp.tv_sec - EarlierTime.m_TimeStamp.tv_sec; 00067 Res.tv_nsec = m_TimeStamp.tv_nsec - EarlierTime.m_TimeStamp.tv_nsec; 00068 00069 if (Res.tv_nsec < 0) 00070 { 00071 Res.tv_sec--; 00072 Res.tv_nsec += 1000000000; 00073 } 00074 00075 return TimespecToDouble(Res); 00076 } 00077 00078 void TimeStamp::operator+=(double TimeS) 00079 { 00080 ::timespec Dbl = DoubleToTimespec(TimeS); 00081 m_TimeStamp.tv_sec += Dbl.tv_sec; 00082 m_TimeStamp.tv_nsec += Dbl.tv_nsec; 00083 if (m_TimeStamp.tv_nsec > 1000000000) 00084 { 00085 m_TimeStamp.tv_sec++; 00086 m_TimeStamp.tv_nsec -= 1000000000; 00087 } 00088 } 00089 00090 void TimeStamp::operator-=(double TimeS) 00091 { 00092 ::timespec Dbl = DoubleToTimespec(TimeS); 00093 m_TimeStamp.tv_sec -= Dbl.tv_sec; 00094 m_TimeStamp.tv_nsec -= Dbl.tv_nsec; 00095 if (m_TimeStamp.tv_nsec < 0.0) 00096 { 00097 m_TimeStamp.tv_sec--; 00098 m_TimeStamp.tv_nsec += 1000000000; 00099 } 00100 } 00101 00102 bool TimeStamp::operator>(const TimeStamp& Time) 00103 { 00104 if (m_TimeStamp.tv_sec > Time.m_TimeStamp.tv_sec) 00105 return true; 00106 if ((m_TimeStamp.tv_sec == Time.m_TimeStamp.tv_sec) && (m_TimeStamp.tv_nsec > Time.m_TimeStamp.tv_nsec)) 00107 return true; 00108 return false; 00109 } 00110 00111 bool TimeStamp::operator<(const TimeStamp& Time) 00112 { 00113 if (m_TimeStamp.tv_sec < Time.m_TimeStamp.tv_sec) 00114 return true; 00115 if ((m_TimeStamp.tv_sec == Time.m_TimeStamp.tv_sec) && (m_TimeStamp.tv_nsec < Time.m_TimeStamp.tv_nsec)) 00116 return true; 00117 return false; 00118 } 00119 00120 void TimeStamp::getTimeStamp(long& lSeconds, long& lNanoSeconds) 00121 { 00122 lSeconds = m_TimeStamp.tv_sec; 00123 lNanoSeconds = m_TimeStamp.tv_nsec; 00124 }; 00125 00126 void TimeStamp::setTimeStamp(const long& lSeconds, const long& lNanoSeconds) 00127 { 00128 m_TimeStamp.tv_sec = lSeconds; 00129 m_TimeStamp.tv_nsec = lNanoSeconds; 00130 }; 00131 00132 #else 00133 //------------------------------------------------------------------- 00134 00135 LARGE_INTEGER ClockFrequency; 00136 BOOL CounterHW = QueryPerformanceFrequency(&ClockFrequency); 00137 double TimeStamp::m_SecPerCount = 1.0 / TimeStamp::LargeIntToDouble(ClockFrequency); 00138 double TimeStamp::m_DwordSize = double(0xFFFFFFFF); 00139 00140 //----------------------------------------------------------------------------- 00141 00142 TimeStamp::TimeStamp() 00143 { 00144 m_TimeStamp.HighPart = 0; 00145 m_TimeStamp.LowPart = 0; 00146 } 00147 00148 void TimeStamp::SetNow() 00149 { 00150 QueryPerformanceCounter(&m_TimeStamp); 00151 } 00152 00153 double TimeStamp::LargeIntToDouble(const LARGE_INTEGER& LargeInt) 00154 { 00155 return LargeInt.HighPart * m_DwordSize + LargeInt.LowPart; 00156 } 00157 00158 LARGE_INTEGER TimeStamp::DoubleToLargeInt(double TimeS) 00159 { 00160 LARGE_INTEGER DeltaTime; 00161 DeltaTime.LowPart = 0; 00162 DeltaTime.HighPart = 0; 00163 if (!(TimeS < 4e9 && TimeS > 0.0)) 00164 return DeltaTime; 00165 DeltaTime.LowPart = (unsigned long)(TimeS / m_SecPerCount); 00166 return DeltaTime; 00167 } 00168 00169 double TimeStamp::operator-(const TimeStamp& EarlierTime) const 00170 { 00171 LARGE_INTEGER Res; 00172 Res.HighPart = m_TimeStamp.HighPart - EarlierTime.m_TimeStamp.HighPart; 00173 Res.LowPart = m_TimeStamp.LowPart - EarlierTime.m_TimeStamp.LowPart; 00174 if (EarlierTime.m_TimeStamp.LowPart > m_TimeStamp.LowPart) 00175 Res.HighPart--; 00176 return LargeIntToDouble(Res) * m_SecPerCount; 00177 } 00178 00179 void TimeStamp::operator+=(double TimeS) 00180 { 00181 LARGE_INTEGER& Dbl = DoubleToLargeInt(TimeS); 00182 LARGE_INTEGER Res; 00183 Res.LowPart = m_TimeStamp.LowPart + Dbl.LowPart; 00184 if ((Res.LowPart < m_TimeStamp.LowPart) || (Res.LowPart < Dbl.LowPart)) 00185 m_TimeStamp.HighPart++; 00186 m_TimeStamp.LowPart = Res.LowPart; 00187 } 00188 00189 void TimeStamp::operator-=(double TimeS) 00190 { 00191 LARGE_INTEGER& Dbl = DoubleToLargeInt(TimeS); 00192 LARGE_INTEGER Res; 00193 Res.LowPart = m_TimeStamp.LowPart - Dbl.LowPart; 00194 Res.HighPart = m_TimeStamp.HighPart - Dbl.HighPart; 00195 if ((Res.LowPart > m_TimeStamp.LowPart)) 00196 Res.HighPart--; 00197 m_TimeStamp.LowPart = Res.LowPart; 00198 m_TimeStamp.HighPart = Res.HighPart; 00199 } 00200 00201 bool TimeStamp::operator>(const TimeStamp& Time) 00202 { 00203 if (m_TimeStamp.HighPart > Time.m_TimeStamp.HighPart) 00204 return true; 00205 if ((m_TimeStamp.HighPart == Time.m_TimeStamp.HighPart) && (m_TimeStamp.LowPart > Time.m_TimeStamp.LowPart)) 00206 return true; 00207 return false; 00208 } 00209 00210 bool TimeStamp::operator<(const TimeStamp& Time) 00211 { 00212 if (m_TimeStamp.HighPart < Time.m_TimeStamp.HighPart) 00213 return true; 00214 if ((m_TimeStamp.HighPart == Time.m_TimeStamp.HighPart) && (m_TimeStamp.LowPart < Time.m_TimeStamp.LowPart)) 00215 return true; 00216 return false; 00217 } 00218 } 00219 #endif