tmc2130: Rework current selection to prefer vsense=1

It is preferable to program the tmc drivers with an irun (or cs)
setting near 31, as otherwise the driver may have reduced microstep
precision.  It was possible for the driver to be programmed with
irun=16 or irun=17 when it could have been configured with irun=31
with vsense=1 instead.  This would occur on tmc2130/tmc2208/tmc2209
drivers for values around 0.900 to 1.000 amps (when using a typical
sense_resistor settings of 0.110 Ohms).

Change the code to prefer using vsense=1 to avoid this issue.

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

3 files changed, 45 insertions, 27 deletions

diff --git a/docs/Config_Changes.md b/docs/Config_Changes.md
index e9bcd120..ead2445c 100644
--- a/docs/Config_Changes.md
+++ b/docs/Config_Changes.md
@@ -8,6 +8,12 @@ All dates in this document are approximate.
 
 ## Changes
 
+20220116: The tmc2130, tmc2208, tmc2209, and tmc2660 `run_current`
+calculation code has changed. For some `run_current` settings the
+drivers may now be configured differently. This new configuration
+should be more accurate, but it may invalidate previous tmc driver
+tuning.
+
 20211230: Scripts to tune input shaper (`scripts/calibrate_shaper.py`
 and `scripts/graph_accelerometer.py`) were migrated to use Python3
 by default. As a result, users must install Python3 versions of certain
diff --git a/klippy/extras/tmc2130.py b/klippy/extras/tmc2130.py
index c6a54561..f2142d26 100644
--- a/klippy/extras/tmc2130.py
+++ b/klippy/extras/tmc2130.py
@@ -116,30 +116,33 @@ class TMCCurrentHelper:
         vref = 0.32
         if vsense:
             vref = 0.18
-        cs = int(32. * current * sense_resistor * math.sqrt(2.) / vref
-                 - 1. + .5)
+        cs = int(32. * sense_resistor * current * math.sqrt(2.) / vref + .5) - 1
         return max(0, min(31, cs))
-    def _calc_current(self, run_current, hold_current):
-        vsense = False
-        irun = self._calc_current_bits(run_current, vsense)
-        ihold = self._calc_current_bits(min(hold_current, run_current),
-                                        vsense)
-        if irun < 16 and ihold < 16:
-            vsense = True
-            irun = self._calc_current_bits(run_current, vsense)
-            ihold = self._calc_current_bits(min(hold_current, run_current),
-                                            vsense)
-        return vsense, irun, ihold
-    def _calc_current_from_field(self, field_name):
-        bits = self.fields.get_field(field_name)
+    def _calc_current_from_bits(self, cs, vsense):
         sense_resistor = self.sense_resistor + 0.020
         vref = 0.32
-        if self.fields.get_field("vsense"):
+        if vsense:
             vref = 0.18
-        return (bits + 1) * vref / (32 * sense_resistor * math.sqrt(2.))
+        return (cs + 1) * vref / (32. * sense_resistor * math.sqrt(2.))
+    def _calc_current(self, run_current, hold_current):
+        vsense = True
+        irun = self._calc_current_bits(run_current, True)
+        if irun == 31:
+            cur = self._calc_current_from_bits(irun, True)
+            if cur < run_current:
+                irun2 = self._calc_current_bits(run_current, False)
+                cur2 = self._calc_current_from_bits(irun2, False)
+                if abs(run_current - cur2) < abs(run_current - cur):
+                    vsense = False
+                    irun = irun2
+        ihold = self._calc_current_bits(min(hold_current, run_current), vsense)
+        return vsense, irun, ihold
     def get_current(self):
-        run_current = self._calc_current_from_field("irun")
-        hold_current = self._calc_current_from_field("ihold")
+        irun = self.fields.get_field("irun")
+        ihold = self.fields.get_field("ihold")
+        vsense = self.fields.get_field("vsense")
+        run_current = self._calc_current_from_bits(irun, vsense)
+        hold_current = self._calc_current_from_bits(ihold, vsense)
         return run_current, hold_current, self.req_hold_current, MAX_CURRENT
     def set_current(self, run_current, hold_current, print_time):
         self.req_hold_current = hold_current
diff --git a/klippy/extras/tmc2660.py b/klippy/extras/tmc2660.py
index c0d8fa9b..e873c608 100644
--- a/klippy/extras/tmc2660.py
+++ b/klippy/extras/tmc2660.py
@@ -136,17 +136,26 @@ class TMC2660CurrentHelper:
 
     def _calc_current_bits(self, current, vsense):
         vref = 0.165 if vsense else 0.310
-        cs = int(32 * current * self.sense_resistor * math.sqrt(2.) / vref
-                 - 1. + .5)
+        sr = self.sense_resistor
+        cs = int(32. * sr * current * math.sqrt(2.) / vref + .5) - 1
         return max(0, min(31, cs))
 
+    def _calc_current_from_bits(self, cs, vsense):
+        vref = 0.165 if vsense else 0.310
+        return (cs + 1) * vref / (32. * self.sense_resistor * math.sqrt(2.))
+
     def _calc_current(self, run_current):
-        vsense = False
-        cs = self._calc_current_bits(run_current, vsense)
-        if cs < 16:
-            vsense = True
-            cs = self._calc_current_bits(run_current, vsense)
-        return vsense, cs
+        vsense = True
+        irun = self._calc_current_bits(run_current, True)
+        if irun == 31:
+            cur = self._calc_current_from_bits(irun, True)
+            if cur < run_current:
+                irun2 = self._calc_current_bits(run_current, False)
+                cur2 = self._calc_current_from_bits(irun2, False)
+                if abs(run_current - cur2) < abs(run_current - cur):
+                    vsense = False
+                    irun = irun2
+        return vsense, irun
 
     def _handle_printing(self, print_time):
         print_time -= 0.100 # Schedule slightly before deadline