mirror of
https://github.com/Klipper3d/klipper.git
synced 2025-07-18 04:08:04 -06:00
adxl345: Move sample timestamp calculation to reusable code
Add a new extract_samples() method to the ChipClockUpdater class that calculates the sample timestamp for each sample in a list of bulk sensor reports. Update the adxl345 code to use that extract_samples() code. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
c106955850
commit
95fdb68587
2 changed files with 44 additions and 31 deletions
|
@ -248,37 +248,25 @@ class ADXL345:
|
|||
return aqh
|
||||
# Measurement decoding
|
||||
def _extract_samples(self, raw_samples):
|
||||
# Load variables to optimize inner loop below
|
||||
# Convert messages to samples
|
||||
samples = self.clock_updater.extract_samples("BBBBB", raw_samples)
|
||||
# Convert samples
|
||||
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
|
||||
last_sequence = self.clock_updater.get_last_sequence()
|
||||
time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
|
||||
# Process every message in raw_samples
|
||||
count = seq = 0
|
||||
samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK)
|
||||
for params in raw_samples:
|
||||
seq_diff = (params['sequence'] - last_sequence) & 0xffff
|
||||
seq_diff -= (seq_diff & 0x8000) << 1
|
||||
seq = last_sequence + seq_diff
|
||||
d = bytearray(params['data'])
|
||||
msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base
|
||||
for i in range(len(d) // BYTES_PER_SAMPLE):
|
||||
d_xyz = d[i*BYTES_PER_SAMPLE:(i+1)*BYTES_PER_SAMPLE]
|
||||
xlow, ylow, zlow, xzhigh, yzhigh = d_xyz
|
||||
if yzhigh & 0x80:
|
||||
self.last_error_count += 1
|
||||
continue
|
||||
rx = (xlow | ((xzhigh & 0x1f) << 8)) - ((xzhigh & 0x10) << 9)
|
||||
ry = (ylow | ((yzhigh & 0x1f) << 8)) - ((yzhigh & 0x10) << 9)
|
||||
rz = ((zlow | ((xzhigh & 0xe0) << 3) | ((yzhigh & 0xe0) << 6))
|
||||
- ((yzhigh & 0x40) << 7))
|
||||
raw_xyz = (rx, ry, rz)
|
||||
x = round(raw_xyz[x_pos] * x_scale, 6)
|
||||
y = round(raw_xyz[y_pos] * y_scale, 6)
|
||||
z = round(raw_xyz[z_pos] * z_scale, 6)
|
||||
ptime = round(time_base + (msg_cdiff + i) * inv_freq, 6)
|
||||
samples[count] = (ptime, x, y, z)
|
||||
count += 1
|
||||
self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
|
||||
count = 0
|
||||
for ptime, xlow, ylow, zlow, xzhigh, yzhigh in samples:
|
||||
if yzhigh & 0x80:
|
||||
self.last_error_count += 1
|
||||
continue
|
||||
rx = (xlow | ((xzhigh & 0x1f) << 8)) - ((xzhigh & 0x10) << 9)
|
||||
ry = (ylow | ((yzhigh & 0x1f) << 8)) - ((yzhigh & 0x10) << 9)
|
||||
rz = ((zlow | ((xzhigh & 0xe0) << 3) | ((yzhigh & 0xe0) << 6))
|
||||
- ((yzhigh & 0x40) << 7))
|
||||
raw_xyz = (rx, ry, rz)
|
||||
x = round(raw_xyz[x_pos] * x_scale, 6)
|
||||
y = round(raw_xyz[y_pos] * y_scale, 6)
|
||||
z = round(raw_xyz[z_pos] * z_scale, 6)
|
||||
samples[count] = (round(ptime, 6), x, y, z)
|
||||
count += 1
|
||||
del samples[count:]
|
||||
return samples
|
||||
# Start, stop, and process message batches
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue