Merge branch 'master' of https://github.com/Klipper3d/klipper

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;

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):

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,23 +343,19 @@ 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
             self.max_query_duration = max(2 * self.max_query_duration,
                                           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

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

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

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,23 +156,19 @@ 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
             self.max_query_duration = max(2 * self.max_query_duration,
                                           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

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,23 +168,19 @@ 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
             self.max_query_duration = max(2 * self.max_query_duration,
                                           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