diff --git a/src/hal/avr/tmc2130.cpp b/src/hal/avr/tmc2130.cpp
index c1d95f6..c342029 100644
--- a/src/hal/avr/tmc2130.cpp
+++ b/src/hal/avr/tmc2130.cpp
@@ -36,11 +36,7 @@ void TMC2130::Init(const MotorParams &params) {
     WriteRegister(params, Registers::CHOPCONF, chopconf);
 
     ///apply currents
-    uint32_t ihold_irun = 0;
-    ihold_irun |= (uint32_t)(currents.iHold & 0x1F) << 0; //ihold
-    ihold_irun |= (uint32_t)(currents.iRun & 0x1F) << 8; //irun
-    ihold_irun |= (uint32_t)(15 & 0x0F) << 16; //IHOLDDELAY
-    WriteRegister(params, Registers::IHOLD_IRUN, ihold_irun);
+    SetCurrents(params, currents);
 
     ///instant powerdown ramp
     WriteRegister(params, Registers::TPOWERDOWN, 0);
@@ -64,11 +60,27 @@ void TMC2130::Init(const MotorParams &params) {
     pwmconf |= (uint32_t)(1 & 0x01) << 18; //pwm_autoscale
     WriteRegister(params, Registers::PWMCONF, pwmconf);
 
-    ///TPWMTHRS: switching velocity between stealthChop and spreadCycle. Stallguard is also disabled if the velocity falls below this. Should be set to 0 when homing.
+    ///TPWMTHRS: switching velocity between stealthChop and spreadCycle. Stallguard is also disabled if the velocity falls below this. Should be set as high as possible when homing.
+    SetMode(params, mode);
+}
+
+void TMC2130::SetMode(const MotorParams &params, MotorMode mode) {
+    this->mode = mode;
+
     ///0xFFF00 is used as a "Normal" mode threshold since stealthchop will be used at standstill.
     WriteRegister(params, Registers::TPWMTHRS, (mode == Stealth) ? 70 : 0xFFF00); // @todo should be configurable
 }
 
+void TMC2130::SetCurrents(const MotorParams &params, const MotorCurrents &currents) {
+    this->currents = currents;
+
+    uint32_t ihold_irun = 0;
+    ihold_irun |= (uint32_t)(currents.iHold & 0x1F) << 0; //ihold
+    ihold_irun |= (uint32_t)(currents.iRun & 0x1F) << 8; //irun
+    ihold_irun |= (uint32_t)(15 & 0x0F) << 16; //IHOLDDELAY
+    WriteRegister(params, Registers::IHOLD_IRUN, ihold_irun);
+}
+
 uint32_t TMC2130::ReadRegister(const MotorParams &params, Registers reg) {
     uint8_t pData[5] = { (uint8_t)reg };
     _spi_tx_rx(params, pData);
@@ -78,6 +90,11 @@ uint32_t TMC2130::ReadRegister(const MotorParams &params, Registers reg) {
     return ((uint32_t)pData[1] << 24 | (uint32_t)pData[2] << 16 | (uint32_t)pData[3] << 8 | (uint32_t)pData[4]);
 }
 
+void TMC2130::SetEnabled(const MotorParams &params, bool enabled) {
+    hal::shr16::shr16.SetTMCDir(params.idx, enabled);
+    this->enabled = enabled;
+}
+
 void TMC2130::WriteRegister(const MotorParams &params, Registers reg, uint32_t data) {
     uint8_t pData[5] = { (uint8_t)((uint8_t)(reg) | 0x80), (uint8_t)(data >> 24), (uint8_t)(data >> 16), (uint8_t)(data >> 8), (uint8_t)data };
     _spi_tx_rx(params, pData);
diff --git a/src/hal/tmc2130.h b/src/hal/tmc2130.h
index 437423c..ce0b713 100644
--- a/src/hal/tmc2130.h
+++ b/src/hal/tmc2130.h
@@ -43,6 +43,7 @@ class TMC2130 {
         uint8_t otpw : 1;
         uint8_t ot : 1;
     } errorFlags;
+    bool enabled = false;
 
 public:
     enum class Registers : uint8_t {
@@ -81,7 +82,7 @@ public:
     }
 
     /// Set the current motor mode
-    void SetMode(MotorMode mode);
+    void SetMode(const MotorParams &params, MotorMode mode);
 
     /// Get the current motor currents
     const MotorCurrents &Currents() const {
@@ -89,15 +90,15 @@ public:
     }
 
     /// Set the current motor currents
-    void SetCurrents(const MotorCurrents &currents);
+    void SetCurrents(const MotorParams &params, const MotorCurrents &currents);
 
-    /// Return enabled state (TODO)
-    static bool Enabled(const MotorParams &params);
+    /// Return enabled state
+    const bool Enabled() const {
+        return enabled;
+    }
 
     /// Enable/Disable the motor
-    static void SetEnabled(const MotorParams &params, bool enabled) {
-        hal::shr16::shr16.SetTMCDir(params.idx, enabled);
-    }
+    void SetEnabled(const MotorParams &params, bool enabled);
 
     /// Set direction
     static inline void SetDir(const MotorParams &params, bool dir) {
diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index 50c5a4e..9d048ea 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -29,7 +29,7 @@ void Motion::SetEnabled(Axis axis, bool enabled) {
 
 void Motion::SetMode(Axis axis, MotorMode mode) {
     for (uint8_t i = 0; i != NUM_AXIS; ++i)
-        axisData[axis].drv.SetMode(mode);
+        axisData[axis].drv.SetMode(axisParams[axis].params, mode);
 }
 
 bool Motion::StallGuard(Axis axis) {