From ecc148bde636b804f22bd008db873572d021a5eb Mon Sep 17 00:00:00 2001
From: victhor <victorlopezcortes25@gmail.com>
Date: Thu, 16 Apr 2026 18:59:04 +0200
Subject: [PATCH] implement an array of the last 2^n task times

---
 Inc/HALAL/Services/Time/Scheduler.hpp | 10 ++++++++
 Src/HALAL/Services/Time/Scheduler.cpp | 37 +++++++++++++++++++++++++++
 2 files changed, 47 insertions(+)

diff --git a/Inc/HALAL/Services/Time/Scheduler.hpp b/Inc/HALAL/Services/Time/Scheduler.hpp
index ceb16e198..40741238f 100644
--- a/Inc/HALAL/Services/Time/Scheduler.hpp
+++ b/Inc/HALAL/Services/Time/Scheduler.hpp
@@ -27,6 +27,16 @@
 #include "stm32h7xx_hal_tim.h"
 #endif
 
+//#define SLOW_CHECK_USE_READY_BITMAP
+#define SLOW_CHECK_USE_LAST_N_TASKS
+
+#define SLOW_CHECK_USE_LAST_N_TASKS_COUNT 512
+
+#if defined(SLOW_CHECK_USE_LAST_N_TASKS)
+// Must be a 32 bit timer and not be the same as the scheduler timer
+extern TIM_TypeDef* perf_timer;
+#endif
+
 extern TIM_TypeDef* Scheduler_global_timer;
 void Scheduler_global_timer_callback(void* raw);
 void Scheduler_start(void);
diff --git a/Src/HALAL/Services/Time/Scheduler.cpp b/Src/HALAL/Services/Time/Scheduler.cpp
index ac215b4b4..32f1b8bf5 100644
--- a/Src/HALAL/Services/Time/Scheduler.cpp
+++ b/Src/HALAL/Services/Time/Scheduler.cpp
@@ -31,8 +31,30 @@ uint64_t Scheduler::global_tick_us_{0};
 uint32_t Scheduler::current_interval_us_{0};
 uint16_t Scheduler::timeout_idx_{1};
 
+#if (SLOW_CHECK_USE_LAST_N_TASKS_COUNT & (SLOW_CHECK_USE_LAST_N_TASKS_COUNT - 1)) != 0
+// NOTE: Performance: this is to use an and instead of a mod when storing the data
+#error SLOW_CHECK_USE_LAST_N_TASKS_COUNT must be a power of 2
+#endif
+
+#if defined(SLOW_CHECK_USE_READY_BITMAP)
 uint16_t failing_id = Scheduler::INVALID_ID;
 
+#elif defined(SLOW_CHECK_USE_LAST_N_TASKS)
+struct TaskTimeInfo {
+    uint16_t id;
+    uint32_t start_time;
+    uint32_t end_time;
+};
+TaskTimeInfo time_info[SLOW_CHECK_USE_LAST_N_TASKS_COUNT]{};
+#if SLOW_CHECK_USE_LAST_N_TASKS_COUNT > 65536
+#error Use a uint32_t instead and remove these lines
+#endif
+uint16_t current_time_info{0};
+
+// Must be a 32 bit timer and not be the same as the scheduler timer
+TIM_TypeDef *perf_timer;
+#endif
+
 // ----------------------------
 
 inline void Scheduler::global_timer_disable() {
@@ -75,19 +97,32 @@ void Scheduler_start(void) {
 }
 
 void Scheduler::update() {
+#ifdef SLOW_CHECK_USE_READY_BITMAP
     // NOTE: Only _one_ id will be shown per call to update()
     if (failing_id != Scheduler::INVALID_ID) [[unlikely]] {
         WARNING("Too slow, could not execute task %u in time", failing_id);
         failing_id = Scheduler::INVALID_ID;
     }
+#endif
 
     while (ready_bitmap_ != 0u) {
         uint32_t bit_index = static_cast<uint32_t>(__builtin_ctz(ready_bitmap_));
 
         Task& task = tasks_[bit_index];
 
+#if defined(SLOW_CHECK_USE_LAST_N_TASKS)
+        TaskTimeInfo *info = time_info + current_time_info;
+        info->id = bit_index;
+        info->start_time = perf_timer->CNT;
+#endif
+
         task.callback();
 
+#if defined(SLOW_CHECK_USE_LAST_N_TASKS)
+        info->end_time = perf_timer->CNT;
+        current_time_info = (current_time_info + 1) & SLOW_CHECK_USE_LAST_N_TASKS_COUNT;
+#endif
+
         SchedLock();
         CLEAR_BIT(ready_bitmap_, 1u << bit_index);
         if (!task.repeating) [[unlikely]] {
@@ -246,9 +281,11 @@ inline void Scheduler::on_timer_update() {
         SchedLock();
         pop_front();
         // mark task as ready
+#ifdef SLOW_CHECK_USE_READY_BITMAP
         if ((ready_bitmap_ & task_bit) != 0) [[unlikely]] {
             failing_id = candidate_id;
         }
+#endif
         SET_BIT(ready_bitmap_, task_bit);
         if (task.repeating) [[likely]] {
             task.next_fire_us = static_cast<uint32_t>(global_tick_us_ + task.period_us);