endstop_phase: Unify phase conversion code

Introduce a new PhaseCalc class and use that for both homing and
ENDSTOP_PHASE_CALIBRATE.

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

1 files changed, 87 insertions, 80 deletions

diff --git a/klippy/extras/endstop_phase.py b/klippy/extras/endstop_phase.py
index ed6c5960..5c762b2e 100644
--- a/klippy/extras/endstop_phase.py
+++ b/klippy/extras/endstop_phase.py
@@ -8,23 +8,65 @@ import stepper
 
 TRINAMIC_DRIVERS = ["tmc2130", "tmc2208", "tmc2209", "tmc2660", "tmc5160"]
 
-def convert_phase(driver_phase, driver_phases, phases):
-    return (int(float(driver_phase) / driver_phases * phases + .5) % phases)
+# Calculate the trigger phase of a stepper motor
+class PhaseCalc:
+    def __init__(self, printer, name, phases=None):
+        self.printer = printer
+        self.name = name
+        self.phases = phases
+        self.tmc_module = None
+        # Statistics tracking for ENDSTOP_PHASE_CALIBRATE
+        self.phase_history = self.last_phase = self.last_mcu_position = None
+        self.is_primary = self.stats_only = False
+    def lookup_tmc(self):
+        for driver in TRINAMIC_DRIVERS:
+            driver_name = "%s %s" % (driver, self.name)
+            module = self.printer.lookup_object(driver_name, None)
+            if module is not None:
+                self.tmc_module = module
+                if self.phases is None:
+                    self.phases = module.get_microsteps() * 4
+                break
+        if self.phases is not None:
+            self.phase_history = [0] * self.phases
+    def convert_phase(self, driver_phase, driver_phases):
+        phases = self.phases
+        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)
+        self.phase_history[phase] += 1
+        self.last_phase = phase
+        self.last_mcu_position = mcu_pos
+        return phase
 
+# Adjusted endstop trigger positions
 class EndstopPhase:
     def __init__(self, config):
         self.printer = config.get_printer()
         self.name = config.get_name().split()[1]
+        # Obtain step_distance and microsteps from stepper config section
+        sconfig = config.getsection(self.name)
+        self.step_dist = stepper.parse_step_distance(sconfig)
+        self.phases = sconfig.getint("microsteps", note_valid=False) * 4
+        self.phase_calc = PhaseCalc(self.printer, self.name, self.phases)
         # Register event handlers
         self.printer.register_event_handler("klippy:connect",
-                                            self.handle_connect)
+                                            self.phase_calc.lookup_tmc)
         self.printer.register_event_handler("homing:home_rails_end",
                                             self.handle_home_rails_end)
         self.printer.load_object(config, "endstop_phase")
-        # Obtain step_distance and microsteps from stepper config section
-        sconfig = config.getsection(self.name)
-        self.step_dist = stepper.parse_step_distance(sconfig)
-        self.phases = sconfig.getint("microsteps", note_valid=False) * 4
         # Read config
         self.endstop_phase = None
         trigger_phase = config.get('trigger_phase', None)
@@ -37,7 +79,7 @@ class EndstopPhase:
             if p >= ps:
                 raise config.error("Invalid trigger_phase '%s'"
                                    % (trigger_phase,))
-            self.endstop_phase = convert_phase(p, ps, self.phases)
+            self.endstop_phase = self.phase_calc.convert_phase(p, ps)
         self.endstop_align_zero = config.getboolean('endstop_align_zero', False)
         self.endstop_accuracy = config.getfloat('endstop_accuracy', None,
                                                 above=0.)
@@ -55,16 +97,6 @@ class EndstopPhase:
                                " stepper phase adjustment" % (self.name,))
         if self.printer.get_start_args().get('debugoutput') is not None:
             self.endstop_phase_accuracy = self.phases
-        self.phase_history = [0] * self.phases
-        self.get_phase = None
-    def handle_connect(self):
-        # Check for trinamic driver with get_phase() method
-        for driver in TRINAMIC_DRIVERS:
-            driver_name = "%s %s" % (driver, self.name)
-            module = self.printer.lookup_object(driver_name, None)
-            if module is not None:
-                self.get_phase = module.get_phase
-                break
     def align_endstop(self, pos):
         if not self.endstop_align_zero or self.endstop_phase is None:
             return pos
@@ -76,19 +108,7 @@ class EndstopPhase:
         full_step = microsteps * self.step_dist
         return int(pos / full_step + .5) * full_step + phase_offset
     def get_homed_offset(self, stepper):
-        if self.get_phase is not None:
-            try:
-                driver_phase, driver_phases = self.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 = convert_phase(driver_phase, driver_phases, self.phases)
-        else:
-            phase = stepper.get_mcu_position() % self.phases
-        self.phase_history[phase] += 1
+        phase = self.phase_calc.calc_phase(stepper)
         if self.endstop_phase is None:
             logging.info("Setting %s endstop phase to %d", self.name, phase)
             self.endstop_phase = phase
@@ -113,11 +133,11 @@ class EndstopPhase:
                 kin_spos[self.name] = self.align_endstop(orig_pos) + offset
                 return
 
+# Support for ENDSTOP_PHASE_CALIBRATE command
 class EndstopPhases:
     def __init__(self, config):
         self.printer = config.get_printer()
         self.tracking = {}
-        self.last_home_info = {}
         # Register handlers
         self.printer.register_event_handler("homing:home_rails_end",
                                             self.handle_home_rails_end)
