bulk_sensor: Add new ChipClockUpdater helper class

All the accelerometers use a standard response for their query_status
messages.  Create a common helper class to process those responses.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 48 insertions, 0 deletions

diff --git a/klippy/extras/bulk_sensor.py b/klippy/extras/bulk_sensor.py
index 15749051..28aed48a 100644
--- a/klippy/extras/bulk_sensor.py
+++ b/klippy/extras/bulk_sensor.py
@@ -71,3 +71,51 @@ class ClockSyncRegression:
         base_time = clock_to_print_time(base_mcu)
         inv_freq = clock_to_print_time(base_mcu + inv_cfreq) - base_time
         return base_time, base_chip, inv_freq
+
+MAX_BULK_MSG_SIZE = 52
+
+# Handle common periodic chip status query responses
+class ChipClockUpdater:
+    def __init__(self, clock_sync, bytes_per_sample):
+        self.clock_sync = clock_sync
+        self.bytes_per_sample = bytes_per_sample
+        self.samples_per_block = MAX_BULK_MSG_SIZE // bytes_per_sample
+        self.mcu = clock_sync.mcu
+        self.last_sequence = self.max_query_duration = 0
+        self.last_limit_count = 0
+    def get_last_sequence(self):
+        return self.last_sequence
+    def get_last_limit_count(self):
+        return self.last_limit_count
+    def clear_duration_filter(self):
+        self.max_query_duration = 1 << 31
+    def note_start(self, reqclock):
+        self.last_sequence = 0
+        self.last_limit_count = 0
+        self.clock_sync.reset(reqclock, 0)
+        self.clear_duration_filter()
+    def update_clock(self, params):
+        # Handle a status response message of the form:
+        #   adxl345_status oid=x clock=x query_ticks=x next_sequence=x
+        #     buffered=x fifo=x limit_count=x
+        fifo = params['fifo']
+        mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
+        seq_diff = (params['next_sequence'] - self.last_sequence) & 0xffff
+        self.last_sequence += seq_diff
+        buffered = params['buffered']
+        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 = (self.last_sequence * self.samples_per_block
+                     + buffered // self.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
+        # of hardware processing time.
+        chip_clock = msg_count + 1
+        self.clock_sync.update(mcu_clock + duration // 2, chip_clock)