common: C++ RateKeeper (#29374)

* c++ RateKeeper

* add to files_common

* use util::random_int

* improve monotor_time

* remove ~ratekeeper

.github/workflows/selfdrive_tests.yaml

@@ -275,6 +275,7 @@ jobs:
                         QT_QPA_PLATFORM=offscreen ./selfdrive/ui/tests/test_translations && \
                         ./selfdrive/ui/tests/test_translations.py && \
                         ./common/tests/test_util && \
+                        ./common/tests/test_ratekeeper && \
                         ./common/tests/test_swaglog && \
                         ./selfdrive/boardd/tests/test_boardd_usbprotocol && \
                         ./system/loggerd/tests/test_logger &&\

common/SConscript

@@ -12,6 +12,7 @@ common_libs = [
   'util.cc',
   'i2c.cc',
   'watchdog.cc',
+  'ratekeeper.cc'
 ]
 
 if arch != "Darwin":
@@ -29,6 +30,7 @@ Export('_common', '_gpucommon')
 if GetOption('test'):
   env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common])
   env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread'])
+  env.Program('tests/test_ratekeeper', ['tests/test_ratekeeper.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread'])
 
 # Cython
 envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11'])

common/ratekeeper.cc

@@ -0,0 +1,38 @@
+#include "common/ratekeeper.h"
+
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+RateKeeper::RateKeeper(const std::string &name, float rate, float print_delay_threshold)
+    : name(name),
+      print_delay_threshold(std::max(0.f, print_delay_threshold)) {
+  interval = 1 / rate;
+  last_monitor_time = seconds_since_boot();
+  next_frame_time = last_monitor_time + interval;
+}
+
+bool RateKeeper::keepTime() {
+  bool lagged = monitorTime();
+  if (remaining_ > 0) {
+    util::sleep_for(remaining_ * 1000);
+  }
+  return lagged;
+}
+
+bool RateKeeper::monitorTime() {
+  ++frame_;
+  last_monitor_time = seconds_since_boot();
+  remaining_ = next_frame_time - last_monitor_time;
+
+  bool lagged = remaining_ < 0;
+  if (lagged) {
+    if (print_delay_threshold > 0 && remaining_ < -print_delay_threshold) {
+      LOGW("%s lagging by %.2f ms", name.c_str(), -remaining_ * 1000);
+    }
+    next_frame_time = last_monitor_time + interval;
+  } else {
+    next_frame_time += interval;
+  }
+  return lagged;
+}

common/ratekeeper.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <string>
+
+class RateKeeper {
+public:
+  RateKeeper(const std::string &name, float rate, float print_delay_threshold = 0);
+  ~RateKeeper() {}
+  bool keepTime();
+  bool monitorTime();
+  inline double frame() const { return frame_; }
+  inline double remaining() const { return remaining_; }
+
+private:
+  double interval;
+  double next_frame_time;
+  double last_monitor_time;
+  double remaining_ = 0;
+  float print_delay_threshold = 0;
+  uint64_t frame_ = 0;
+  std::string name;
+};

common/tests/.gitignore

@@ -1,2 +1,3 @@
+test_ratekeeper
 test_util
 test_swaglog

common/tests/test_ratekeeper.cc

@@ -0,0 +1,17 @@
+#define CATCH_CONFIG_MAIN
+#include "catch2/catch.hpp"
+#include "common/ratekeeper.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+TEST_CASE("RateKeeper") {
+  float freq = GENERATE(10, 50, 100);
+  RateKeeper rk("Test RateKeeper", freq);
+  for (int i = 0; i < freq; ++i) {
+    double begin = seconds_since_boot();
+    util::sleep_for(util::random_int(0, 1000.0 / freq - 1));
+    bool lagged = rk.keepTime();
+    REQUIRE(std::abs(seconds_since_boot() - begin - (1 / freq)) < 1e-3);
+    REQUIRE(lagged == false);
+  }
+}

release/files_common

@@ -162,6 +162,8 @@ common/clutil.cc
 common/clutil.h
 common/params.h
 common/params.cc
+common/ratekeeper.cc
+common/ratekeeper.h
 common/watchdog.cc
 common/watchdog.h
 

selfdrive/boardd/boardd.cc

