MRPT  2.0.4
CControlledRateTimer.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "system-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/core/bits_math.h>
13 #include <mrpt/core/exceptions.h>
15 
16 #include <cmath> // std::abs(double)
17 
18 using namespace mrpt::system;
19 
21  : mrpt::system::COutputLogger("CControlledRateTimer")
22 {
23  m_tic.Tic();
24  setRate(rate_hz);
25 }
26 void CControlledRateTimer::setRate(const double rate_hz)
27 {
28  if (rate_hz == m_rate_hz) return;
29 
30  ASSERT_ABOVE_(rate_hz, 0.0);
31  m_rate_hz = rate_hz;
34  m_lastTic = 0;
35 }
36 
38 {
39  const bool validRateEstimate = internalUpdateRateEstimate();
40 
41  const double rawError = m_rate_hz - m_currentEstimatedRate;
42 
43  const double controlError =
44  mrpt::saturate_val(rawError, -0.2 * m_rate_hz, 0.2 * m_rate_hz);
45 
46  if (std::abs(rawError) / m_rate_hz > m_followErrorRatioForWarning)
47  {
49  2.0,
50  "Cannot run at the expected rate: actual_rate=%.03f Hz "
51  "desired_rate=%.03f Hz",
53  }
54 
55  // Trapezoidal approx. of PI(s) controller equation
56  // integral e(t)->s=(2/T)*(1-z^-1)/(1+z^-1)
57  // derivative e(t)->s=(1/T)*(1-z^-1)
58  const double q0 = m_Kp * (1 + 1.0 / (m_rate_hz * 2 * m_Ti));
59  const double q1 = m_Kp * (-1 + 1.0 / (m_rate_hz * 2 * m_Ti));
60 
61  double newRate;
62  if (validRateEstimate)
63  {
64  newRate =
65  m_ratetimer.rate() + q0 * controlError + q1 * m_lastControlError;
66  }
67  else
68  {
69  newRate = m_rate_hz;
70  }
71 
72  // Set control output:
73  m_ratetimer.setRate(newRate);
74 
75  m_lastControlError = controlError;
76 
77  return m_ratetimer.sleep();
78 }
79 
81 {
82  bool valid = false;
83  const double t_now = m_tic.Tac();
84 
85  if (m_lastTic > 0 && t_now > m_lastTic)
86  {
87  const double measuredPeriod = t_now - m_lastTic;
88  m_lastRawRate = (1.0 / measuredPeriod);
89 
90  // Filter:
92  (1.0 - m_lowPass_a0) * m_lastRawRate;
93  valid = true;
94  }
95 
96  m_lastTic = t_now;
97  return valid;
98 }
CControlledRateTimer.h
ASSERT_ABOVE_
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
exceptions.h
mrpt::system::CRateTimer::rate
double rate() const
Gets current rate (Hz)
Definition: system/CRateTimer.h:33
mrpt::system::CControlledRateTimer::m_lowPass_a0
double m_lowPass_a0
Definition: CControlledRateTimer.h:130
mrpt::system::CControlledRateTimer::m_currentEstimatedRate
double m_currentEstimatedRate
Definition: CControlledRateTimer.h:138
system-precomp.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::system::CControlledRateTimer::m_lastTic
double m_lastTic
Definition: CControlledRateTimer.h:139
mrpt::saturate_val
T saturate_val(const T &value, const T sat_min, const T sat_max)
Like saturate() but it returns the value instead of modifying the variable.
Definition: core/include/mrpt/core/bits_math.h:167
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:87
mrpt::system::CControlledRateTimer::m_tic
mrpt::system::CTicTac m_tic
Definition: CControlledRateTimer.h:140
mrpt::system::CControlledRateTimer::m_lastControlError
double m_lastControlError
Definition: CControlledRateTimer.h:135
mrpt::system::CRateTimer::sleep
bool sleep()
Sleeps for some time, such as the return of this method is 1/rate (seconds) after the return of the p...
Definition: CRateTimer.cpp:30
MRPT_LOG_THROTTLE_WARN_FMT
#define MRPT_LOG_THROTTLE_WARN_FMT(_PERIOD_SECONDS, _FMT_STRING,...)
Definition: system/COutputLogger.h:502
mrpt::system::CControlledRateTimer::m_ratetimer
mrpt::system::CRateTimer m_ratetimer
the one control acts on
Definition: CControlledRateTimer.h:128
mrpt::system::CRateTimer::setRate
void setRate(const double rate_hz)
Changes the object loop rate (Hz)
Definition: CRateTimer.cpp:25
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:76
mrpt::system::CControlledRateTimer::setRate
void setRate(const double rate_hz)
Changes the object loop rate (Hz)
Definition: CControlledRateTimer.cpp:26
mrpt::system::CControlledRateTimer::internalUpdateRateEstimate
bool internalUpdateRateEstimate()
Definition: CControlledRateTimer.cpp:80
mrpt::system::COutputLogger
Versatile class for consistent logging and management of output messages.
Definition: system/COutputLogger.h:117
bits_math.h
mrpt::system::CControlledRateTimer::sleep
bool sleep()
Sleeps for some time, such as the return of this method is 1/rate (seconds) after the return of the p...
Definition: CControlledRateTimer.cpp:37
mrpt::system::CControlledRateTimer::CControlledRateTimer
CControlledRateTimer(const double rate_hz=1.0)
Ctor: specifies the desired rate (Hz)
Definition: CControlledRateTimer.cpp:20
mrpt::system::CControlledRateTimer::m_rate_hz
double m_rate_hz
Definition: CControlledRateTimer.h:127
mrpt::system::CControlledRateTimer::m_Ti
double m_Ti
Definition: CControlledRateTimer.h:132
mrpt::system::CControlledRateTimer::m_followErrorRatioForWarning
double m_followErrorRatioForWarning
Definition: CControlledRateTimer.h:134
mrpt::system::CControlledRateTimer::m_Kp
double m_Kp
Definition: CControlledRateTimer.h:131
mrpt::system::CControlledRateTimer::m_lastRawRate
double m_lastRawRate
Definition: CControlledRateTimer.h:138
mrpt::system
Definition: backtrace.h:14



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020