From 8f0732a0ccdb132907dd5b5a00da47a89ccfd29a Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 13 Jul 2021 20:36:43 +0200
Subject: [PATCH] Motion: initial StallGuard support

Add the code in the right position to sample StallGuard and set/reset
the trigger flag.
---
 src/modules/motion.cpp | 19 ++++++++++++++++---
 src/modules/motion.h   |  2 ++
 2 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index 26386fd..36ca904 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -11,6 +11,12 @@ void Motion::InitAxis(Axis axis) {}
 void Motion::SetEnabled(Axis axis, bool enabled) {
     axisData[axis].drv.SetEnabled(axisParams[axis].params, enabled);
     axisData[axis].enabled = enabled;
+
+    if (!axisData[axis].enabled) {
+        // axis is powered off, clear internal StallGuard counters
+        axisData[axis].stall_trig = false;
+        axisData[axis].stall_cnt = 0;
+    }
 }
 
 void Motion::SetMode(Axis axis, MotorMode mode) {
@@ -18,13 +24,12 @@ void Motion::SetMode(Axis axis, MotorMode mode) {
         axisData[axis].drv.SetMode(mode);
 }
 
-// TODO: not implemented
 bool Motion::StallGuard(Axis axis) {
-    return false;
+    return axisData[axis].stall_trig;
 }
 
-// TODO: not implemented
 void Motion::ClearStallGuardFlag(Axis axis) {
+    axisData[axis].stall_trig = false;
 }
 
 // TODO: not implemented
@@ -63,6 +68,14 @@ st_timer_t Motion::Step() {
         timers[i] = axisData[i].residual;
         if (timers[i] <= config::stepTimerQuantum) {
             timers[i] += axisData[i].ctrl.Step(axisParams[i].params);
+
+            // axis has been moved, sample StallGuard
+            if (hal::tmc2130::TMC2130::Stall(axisParams[i].params)) {
+                // TODO: on the MK3 a stall is marked as such as 1/2 of a full step is
+                // lost: this is too simplistic for production
+                ++axisData[i].stall_cnt;
+                axisData[i].stall_trig = true;
+            }
         }
     }
 
diff --git a/src/modules/motion.h b/src/modules/motion.h
index 7a864e1..2c0b311 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -175,6 +175,8 @@ private:
         pulse_gen::PulseGen ctrl; ///< Motor controller
         bool enabled; ///< Axis enabled
         st_timer_t residual; ///< Axis timer residual
+        uint8_t stall_cnt; ///< Underlying StallGuard lost ustep count
+        bool stall_trig; ///< StallGuard trigger flag
     };
 
     /// Helper to initialize AxisData members