diff --git a/src/common/pico_util/include/pico/util/pheap.h b/src/common/pico_util/include/pico/util/pheap.h
index 5dc5133..2e0d778 100644
--- a/src/common/pico_util/include/pico/util/pheap.h
+++ b/src/common/pico_util/include/pico/util/pheap.h
@@ -247,6 +247,10 @@ static inline void ph_free_node(pheap_t *heap, pheap_node_id_t id) {
     if (heap->free_tail_id) {
         ph_get_node(heap, heap->free_tail_id)->sibling = id;
     }
+    if (!heap->free_head_id) {
+        assert(!heap->free_tail_id);
+        heap->free_head_id = id;
+    }
     heap->free_tail_id = id;
 }
 
diff --git a/src/common/pico_util/pheap.c b/src/common/pico_util/pheap.c
index fc80435..c7c9575 100644
--- a/src/common/pico_util/pheap.c
+++ b/src/common/pico_util/pheap.c
@@ -62,6 +62,10 @@ static pheap_node_id_t ph_remove_any_head(pheap_t *heap, pheap_node_id_t root_id
         if (heap->free_tail_id) {
             ph_get_node(heap, heap->free_tail_id)->sibling = root_id;
         }
+        if (!heap->free_head_id) {
+            assert(!heap->free_tail_id);
+            heap->free_head_id = root_id;
+        }
         heap->free_tail_id = root_id;
     }
     if (new_root_id) ph_get_node(heap, new_root_id)->parent = 0;
diff --git a/test/pico_time_test/pico_time_test.c b/test/pico_time_test/pico_time_test.c
index 2209fd5..bd419b6 100644
--- a/test/pico_time_test/pico_time_test.c
+++ b/test/pico_time_test/pico_time_test.c
@@ -18,7 +18,11 @@ PICOTEST_MODULE_NAME("pico_time_test", "pico_time test harness");
 static_assert(PICO_TIME_DEFAULT_ALARM_POOL_MAX_TIMERS >= MAX_TIMERS_PER_POOL, "");
 #define TEST_LENGTH_US 2000000
 
+#ifndef NDEBUG
+#define NUM_REPEATING_TIMERS 30
+#else
 #define NUM_REPEATING_TIMERS 50
+#endif
 static struct repeating_timer repeating_timers[NUM_REPEATING_TIMERS];
 static uint repeating_timer_callback_count[NUM_REPEATING_TIMERS];