From 2c2bb720fae54422cf10a71569ce40476e909c09 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 4 Dec 2023 14:13:37 -0500 Subject: [PATCH] adxl345: Simplify sequence and limit_count upconversion Signed-off-by: Kevin O'Connor --- klippy/extras/adxl345.py | 18 +++++++----------- klippy/extras/lis2dw.py | 18 +++++++----------- klippy/extras/mpu9250.py | 18 +++++++----------- 3 files changed, 21 insertions(+), 33 deletions(-) diff --git a/klippy/extras/adxl345.py b/klippy/extras/adxl345.py index f5af2f16..52698cb6 100644 --- a/klippy/extras/adxl345.py +++ b/klippy/extras/adxl345.py @@ -307,9 +307,9 @@ class ADXL345: count = seq = 0 samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK) 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 = last_sequence - seq_diff + 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): @@ -343,15 +343,11 @@ class ADXL345: else: raise self.printer.command_error("Unable to query adxl345 fifo") mcu_clock = self.mcu.clock32_to_clock64(params['clock']) - sequence = (self.last_sequence & ~0xffff) | params['next_sequence'] - if sequence < self.last_sequence: - sequence += 0x10000 - self.last_sequence = sequence + seq_diff = (params['next_sequence'] - self.last_sequence) & 0xffff + self.last_sequence += seq_diff buffered = params['buffered'] - limit_count = (self.last_limit_count & ~0xffff) | params['limit_count'] - if limit_count < self.last_limit_count: - limit_count += 0x10000 - self.last_limit_count = limit_count + lc_diff = (params['limit_count'] - self.last_limit_count) & 0xffff + self.last_limit_count += lc_diff duration = params['query_ticks'] if duration > self.max_query_duration: # Skip measurement as a high query time could skew clock tracking @@ -359,7 +355,7 @@ class ADXL345: self.mcu.seconds_to_clock(.000005)) return 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) # The "chip clock" is the message counter plus .5 for average # inaccuracy of query responses and plus .5 for assumed offset diff --git a/klippy/extras/lis2dw.py b/klippy/extras/lis2dw.py index 5cf67639..ae62c22f 100644 --- a/klippy/extras/lis2dw.py +++ b/klippy/extras/lis2dw.py @@ -118,9 +118,9 @@ class LIS2DW: count = seq = 0 samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK) 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 = last_sequence - seq_diff + seq = last_sequence + seq_diff d = bytearray(params['data']) msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base @@ -156,15 +156,11 @@ class LIS2DW: else: raise self.printer.command_error("Unable to query lis2dw fifo") mcu_clock = self.mcu.clock32_to_clock64(params['clock']) - sequence = (self.last_sequence & ~0xffff) | params['next_sequence'] - if sequence < self.last_sequence: - sequence += 0x10000 - self.last_sequence = sequence + seq_diff = (params['next_sequence'] - self.last_sequence) & 0xffff + self.last_sequence += seq_diff buffered = params['buffered'] - limit_count = (self.last_limit_count & ~0xffff) | params['limit_count'] - if limit_count < self.last_limit_count: - limit_count += 0x10000 - self.last_limit_count = limit_count + lc_diff = (params['limit_count'] - self.last_limit_count) & 0xffff + self.last_limit_count += lc_diff duration = params['query_ticks'] if duration > self.max_query_duration: # Skip measurement as a high query time could skew clock tracking @@ -172,7 +168,7 @@ class LIS2DW: self.mcu.seconds_to_clock(.000005)) return 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) # The "chip clock" is the message counter plus .5 for average # inaccuracy of query responses and plus .5 for assumed offset diff --git a/klippy/extras/mpu9250.py b/klippy/extras/mpu9250.py index dc25449b..72c1f681 100644 --- a/klippy/extras/mpu9250.py +++ b/klippy/extras/mpu9250.py @@ -132,9 +132,9 @@ class MPU9250: count = seq = 0 samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK) 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 = last_sequence - seq_diff + seq = last_sequence + seq_diff d = bytearray(params['data']) msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base @@ -168,15 +168,11 @@ class MPU9250: else: raise self.printer.command_error("Unable to query mpu9250 fifo") mcu_clock = self.mcu.clock32_to_clock64(params['clock']) - sequence = (self.last_sequence & ~0xffff) | params['next_sequence'] - if sequence < self.last_sequence: - sequence += 0x10000 - self.last_sequence = sequence + seq_diff = (params['next_sequence'] - self.last_sequence) & 0xffff + self.last_sequence += seq_diff buffered = params['buffered'] - limit_count = (self.last_limit_count & ~0xffff) | params['limit_count'] - if limit_count < self.last_limit_count: - limit_count += 0x10000 - self.last_limit_count = limit_count + lc_diff = (params['limit_count'] - self.last_limit_count) & 0xffff + self.last_limit_count += lc_diff duration = params['query_ticks'] if duration > self.max_query_duration: # Skip measurement as a high query time could skew clock tracking @@ -184,7 +180,7 @@ class MPU9250: self.mcu.seconds_to_clock(.000005)) return 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) # The "chip clock" is the message counter plus .5 for average # inaccuracy of query responses and plus .5 for assumed offset