endstop_phase: Convert to using tmc mcu_phase_offset

Now that the TMC drivers track the phase offset, use that to implement
endstop phase.

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

7 files changed, 26 insertions, 33 deletions

diff --git a/klippy/extras/endstop_phase.py b/klippy/extras/endstop_phase.py
index 5c762b2e..fa57a0fc 100644
--- a/klippy/extras/endstop_phase.py
+++ b/klippy/extras/endstop_phase.py
@@ -25,7 +25,7 @@ class PhaseCalc:
             if module is not None:
                 self.tmc_module = module
                 if self.phases is None:
-                    self.phases = module.get_microsteps() * 4
+                    phase_offset, self.phases = module.get_phase_offset()
                 break
         if self.phases is not None:
             self.phase_history = [0] * self.phases
@@ -34,18 +34,15 @@ class PhaseCalc:
         return (int(float(driver_phase) / driver_phases * phases + .5) % phases)
     def calc_phase(self, stepper):
         mcu_pos = stepper.get_mcu_position()
-        if self.tmc_module is None:
-            phase = mcu_pos % self.phases
-        else:
-            try:
-                driver_phase, driver_phases = self.tmc_module.get_phase()
-            except Exception as e:
-                msg = "Unable to get stepper %s phase: %s" % (self.name, str(e))
-                logging.exception(msg)
-                raise self.printer.command_error(msg)
-            if stepper.is_dir_inverted():
-                driver_phase = (driver_phases - 1) - driver_phase
-            phase = self.convert_phase(driver_phase, driver_phases)
+        mcu_phase_offset = 0
+        if self.tmc_module is not None:
+            mcu_phase_offset, phases = self.tmc_module.get_phase_offset()
+            if mcu_phase_offset is None:
+                if self.printer.get_start_args().get('debugoutput') is None:
+                    raise self.printer.command_error("Stepper %s phase unknown"
+                                                     % (self.name,))
+                mcu_phase_offset = 0
+        phase = (mcu_pos + mcu_phase_offset) % self.phases
         self.phase_history[phase] += 1
         self.last_phase = phase
         self.last_mcu_position = mcu_pos
diff --git a/klippy/extras/tmc.py b/klippy/extras/tmc.py
index f1b91dc1..6ed45011 100644
--- a/klippy/extras/tmc.py
+++ b/klippy/extras/tmc.py
@@ -266,21 +266,22 @@ class TMCCommandHelper:
             gcmd.respond_info("Run Current: %0.2fA Hold Current: %0.2fA"
                               % (prev_run_current, prev_hold_current))
     # Stepper phase tracking
-    def get_microsteps(self):
-        return 256 >> self.fields.get_field("mres")
-    def get_phase(self):
+    def _get_phases(self):
+        return (256 >> self.fields.get_field("mres")) * 4
+    def get_phase_offset(self):
+        return self.mcu_phase_offset, self._get_phases()
+    def _query_phase(self):
         field_name = "mscnt"
         if self.fields.lookup_register(field_name, None) is None:
             # TMC2660 uses MSTEP
             field_name = "mstep"
         reg = self.mcu_tmc.get_register(self.fields.lookup_register(field_name))
-        mscnt = self.fields.get_field(field_name, reg)
-        return 1023 - mscnt, 1024
+        return self.fields.get_field(field_name, reg)
     def _handle_sync_mcu_pos(self, stepper):
         if stepper.get_name() != self.stepper_name:
             return
         try:
-            driver_phase, driver_phases = self.get_phase()
+            driver_phase = self._query_phase()
         except self.printer.command_error as e:
             logging.info("Unable to obtain tmc %s phase", self.stepper_name)
             self.mcu_phase_offset = None
@@ -288,10 +289,10 @@ class TMCCommandHelper:
             if enable_line.is_motor_enabled():
                 raise
             return
