diff --git a/klippy/extras/angle.py b/klippy/extras/angle.py index 1eec3cf4..c61d2d01 100644 --- a/klippy/extras/angle.py +++ b/klippy/extras/angle.py @@ -263,6 +263,7 @@ class HelperA1333: SPI_SPEED = 10000000 def __init__(self, config, spi, oid): self.spi = spi + self.is_tcode_absolute = False def get_static_delay(self): return .000001 def start(self): @@ -274,6 +275,7 @@ class HelperAS5047D: SPI_SPEED = int(1. / .000000350) def __init__(self, config, spi, oid): self.spi = spi + self.is_tcode_absolute = False def get_static_delay(self): return .000100 def start(self): @@ -286,12 +288,71 @@ class HelperTLE5012B: SPI_MODE = 1 SPI_SPEED = 4000000 def __init__(self, config, spi, oid): + self.printer = config.get_printer() self.spi = spi - def get_static_delay(self): - return .000042700 * 2.5 + self.oid = oid + self.is_tcode_absolute = True + self.mcu = spi.get_mcu() + self.mcu.register_config_callback(self._build_config) + self.spi_angle_transfer_cmd = None + self.last_chip_mcu_clock = self.last_chip_clock = 0 + self.chip_freq = 0. + 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 get_tcode_params(self): + return self.last_chip_mcu_clock, self.last_chip_clock, self.chip_freq + def _calc_crc(self, data): + crc = 0xff + for d in data: + crc ^= d + for i in range(8): + if crc & 0x80: + crc = (crc << 1) ^ 0x1d + else: + crc <<= 1 + return (~crc) & 0xff + def _send_spi(self, msg): + for retry in range(5): + if msg[0] & 0x04: + params = self.spi_angle_transfer_cmd.send([self.oid, msg]) + else: + params = self.spi.spi_transfer(msg) + resp = bytearray(params['response']) + crc = self._calc_crc(bytearray(msg[:2]) + resp[2:-2]) + if crc == resp[-1]: + return params + raise self.printer.command_error("Unable to query tle5012b chip") + def _query_clock(self): + # Read frame counter (and normalize to a 16bit counter) + msg = [0x84, 0x42, 0, 0, 0, 0, 0, 0] # Read with latch, AREV and FSYNC + params = self._send_spi(msg) + resp = bytearray(params['response']) + mcu_clock = self.mcu.clock32_to_clock64(params['clock']) + chip_clock = ((resp[2] & 0x7e) << 9) | ((resp[4] & 0x3e) << 4) + return mcu_clock, chip_clock + def update_clock(self): + mcu_clock, chip_clock = self._query_clock() + mdiff = mcu_clock - self.last_chip_mcu_clock + chip_mclock = self.last_chip_clock + int(mdiff * self.chip_freq + .5) + cdiff = (chip_mclock - chip_clock) & 0xffff + cdiff -= (cdiff & 0x8000) << 1 + new_chip_clock = chip_mclock - cdiff + self.chip_freq = float(new_chip_clock - self.last_chip_clock) / mdiff + self.last_chip_clock = new_chip_clock + self.last_chip_mcu_clock = mcu_clock def start(self): # Clear any errors from device - self.spi.spi_transfer([0x80, 0x01, 0x00, 0x00, 0x00, 0x00]) # Read STAT + self._send_spi([0x80, 0x01, 0x00, 0x00, 0x00, 0x00]) # Read STAT + # Setup starting clock values + mcu_clock, chip_clock = self._query_clock() + self.last_chip_clock = chip_clock + self.last_chip_mcu_clock = mcu_clock + self.chip_freq = float(1<<5) / self.mcu.seconds_to_clock(1. / 750000.) + self.update_clock() SAMPLE_PERIOD = 0.000400 @@ -359,8 +420,17 @@ class Angle: clock_to_print_time = self.mcu.clock_to_print_time last_sequence = self.last_sequence last_angle = self.last_angle - time_shift = self.time_shift - static_delay = self.sensor_helper.get_static_delay() + time_shift = 0 + static_delay = 0. + last_chip_mcu_clock = last_chip_clock = chip_freq = inv_chip_freq = 0. + is_tcode_absolute = self.sensor_helper.is_tcode_absolute + if is_tcode_absolute: + tparams = self.sensor_helper.get_tcode_params() + last_chip_mcu_clock, last_chip_clock, chip_freq = tparams + inv_chip_freq = 1. / chip_freq + else: + time_shift = self.time_shift + static_delay = self.sensor_helper.get_static_delay() # Process every message in raw_samples count = error_count = 0 samples = [None] * (len(raw_samples) * 16) @@ -380,8 +450,18 @@ class Angle: angle_diff = (last_angle - raw_angle) & 0xffff angle_diff -= (angle_diff & 0x8000) << 1 last_angle -= angle_diff - mclock = msg_mclock + i*sample_ticks + (tcode<data[sa->data_count] = tcode; + sa->data[sa->data_count + 1] = data; + sa->data[sa->data_count + 2] = data >> 8; + sa->data_count += 3; +} + // Add an error indicator to the measurement buffer static void angle_add_error(struct spi_angle *sa, uint_fast8_t error_code) { - sa->data[sa->data_count] = TCODE_ERROR; - sa->data[sa->data_count + 1] = error_code; - sa->data[sa->data_count + 2] = 0; - sa->data_count += 3; + angle_add(sa, TCODE_ERROR, error_code); } // Add a measurement to the buffer @@ -112,10 +119,7 @@ angle_add_data(struct spi_angle *sa, uint32_t stime, uint32_t mtime angle_add_error(sa, SE_SCHEDULE); return; } - sa->data[sa->data_count] = tdiff; - sa->data[sa->data_count + 1] = angle; - sa->data[sa->data_count + 2] = angle >> 8; - sa->data_count += 3; + angle_add(sa, tdiff, angle); } // a1333 sensor query @@ -200,18 +204,22 @@ tle5012b_query(struct spi_angle *sa, uint32_t stime) uint32_t mtime = timer_read_time(); irq_enable(); - uint8_t msg[6] = { TLE_READ_LATCH, (TLE_REG_AVAL << 4) | 0x01, 0, 0, 0, 0 }; - uint8_t start_crc = 0x3f; // 0x3f == crc8(crc8(0xff, msg[0]), msg[1]) + uint8_t msg[10] = { TLE_READ_LATCH, (TLE_REG_AVAL << 4) | 0x03 }; + uint8_t crc = 0x05; // 0x05 == crc8(crc8(0xff, msg[0]), msg[1]) spidev_transfer(sa->spi, 1, sizeof(msg), msg); - uint8_t crc = ~crc8(crc8(start_crc, msg[2]), msg[3]); - if (crc != msg[5]) + int i; + for (i=2; i<8; i++) + crc = crc8(crc, msg[i]); + if (((~crc) & 0xff) != msg[9]) angle_add_error(sa, SE_CRC); - else if (!(msg[4] & (1<<4))) + else if (!(msg[8] & (1<<4))) angle_add_error(sa, SE_NO_ANGLE); else if (!(msg[2] & 0x80)) angle_add_error(sa, SE_DUP); + else if (mtime - stime > timer_from_us(32 * 32 * 1000000UL / 750000)) + angle_add_error(sa, SE_SCHEDULE); else - angle_add_data(sa, stime, mtime, (msg[2] << 9) | (msg[3] << 1)); + angle_add(sa, (msg[6] >> 1) & 0x3f, (msg[2] << 9) | (msg[3] << 1)); } void @@ -240,6 +248,41 @@ command_query_spi_angle(uint32_t *args) DECL_COMMAND(command_query_spi_angle, "query_spi_angle oid=%c clock=%u rest_ticks=%u time_shift=%c"); +void +command_spi_angle_transfer(uint32_t *args) +{ + uint8_t oid = args[0]; + struct spi_angle *sa = oid_lookup(oid, command_config_spi_angle); + uint8_t data_len = args[1]; + uint8_t *data = command_decode_ptr(args[2]); + uint32_t mtime; + uint_fast8_t chip = sa->chip_type; + if (chip == SA_CHIP_TLE5012B) { + // Latch data (data is latched on rising CS of a NULL message) + struct gpio_out cs_pin = spidev_get_cs_pin(sa->spi); + gpio_out_write(cs_pin, 0); + udelay(1); + irq_disable(); + gpio_out_write(cs_pin, 1); + mtime = timer_read_time(); + irq_enable(); + spidev_transfer(sa->spi, 1, data_len, data); + } else { + uint32_t mtime1 = timer_read_time(); + spidev_transfer(sa->spi, 1, data_len, data); + uint32_t mtime2 = timer_read_time(); + if (mtime2 - mtime1 > MAX_SPI_READ_TIME) + data_len = 0; + if (chip == SA_CHIP_AS5047D) + mtime = mtime2; + else + mtime = mtime1; + } + sendf("spi_angle_transfer_response oid=%c clock=%u response=%*s" + , oid, mtime, data_len, data); +} +DECL_COMMAND(command_spi_angle_transfer, "spi_angle_transfer oid=%c data=%*s"); + // Background task that performs measurements void spi_angle_task(void)