From 8c695fa834c070ae99e5211b6dc2700f2cd0c96f Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Mon, 7 Dec 2020 12:21:45 +0100
Subject: [PATCH] fix sensorless estimator error

---
 Firmware/MotorControl/sensorless_estimator.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp
index 84f12f55..94c09c68 100644
--- a/Firmware/MotorControl/sensorless_estimator.cpp
+++ b/Firmware/MotorControl/sensorless_estimator.cpp
@@ -36,6 +36,11 @@ bool SensorlessEstimator::update() {
     // TODO: we read values here which are modified by a higher priority interrupt.
     // This is not thread-safe.    
     auto current_meas = axis_->motor_.current_meas_;
+    if (!axis_->motor_.is_armed_) {
+        // While the motor is disarmed the current is not measurable so we
+        // assume that it's zero.
+        current_meas = {0.0f, 0.0f};
+    }
     if (!current_meas.has_value()) {
         error_ |= ERROR_UNKNOWN_CURRENT_MEASUREMENT;
         reset(); // Reset state for when the next valid current measurement comes in.