20 #include <baseapp/timing_thread.h>
24 #define CFG_PREFIX "/fawkes/mainapp/"
37 FawkesTimingThread::FawkesTimingThread() : Thread(
"FawkesTimingThread", Thread::OPMODE_CONTINUOUS)
49 loop_start_ =
new Time(clock_);
50 loop_end_ =
new Time(clock_);
56 desired_loop_time_usec_ =
config->
get_uint(
"/fawkes/mainapp/desired_loop_time");
58 desired_loop_time_usec_ = 0;
61 desired_loop_time_sec_ = (float)desired_loop_time_usec_ / 1000000.f;
64 min_loop_time_usec_ =
config->
get_uint(
"/fawkes/mainapp/min_loop_time");
66 min_loop_time_usec_ = 0;
69 min_loop_time_sec_ = (float)min_loop_time_usec_ / 1000000.f;
72 enable_looptime_warnings_ =
config->
get_bool(
"/fawkes/mainapp/enable_looptime_warnings");
73 if (!enable_looptime_warnings_) {
77 enable_looptime_warnings_ =
true;
94 float loop_time = *loop_end_ - loop_start_;
96 if (loop_time < min_loop_time_usec_) {
98 usleep(min_loop_time_usec_ - loop_time);
100 loop_time = *loop_end_ - loop_start_;
103 if (desired_loop_time_sec_ > 0) {
104 if (enable_looptime_warnings_) {
107 if (loop_time > 1.1 * desired_loop_time_sec_) {
109 "Loop time exceeded, "
110 "desired: %f sec (%u usec), actual: %f sec",
111 desired_loop_time_sec_,
112 desired_loop_time_usec_,
116 "Desired loop time achieved, "
117 "desired: %f sec (%u usec), actual: %f sec",
118 desired_loop_time_sec_,
119 desired_loop_time_usec_,