From 5a19799613e0ff6b9eefdfdd48332d2024fb16a2 Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Mon, 16 Nov 2020 21:03:41 +0100
Subject: [PATCH] prevent motor arming if an error is set

---
 Firmware/MotorControl/axis.cpp | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index 6c8e6663..9b12a3c4 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -481,10 +481,18 @@ void Axis::run_state_machine_loop() {
         bool status;
         switch (current_state_) {
             case AXIS_STATE_MOTOR_CALIBRATION: {
+                // These error checks are a hacky way to force legacy behavior
+                // when an error is raised. TODO: remove this when we overhaul
+                // the error architecture
+                // (https://github.com/madcowswe/ODrive/issues/526).
+                if (odrv.any_error())
+                    goto invalid_state_label;
                 status = motor_.run_calibration();
             } break;
 
             case AXIS_STATE_ENCODER_INDEX_SEARCH: {
+                if (odrv.any_error())
+                    goto invalid_state_label;
                 if (!motor_.is_calibrated_)
                     goto invalid_state_label;
 
@@ -492,6 +500,8 @@ void Axis::run_state_machine_loop() {
             } break;
 
             case AXIS_STATE_ENCODER_DIR_FIND: {
+                if (odrv.any_error())
+                    goto invalid_state_label;
                 if (!motor_.is_calibrated_)
                     goto invalid_state_label;
 
@@ -499,22 +509,30 @@ void Axis::run_state_machine_loop() {
             } break;
 
             case AXIS_STATE_HOMING: {
+                if (odrv.any_error())
+                    goto invalid_state_label;
                 status = run_homing();
             } break;
 
             case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
+                if (odrv.any_error())
+                    goto invalid_state_label;
                 if (!motor_.is_calibrated_)
                     goto invalid_state_label;
                 status = encoder_.run_offset_calibration();
             } break;
 
             case AXIS_STATE_LOCKIN_SPIN: {
+                if (odrv.any_error())
+                    goto invalid_state_label;
                 if (!motor_.is_calibrated_ || encoder_.config_.direction==0)
                     goto invalid_state_label;
                 status = run_lockin_spin(config_.general_lockin, false);
             } break;
 
             case AXIS_STATE_CLOSED_LOOP_CONTROL: {
+                if (odrv.any_error())
+                    goto invalid_state_label;
                 if (!motor_.is_calibrated_ || (encoder_.config_.direction==0 && !config_.enable_sensorless_mode))
                     goto invalid_state_label;
                 watchdog_feed();