diff --git a/klippy/extras/tmc5160.py b/klippy/extras/tmc5160.py
index b773135c2..45fd84fca 100644
--- a/klippy/extras/tmc5160.py
+++ b/klippy/extras/tmc5160.py
@@ -2,6 +2,8 @@
 #
 # Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
 #
+# Modified by Honest Brothers (C) 2024
+#
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import math, logging
 from . import bus, tmc, tmc2130
@@ -246,6 +248,7 @@ Fields["THIGH"] = {
     "thigh":                    0xfffff << 0
 }
 
+
 SignedFields = ["cur_a", "cur_b", "sgt", "xactual", "vactual", "pwm_scale_auto"]
 
 FieldFormatters = dict(tmc2130.FieldFormatters)
@@ -260,7 +263,7 @@ FieldFormatters.update({
 ######################################################################
 
 VREF = 0.325
-MAX_CURRENT = 10.000 # Maximum dependent on board, but 10 is safe sanity check
+MAX_CURRENT = 10.000  # Maximum dependent on board; 10 is safe sanity check
 
 class TMC5160CurrentHelper:
     def __init__(self, config, mcu_tmc):
@@ -272,6 +275,7 @@ class TMC5160CurrentHelper:
                                       above=0., maxval=MAX_CURRENT)
         hold_current = config.getfloat('hold_current', MAX_CURRENT,
                                        above=0., maxval=MAX_CURRENT)
+        self.cs = config.getint('driver_cs', 31, -1)
         self.req_hold_current = hold_current
         self.sense_resistor = config.getfloat('sense_resistor', 0.075, above=0.)
         gscaler, irun, ihold = self._calc_current(run_current, hold_current)
@@ -279,23 +283,26 @@ class TMC5160CurrentHelper:
         self.fields.set_field("ihold", ihold)
         self.fields.set_field("irun", irun)
     def _calc_globalscaler(self, current):
-        globalscaler = int((current * 256. * math.sqrt(2.)
-                            * self.sense_resistor / VREF) + .5)
+        cs = self._calc_current_bits(current)
+        globalscaler = int(math.ceil(
+            (current * 256.0 * math.sqrt(2.0) * self.sense_resistor * 32 / (
+            VREF * (1 + cs)))))
         globalscaler = max(32, globalscaler)
         if globalscaler >= 256:
             globalscaler = 0
         return globalscaler
-    def _calc_current_bits(self, current, globalscaler):
-        if not globalscaler:
-            globalscaler = 256
-        cs = int((current * 256. * 32. * math.sqrt(2.) * self.sense_resistor)
-                 / (globalscaler * VREF)
-                 - 1. + .5)
+    def _calc_current_bits(self, current):
+        if self.cs == -1:
+            Ipeak = current * math.sqrt(2)
+            Rsens = self.sense_resistor
+            cs = int(math.ceil(Rsens * 32 * Ipeak / 0.32) - 1)
+        else:
+            cs = self.cs
         return max(0, min(31, cs))
     def _calc_current(self, run_current, hold_current):
         gscaler = self._calc_globalscaler(run_current)
-        irun = self._calc_current_bits(run_current, gscaler)
-        ihold = self._calc_current_bits(min(hold_current, run_current), gscaler)
+        irun = self._calc_current_bits(run_current)
+        ihold = int(min((hold_current / run_current) * irun, irun))
         return gscaler, irun, ihold
     def _calc_current_from_field(self, field_name):
         globalscaler = self.fields.get_field("globalscaler")