diff --git a/src/hal/tmc2130.cpp b/src/hal/tmc2130.cpp
index d13b221..3b0971a 100644
--- a/src/hal/tmc2130.cpp
+++ b/src/hal/tmc2130.cpp
@@ -64,7 +64,7 @@ bool TMC2130::Init(const MotorParams &params, const MotorCurrents &currents, Mot
 
 void TMC2130::SetMode(const MotorParams &params, MotorMode 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
+    WriteRegister(params, Registers::TPWMTHRS, (mode == Stealth) ? 70 : 0xFFFF0); // @@TODO should be configurable
 }
 
 void TMC2130::SetCurrents(const MotorParams &params, const MotorCurrents &currents) {