@@ -125,62 +145,45 @@ class EndstopPhases:
         self.gcode.register_command("ENDSTOP_PHASE_CALIBRATE",
                                     self.cmd_ENDSTOP_PHASE_CALIBRATE,
                                     desc=self.cmd_ENDSTOP_PHASE_CALIBRATE_help)
-    def lookup_stepper(self, stepper, stepper_name):
-        mod_name = "endstop_phase %s" % (stepper_name,)
-        m = self.printer.lookup_object(mod_name, None)
-        if m is not None:
-            return {"get_phase": None, "is_rail": False,
-                    "phase_history": m.phase_history}
-        for driver in TRINAMIC_DRIVERS:
-            mod_name = "%s %s" % (driver, stepper_name)
+    def update_stepper(self, stepper, is_primary):
+        stepper_name = stepper.get_name()
+        phase_calc = self.tracking.get(stepper_name)
+        if phase_calc is None:
+            # Check if stepper has an endstop_phase config section defined
+            mod_name = "endstop_phase %s" % (stepper_name,)
             m = self.printer.lookup_object(mod_name, None)
             if m is not None:
-                return {"get_phase": m.get_phase, "is_rail": False,
-                        "phase_history": [0] * (m.get_microsteps() * 4)}
-        return None
-    def update_stepper(self, stepper, is_rail):
-        stepper_name = stepper.get_name()
-        if stepper_name not in self.tracking:
-            info = self.lookup_stepper(stepper, stepper_name)
-            self.tracking[stepper_name] = info
-        info = self.tracking[stepper_name]
-        if info is None:
-            return
-        if is_rail:
-            info["is_rail"] = True
-        get_phase = info["get_phase"]
-        if get_phase is None:
-            return
-        try:
-            driver_phase, driver_phases = get_phase()
-        except:
-            logging.exception("Error in EndstopPhases get_phase")
+                phase_calc = m.phase_calc
+            else:
+                # Create new PhaseCalc tracker
+                phase_calc = PhaseCalc(self.printer, stepper_name)
+                phase_calc.stats_only = True
+                phase_calc.lookup_tmc()
+            self.tracking[stepper_name] = phase_calc
+        if phase_calc.phase_history is None:
             return
-        phase_history = info["phase_history"]
-        phase = convert_phase(driver_phase, driver_phases, len(phase_history))
-        phase_history[phase] += 1
-        self.last_home_info[stepper.get_name()] = {
-            'phase': phase, 'phases': len(phase_history),
-            'mcu_position': stepper.get_mcu_position()
-        }
+        if is_primary:
+            phase_calc.is_primary = True
+        if phase_calc.stats_only:
+            phase_calc.calc_phase(stepper)
     def handle_home_rails_end(self, homing_state, rails):
         for rail in rails:
-            is_rail = True
+            is_primary = True
             for stepper in rail.get_steppers():
-                self.update_stepper(stepper, is_rail)
-                is_rail = False
+                self.update_stepper(stepper, is_primary)
+                is_primary = False
     cmd_ENDSTOP_PHASE_CALIBRATE_help = "Calibrate stepper phase"
     def cmd_ENDSTOP_PHASE_CALIBRATE(self, gcmd):
         stepper_name = gcmd.get('STEPPER', None)
         if stepper_name is None:
             self.report_stats()
             return
-        info = self.tracking.get(stepper_name)
-        if info is None:
+        phase_calc = self.tracking.get(stepper_name)
+        if phase_calc is None or phase_calc.phase_history is None:
             raise gcmd.error("Stats not available for stepper %s"
                              % (stepper_name,))
-        endstop_phase, phases = self.generate_stats(stepper_name, info)
-        if not info["is_rail"]:
+        endstop_phase, phases = self.generate_stats(stepper_name, phase_calc)
+        if not phase_calc.is_primary:
             return
         configfile = self.printer.lookup_object('configfile')
         section = 'endstop_phase %s' % (stepper_name,)
@@ -190,8 +193,8 @@ class EndstopPhases:
         gcmd.respond_info(
             "The SAVE_CONFIG command will update the printer config\n"
             "file with these parameters and restart the printer.")
-    def generate_stats(self, stepper_name, info):
-        phase_history = info["phase_history"]
+    def generate_stats(self, stepper_name, phase_calc):
+        phase_history = phase_calc.phase_history
         wph = phase_history + phase_history
         count = sum(phase_history)
         phases = len(phase_history)
@@ -216,12 +219,16 @@ class EndstopPhases:
                 "No steppers found. (Be sure to home at least once.)")
             return
         for stepper_name in sorted(self.tracking.keys()):
-            info = self.tracking[stepper_name]
-            if info is None or not info["is_rail"]:
+            phase_calc = self.tracking[stepper_name]
+            if phase_calc is None or not phase_calc.is_primary:
                 continue
-            self.generate_stats(stepper_name, info)
+            self.generate_stats(stepper_name, phase_calc)
     def get_status(self, eventtime):
-        return { 'last_home': dict(self.last_home_info) }
+        lh = { name: {'phase': pc.last_phase, 'phases': pc.phases,
+                      'mcu_position': pc.last_mcu_position}
+               for name, pc in self.tracking.items()
+               if pc.phase_history is not None }
+        return { 'last_home': lh }
 
 def load_config_prefix(config):
     return EndstopPhase(config)