From 4160d019be1625e5f2541a6b6072a56403dec840 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 13 Jul 2021 20:44:09 +0200
Subject: [PATCH] Motion: implement InitAxis

---
 src/modules/motion.cpp | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index 36ca904..b358149 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -5,8 +5,14 @@ namespace motion {
 
 Motion motion;
 
-// TODO: not implemented
-void Motion::InitAxis(Axis axis) {}
+void Motion::InitAxis(Axis axis) {
+    for (uint8_t i = 0; i != NUM_AXIS; ++i) {
+        // disable the axis and re-init the driver: this will clear the internal
+        // StallGuard data as a result without special handling
+        Disable(axis);
+        axisData[axis].drv.Init(axisParams[axis].params);
+    }
+}
 
 void Motion::SetEnabled(Axis axis, bool enabled) {
     axisData[axis].drv.SetEnabled(axisParams[axis].params, enabled);