angle: mt6826s added support
Signed-off-by: Timofey Titovets <nefelim4ag@gmail.com>
This commit is contained in:
Timofey Titovets
committed by
KevinOConnor
KevinOConnor
parent
90c1b82baa
commit
2b9e041a86
@@ -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,
|
||||
|
||||
Reference in New Issue
Block a user