diff options
| -rw-r--r-- | docs/Config_Reference.md | 5 | ||||
| -rw-r--r-- | docs/G-Codes.md | 8 | ||||
| -rw-r--r-- | klippy/extras/angle.py | 151 | ||||
| -rw-r--r-- | src/sensor_angle.c | 38 |
4 files changed, 198 insertions, 4 deletions
diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index 33cb153b..f797d2b0 100644 --- a/docs/Config_Reference.md +++ b/docs/Config_Reference.md @@ -5040,7 +5040,8 @@ serial: ### [angle] Magnetic hall angle sensor support for reading stepper motor angle -shaft measurements using a1333, as5047d, mt6816, or tle5012b SPI chips. +shaft measurements using a1333, as5047d, mt6816, mt6826s, +or tle5012b SPI chips. The measurements are available via the [API Server](API_Server.md) and [motion analysis tool](Debugging.md#motion-analysis-and-data-logging). See the [G-Code reference](G-Codes.md#angle) for available commands. @@ -5049,7 +5050,7 @@ See the [G-Code reference](G-Codes.md#angle) for available commands. [angle my_angle_sensor] sensor_type: # The type of the magnetic hall sensor chip. Available choices are -# "a1333", "as5047d", "mt6816", and "tle5012b". This parameter must be +# "a1333", "as5047d", "mt6816", "mt6826s", and "tle5012b". This parameter must be # specified. #sample_period: 0.000400 # The query period (in seconds) to use during measurements. The diff --git a/docs/G-Codes.md b/docs/G-Codes.md index 7c90417c..94a6e9a9 100644 --- a/docs/G-Codes.md +++ b/docs/G-Codes.md @@ -127,6 +127,14 @@ use this tool the Python "numpy" package must be installed (see the [measuring resonance document](Measuring_Resonances.md#software-installation) for more information). +#### ANGLE_CHIP_CALIBRATE +`ANGLE_CHIP_CALIBRATE CHIP=<chip_name>`: Perform internal sensor calibration, +if implemented (MT6826S/MT6835). + +- **MT68XX**: The motor should be disconnected +from any printer carriage before performing calibration. +After calibration, the sensor should be reset by disconnecting the power. + #### ANGLE_DEBUG_READ `ANGLE_DEBUG_READ CHIP=<config_name> REG=<register>`: Queries sensor register "register" (e.g. 44 or 0x2C). Can be useful for debugging 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, diff --git a/src/sensor_angle.c b/src/sensor_angle.c index 3dfd6009..d0183579 100644 --- a/src/sensor_angle.c +++ b/src/sensor_angle.c @@ -18,6 +18,7 @@ enum { SA_CHIP_AS5047D, SA_CHIP_TLE5012B, SA_CHIP_MT6816, + SA_CHIP_MT6826S, SA_CHIP_MAX }; @@ -25,6 +26,7 @@ DECL_ENUMERATION("spi_angle_type", "a1333", SA_CHIP_A1333); DECL_ENUMERATION("spi_angle_type", "as5047d", SA_CHIP_AS5047D); DECL_ENUMERATION("spi_angle_type", "tle5012b", SA_CHIP_TLE5012B); DECL_ENUMERATION("spi_angle_type", "mt6816", SA_CHIP_MT6816); +DECL_ENUMERATION("spi_angle_type", "mt6826s", SA_CHIP_MT6826S); enum { TCODE_ERROR = 0xff }; enum { @@ -192,6 +194,40 @@ static void mt6816_query(struct spi_angle *sa, uint32_t stime) angle_add_data(sa, stime, mtime2, (msg[1] << 8) | (msg[2] & 0xfc)); } +static uint8_t +crc8_mt(uint8_t crc, uint8_t data) +{ + crc ^= data; + int i; + for (i = 0; i < 8; i++) + crc = crc & 0x80 ? (crc << 1) ^ 0x07 : crc << 1; + return crc; +} + +static void mt6826s_query(struct spi_angle *sa, uint32_t stime) +{ + uint8_t msg[6] = {0x30, 0x03, 0x00, 0x00, 0x00, 0x00}; + uint32_t mtime1 = timer_read_time(); + spidev_transfer(sa->spi, 1, sizeof(msg), msg); + uint32_t mtime2 = timer_read_time(); + // Data is latched on first sclk edge of response + if (mtime2 - mtime1 > MAX_SPI_READ_TIME) { + angle_add_error(sa, SE_SPI_TIME); + return; + } + uint8_t crc = 0; + for (int i = 2; i < 5; i++) + crc = crc8_mt(crc, msg[i]); + + if (crc != msg[5]) + angle_add_error(sa, SE_CRC); + else if (msg[4] & 0x02) + angle_add_error(sa, SE_NO_ANGLE); + else + angle_add_data(sa, stime, mtime2, (msg[2] << 8) | msg[3]); +} + + #define TLE_READ 0x80 #define TLE_READ_LATCH (TLE_READ | 0x04) #define TLE_REG_AVAL 0x02 @@ -336,6 +372,8 @@ spi_angle_task(void) tle5012b_query(sa, stime); else if (chip == SA_CHIP_MT6816) mt6816_query(sa, stime); + else if (chip == SA_CHIP_MT6826S) + mt6826s_query(sa, stime); angle_check_report(sa, oid); } } |