MRPT  2.0.4
CControlledRateTimer.h
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 #pragma once
10 
11 #include <mrpt/core/exceptions.h>
13 #include <mrpt/system/CRateTimer.h>
14 #include <mrpt/system/CTicTac.h>
15 
16 namespace mrpt::system
17 {
18 /** A class for calling sleep() in a loop, such that the amount of sleep time
19  * will be computed to make the loop run at the desired rate (in Hz).
20  * This class implements a PI controller on top of a vanilla CRateTimer object,
21  * ensuring a high accuracy in achieved execution rates. Note that this is done
22  * by setting a slightly-higher rate ("control action") to the internal
23  * CRateTimer, such that the error between the user-provided expected rate and
24  * the actual measured rate (low-pass filtered) is decreased by means of a PI
25  * controller.
26  *
27  * Note that rates higher than a few kHz are not attainable in all CPUs and/or
28  * kernel versions. Find below some graphs illustrating how this class tries to
29  * achieve a constant setpoint rate (given by the user), reacting to changes in
30  * the setpoint values:
31  *
32  * ![controlled rate timer plots](CControlledRateTimer_example.png)
33  *
34  * This graphs is generated with the example:
35  *
36  * `system_control_rate_timer_example --rate1 2000.0 --rate2 4000.0`
37  *
38  * All the parameters for the PI controller and low-pass filter (rate estimator)
39  * are settable by the user to adapt them to specific needs.
40  *
41  * \note Control law by [Francisco Jose MaƱas
42  * Alvarez](https://github.com/FranciscoJManasAlvarez)
43  *
44  * \note [New in MRPT 2.0.4]
45  * \ingroup mrpt_system_grp
46  */
48 {
49  public:
50  /** @name Main API
51  * @{ */
52 
53  /** Ctor: specifies the desired rate (Hz) */
54  CControlledRateTimer(const double rate_hz = 1.0);
55  /** Dtor */
56  virtual ~CControlledRateTimer() = default;
57 
58  /** Changes the object loop rate (Hz) */
59  void setRate(const double rate_hz);
60 
61  /** Sleeps for some time, such as the return of this method is 1/rate
62  * (seconds)
63  * after the return of the previous call.
64  * \return false if the rate could not be achieved ("we are already late"),
65  * true if all went right. */
66  bool sleep();
67 
68  /** @} */
69 
70  /** @name PI control parameters
71  * @{ */
72 
73  /** PI controller Kp parameter [default=1.0] */
74  double controllerParam_Kp() const { return m_Kp; }
75  void controllerParam_Kp(double v)
76  {
77  ASSERT_ABOVE_(v, .0);
78  m_Kp = v;
79  }
80 
81  /** PI controller Ti parameter [default=0.0194] */
82  double controllerParam_Ti() const { return m_Ti; }
83  void controllerParam_Ti(double v)
84  {
85  ASSERT_ABOVEEQ_(v, .0);
86  m_Ti = v;
87  }
88 
89  /** Low-pass filter a0 value [default=0.9]:
90  * estimation = a0*input + (1-a0)*former_estimation */
91  double lowPassParam_a0() const { return m_lowPass_a0; }
92  void lowPassParam_a0(double v)
93  {
94  ASSERT_ABOVE_(v, .0);
95  ASSERT_BELOWEQ_(v, 1.0);
96  m_lowPass_a0 = v;
97  }
98 
99  /** Get/set ratio threshold for issuing a warning (via COutputLogger
100  * interface) if the achieved rate is not this close to the set-point
101  * [Default=0.2, =20%]
102  */
104  {
106  }
108  {
109  ASSERT_ABOVE_(v, .0);
110  ASSERT_BELOWEQ_(v, 1.0);
112  }
113 
114  /** Gets the actual controller output: the rate (Hz) of the internal
115  * CRateTimer object. */
116  double actualControlledRate() const { return m_ratetimer.rate(); }
117 
118  /** Gets the latest estimated run rate (Hz), which comes from actual period
119  * measurement, low-pass filtered. */
120  double estimatedRate() const { return m_currentEstimatedRate; }
121 
122  /** Last actual execution rate measured (Hz), without low-pass filtering */
123  double estimatedRateRaw() const { return m_lastRawRate; }
124 
125  /** @} */
126  private:
127  double m_rate_hz = 1.0;
128  mrpt::system::CRateTimer m_ratetimer; //!< the one control acts on
129 
130  double m_lowPass_a0 = 0.99;
131  double m_Kp = 1.0;
132  double m_Ti = 0.1;
133 
135  double m_lastControlError = .0;
136 
139  double m_lastTic = 0;
141 
142 }; // End of class def.
143 
144 } // namespace mrpt::system
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::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
mrpt::system::CControlledRateTimer::m_lowPass_a0
double m_lowPass_a0
Definition: CControlledRateTimer.h:130
mrpt::system::CControlledRateTimer
A class for calling sleep() in a loop, such that the amount of sleep time will be computed to make th...
Definition: CControlledRateTimer.h:47
mrpt::system::CControlledRateTimer::m_currentEstimatedRate
double m_currentEstimatedRate
Definition: CControlledRateTimer.h:138
mrpt::system::CControlledRateTimer::controllerParam_Kp
double controllerParam_Kp() const
PI controller Kp parameter [default=1.0].
Definition: CControlledRateTimer.h:74
mrpt::system::CControlledRateTimer::m_lastTic
double m_lastTic
Definition: CControlledRateTimer.h:139
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::CControlledRateTimer::followErrorRatioToRaiseWarning
void followErrorRatioToRaiseWarning(double v)
Definition: CControlledRateTimer.h:107
ASSERT_BELOWEQ_
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:161
COutputLogger.h
mrpt::system::CRateTimer
A class for calling sleep() in a loop, such that the amount of sleep time will be computed to make th...
Definition: system/CRateTimer.h:21
mrpt::system::CControlledRateTimer::m_ratetimer
mrpt::system::CRateTimer m_ratetimer
the one control acts on
Definition: CControlledRateTimer.h:128
mrpt::system::CControlledRateTimer::controllerParam_Ti
void controllerParam_Ti(double v)
Definition: CControlledRateTimer.h:83
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
ASSERT_ABOVEEQ_
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
mrpt::system::CControlledRateTimer::~CControlledRateTimer
virtual ~CControlledRateTimer()=default
Dtor.
mrpt::system::CControlledRateTimer::actualControlledRate
double actualControlledRate() const
Gets the actual controller output: the rate (Hz) of the internal CRateTimer object.
Definition: CControlledRateTimer.h:116
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
CTicTac.h
mrpt::system::CControlledRateTimer::controllerParam_Kp
void controllerParam_Kp(double v)
Definition: CControlledRateTimer.h:75
mrpt::system::CControlledRateTimer::controllerParam_Ti
double controllerParam_Ti() const
PI controller Ti parameter [default=0.0194].
Definition: CControlledRateTimer.h:82
mrpt::system::CControlledRateTimer::lowPassParam_a0
void lowPassParam_a0(double v)
Definition: CControlledRateTimer.h:92
mrpt::system::CControlledRateTimer::lowPassParam_a0
double lowPassParam_a0() const
Low-pass filter a0 value [default=0.9]: estimation = a0*input + (1-a0)*former_estimation.
Definition: CControlledRateTimer.h:91
CRateTimer.h
mrpt::system::CControlledRateTimer::m_rate_hz
double m_rate_hz
Definition: CControlledRateTimer.h:127
mrpt::system::CControlledRateTimer::estimatedRateRaw
double estimatedRateRaw() const
Last actual execution rate measured (Hz), without low-pass filtering.
Definition: CControlledRateTimer.h:123
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::followErrorRatioToRaiseWarning
double followErrorRatioToRaiseWarning() const
Get/set ratio threshold for issuing a warning (via COutputLogger interface) if the achieved rate is n...
Definition: CControlledRateTimer.h:103
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::CControlledRateTimer::estimatedRate
double estimatedRate() const
Gets the latest estimated run rate (Hz), which comes from actual period measurement,...
Definition: CControlledRateTimer.h:120
mrpt::system
Definition: backtrace.h:14



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020