tmc: Implement TMCtstepHelper

Implement a helper for calculating velocity based thresholds for tmc drivers. This code was written in such a way that it can be used with more fields than just tpwmthrs.

Signed-off-by: Alex Voinea <voinea.dragos.alexandru@gmail.com>

1 files changed, 20 insertions, 7 deletions

diff --git a/klippy/extras/tmc.py b/klippy/extras/tmc.py
index ba1bc41c..f9f2d150 100644
--- a/klippy/extras/tmc.py
+++ b/klippy/extras/tmc.py
@@ -538,20 +538,33 @@ def TMCMicrostepHelper(config, mcu_tmc):
     fields.set_field("mres", mres)
     fields.set_field("intpol", config.getboolean("interpolate", True))
 
-# Helper to configure "stealthchop" mode
+# Helper for calculating TSTEP based values from velocity
+def TMCtstepHelper(step_dist, mres, tmc_freq, velocity):
+    if velocity > 0.:
+        step_dist_256 = step_dist / (1 << mres)
+        threshold = int(tmc_freq * step_dist_256 / velocity + .5)
+        return max(0, min(0xfffff, threshold))
+    else:
+        return 0xfffff
+
+# Helper to configure stealthChop-spreadCycle transition velocity
 def TMCStealthchopHelper(config, mcu_tmc, tmc_freq):
     fields = mcu_tmc.get_fields()
     en_pwm_mode = False
-    velocity = config.getfloat('stealthchop_threshold', 0., minval=0.)
-    if velocity:
+    velocity = config.getfloat('stealthchop_threshold', None, minval=0.)
+    tpwmthrs = 0xfffff
+
+    if velocity is not None:
+        en_pwm_mode = True
+
         stepper_name = " ".join(config.get_name().split()[1:])
         sconfig = config.getsection(stepper_name)
         rotation_dist, steps_per_rotation = stepper.parse_step_distance(sconfig)
         step_dist = rotation_dist / steps_per_rotation
-        step_dist_256 = step_dist / (1 << fields.get_field("mres"))
-        threshold = int(tmc_freq * step_dist_256 / velocity + .5)
-        fields.set_field("tpwmthrs", max(0, min(0xfffff, threshold)))
-        en_pwm_mode = True
+        mres = fields.get_field("mres")
+        tpwmthrs = TMCtstepHelper(step_dist, mres, tmc_freq, velocity)
+    fields.set_field("tpwmthrs", tpwmthrs)
+
     reg = fields.lookup_register("en_pwm_mode", None)
     if reg is not None:
         fields.set_field("en_pwm_mode", en_pwm_mode)