-        if stepper.is_dir_inverted():
-            driver_phase = (driver_phases - 1) - driver_phase
-        phases = self.get_microsteps() * 4
-        phase = int(float(driver_phase) / driver_phases * phases + .5) % phases
+        if not stepper.is_dir_inverted():
+            driver_phase = 1023 - driver_phase
+        phases = self._get_phases()
+        phase = int(float(driver_phase) / 1024 * phases + .5) % phases
         moff = (phase - stepper.get_mcu_position()) % phases
         if self.mcu_phase_offset is not None and self.mcu_phase_offset != moff:
             logging.warning("Stepper %s phase change (was %d now %d)",
diff --git a/klippy/extras/tmc2130.py b/klippy/extras/tmc2130.py
index 7d9e554f..b23b2a64 100644
--- a/klippy/extras/tmc2130.py
+++ b/klippy/extras/tmc2130.py
@@ -260,8 +260,7 @@ class TMC2130:
         current_helper = TMCCurrentHelper(config, self.mcu_tmc)
         cmdhelper = tmc.TMCCommandHelper(config, self.mcu_tmc, current_helper)
         cmdhelper.setup_register_dump(ReadRegisters)
-        self.get_microsteps = cmdhelper.get_microsteps
-        self.get_phase = cmdhelper.get_phase
+        self.get_phase_offset = cmdhelper.get_phase_offset
         # Setup basic register values
         tmc.TMCStealthchopHelper(config, self.mcu_tmc, TMC_FREQUENCY)
         # Allow other registers to be set from the config
diff --git a/klippy/extras/tmc2208.py b/klippy/extras/tmc2208.py
index 8287712c..fcc957a5 100644
--- a/klippy/extras/tmc2208.py
+++ b/klippy/extras/tmc2208.py
@@ -192,8 +192,7 @@ class TMC2208:
         current_helper = tmc2130.TMCCurrentHelper(config, self.mcu_tmc)
         cmdhelper = tmc.TMCCommandHelper(config, self.mcu_tmc, current_helper)
         cmdhelper.setup_register_dump(ReadRegisters, self.read_translate)
-        self.get_microsteps = cmdhelper.get_microsteps
-        self.get_phase = cmdhelper.get_phase
+        self.get_phase_offset = cmdhelper.get_phase_offset
         # Setup basic register values
         self.fields.set_field("mstep_reg_select", True)
         self.fields.set_field("multistep_filt", True)
diff --git a/klippy/extras/tmc2209.py b/klippy/extras/tmc2209.py
index 97a70293..e0d64c31 100644
--- a/klippy/extras/tmc2209.py
+++ b/klippy/extras/tmc2209.py
@@ -68,8 +68,7 @@ class TMC2209:
         current_helper = tmc2130.TMCCurrentHelper(config, self.mcu_tmc)
         cmdhelper = tmc.TMCCommandHelper(config, self.mcu_tmc, current_helper)
         cmdhelper.setup_register_dump(ReadRegisters)
-        self.get_microsteps = cmdhelper.get_microsteps
-        self.get_phase = cmdhelper.get_phase
+        self.get_phase_offset = cmdhelper.get_phase_offset
         # Setup basic register values
         self.fields.set_field("pdn_disable", True)
         self.fields.set_field("mstep_reg_select", True)
diff --git a/klippy/extras/tmc2660.py b/klippy/extras/tmc2660.py
index 125d7160..8cb1d1c1 100644
--- a/klippy/extras/tmc2660.py
+++ b/klippy/extras/tmc2660.py
@@ -229,8 +229,7 @@ class TMC2660:
         current_helper = TMC2660CurrentHelper(config, self.mcu_tmc)
         cmdhelper = tmc.TMCCommandHelper(config, self.mcu_tmc, current_helper)
         cmdhelper.setup_register_dump(ReadRegisters)
-        self.get_microsteps = cmdhelper.get_microsteps
-        self.get_phase = cmdhelper.get_phase
+        self.get_phase_offset = cmdhelper.get_phase_offset
 
         # CHOPCONF
         set_config_field = self.fields.set_config_field
diff --git a/klippy/extras/tmc5160.py b/klippy/extras/tmc5160.py
index dfd059ee..76c939d4 100644
--- a/klippy/extras/tmc5160.py
+++ b/klippy/extras/tmc5160.py
@@ -294,8 +294,7 @@ class TMC5160:
         current_helper = TMC5160CurrentHelper(config, self.mcu_tmc)
         cmdhelper = tmc.TMCCommandHelper(config, self.mcu_tmc, current_helper)
         cmdhelper.setup_register_dump(ReadRegisters)
-        self.get_microsteps = cmdhelper.get_microsteps
-        self.get_phase = cmdhelper.get_phase
+        self.get_phase_offset = cmdhelper.get_phase_offset
         # Setup basic register values
         tmc.TMCStealthchopHelper(config, self.mcu_tmc, TMC_FREQUENCY)
         #   CHOPCONF