3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

adxl345: Simplify sequence and limit_count upconversion 

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2023-12-04 14:13:37 -05:00
parent 1a83845c9f
commit 2c2bb720fa
3 changed files with 21 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
klippy/extras/adxl345.py
View File

@ -307,9 +307,9 @@ class ADXL345:
count = seq = 0 count = seq = 0
samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK) samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK)
for params in raw_samples: for params in raw_samples:
seq_diff = (last_sequence - params['sequence']) & 0xffff seq_diff = (params['sequence'] - last_sequence) & 0xffff
seq_diff -= (seq_diff & 0x8000) << 1 seq_diff -= (seq_diff & 0x8000) << 1
seq = last_sequence - seq_diff seq = last_sequence + seq_diff
d = bytearray(params['data']) d = bytearray(params['data'])
msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base
for i in range(len(d) // BYTES_PER_SAMPLE): for i in range(len(d) // BYTES_PER_SAMPLE):
@ -343,15 +343,11 @@ class ADXL345:
else: else:
raise self.printer.command_error("Unable to query adxl345 fifo") raise self.printer.command_error("Unable to query adxl345 fifo")
mcu_clock = self.mcu.clock32_to_clock64(params['clock']) mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
sequence = (self.last_sequence & ~0xffff) | params['next_sequence'] seq_diff = (params['next_sequence'] - self.last_sequence) & 0xffff
if sequence < self.last_sequence: self.last_sequence += seq_diff
sequence += 0x10000
self.last_sequence = sequence
buffered = params['buffered'] buffered = params['buffered']
limit_count = (self.last_limit_count & ~0xffff) | params['limit_count'] lc_diff = (params['limit_count'] - self.last_limit_count) & 0xffff
if limit_count < self.last_limit_count: self.last_limit_count += lc_diff
limit_count += 0x10000
self.last_limit_count = limit_count
duration = params['query_ticks'] duration = params['query_ticks']
if duration > self.max_query_duration: if duration > self.max_query_duration:
# Skip measurement as a high query time could skew clock tracking # Skip measurement as a high query time could skew clock tracking
@ -359,7 +355,7 @@ class ADXL345:
self.mcu.seconds_to_clock(.000005)) self.mcu.seconds_to_clock(.000005))
return return
self.max_query_duration = 2 * duration self.max_query_duration = 2 * duration
msg_count = (sequence * SAMPLES_PER_BLOCK msg_count = (self.last_sequence * SAMPLES_PER_BLOCK
+ buffered // BYTES_PER_SAMPLE + fifo) + buffered // BYTES_PER_SAMPLE + fifo)
# The "chip clock" is the message counter plus .5 for average # The "chip clock" is the message counter plus .5 for average
# inaccuracy of query responses and plus .5 for assumed offset # inaccuracy of query responses and plus .5 for assumed offset

18
klippy/extras/lis2dw.py
View File

@ -118,9 +118,9 @@ class LIS2DW:
count = seq = 0 count = seq = 0
samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK) samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK)
for params in raw_samples: for params in raw_samples:
seq_diff = (last_sequence - params['sequence']) & 0xffff seq_diff = (params['sequence'] - last_sequence) & 0xffff
seq_diff -= (seq_diff & 0x8000) << 1 seq_diff -= (seq_diff & 0x8000) << 1
seq = last_sequence - seq_diff seq = last_sequence + seq_diff
d = bytearray(params['data']) d = bytearray(params['data'])
msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base
@ -156,15 +156,11 @@ class LIS2DW:
else: else:
raise self.printer.command_error("Unable to query lis2dw fifo") raise self.printer.command_error("Unable to query lis2dw fifo")
mcu_clock = self.mcu.clock32_to_clock64(params['clock']) mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
sequence = (self.last_sequence & ~0xffff) | params['next_sequence'] seq_diff = (params['next_sequence'] - self.last_sequence) & 0xffff
if sequence < self.last_sequence: self.last_sequence += seq_diff
sequence += 0x10000
self.last_sequence = sequence
buffered = params['buffered'] buffered = params['buffered']
limit_count = (self.last_limit_count & ~0xffff) | params['limit_count'] lc_diff = (params['limit_count'] - self.last_limit_count) & 0xffff
if limit_count < self.last_limit_count: self.last_limit_count += lc_diff
limit_count += 0x10000
self.last_limit_count = limit_count
duration = params['query_ticks'] duration = params['query_ticks']
if duration > self.max_query_duration: if duration > self.max_query_duration:
# Skip measurement as a high query time could skew clock tracking # Skip measurement as a high query time could skew clock tracking
@ -172,7 +168,7 @@ class LIS2DW:
self.mcu.seconds_to_clock(.000005)) self.mcu.seconds_to_clock(.000005))
return return
self.max_query_duration = 2 * duration self.max_query_duration = 2 * duration
msg_count = (sequence * SAMPLES_PER_BLOCK msg_count = (self.last_sequence * SAMPLES_PER_BLOCK
+ buffered // BYTES_PER_SAMPLE + fifo) + buffered // BYTES_PER_SAMPLE + fifo)
# The "chip clock" is the message counter plus .5 for average # The "chip clock" is the message counter plus .5 for average
# inaccuracy of query responses and plus .5 for assumed offset # inaccuracy of query responses and plus .5 for assumed offset

18
klippy/extras/mpu9250.py
View File

@ -132,9 +132,9 @@ class MPU9250:
count = seq = 0 count = seq = 0
samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK) samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK)
for params in raw_samples: for params in raw_samples:
seq_diff = (last_sequence - params['sequence']) & 0xffff seq_diff = (params['sequence'] - last_sequence) & 0xffff
seq_diff -= (seq_diff & 0x8000) << 1 seq_diff -= (seq_diff & 0x8000) << 1
seq = last_sequence - seq_diff seq = last_sequence + seq_diff
d = bytearray(params['data']) d = bytearray(params['data'])
msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base
@ -168,15 +168,11 @@ class MPU9250:
else: else:
raise self.printer.command_error("Unable to query mpu9250 fifo") raise self.printer.command_error("Unable to query mpu9250 fifo")
mcu_clock = self.mcu.clock32_to_clock64(params['clock']) mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
sequence = (self.last_sequence & ~0xffff) | params['next_sequence'] seq_diff = (params['next_sequence'] - self.last_sequence) & 0xffff
if sequence < self.last_sequence: self.last_sequence += seq_diff
sequence += 0x10000
self.last_sequence = sequence
buffered = params['buffered'] buffered = params['buffered']
limit_count = (self.last_limit_count & ~0xffff) | params['limit_count'] lc_diff = (params['limit_count'] - self.last_limit_count) & 0xffff
if limit_count < self.last_limit_count: self.last_limit_count += lc_diff
limit_count += 0x10000
self.last_limit_count = limit_count
duration = params['query_ticks'] duration = params['query_ticks']
if duration > self.max_query_duration: if duration > self.max_query_duration:
# Skip measurement as a high query time could skew clock tracking # Skip measurement as a high query time could skew clock tracking
@ -184,7 +180,7 @@ class MPU9250:
self.mcu.seconds_to_clock(.000005)) self.mcu.seconds_to_clock(.000005))
return return
self.max_query_duration = 2 * duration self.max_query_duration = 2 * duration
msg_count = (sequence * SAMPLES_PER_BLOCK msg_count = (self.last_sequence * SAMPLES_PER_BLOCK
+ buffered // BYTES_PER_SAMPLE + fifo) + buffered // BYTES_PER_SAMPLE + fifo)
# The "chip clock" is the message counter plus .5 for average # The "chip clock" is the message counter plus .5 for average
# inaccuracy of query responses and plus .5 for assumed offset # inaccuracy of query responses and plus .5 for assumed offset