diff options
Diffstat (limited to 'klippy/extras/angle.py')
| -rw-r--r-- | klippy/extras/angle.py | 151 |
1 files changed, 149 insertions, 2 deletions
diff --git a/klippy/extras/angle.py b/klippy/extras/angle.py index aaaa8300..3b1f323e 100644 --- a/klippy/extras/angle.py +++ b/klippy/extras/angle.py @@ -457,6 +457,150 @@ class HelperMT6816: gcmd.respond_info("No Mag: %i" % (val >> 1 & 0x1)) gcmd.respond_info("Parity: %i == %i" % (parity, val & 0x1)) +class HelperMT6826S: + SPI_MODE = 3 + SPI_SPEED = 10000000 + def __init__(self, config, spi, oid): + self.printer = config.get_printer() + self.stepper_name = config.get('stepper', None) + self.spi = spi + self.oid = oid + self.mcu = spi.get_mcu() + self.mcu.register_config_callback(self._build_config) + self.spi_angle_transfer_cmd = None + self.is_tcode_absolute = False + self.last_temperature = None + name = config.get_name().split()[-1] + gcode = self.printer.lookup_object("gcode") + gcode.register_mux_command("ANGLE_DEBUG_READ", "CHIP", name, + self.cmd_ANGLE_DEBUG_READ, + desc=self.cmd_ANGLE_DEBUG_READ_help) + gcode.register_mux_command("ANGLE_CHIP_CALIBRATE", "CHIP", name, + self.cmd_ANGLE_CHIP_CALIBRATE, + desc=self.cmd_ANGLE_CHIP_CALIBRATE_help) + self.status_map = { + 0: "No Calibration", + 1: "Running Calibration", + 2: "Calibration Failed", + 3: "Calibration Successful" + } + def _build_config(self): + cmdqueue = self.spi.get_command_queue() + self.spi_angle_transfer_cmd = self.mcu.lookup_query_command( + "spi_angle_transfer oid=%c data=%*s", + "spi_angle_transfer_response oid=%c clock=%u response=%*s", + oid=self.oid, cq=cmdqueue) + def _send_spi(self, msg): + params = self.spi.spi_transfer(msg) + return params + def get_static_delay(self): + return .00001 + def _read_reg(self, reg): + reg = 0x3000 | reg + msg = [reg >> 8, reg & 0xff, 0] + params = self._send_spi(msg) + resp = bytearray(params['response']) + return resp[2] + def _write_reg(self, reg, data): + reg = 0x6000 | reg + msg = [reg >> 8, reg & 0xff, data] + self._send_spi(msg) + def crc8(self, data): + polynomial = 0x07 + crc = 0x00 + for byte in data: + crc ^= byte + for _ in range(8): + if crc & 0x80: + crc = (crc << 1) ^ polynomial + else: + crc <<= 1 + crc &= 0xFF + return crc + def _read_angle(self, reg): + reg = 0x3000 | reg + msg = [reg >> 8, reg & 0xff, 0, 0, 0, 0] + params = self._send_spi(msg) + resp = bytearray(params['response']) + angle = (resp[2] << 7) | (resp[3] >> 1) + status = resp[4] + crc_computed = self.crc8([resp[2], resp[3], resp[4]]) + crc = resp[5] + return angle, status, crc, crc_computed + def start(self): + val = self._read_reg(0x00d) + # Set histeresis to 0.003 degree + self._write_reg(0x00d, (val & 0xf8) | 0x5) + def get_microsteps(self): + configfile = self.printer.lookup_object('configfile') + sconfig = configfile.get_status(None)['settings'] + stconfig = sconfig.get(self.stepper_name, {}) + microsteps = stconfig['microsteps'] + full_steps = stconfig['full_steps_per_rotation'] + return microsteps, full_steps + cmd_ANGLE_CHIP_CALIBRATE_help = "Run MT6826s calibration sequence" + def cmd_ANGLE_CHIP_CALIBRATE(self, gcmd): + fmove = self.printer.lookup_object('force_move') + mcu_stepper = fmove.lookup_stepper(self.stepper_name) + if self.stepper_name is None: + gcmd.respond_info("stepper not defined") + return + + gcmd.respond_info("MT6826S Run calibration sequence") + gcmd.respond_info("Motor will do 18+ rotations -" + + " ensure pulley is disconnected") + req_freq = self._read_reg(0x00e) >> 4 & 0x7 + # Minimal calibration speed + rpm = (3200 >> req_freq) + 1 + rps = rpm / 60 + move = fmove.manual_move + # Move stepper several turns (to allow internal sensor calibration) + microsteps, full_steps = self.get_microsteps() + step_dist = mcu_stepper.get_step_dist() + full_step_dist = step_dist * microsteps + rotation_dist = full_steps * full_step_dist + move(mcu_stepper, 2 * rotation_dist, rps * rotation_dist) + self._write_reg(0x155, 0x5e) + move(mcu_stepper, 20 * rotation_dist, rps * rotation_dist) + val = self._read_reg(0x113) + code = val >> 6 + gcmd.respond_info("Status: %s" % (self.status_map[code])) + while code == 1: + move(mcu_stepper, 5 * rotation_dist, rps * rotation_dist) + val = self._read_reg(0x113) + code = val >> 6 + gcmd.respond_info("Status: %s" % (self.status_map[code])) + if code == 2: + gcmd.respond_info("Calibration failed") + if code == 3: + gcmd.respond_info("Calibration success, please poweroff sensor") + cmd_ANGLE_DEBUG_READ_help = "Query low-level angle sensor register" + def cmd_ANGLE_DEBUG_READ(self, gcmd): + reg = gcmd.get("REG", minval=0, maxval=0x155, + parser=lambda x: int(x, 0)) + if reg == 0x003: + angle, status, crc1, crc2 = self._read_angle(reg) + gcmd.respond_info("ANGLE REG[0x003] = 0x%02x" % + (angle >> 7)) + gcmd.respond_info("ANGLE REG[0x004] = 0x%02x" % + ((angle << 1) & 0xff)) + gcmd.respond_info("Angle %i ~ %.2f" % (angle, + angle * 360 / (1 << 15))) + gcmd.respond_info("Weak Mag: %i" % (status >> 1 & 0x1)) + gcmd.respond_info("Under Voltage: %i" % (status >> 2 & 0x1)) + gcmd.respond_info("CRC: 0x%02x == 0x%02x" % (crc1, crc2)) + elif reg == 0x00e: + val = self._read_reg(reg) + gcmd.respond_info("GPIO_DS = %i" % (val >> 7)) + gcmd.respond_info("AUTOCAL_FREQ = %i" % (val >> 4 & 0x7)) + elif reg == 0x113: + val = self._read_reg(reg) + gcmd.respond_info("Status: %s" % (self.cal_status[val >> 6])) + else: + val = self._read_reg(reg) + gcmd.respond_info("REG[0x%04x] = 0x%02x" % (reg, val)) + + BYTES_PER_SAMPLE = 3 SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE @@ -473,8 +617,11 @@ class Angle: self.start_clock = self.time_shift = self.sample_ticks = 0 self.last_sequence = self.last_angle = 0 # Sensor type - sensors = { "a1333": HelperA1333, "as5047d": HelperAS5047D, - "tle5012b": HelperTLE5012B, "mt6816": HelperMT6816 } + sensors = { "a1333": HelperA1333, + "as5047d": HelperAS5047D, + "tle5012b": HelperTLE5012B, + "mt6816": HelperMT6816, + "mt6826s": HelperMT6826S } sensor_type = config.getchoice('sensor_type', {s: s for s in sensors}) sensor_class = sensors[sensor_type] self.spi = bus.MCU_SPI_from_config(config, sensor_class.SPI_MODE, |