Angle: add support for MT6816, MT6826S sensors

docs/Config_Reference.md

@@ -5030,16 +5030,17 @@ 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, 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.
 
 ```
 [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", "mt6826s", and "tle5012b". This parameter must be
 #   specified.
 #sample_period: 0.000400
 #   The query period (in seconds) to use during measurements. The

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

klippy/extras/angle.py

@@ -411,6 +411,196 @@ def cmd_ANGLE_DEBUG_WRITE(self, gcmd):
                        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))
+
+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
 
@@ -427,8 +617,11 @@ def __init__(self, config):
         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 }
+        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,

src/Kconfig

@@ -96,9 +96,21 @@ config WANT_DISPLAYS
     bool
     depends on HAVE_GPIO
     default y
-config WANT_SENSORS
+config WANT_THERMOCOUPLE
     bool
-    depends on HAVE_GPIO_I2C || HAVE_GPIO_SPI
+    depends on HAVE_GPIO_SPI
+    default y
+config WANT_SENSOR_ADXL345
+    bool
+    depends on HAVE_GPIO_SPI
+    default y
+config WANT_SENSOR_ANGLE
+    bool
+    depends on HAVE_GPIO_SPI
+    default y
+config WANT_SENSOR_MPU9250
+    bool
+    depends on HAVE_GPIO_I2C
     default y
 config WANT_LIS2DW
     bool
@@ -126,8 +138,10 @@ config WANT_SOFTWARE_SPI
     default y
 config NEED_SENSOR_BULK
     bool
-    depends on WANT_SENSORS || WANT_LIS2DW || WANT_LDC1612 || WANT_HX71X \
-        || WANT_ADS1220
+    depends on WANT_LIS2DW || WANT_LDC1612 || WANT_HX71X \
+        || WANT_ADS1220 || WANT_THERMOCOUPLE \
+        || WANT_SENSOR_ADXL345 || WANT_SENSOR_ANGLE \
+        || WANT_SENSOR_MPU9250
     default y
 menu "Optional features (to reduce code size)"
     depends on HAVE_LIMITED_CODE_SIZE
@@ -137,9 +151,18 @@ config WANT_GPIO_BITBANGING
 config WANT_DISPLAYS
     bool "Support LCD devices"
     depends on HAVE_GPIO
-config WANT_SENSORS
-    bool "Support external sensor devices"
-    depends on HAVE_GPIO_I2C || HAVE_GPIO_SPI
+config WANT_THERMOCOUPLE
+    bool "Support thermocouple MAX sensors"
+    depends on HAVE_GPIO_SPI
+config WANT_SENSOR_ADXL345
+    bool "Support adxl accelerometers"
+    depends on HAVE_GPIO_SPI
+config WANT_SENSOR_ANGLE
+    bool "Support angle sensors"
+    depends on HAVE_GPIO_SPI
+config WANT_SENSOR_MPU9250
+    bool "Support MPU accelerometers"
+    depends on HAVE_GPIO_I2C
 config WANT_LIS2DW
     bool "Support lis2dw and lis3dh 3-axis accelerometers"
     depends on HAVE_GPIO_SPI || HAVE_GPIO_I2C

src/Makefile

@@ -14,10 +14,13 @@ src-$(CONFIG_WANT_GPIO_BITBANGING) += buttons.c tmcuart.c neopixel.c \
 src-$(CONFIG_WANT_DISPLAYS) += lcd_st7920.c lcd_hd44780.c
 src-$(CONFIG_WANT_SOFTWARE_SPI) += spi_software.c
 src-$(CONFIG_WANT_SOFTWARE_I2C) += i2c_software.c
-sensors-src-$(CONFIG_HAVE_GPIO_SPI) := thermocouple.c sensor_adxl345.c \
-    sensor_angle.c
-sensors-src-$(CONFIG_HAVE_GPIO_I2C) += sensor_mpu9250.c
-src-$(CONFIG_WANT_SENSORS) += $(sensors-src-y)
+src-thermocouple-$(CONFIG_WANT_THERMOCOUPLE) := thermocouple.c
+src-adxl345-$(CONFIG_WANT_SENSOR_ADXL345) := sensor_adxl345.c
+src-angle-$(CONFIG_WANT_SENSOR_ANGLE) := sensor_angle.c
+src-mpu9250-$(CONFIG_WANT_SENSOR_MPU9250) := sensor_mpu9250.c
+src-$(CONFIG_HAVE_GPIO_SPI) += $(src-thermocouple-y) $(src-adxl345-y) \
+    $(src-angle-y)
+src-$(CONFIG_HAVE_GPIO_I2C) += $(src-mpu9250-y)
 src-$(CONFIG_WANT_LIS2DW) += sensor_lis2dw.c
 src-$(CONFIG_WANT_LDC1612) += sensor_ldc1612.c
 src-$(CONFIG_WANT_HX71X) += sensor_hx71x.c