From 712b67beb4ade46ee8aaac6c6081ccfcde169c70 Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Fri, 27 Aug 2021 09:29:10 +0200
Subject: [PATCH] Add checking of TMC2130 error states for Idler and Selector

we shall think about the Pulley as well, it looks like it should get its
own class just like Idler and Selector as it behaves very similarly in terms
of stepping and error checking
---
 src/hal/tmc2130.h            |  1 +
 src/modules/idler.cpp        |  5 +----
 src/modules/movable_base.cpp | 10 ++++++++++
 src/modules/movable_base.h   |  2 ++
 src/modules/selector.cpp     |  5 +----
 5 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/src/hal/tmc2130.h b/src/hal/tmc2130.h
index f78ede8..68af305 100644
--- a/src/hal/tmc2130.h
+++ b/src/hal/tmc2130.h
@@ -45,6 +45,7 @@ struct __attribute__((packed)) ErrorFlags {
         , s2g(0)
         , otpw(0)
         , ot(0) {}
+    inline bool Good() const { return reset_flag == 0 && uv_cp == 0 && s2g == 0 && otpw == 0 && ot == 0; }
 };
 
 /// TMC2130 interface - instances of this class are hidden in modules::motion::Motion::AxisData
diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp
index c27c0a7..23aece3 100644
--- a/src/modules/idler.cpp
+++ b/src/modules/idler.cpp
@@ -51,10 +51,7 @@ bool Idler::Home() {
 bool Idler::Step() {
     switch (state) {
     case Moving:
-        if (mm::motion.QueueEmpty(mm::Idler)) {
-            // move finished
-            state = Ready;
-        }
+        PerformMove(mm::Idler);
         return false;
     case Ready:
         currentlyEngaged = plannedEngage;
diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp
index 8450827..52d8378 100644
--- a/src/modules/movable_base.cpp
+++ b/src/modules/movable_base.cpp
@@ -15,5 +15,15 @@ MovableBase::OperationResult MovableBase::InitMovement(config::Axis axis) {
     }
 }
 
+void MovableBase::PerformMove(config::Axis axis) {
+    if (!mm::motion.DriverForAxis(axis).GetErrorFlags().Good()) {
+        // TMC2130 entered some error state, the planned move couldn't have been finished - result of operation is Failed
+        state = Failed;
+    } else if (mm::motion.QueueEmpty(axis)) {
+        // move finished
+        state = Ready;
+    }
+}
+
 } // namespace motion
 } // namespace modules
diff --git a/src/modules/movable_base.h b/src/modules/movable_base.h
index 12033ea..d02d9ca 100644
--- a/src/modules/movable_base.h
+++ b/src/modules/movable_base.h
@@ -50,6 +50,8 @@ protected:
     virtual void PrepareMoveToPlannedSlot() = 0;
 
     OperationResult InitMovement(config::Axis axis);
+
+    void PerformMove(config::Axis axis);
 };
 
 } // namespace motion
diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp
index 9cb77b0..edd035d 100644
--- a/src/modules/selector.cpp
+++ b/src/modules/selector.cpp
@@ -36,10 +36,7 @@ bool Selector::Home() {
 bool Selector::Step() {
     switch (state) {
     case Moving:
-        if (mm::motion.QueueEmpty(mm::Selector)) {
-            // move finished
-            state = Ready;
-        }
+        PerformMove(mm::Selector);
         return false;
     case Ready:
         currentSlot = plannedSlot;