stepper: Calculate step_distance from rotation_distance

Add support for automatically calculating the internal step_distance
from new config parameters - rotation_distance, microsteps,
full_steps_per_rotation, and gear_ratio.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

2 files changed, 51 insertions, 4 deletions

diff --git a/klippy/extras/tmc.py b/klippy/extras/tmc.py
index 7b9280dd..c14a5f0f 100644
--- a/klippy/extras/tmc.py
+++ b/klippy/extras/tmc.py
@@ -1,9 +1,10 @@
 # Common helper code for TMC stepper drivers
 #
-# Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2018-2020  Kevin O'Connor <kevin@koconnor.net>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import logging, collections
+import stepper
 
 
 ######################################################################
@@ -263,9 +264,14 @@ class TMCMicrostepHelper:
     def __init__(self, config, mcu_tmc):
         self.mcu_tmc = mcu_tmc
         self.fields = mcu_tmc.get_fields()
+        stepper_name = " ".join(config.get_name().split()[1:])
+        stepper_config = ms_config = config.getsection(stepper_name)
+        if stepper_config.get('microsteps', None, note_valid=False) is None:
+            # Older config format with microsteps in tmc config section
+            ms_config = config
         steps = {'256': 0, '128': 1, '64': 2, '32': 3, '16': 4,
                  '8': 5, '4': 6, '2': 7, '1': 8}
-        mres = config.getchoice('microsteps', steps)
+        mres = ms_config.getchoice('microsteps', steps)
         self.fields.set_field("MRES", mres)
         self.fields.set_field("intpol", config.getboolean("interpolate", True))
     def get_microsteps(self):
@@ -287,7 +293,7 @@ def TMCStealthchopHelper(config, mcu_tmc, tmc_freq):
     if velocity:
         stepper_name = " ".join(config.get_name().split()[1:])
         stepper_config = config.getsection(stepper_name)
-        step_dist = stepper_config.getfloat('step_distance', note_valid=False)
+        step_dist = stepper.parse_step_distance(stepper_config)
         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)))
diff --git a/klippy/stepper.py b/klippy/stepper.py
index a8ab2298..1b1ad7ad 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -183,7 +183,7 @@ def PrinterStepper(config, units_in_radians=False):
     step_pin_params = ppins.lookup_pin(step_pin, can_invert=True)
     dir_pin = config.get('dir_pin')
     dir_pin_params = ppins.lookup_pin(dir_pin, can_invert=True)
-    step_dist = config.getfloat('step_distance', above=0.)
+    step_dist = parse_step_distance(config, units_in_radians, True)
     mcu_stepper = MCU_stepper(name, step_pin_params, dir_pin_params, step_dist,
                               units_in_radians)
     # Support for stepper enable pin handling
@@ -194,6 +194,47 @@ def PrinterStepper(config, units_in_radians=False):
     force_move.register_stepper(mcu_stepper)
     return mcu_stepper
 
+# Parse stepper gear_ratio config parameter
+def parse_gear_ratio(config, note_valid):
+    gear_ratio = config.get('gear_ratio', None, note_valid=note_valid)
+    if gear_ratio is None:
+        return 1.
+    result = 1.
+    try:
+        gears = gear_ratio.split(',')
+        for gear in gears:
+            g1, g2 = [float(v.strip()) for v in gear.split(':')]
+            result *= g1 / g2
+    except:
+        raise config.error("Unable to parse gear_ratio: %s" % (gear_ratio,))
+    return result
+
+# Obtain "step distance" information from a config section
+def parse_step_distance(config, units_in_radians=None, note_valid=False):
+    if units_in_radians is None:
+        # Caller doesn't know if units are in radians - infer it
+        rd = config.get('rotation_distance', None, note_valid=False)
+        gr = config.get('gear_ratio', None, note_valid=False)
+        units_in_radians = rd is None and gr is not None
+    if units_in_radians:
+        rotation_dist = 2. * math.pi
+        config.get('gear_ratio', note_valid=note_valid)
+    else:
+        rotation_dist = config.getfloat('rotation_distance', None,
+                                        above=0., note_valid=note_valid)
+    if rotation_dist is None:
+        # Older config format with step_distance
+        return config.getfloat('step_distance', above=0., note_valid=note_valid)
+    # Newer config format with rotation_distance
+    microsteps = config.getint('microsteps', minval=1, note_valid=note_valid)
+    full_steps = config.getint('full_steps_per_rotation', 200, minval=1,
+                               note_valid=note_valid)
+    if full_steps % 4:
+        raise config.error("full_steps_per_rotation invalid in section '%s'"
+                           % (config.get_name(),))
+    gearing = parse_gear_ratio(config, note_valid)
+    return rotation_dist / (full_steps * microsteps * gearing)
+
 
 ######################################################################
 # Stepper controlled rails