Merge pull request #144 from resibots/fix_rt_factor

Fix real-time factor reporting

Author: costashatz

src/robot_dart/scheduler.cpp

@@ -30,6 +30,7 @@ namespace robot_dart {
         _real_start_time = real_time;
         _current_step = 0;
         _max_frequency = -1;
+        _average_it_duration = 0.;
 
         _dt = dt;
         _sync = sync;
@@ -43,6 +44,7 @@ namespace robot_dart {
         auto end = clock_t::now();
         _it_duration = std::chrono::duration<double, std::micro>(end - _last_iteration_time).count();
         _last_iteration_time = end;
+        _average_it_duration = _average_it_duration + (_it_duration - _average_it_duration) / _current_step;
         std::chrono::duration<double, std::micro> real = end - _start_time;
         if (_sync) {
             auto expected = std::chrono::microseconds(int(_current_time * 1e6));

src/robot_dart/scheduler.hpp

@@ -45,10 +45,13 @@ namespace robot_dart {
         // 0.8x => we are simulating at 80% of real time
         double real_time_factor() const { return _dt / it_duration(); }
         // time for a single iteration (wall-clock)
-        double it_duration() const { return _it_duration * 1e-6; }
+        double it_duration() const { return _average_it_duration * 1e-6; }
+        // time of the last iteration (wall-clock)
+        double last_it_duration() const { return _it_duration * 1e-6; }
 
     protected:
         double _current_time = 0., _simu_start_time = 0., _real_time = 0., _real_start_time = 0., _it_duration = 0.;
+        double _average_it_duration = 0.;
         double _dt;
         int _current_step = 0;
         bool _sync;