diff --git a/components/esp32/test/test_fp.c b/components/esp32/test/test_fp.c
index c9454107a6..81dd1a2543 100644
--- a/components/esp32/test/test_fp.c
+++ b/components/esp32/test/test_fp.c
@@ -3,6 +3,7 @@
 #include "soc/cpu.h"
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
+#include "freertos/semphr.h"
 #include "unity.h"
 #include "test_utils.h"
 
@@ -155,7 +156,7 @@ TEST_CASE("test FP sqrt", "[fp]")
 
 struct TestFPState {
     int fail;
-    int done;
+    SemaphoreHandle_t done;
 };
 
 static const int testFpIter = 100000;
@@ -175,22 +176,25 @@ static void tskTestFP(void *pvParameters)
             printf("%s: i=%d y=%f eps=%f\r\n", __func__, i, y, eps);
         }
     }
-    state->done++;
+    TEST_ASSERT(xSemaphoreGive(state->done));
     vTaskDelete(NULL);
 }
 
 TEST_CASE("context switch saves FP registers", "[fp]")
 {
-    struct TestFPState state;
-    state.done = 0;
-    state.fail = 0;
-    xTaskCreatePinnedToCore(tskTestFP, "tsk1", 2048, &state, 3, NULL, 0);
-    xTaskCreatePinnedToCore(tskTestFP, "tsk2", 2048, &state, 3, NULL, 0);
-    xTaskCreatePinnedToCore(tskTestFP, "tsk3", 2048, &state, 3, NULL, portNUM_PROCESSORS - 1);
-    xTaskCreatePinnedToCore(tskTestFP, "tsk4", 2048, &state, 3, NULL, 0);
-    while (state.done != 4) {
-        vTaskDelay(100 / portTICK_PERIOD_MS);
+    struct TestFPState state = {
+        .done = xSemaphoreCreateCounting(4, 0)
+    };
+    TEST_ASSERT_NOT_NULL(state.done);
+    const int prio = UNITY_FREERTOS_PRIORITY + 1;
+    TEST_ASSERT(xTaskCreatePinnedToCore(tskTestFP, "tsk1", 2048, &state, prio, NULL, 0));
+    TEST_ASSERT(xTaskCreatePinnedToCore(tskTestFP, "tsk2", 2048, &state, prio, NULL, 0));
+    TEST_ASSERT(xTaskCreatePinnedToCore(tskTestFP, "tsk3", 2048, &state, prio, NULL, portNUM_PROCESSORS - 1));
+    TEST_ASSERT(xTaskCreatePinnedToCore(tskTestFP, "tsk4", 2048, &state, prio, NULL, 0));
+    for (int i = 0; i < 4; ++i) {
+        TEST_ASSERT(xSemaphoreTake(state.done, pdMS_TO_TICKS(5000)));
     }
+    vSemaphoreDelete(state.done);
     if (state.fail) {
         const int total = testFpIter * 4;
         printf("Failed: %d, total: %d\r\n", state.fail, total);