sensor_angle: Support TLE5012b frame counter for timing
Use the tle5012b internal frame counter to calculate the time of each measurement. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor
parent
76558168d9
commit
f9d505e376
|
|
@ -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,6 +420,15 @@ class Angle:
|
|||
clock_to_print_time = self.mcu.clock_to_print_time
|
||||
last_sequence = self.last_sequence
|
||||
last_angle = self.last_angle
|
||||
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
|
||||
|
|
@ -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<<time_shift)
|
||||
ptime = round(clock_to_print_time(mclock) - static_delay, 6)
|
||||
mclock = msg_mclock + i*sample_ticks
|
||||
if is_tcode_absolute:
|
||||
# tcode is tle5012b frame counter
|
||||
mdiff = mclock - last_chip_mcu_clock
|
||||
chip_mclock = last_chip_clock + int(mdiff * chip_freq + .5)
|
||||
cdiff = ((tcode << 10) - chip_mclock) & 0xffff
|
||||
cdiff -= (cdiff & 0x8000) << 1
|
||||
sclock = mclock + (cdiff - 0x800) * inv_chip_freq
|
||||
else:
|
||||
# tcode is mcu clock offset shifted by time_shift
|
||||
sclock = mclock + (tcode<<time_shift)
|
||||
ptime = round(clock_to_print_time(sclock) - static_delay, 6)
|
||||
samples[count] = (ptime, last_angle)
|
||||
count += 1
|
||||
self.last_sequence = last_sequence
|
||||
|
|
@ -390,6 +470,8 @@ class Angle:
|
|||
return samples, error_count
|
||||
# API interface
|
||||
def _api_update(self, eventtime):
|
||||
if self.sensor_helper.is_tcode_absolute:
|
||||
self.sensor_helper.update_clock()
|
||||
with self.lock:
|
||||
raw_samples = self.raw_samples
|
||||
self.raw_samples = []
|
||||
|
|
|
|||
|
|
@ -90,14 +90,21 @@ angle_check_report(struct spi_angle *sa, uint8_t oid)
|
|||
angle_report(sa, oid);
|
||||
}
|
||||
|
||||
// Add an entry to the measurement buffer
|
||||
static void
|
||||
angle_add(struct spi_angle *sa, uint_fast8_t tcode, uint_fast16_t data)
|
||||
{
|
||||
sa->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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue