diff options
Diffstat (limited to 'klippy/extras/tmc.py')
| -rw-r--r-- | klippy/extras/tmc.py | 242 |
1 files changed, 242 insertions, 0 deletions
diff --git a/klippy/extras/tmc.py b/klippy/extras/tmc.py new file mode 100644 index 00000000..ad6085ff --- /dev/null +++ b/klippy/extras/tmc.py @@ -0,0 +1,242 @@ +# Common helper code for TMC stepper drivers +# +# Copyright (C) 2018-2019 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import logging, collections + + +###################################################################### +# Field helpers +###################################################################### + +# Return the position of the first bit set in a mask +def ffs(mask): + return (mask & -mask).bit_length() - 1 + +class FieldHelper: + def __init__(self, all_fields, signed_fields=[], field_formatters={}, + registers=None): + self.all_fields = all_fields + self.signed_fields = {sf: 1 for sf in signed_fields} + self.field_formatters = field_formatters + self.registers = registers + if self.registers is None: + self.registers = collections.OrderedDict() + self.field_to_register = { f: r for r, fields in self.all_fields.items() + for f in fields } + def lookup_register(self, field_name, default=None): + return self.field_to_register.get(field_name, default) + def get_field(self, field_name, reg_value=None, reg_name=None): + # Returns value of the register field + if reg_name is None: + reg_name = self.field_to_register[field_name] + if reg_value is None: + reg_value = self.registers[reg_name] + mask = self.all_fields[reg_name][field_name] + field_value = (reg_value & mask) >> ffs(mask) + if field_name in self.signed_fields and ((reg_value & mask)<<1) > mask: + field_value -= (1 << field_value.bit_length()) + return field_value + def set_field(self, field_name, field_value, reg_value=None, reg_name=None): + # Returns register value with field bits filled with supplied value + if reg_name is None: + reg_name = self.field_to_register[field_name] + if reg_value is None: + reg_value = self.registers.get(reg_name, 0) + mask = self.all_fields[reg_name][field_name] + new_value = (reg_value & ~mask) | ((field_value << ffs(mask)) & mask) + self.registers[reg_name] = new_value + return new_value + def set_config_field(self, config, field_name, default, config_name=None): + # Allow a field to be set from the config file + if config_name is None: + config_name = "driver_" + field_name.upper() + reg_name = self.field_to_register[field_name] + mask = self.all_fields[reg_name][field_name] + maxval = mask >> ffs(mask) + if maxval == 1: + val = config.getboolean(config_name, default) + elif field_name in self.signed_fields: + val = config.getint(config_name, default, + minval=-(maxval//2 + 1), maxval=maxval//2) + else: + val = config.getint(config_name, default, minval=0, maxval=maxval) + return self.set_field(field_name, val) + def pretty_format(self, reg_name, reg_value): + # Provide a string description of a register + reg_fields = self.all_fields.get(reg_name, {}) + reg_fields = sorted([(mask, name) for name, mask in reg_fields.items()]) + fields = [] + for mask, field_name in reg_fields: + field_value = self.get_field(field_name, reg_value, reg_name) + sval = self.field_formatters.get(field_name, str)(field_value) + if sval and sval != "0": + fields.append(" %s=%s" % (field_name, sval)) + return "%-11s %08x%s" % (reg_name + ":", reg_value, "".join(fields)) + + +###################################################################### +# G-Code command helpers +###################################################################### + +class TMCCommandHelper: + def __init__(self, config, mcu_tmc): + self.printer = config.get_printer() + self.name = config.get_name().split()[-1] + self.mcu_tmc = mcu_tmc + self.fields = mcu_tmc.get_fields() + self.query_registers = None + self.gcode = self.printer.lookup_object("gcode") + self.gcode.register_mux_command( + "SET_TMC_FIELD", "STEPPER", self.name, + self.cmd_SET_TMC_FIELD, desc=self.cmd_SET_TMC_FIELD_help) + self.gcode.register_mux_command( + "INIT_TMC", "STEPPER", self.name, + self.cmd_INIT_TMC, desc=self.cmd_INIT_TMC_help) + self.printer.register_event_handler("klippy:connect", + self._handle_connect) + def _init_registers(self, print_time): + # Send registers + for reg_name, val in self.fields.registers.items(): + self.mcu_tmc.set_register(reg_name, val, print_time) + def _handle_connect(self): + try: + self._init_registers(0.) + except self.printer.command_error as e: + raise self.printer.config_error(str(e)) + cmd_INIT_TMC_help = "Initialize TMC stepper driver registers" + def cmd_INIT_TMC(self, params): + logging.info("INIT_TMC %s", self.name) + print_time = self.printer.lookup_object('toolhead').get_last_move_time() + self._init_registers(print_time) + cmd_SET_TMC_FIELD_help = "Set a register field of a TMC driver" + def cmd_SET_TMC_FIELD(self, params): + if 'FIELD' not in params or 'VALUE' not in params: + raise self.gcode.error("Invalid command format") + field_name = self.gcode.get_str('FIELD', params) + reg_name = self.fields.lookup_register(field_name, None) + if reg_name is None: + raise self.gcode.error("Unknown field name '%s'" % (field_name,)) + value = self.gcode.get_int('VALUE', params) + reg_val = self.fields.set_field(field_name, value) + print_time = self.printer.lookup_object('toolhead').get_last_move_time() + self.mcu_tmc.set_register(reg_name, reg_val, print_time) + # DUMP_TMC support + def setup_register_dump(self, query_registers): + self.query_registers = query_registers + self.gcode.register_mux_command( + "DUMP_TMC", "STEPPER", self.name, + self.cmd_DUMP_TMC, desc=self.cmd_DUMP_TMC_help) + cmd_DUMP_TMC_help = "Read and display TMC stepper driver registers" + def cmd_DUMP_TMC(self, params): + logging.info("DUMP_TMC %s", self.name) + print_time = self.printer.lookup_object('toolhead').get_last_move_time() + read_regs = self.query_registers(print_time) + read_regs_by_name = { reg_name: val for reg_name, val in read_regs } + self.gcode.respond_info("========== Write-only registers ==========") + for reg_name, val in self.fields.registers.items(): + if reg_name not in read_regs_by_name: + self.gcode.respond_info( + self.fields.pretty_format(reg_name, val)) + self.gcode.respond_info("========== Queried registers ==========") + for reg_name, val in read_regs: + self.gcode.respond_info(self.fields.pretty_format(reg_name, val)) + + +###################################################################### +# TMC virtual endstops +###################################################################### + +# Endstop wrapper that enables "sensorless homing" +class TMCVirtualEndstop: + def __init__(self, mcu_tmc, mcu_endstop): + self.mcu_tmc = mcu_tmc + self.fields = mcu_tmc.get_fields() + self.mcu_endstop = mcu_endstop + self.en_pwm = self.fields.get_field("en_pwm_mode") + # Wrappers + self.get_mcu = self.mcu_endstop.get_mcu + self.add_stepper = self.mcu_endstop.add_stepper + self.get_steppers = self.mcu_endstop.get_steppers + self.home_start = self.mcu_endstop.home_start + self.home_wait = self.mcu_endstop.home_wait + self.query_endstop = self.mcu_endstop.query_endstop + self.query_endstop_wait = self.mcu_endstop.query_endstop_wait + self.TimeoutError = self.mcu_endstop.TimeoutError + def home_prepare(self): + self.fields.set_field("en_pwm_mode", 0) + val = self.fields.set_field("diag1_stall", 1) + self.mcu_tmc.set_register("GCONF", val) + self.mcu_tmc.set_register("TCOOLTHRS", 0xfffff) + self.mcu_endstop.home_prepare() + def home_finalize(self): + self.fields.set_field("en_pwm_mode", self.en_pwm) + val = self.fields.set_field("diag1_stall", 0) + self.mcu_tmc.set_register("GCONF", val) + self.mcu_tmc.set_register("TCOOLTHRS", 0) + self.mcu_endstop.home_finalize() + +class TMCEndstopHelper: + def __init__(self, config, mcu_tmc, diag_pin): + self.printer = config.get_printer() + self.mcu_tmc = mcu_tmc + self.diag_pin = diag_pin + name_parts = config.get_name().split() + ppins = self.printer.lookup_object("pins") + ppins.register_chip("%s_%s" % (name_parts[0], name_parts[-1]), self) + def setup_pin(self, pin_type, pin_params): + ppins = self.printer.lookup_object('pins') + if self.diag_pin is None: + raise ppins.error("tmc virtual endstop requires diag pin config") + if pin_type != 'endstop' or pin_params['pin'] != 'virtual_endstop': + raise ppins.error("tmc virtual endstop only useful as endstop") + if pin_params['invert'] or pin_params['pullup']: + raise ppins.error("Can not pullup/invert tmc virtual endstop") + mcu_endstop = ppins.setup_pin('endstop', self.diag_pin) + return TMCVirtualEndstop(self.mcu_tmc, mcu_endstop) + + +###################################################################### +# Config reading helpers +###################################################################### + +# Helper to configure and query the microstep settings +class TMCMicrostepHelper: + def __init__(self, config, mcu_tmc): + self.mcu_tmc = mcu_tmc + self.fields = mcu_tmc.get_fields() + steps = {'256': 0, '128': 1, '64': 2, '32': 3, '16': 4, + '8': 5, '4': 6, '2': 7, '1': 8} + mres = config.getchoice('microsteps', steps) + self.fields.set_field("MRES", mres) + def get_microsteps(self): + return 256 >> self.fields.get_field("MRES") + def get_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 mscnt >> self.fields.get_field("MRES") + +# Helper to configure "stealthchop" mode +def TMCStealthchopHelper(config, mcu_tmc, tmc_freq): + fields = mcu_tmc.get_fields() + en_pwm_mode = False + velocity = config.getfloat('stealthchop_threshold', 0., minval=0.) + if velocity: + stepper_name = " ".join(config.get_name().split()[1:]) + stepper_config = config.getsection(stepper_name) + step_dist = stepper_config.getfloat('step_distance') + step_dist_256 = step_dist / (1 << fields.get_field("MRES")) + threshold = int(tmc_freq * step_dist_256 / velocity + .5) + fields.set_field("TPWMTHRS", max(0, min(0xfffff, threshold))) + en_pwm_mode = True + reg = fields.lookup_register("en_pwm_mode", None) + if reg is not None: + fields.set_field("en_pwm_mode", en_pwm_mode) + else: + # TMC2208 uses en_spreadCycle + fields.set_field("en_spreadCycle", not en_pwm_mode) |