rate.cpp
Go to the documentation of this file.
1 #include "rate.h"
2 #include <argos3/core/utility/logging/argos_log.h>
3 
4 using namespace argos;
5 
6 /****************************************/
7 /****************************************/
8 
9 CRate::CRate(Real f_rate) {
10  SetRate(f_rate);
11 }
12 
13 /****************************************/
14 /****************************************/
15 
17  /* Get current time */
18  ::timeval tNow;
19  ::gettimeofday(&tNow, nullptr);
20  /* Calculate difference between past call and now */
21  ::timeval tDiff;
22  timersub(&tNow, &m_tPast, &tDiff);
23  return tDiff.tv_sec * 1000000 + tDiff.tv_usec;
24 }
25 
26 /****************************************/
27 /****************************************/
28 
30  UInt64 unValue = ElapsedUS();
31  Real fValue = static_cast<Real>(unValue) / 1e6;
32  return fValue;
33 }
34 
35 /****************************************/
36 /****************************************/
37 
38 void CRate::Sleep() {
39  UInt64 unMicroSecDiff = ElapsedUS();
40  /* Sleep if necessary */
41  if(unMicroSecDiff < m_unNominalPeriod) {
42  ::timespec tSleepPeriod;
43  tSleepPeriod.tv_sec = static_cast<UInt32>((m_unNominalPeriod - unMicroSecDiff) / 1e6);
44  tSleepPeriod.tv_nsec = (m_unNominalPeriod - unMicroSecDiff) * 1000;
45  ::nanosleep(&tSleepPeriod, nullptr);
46  }
47  else {
48  LOGERR << "[WARNING] Nominal rate "
49  << m_fNominalRate
50  << " loops per sec delayed by "
51  << (unMicroSecDiff - m_unNominalPeriod)
52  << " microseconds"
53  << std::endl;
54  }
55  /* Update past for next call */
56  ::gettimeofday(&m_tPast, nullptr);
57 }
58 
59 /****************************************/
60 /****************************************/
61 
62 void CRate::SetRate(Real f_rate) {
63  m_fNominalRate = Abs(f_rate);
64  m_unNominalPeriod = 1e6 / m_fNominalRate;
65  ::gettimeofday(&m_tPast, nullptr);
66 }
67 
68 /****************************************/
69 /****************************************/
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
unsigned long long UInt64
64-bit unsigned integer.
Definition: datatypes.h:107
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
CARGoSLog LOGERR(std::cerr, SLogColor(ARGOS_LOG_ATTRIBUTE_BRIGHT, ARGOS_LOG_COLOR_RED))
Definition: argos_log.h:180
T Abs(const T &t_v)
Returns the absolute value of the passed argument.
Definition: general.h:25
CRate(Real f_rate)
Class constructor.
Definition: rate.cpp:9
UInt64 ElapsedUS() const
Returns the time elapsed since the last sleep in microseconds.
Definition: rate.cpp:16
void Sleep()
Sleeps for the appropriate time to complete the period.
Definition: rate.cpp:38
void SetRate(Real f_rate)
Sets the rate and resets the internal clock.
Definition: rate.cpp:62
Real ElapsedS() const
Returns the time elapsed since the last sleep in seconds.
Definition: rate.cpp:29