diff --git a/esp32/include/utils/timing.h b/esp32/include/utils/timing.h index 5b94b2f..bfaa5d0 100644 --- a/esp32/include/utils/timing.h +++ b/esp32/include/utils/timing.h @@ -39,3 +39,16 @@ name##_count = 0; \ last_time = esp_timer_get_time() / 1000; \ } + +#define WARN_IF_SLOW(name, period_ms) \ + static uint64_t name##_slow_count = 0; \ + static uint64_t name##_slow_last_time = 0; \ + name##_slow_count++; \ + if (esp_timer_get_time() / 1000 - name##_slow_last_time >= 1000) { \ + uint64_t expected_hz = 1000 / (period_ms); \ + if (name##_slow_count < expected_hz) { \ + ESP_LOGW("Timing", "%s: %llu Hz (expected %llu Hz)", #name, name##_slow_count, expected_hz); \ + } \ + name##_slow_count = 0; \ + name##_slow_last_time = esp_timer_get_time() / 1000; \ + } diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp index 660ba76..58b8949 100644 --- a/esp32/src/main.cpp +++ b/esp32/src/main.cpp @@ -253,7 +253,7 @@ void IRAM_ATTR SpotControlLoopEntry(void *) { peripherals.calibrateIMU(); for (;;) { - CALLS_PER_SECOND(SpotControlLoopEntry); + WARN_IF_SLOW(SpotControlLoopEntry, 5); peripherals.update(); motionService.update(&peripherals); servoController.setAngles(motionService.getAngles());