From cefeb11e1045fa977a07aebafdfe028db7779535 Mon Sep 17 00:00:00 2001 From: Niklas Jensen Date: Fri, 26 Dec 2025 20:19:54 +0100 Subject: [PATCH] Timing expansion: measurements per function (CLAUDE) --- esp32/include/peripherals/imu.h | 2 +- esp32/include/utils/timing.h | 45 +++++++++++++++++++++++++++++++++ esp32/src/main.cpp | 8 +++++- 3 files changed, 53 insertions(+), 2 deletions(-) diff --git a/esp32/include/peripherals/imu.h b/esp32/include/peripherals/imu.h index 018598b..6454412 100644 --- a/esp32/include/peripherals/imu.h +++ b/esp32/include/peripherals/imu.h @@ -42,7 +42,7 @@ class IMU : public SensorBase { #endif #if FT_ENABLED(USE_ICM20948) #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0 - SPI_PORT.begin(SPI_SCK, SPI_MISO, SPI_MOSI, -1); + SPI_PORT.begin(SPI_SCK, SPI_MISO, SPI_MOSI, -1); // TODO: Move to global spi start _imu = (ICM_20948_SPI*)_arg; #ifndef ICM20948_ALIVE #define ICM20948_ALIVE diff --git a/esp32/include/utils/timing.h b/esp32/include/utils/timing.h index bfaa5d0..39e54b7 100644 --- a/esp32/include/utils/timing.h +++ b/esp32/include/utils/timing.h @@ -40,6 +40,51 @@ last_time = esp_timer_get_time() / 1000; \ } +#define CALLS_PER_SECOND_TIMED_START_TICK(name, function) \ + static uint64_t name##_##function##_start = 0; \ + static uint64_t name##_##function##_total_time = 0; \ + static uint64_t name##_##function##_call_count = 0; \ + name##_##function##_start = esp_timer_get_time(); + +#define CALLS_PER_SECOND_TIMED_END_TICK(name, function) \ + name##_##function##_total_time += (esp_timer_get_time() - name##_##function##_start); \ + name##_##function##_call_count++; + +#define CALLS_PER_SECOND_TIMED_CALL(name, function, call) \ + static uint64_t name##_##function##_total_time = 0; \ + static uint64_t name##_##function##_call_count = 0; \ + do { \ + uint64_t name##_##function##_start = esp_timer_get_time(); \ + call; \ + name##_##function##_total_time += (esp_timer_get_time() - name##_##function##_start); \ + name##_##function##_call_count++; \ + } while (0) + +#define CALLS_PER_SECOND_TIMED_FUNC_PRINT(name, function) \ + if (name##_##function##_call_count > 0) { \ + uint64_t avg = name##_##function##_total_time / name##_##function##_call_count; \ + if (avg < 1000) { \ + ESP_LOGI("Timing", " %s: %llu us (avg over %llu calls)", \ + #function, avg, name##_##function##_call_count); \ + } else { \ + ESP_LOGI("Timing", " %s: %llu ms (avg over %llu calls)", \ + #function, avg / 1000, name##_##function##_call_count); \ + } \ + name##_##function##_total_time = 0; \ + name##_##function##_call_count = 0; \ + } + +#define CALLS_PER_SECOND_TIMED(name, ...) \ + do { \ + static uint64_t name##_last_print = 0; \ + uint64_t name##_current_time = esp_timer_get_time() / 1000; \ + if (name##_current_time - name##_last_print >= 1000) { \ + ESP_LOGI("Timing", "=== %s Average Timings ===", #name); \ + __VA_ARGS__ \ + name##_last_print = name##_current_time; \ + } \ + } while (0) + #define WARN_IF_SLOW(name, period_ms) \ static uint64_t name##_slow_count = 0; \ static uint64_t name##_slow_last_time = 0; \ diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp index e539ec0..227c89c 100644 --- a/esp32/src/main.cpp +++ b/esp32/src/main.cpp @@ -264,7 +264,13 @@ void IRAM_ATTR SpotControlLoopEntry(void *) { #if FT_ENABLED(USE_WS2812) ledService.loop(); #endif - vTaskDelayUntil(&xLastWakeTime, xFrequency); + CALLS_PER_SECOND_TIMED(SpotControlLoopEntry, + CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, peripherals_update) + CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, motionService_update) + CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, servoController_setAngles) + CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, servoController_update) + ); + // vTaskDelayUntil(&xLastWakeTime, xFrequency); } }