diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index 8971e415..2a1d5e57 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -104,10 +104,12 @@ void Axis::start_thread() {
  * @brief Blocks until at least one complete control loop has been executed.
  */
 bool Axis::wait_for_control_iteration() {
-    uint16_t control_iteration_num = odrv.n_evt_control_loop_;
-    while (odrv.n_evt_control_loop_ == control_iteration_num) {
-        osDelay(1);
-    }
+    osSignalWait(0x0001, osWaitForever); // this might return instantly
+    osSignalWait(0x0001, osWaitForever); // this might be triggered at the
+                                         // end of a control loop iteration
+                                         // which was started before we entered
+                                         // this function
+    osSignalWait(0x0001, osWaitForever);
     return true;
 }
 
diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp
index e9477e0b..50d506d3 100644
--- a/Firmware/MotorControl/axis.hpp
+++ b/Firmware/MotorControl/axis.hpp
@@ -172,7 +172,7 @@ public:
     MechanicalBrake& mechanical_brake_;
     TaskTimes task_times_;
 
-    osThreadId thread_id_;
+    osThreadId thread_id_ = 0;
     const uint32_t stack_size_ = 2048; // Bytes
     volatile bool thread_id_valid_ = false;
 
diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp
index c2342ebe..3daae2d5 100644
--- a/Firmware/MotorControl/main.cpp
+++ b/Firmware/MotorControl/main.cpp
@@ -378,6 +378,13 @@ void ODrive::control_loop_cb(uint32_t timestamp) {
             axis.motor_.current_control_.update(timestamp); // uses the output of controller_ or open_loop_contoller_ and encoder_ or sensorless_estimator_ or async_estimator_
     }
 
+    // Tell the axis threads that the control loop has finished
+    for (auto& axis: axes) {
+        if (axis.thread_id_) {
+            osSignalSet(axis.thread_id_, 0x0001);
+        }
+    }
+
     get_gpio(odrv.config_.error_gpio_pin).write(odrv.any_error());
 }