From 62bf52bfcf1aab6d97236c3945320d790774ad92 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 4 Dec 2023 14:00:47 -0500 Subject: [PATCH 1/5] serialqueue: Simplify sequence number upconversion Signed-off-by: Kevin O'Connor --- klippy/chelper/serialqueue.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/klippy/chelper/serialqueue.c b/klippy/chelper/serialqueue.c index e6810933aabc..c207495cdc6c 100644 --- a/klippy/chelper/serialqueue.c +++ b/klippy/chelper/serialqueue.c @@ -222,12 +222,11 @@ handle_message(struct serialqueue *sq, double eventtime, int len) pthread_mutex_lock(&sq->lock); // Calculate receive sequence number - uint64_t rseq = ((sq->receive_seq & ~MESSAGE_SEQ_MASK) - | (sq->input_buf[MESSAGE_POS_SEQ] & MESSAGE_SEQ_MASK)); + uint32_t rseq_delta = ((sq->input_buf[MESSAGE_POS_SEQ] - sq->receive_seq) + & MESSAGE_SEQ_MASK); + uint64_t rseq = sq->receive_seq + rseq_delta; if (rseq != sq->receive_seq) { // New sequence number - if (rseq < sq->receive_seq) - rseq += MESSAGE_SEQ_MASK+1; if (rseq > sq->send_seq && sq->receive_seq != 1) { // An ack for a message not sent? Out of order message? sq->bytes_invalid += len; From c5bd813d8b2199b3a1c08d1e29ac0a97d49e3ff8 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 4 Dec 2023 14:01:12 -0500 Subject: [PATCH 2/5] clocksync: Simplify 32bit clock upconversion code Signed-off-by: Kevin O'Connor --- klippy/clocksync.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/klippy/clocksync.py b/klippy/clocksync.py index f32ed3cd6188..80ed9db61f25 100644 --- a/klippy/clocksync.py +++ b/klippy/clocksync.py @@ -66,10 +66,8 @@ def _handle_clock(self, params): self.queries_pending = 0 # Extend clock to 64bit last_clock = self.last_clock - clock = (last_clock & ~0xffffffff) | params['clock'] - if clock < last_clock: - clock += 0x100000000 - self.last_clock = clock + clock_delta = (params['clock'] - last_clock) & 0xffffffff + self.last_clock = clock = last_clock + clock_delta # Check if this is the best round-trip-time seen so far sent_time = params['#sent_time'] if not sent_time: @@ -138,10 +136,9 @@ def estimated_print_time(self, eventtime): # misc commands def clock32_to_clock64(self, clock32): last_clock = self.last_clock - clock_diff = (last_clock - clock32) & 0xffffffff - if clock_diff & 0x80000000: - return last_clock + 0x100000000 - clock_diff - return last_clock - clock_diff + clock_diff = (clock32 - last_clock) & 0xffffffff + clock_diff -= (clock_diff & 0x80000000) << 1 + return last_clock + clock_diff def is_active(self): return self.queries_pending <= 4 def dump_debug(self): From fe7082e4a8e0beb7014584a16aaa22dc7084a01a Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 4 Dec 2023 14:03:27 -0500 Subject: [PATCH 3/5] buttons: Simplify ack upconversion code Signed-off-by: Kevin O'Connor --- klippy/extras/buttons.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/klippy/extras/buttons.py b/klippy/extras/buttons.py index 1a6147d1df28..472742576dae 100644 --- a/klippy/extras/buttons.py +++ b/klippy/extras/buttons.py @@ -57,10 +57,9 @@ def build_config(self): def handle_buttons_state(self, params): # Expand the message ack_count from 8-bit ack_count = self.ack_count - ack_diff = (ack_count - params['ack_count']) & 0xff - if ack_diff & 0x80: - ack_diff -= 0x100 - msg_ack_count = ack_count - ack_diff + ack_diff = (params['ack_count'] - ack_count) & 0xff + ack_diff -= (ack_diff & 0x80) << 1 + msg_ack_count = ack_count + ack_diff # Determine new buttons buttons = bytearray(params['state']) new_count = msg_ack_count + len(buttons) - self.ack_count From 1a83845c9f16577921b09053589e6a90627e3674 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 4 Dec 2023 14:08:22 -0500 Subject: [PATCH 4/5] angle: Simplify sequence upconversion Signed-off-by: Kevin O'Connor --- klippy/extras/angle.py | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/klippy/extras/angle.py b/klippy/extras/angle.py index 2e17749e90c7..30a4447ad65c 100644 --- a/klippy/extras/angle.py +++ b/klippy/extras/angle.py @@ -85,9 +85,9 @@ def apply_calibration(self, samples): cal2 = calibration[bucket + 1] adj = (angle & interp_mask) * (cal2 - cal1) adj = cal1 + ((adj + interp_round) >> interp_bits) - angle_diff = (angle - adj) & 0xffff + angle_diff = (adj - angle) & 0xffff angle_diff -= (angle_diff & 0x8000) << 1 - new_angle = angle - angle_diff + new_angle = angle + angle_diff if calibration_reversed: new_angle = -new_angle samples[i] = (samp_time, new_angle) @@ -375,9 +375,9 @@ 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 = (chip_clock - chip_mclock) & 0xffff cdiff -= (cdiff & 0x8000) << 1 - new_chip_clock = chip_mclock - cdiff + 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 @@ -489,21 +489,19 @@ def _extract_samples(self, raw_samples): count = error_count = 0 samples = [None] * (len(raw_samples) * 16) for params in raw_samples: - seq = (last_sequence & ~0xffff) | params['sequence'] - if seq < last_sequence: - seq += 0x10000 - last_sequence = seq + seq_diff = (params['sequence'] - last_sequence) & 0xffff + last_sequence += seq_diff d = bytearray(params['data']) - msg_mclock = start_clock + seq*16*sample_ticks + msg_mclock = start_clock + last_sequence*16*sample_ticks for i in range(len(d) // 3): tcode = d[i*3] if tcode == TCODE_ERROR: error_count += 1 continue raw_angle = d[i*3 + 1] | (d[i*3 + 2] << 8) - angle_diff = (last_angle - raw_angle) & 0xffff + angle_diff = (raw_angle - last_angle) & 0xffff angle_diff -= (angle_diff & 0x8000) << 1 - last_angle -= angle_diff + last_angle += angle_diff mclock = msg_mclock + i*sample_ticks if is_tcode_absolute: # tcode is tle5012b frame counter From 2c2bb720fae54422cf10a71569ce40476e909c09 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 4 Dec 2023 14:13:37 -0500 Subject: [PATCH 5/5] 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 f5af2f166324..52698cb6cfa7 100644 --- a/klippy/extras/adxl345.py +++ b/klippy/extras/adxl345.py @@ -307,9 +307,9 @@ def _extract_samples(self, raw_samples): 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 @@ def _update_clock(self, minclock=0): 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 @@ def _update_clock(self, minclock=0): 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 5cf6763939c5..ae62c22f67df 100644 --- a/klippy/extras/lis2dw.py +++ b/klippy/extras/lis2dw.py @@ -118,9 +118,9 @@ def _extract_samples(self, raw_samples): 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 @@ def _update_clock(self, minclock=0): 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 @@ def _update_clock(self, minclock=0): 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 dc25449bba7b..72c1f6817ca3 100644 --- a/klippy/extras/mpu9250.py +++ b/klippy/extras/mpu9250.py @@ -132,9 +132,9 @@ def _extract_samples(self, raw_samples): 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 @@ def _update_clock(self, minclock=0): 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 @@ def _update_clock(self, minclock=0): 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