From 287dd47b8f71f6faf6950a3ab844f06ddff5d429 Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Thu, 19 Nov 2020 12:19:02 +0100
Subject: [PATCH] move startup delay to fix race condition

---
 Firmware/MotorControl/axis.cpp | 14 --------------
 Firmware/MotorControl/main.cpp | 21 +++++++++++++++++++--
 2 files changed, 19 insertions(+), 16 deletions(-)

diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index 9b12a3c4..07fd263f 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -428,20 +428,6 @@ bool Axis::run_idle_loop() {
 
 // Infinite loop that does calibration and enters main control loop as appropriate
 void Axis::run_state_machine_loop() {
-
-    // Wait for up to 2s for motor to become ready to allow for error-free
-    // startup. This delay gives the current sensor calibration time to
-    // converge. If the DRV chip is unpowered, the motor will not become ready
-    // but we still enter idle state.
-    for (size_t i = 0; i < 2000; ++i) {
-        if (motor_.current_meas_.has_value()) {
-            break;
-        }
-        osDelay(1);
-    }
-
-    sensorless_estimator_.error_ &= ~SensorlessEstimator::ERROR_UNKNOWN_CURRENT_MEASUREMENT;
-
     for (;;) {
         // Load the task chain if a specific request is pending
         if (requested_state_ != AXIS_STATE_UNDEFINED) {
diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp
index 834ce743..469f71a0 100644
--- a/Firmware/MotorControl/main.cpp
+++ b/Firmware/MotorControl/main.cpp
@@ -485,6 +485,25 @@ static void rtos_main(void*) {
 
     // Start PWM and enable adc interrupts/callbacks
     start_adc_pwm();
+    start_analog_thread();
+
+    // Wait for up to 2s for motor to become ready to allow for error-free
+    // startup. This delay gives the current sensor calibration time to
+    // converge. If the DRV chip is unpowered, the motor will not become ready
+    // but we still enter idle state.
+    for (size_t i = 0; i < 2000; ++i) {
+        bool motors_ready = std::all_of(axes.begin(), axes.end(), [](auto& axis) {
+            return axis.motor_.current_meas_.has_value();
+        });
+        if (motors_ready) {
+            break;
+        }
+        osDelay(1);
+    }
+
+    for (auto& axis: axes) {
+        axis.sensorless_estimator_.error_ &= ~SensorlessEstimator::ERROR_UNKNOWN_CURRENT_MEASUREMENT;
+    }
 
     // Start state machine threads. Each thread will go through various calibration
     // procedures and then run the actual controller loops.
@@ -493,8 +512,6 @@ static void rtos_main(void*) {
         axes[i].start_thread();
     }
 
-    start_analog_thread();
-
     odrv.system_stats_.fully_booted = true;
 
     // Main thread finished starting everything and can delete itself now (yes this is legal).