diff options
| -rw-r--r-- | docs/Config_Reference.md | 6 | ||||
| -rw-r--r-- | klippy/extras/angle.py | 48 | ||||
| -rw-r--r-- | src/sensor_angle.c | 45 |
3 files changed, 90 insertions, 9 deletions
diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index 5d85e483..33cb153b 100644 --- a/docs/Config_Reference.md +++ b/docs/Config_Reference.md @@ -5040,8 +5040,8 @@ serial: ### [angle] Magnetic hall angle sensor support for reading stepper motor angle -shaft measurements using a1333, as5047d, or tle5012b SPI chips. The -measurements are available via the [API Server](API_Server.md) and +shaft measurements using a1333, as5047d, mt6816, 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 +5049,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", and "tle5012b". This parameter must be +# "a1333", "as5047d", "mt6816", and "tle5012b". This parameter must be # specified. #sample_period: 0.000400 # The query period (in seconds) to use during measurements. The diff --git a/klippy/extras/angle.py b/klippy/extras/angle.py index c51d8bf0..aaaa8300 100644 --- a/klippy/extras/angle.py +++ b/klippy/extras/angle.py @@ -411,6 +411,52 @@ class HelperTLE5012B: parser=lambda x: int(x, 0)) self._write_reg(reg, val) +class HelperMT6816: + SPI_MODE = 3 + SPI_SPEED = 10000000 + def __init__(self, config, spi, oid): + self.printer = config.get_printer() + 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) + 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): + return self.spi.spi_transfer(msg) + def get_static_delay(self): + return .000001 + def _read_reg(self, reg): + msg = [reg, 0, 0] + params = self._send_spi(msg) + resp = bytearray(params['response']) + val = (resp[1] << 8) | resp[2] + return val + def start(self): + pass + cmd_ANGLE_DEBUG_READ_help = "Query low-level angle sensor register" + def cmd_ANGLE_DEBUG_READ(self, gcmd): + reg = 0x83 + val = self._read_reg(reg) + gcmd.respond_info("ANGLE REG[0x%02x] = 0x%04x" % (reg, val)) + angle = val >> 2 + parity = bin(val >> 1).count("1") % 2 + gcmd.respond_info("Angle %i ~ %.2f" % (angle, angle * 360 / (1 << 14))) + gcmd.respond_info("No Mag: %i" % (val >> 1 & 0x1)) + gcmd.respond_info("Parity: %i == %i" % (parity, val & 0x1)) + BYTES_PER_SAMPLE = 3 SAMPLES_PER_BLOCK = bulk_sensor.MAX_BULK_MSG_SIZE // BYTES_PER_SAMPLE @@ -428,7 +474,7 @@ class Angle: self.last_sequence = self.last_angle = 0 # Sensor type sensors = { "a1333": HelperA1333, "as5047d": HelperAS5047D, - "tle5012b": HelperTLE5012B } + "tle5012b": HelperTLE5012B, "mt6816": HelperMT6816 } 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 54caecc2..3dfd6009 100644 --- a/src/sensor_angle.c +++ b/src/sensor_angle.c @@ -13,11 +13,18 @@ #include "sensor_bulk.h" // sensor_bulk_report #include "spicmds.h" // spidev_transfer -enum { SA_CHIP_A1333, SA_CHIP_AS5047D, SA_CHIP_TLE5012B, SA_CHIP_MAX }; +enum { + SA_CHIP_A1333, + SA_CHIP_AS5047D, + SA_CHIP_TLE5012B, + SA_CHIP_MT6816, + SA_CHIP_MAX +}; 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); enum { TCODE_ERROR = 0xff }; enum { @@ -131,6 +138,15 @@ a1333_query(struct spi_angle *sa, uint32_t stime) angle_add_data(sa, stime, mtime1, (msg[0] << 9) | (msg[1] << 1)); } +static int bit_parity(uint8_t *msg) +{ + uint_fast8_t parity = msg[0] ^ msg[1]; + parity ^= parity >> 4; + parity ^= parity >> 2; + parity ^= parity >> 1; + return parity; +} + // as5047d sensor query static void as5047d_query(struct spi_angle *sa, uint32_t stime) @@ -147,10 +163,7 @@ as5047d_query(struct spi_angle *sa, uint32_t stime) msg[0] = 0xC0; msg[1] = 0x00; spidev_transfer(sa->spi, 1, sizeof(msg), msg); - uint_fast8_t parity = msg[0] ^ msg[1]; - parity ^= parity >> 4; - parity ^= parity >> 2; - parity ^= parity >> 1; + uint_fast8_t parity = bit_parity(msg); if (parity & 1) angle_add_error(sa, SE_CRC); else if (msg[0] & 0x40) @@ -159,6 +172,26 @@ as5047d_query(struct spi_angle *sa, uint32_t stime) angle_add_data(sa, stime, mtime2, (msg[0] << 10) | (msg[1] << 2)); } +static void mt6816_query(struct spi_angle *sa, uint32_t stime) +{ + uint8_t msg[3] = {0x83, 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; + } + uint_fast8_t parity = bit_parity(&msg[1]); + if (parity & 1) + angle_add_error(sa, SE_CRC); + else if (msg[2] & 0x02) + angle_add_error(sa, SE_NO_ANGLE); + else + angle_add_data(sa, stime, mtime2, (msg[1] << 8) | (msg[2] & 0xfc)); +} + #define TLE_READ 0x80 #define TLE_READ_LATCH (TLE_READ | 0x04) #define TLE_REG_AVAL 0x02 @@ -301,6 +334,8 @@ spi_angle_task(void) as5047d_query(sa, stime); else if (chip == SA_CHIP_TLE5012B) tle5012b_query(sa, stime); + else if (chip == SA_CHIP_MT6816) + mt6816_query(sa, stime); angle_check_report(sa, oid); } } |