From 95fdb68587520e5a36449f58e2d9cf5459924d1c Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Sat, 13 Apr 2024 20:03:47 -0400
Subject: [PATCH] adxl345: Move sample timestamp calculation to reusable code

Add a new extract_samples() method to the ChipClockUpdater class that
calculates the sample timestamp for each sample in a list of bulk
sensor reports.

Update the adxl345 code to use that extract_samples() code.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 klippy/extras/adxl345.py     | 48 ++++++++++++++----------------------
 klippy/extras/bulk_sensor.py | 27 +++++++++++++++++++-
 2 files changed, 44 insertions(+), 31 deletions(-)

diff --git a/klippy/extras/adxl345.py b/klippy/extras/adxl345.py
index 738903f38..623b8e504 100644
--- a/klippy/extras/adxl345.py
+++ b/klippy/extras/adxl345.py
@@ -248,37 +248,25 @@ def start_internal_client(self):
         return aqh
     # Measurement decoding
     def _extract_samples(self, raw_samples):
-        # Load variables to optimize inner loop below
+        # Convert messages to samples
+        samples = self.clock_updater.extract_samples("BBBBB", raw_samples)
+        # Convert samples
         (x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
-        last_sequence = self.clock_updater.get_last_sequence()
-        time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
-        # Process every message in raw_samples
-        count = seq = 0
-        samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK)
-        for params in raw_samples:
-            seq_diff = (params['sequence'] - last_sequence) & 0xffff
-            seq_diff -= (seq_diff & 0x8000) << 1
-            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):
-                d_xyz = d[i*BYTES_PER_SAMPLE:(i+1)*BYTES_PER_SAMPLE]
-                xlow, ylow, zlow, xzhigh, yzhigh = d_xyz
-                if yzhigh & 0x80:
-                    self.last_error_count += 1
-                    continue
-                rx = (xlow | ((xzhigh & 0x1f) << 8)) - ((xzhigh & 0x10) << 9)
-                ry = (ylow | ((yzhigh & 0x1f) << 8)) - ((yzhigh & 0x10) << 9)
-                rz = ((zlow | ((xzhigh & 0xe0) << 3) | ((yzhigh & 0xe0) << 6))
-                      - ((yzhigh & 0x40) << 7))
-                raw_xyz = (rx, ry, rz)
-                x = round(raw_xyz[x_pos] * x_scale, 6)
-                y = round(raw_xyz[y_pos] * y_scale, 6)
-                z = round(raw_xyz[z_pos] * z_scale, 6)
-                ptime = round(time_base + (msg_cdiff + i) * inv_freq, 6)
-                samples[count] = (ptime, x, y, z)
-                count += 1
-        self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
+        count = 0
+        for ptime, xlow, ylow, zlow, xzhigh, yzhigh in samples:
+            if yzhigh & 0x80:
+                self.last_error_count += 1
+                continue
+            rx = (xlow | ((xzhigh & 0x1f) << 8)) - ((xzhigh & 0x10) << 9)
+            ry = (ylow | ((yzhigh & 0x1f) << 8)) - ((yzhigh & 0x10) << 9)
+            rz = ((zlow | ((xzhigh & 0xe0) << 3) | ((yzhigh & 0xe0) << 6))
+                  - ((yzhigh & 0x40) << 7))
+            raw_xyz = (rx, ry, rz)
+            x = round(raw_xyz[x_pos] * x_scale, 6)
+            y = round(raw_xyz[y_pos] * y_scale, 6)
+            z = round(raw_xyz[z_pos] * z_scale, 6)
+            samples[count] = (round(ptime, 6), x, y, z)
+            count += 1
         del samples[count:]
         return samples
     # Start, stop, and process message batches
diff --git a/klippy/extras/bulk_sensor.py b/klippy/extras/bulk_sensor.py
index ad486bc4b..cb4c17ac6 100644
--- a/klippy/extras/bulk_sensor.py
+++ b/klippy/extras/bulk_sensor.py
@@ -3,7 +3,7 @@
 # Copyright (C) 2020-2023  Kevin O'Connor <kevin@koconnor.net>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
-import logging, threading
+import logging, threading, struct
 
 # This "bulk sensor" module facilitates the processing of sensor chip
 # measurements that do not require the host to respond with low
@@ -255,3 +255,28 @@ def update_clock(self, is_reset=False):
             self.clock_sync.reset(avg_mcu_clock, chip_clock)
         else:
             self.clock_sync.update(avg_mcu_clock, chip_clock)
+    # Convert a list of sensor_bulk_data responses into list of samples
+    def extract_samples(self, unpack_fmt, raw_samples):
+        unpack_from = struct.Struct(unpack_fmt).unpack_from
+        # Load variables to optimize inner loop below
+        last_sequence = self.get_last_sequence()
+        time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
+        bytes_per_sample = self.bytes_per_sample
+        samples_per_block = self.samples_per_block
+        # Process every message in raw_samples
+        count = seq = 0
+        samples = [None] * (len(raw_samples) * samples_per_block)
+        for params in raw_samples:
+            seq_diff = (params['sequence'] - last_sequence) & 0xffff
+            seq_diff -= (seq_diff & 0x8000) << 1
+            seq = last_sequence + seq_diff
+            msg_cdiff = seq * samples_per_block - chip_base
+            data = params['data']
+            for i in range(len(data) // bytes_per_sample):
+                ptime = time_base + (msg_cdiff + i) * inv_freq
+                udata = unpack_from(data, i * bytes_per_sample)
+                samples[count] = (ptime,) + udata
+                count += 1
+        self.clock_sync.set_last_chip_clock(seq * samples_per_block + i)
+        del samples[count:]
+        return samples