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] - # Register event handlers - self.printer.register_event_handler("klippy:connect", - self.handle_connect) - 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 + self.phase_calc = PhaseCalc(self.printer, self.name, self.phases) + # Register event handlers + self.printer.register_event_handler("klippy: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") # 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: + 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 - 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") - 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)