@@ -23,6 +23,7 @@
 #include "cereal/gen/cpp/car.capnp.h"
 #include "cereal/messaging/messaging.h"
 #include "common/params.h"
+#include "common/ratekeeper.h"
 #include "common/swaglog.h"
 #include "common/timing.h"
 #include "common/util.h"
@@ -248,8 +249,7 @@ void can_recv_thread(std::vector<Panda *> pandas) {
   PubMaster pm({"can"});
 
   // run at 100Hz
-  const uint64_t dt = 10000000ULL;
-  uint64_t next_frame_time = nanos_since_boot() + dt;
+  RateKeeper rk("boardd_can_recv", 100);
   std::vector<can_frame> raw_can_data;
 
   while (!do_exit && check_all_connected(pandas)) {
@@ -271,18 +271,7 @@ void can_recv_thread(std::vector<Panda *> pandas) {
     }
     pm.send("can", msg);
 
-    uint64_t cur_time = nanos_since_boot();
-    int64_t remaining = next_frame_time - cur_time;
-    if (remaining > 0) {
-      std::this_thread::sleep_for(std::chrono::nanoseconds(remaining));
-    } else {
-      if (ignition) {
-        LOGW("missed cycles (%lu) %lld", (unsigned long)(-1*remaining/dt), (long long)remaining);
-      }
-      next_frame_time = cur_time;
-    }
-
-    next_frame_time += dt;
+    rk.keepTime();
   }
 }
 
@@ -483,9 +472,9 @@ void panda_state_thread(std::vector<Panda *> pandas, bool spoofing_started) {
   LOGD("start panda state thread");
 
   // run at 2hz
-  while (!do_exit && check_all_connected(pandas)) {
-    uint64_t start_time = nanos_since_boot();
+  RateKeeper rk("panda_state_thread", 2);
 
+  while (!do_exit && check_all_connected(pandas)) {
     // send out peripheralState
     send_peripheral_state(&pm, peripheral_panda);
     auto ignition_opt = send_panda_states(&pm, pandas, spoofing_started);
@@ -543,8 +532,7 @@ void panda_state_thread(std::vector<Panda *> pandas, bool spoofing_started) {
       panda->send_heartbeat(engaged);
     }
 
-    uint64_t dt = nanos_since_boot() - start_time;
-    util::sleep_for(500 - dt / 1000000ULL);
+    rk.keepTime();
   }
 }
 

system/proclogd/main.cc

@@ -1,6 +1,7 @@
 
 #include <sys/resource.h>
 
+#include "common/ratekeeper.h"
 #include "common/util.h"
 #include "system/proclogd/proclog.h"
 
@@ -9,13 +10,15 @@ ExitHandler do_exit;
 int main(int argc, char **argv) {
   setpriority(PRIO_PROCESS, 0, -15);
 
+  RateKeeper rk("proclogd", 0.5);
   PubMaster publisher({"procLog"});
+
   while (!do_exit) {
     MessageBuilder msg;
     buildProcLogMessage(msg);
     publisher.send("procLog", msg);
 
-    util::sleep_for(2000);  // 2 secs
+    rk.keepTime();
   }
 
   return 0;

system/sensord/sensors_qcom2.cc

@@ -9,6 +9,7 @@
 
 #include "cereal/messaging/messaging.h"
 #include "common/i2c.h"
+#include "common/ratekeeper.h"
 #include "common/swaglog.h"
 #include "common/timing.h"
 #include "common/util.h"
@@ -173,10 +174,10 @@ int sensor_loop(I2CBus *i2c_bus_imu) {
   std::thread lsm_interrupt_thread(&interrupt_loop, std::ref(lsm_interrupt_sensors),
                                    std::ref(sensor_service));
 
+  RateKeeper rk("sensord", 100);
+
   // polling loop for non interrupt handled sensors
   while (!do_exit) {
-    std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
-
     for (Sensor *sensor : sensors) {
       MessageBuilder msg;
       if (!sensor->get_event(msg)) {
@@ -190,8 +191,7 @@ int sensor_loop(I2CBus *i2c_bus_imu) {
       pm_non_int.send(sensor_service[sensor].c_str(), msg);
     }
 
-    std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
-    std::this_thread::sleep_for(std::chrono::milliseconds(10) - (end - begin));
+    rk.keepTime();
   }
 
   for (Sensor *sensor : sensors) {