Motion: implement InitAxis

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);