diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 435c2df251..90a1c14aa0 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.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 &&\ diff --git a/common/SConscript b/common/SConscript index 71894923a6..bece09947a 100644 --- a/common/SConscript +++ b/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']) diff --git a/common/ratekeeper.cc b/common/ratekeeper.cc new file mode 100644 index 0000000000..c00d72a94a --- /dev/null +++ b/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; +} diff --git a/common/ratekeeper.h b/common/ratekeeper.h new file mode 100644 index 0000000000..ff4cb42c7b --- /dev/null +++ b/common/ratekeeper.h @@ -0,0 +1,22 @@ +#pragma once + +#include + +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; +}; diff --git a/common/tests/.gitignore b/common/tests/.gitignore index 1350b3b825..984c505f92 100644 --- a/common/tests/.gitignore +++ b/common/tests/.gitignore @@ -1,2 +1,3 @@ +test_ratekeeper test_util test_swaglog diff --git a/common/tests/test_ratekeeper.cc b/common/tests/test_ratekeeper.cc new file mode 100644 index 0000000000..66d229bdbb --- /dev/null +++ b/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); + } +} diff --git a/release/files_common b/release/files_common index 023fd9e3af..bac9b382d6 100644 --- a/release/files_common +++ b/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 diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 31ae0a4102..20c2800e8e 100644 --- a/selfdrive/boardd/boardd.cc +++ b/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 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 raw_can_data; while (!do_exit && check_all_connected(pandas)) { @@ -271,18 +271,7 @@ void can_recv_thread(std::vector 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 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 pandas, bool spoofing_started) { panda->send_heartbeat(engaged); } - uint64_t dt = nanos_since_boot() - start_time; - util::sleep_for(500 - dt / 1000000ULL); + rk.keepTime(); } } diff --git a/system/proclogd/main.cc b/system/proclogd/main.cc index c4faa916d9..3f8a889eea 100644 --- a/system/proclogd/main.cc +++ b/system/proclogd/main.cc @@ -1,6 +1,7 @@ #include +#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; diff --git a/system/sensord/sensors_qcom2.cc b/system/sensord/sensors_qcom2.cc index 349c67f498..e09c722fc2 100644 --- a/system/sensord/sensors_qcom2.cc +++ b/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) {