adxl345: Compress each sample from 6 bytes to 5 bytes

Transmit data from mcu to host using 5 bytes per sample and up to 10
samples per message block.  This improves bandwidth efficiency.

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

klippy/extras/adxl345.py

@@ -21,6 +21,7 @@ QUERY_RATES = {
 }
 
 ADXL345_DEV_ID = 0xe5
+SET_FIFO_CTL = 0x90
 
 FREEFALL_ACCEL = 9.80665 * 1000.
 SCALE = 0.0039 * FREEFALL_ACCEL # 3.9mg/LSB * Earth gravity in mm/s**2
@@ -205,6 +206,9 @@ class ClockSyncRegression:
 
 MIN_MSG_TIME = 0.100
 
+BYTES_PER_SAMPLE = 5
+SAMPLES_PER_BLOCK = 10
+
 # Printer class that controls ADXL345 chip
 class ADXL345:
     def __init__(self, config):
@@ -236,7 +240,8 @@ class ADXL345:
         mcu.register_config_callback(self._build_config)
         mcu.register_response(self._handle_adxl345_data, "adxl345_data", oid)
         # Clock tracking
-        self.last_sequence = self.last_limit_count = self.max_query_duration = 0
+        self.last_sequence = self.max_query_duration = 0
+        self.last_limit_count = self.last_error_count = 0
         self.clock_sync = ClockSyncRegression(self.mcu, 640)
         # API server endpoints
         self.api_dump = motion_report.APIDumpHelper(
@@ -285,24 +290,31 @@ class ADXL345:
         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) * 8)
+        samples = [None] * (len(raw_samples) * SAMPLES_PER_BLOCK)
         for params in raw_samples:
             seq_diff = (last_sequence - params['sequence']) & 0xffff
             seq_diff -= (seq_diff & 0x8000) << 1
             seq = last_sequence - seq_diff
             d = bytearray(params['data'])
-            len_d = len(d)
+            msg_cdiff = seq * SAMPLES_PER_BLOCK - chip_base
-            sdata = [(d[i] | ((d[i+1] & 0x1f) << 8)) - ((d[i+1] & 0x10) << 9)
+            for i in range(len(d) // BYTES_PER_SAMPLE):
-                     for i in range(0, len_d-1, 2)]
+                d_xyz = d[i*BYTES_PER_SAMPLE:(i+1)*BYTES_PER_SAMPLE]
-            msg_cdiff = seq * 8 - chip_base
+                xlow, ylow, zlow, xzhigh, yzhigh = d_xyz
-            for i in range(len_d // 6):
+                if yzhigh & 0x80:
-                x = round(sdata[i*3 + x_pos] * x_scale, 6)
+                    self.last_error_count += 1
-                y = round(sdata[i*3 + y_pos] * y_scale, 6)
+                    continue
-                z = round(sdata[i*3 + z_pos] * z_scale, 6)
+                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 * 8 + i)
+        self.clock_sync.set_last_chip_clock(seq * SAMPLES_PER_BLOCK + i)
         del samples[count:]
         return samples
     def _update_clock(self, minclock=0):
@@ -332,7 +344,8 @@ class ADXL345:
                                           self.mcu.seconds_to_clock(.000005))
             return
         self.max_query_duration = 2 * duration
-        msg_count = sequence * 8 + buffered // 6 + fifo
+        msg_count = (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
         # of adxl345 hw processing time.
@@ -352,7 +365,7 @@ class ADXL345:
         self.set_reg(REG_DATA_FORMAT, 0x0B)
         self.set_reg(REG_FIFO_CTL, 0x00)
         self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate])
-        self.set_reg(REG_FIFO_CTL, 0x80)
+        self.set_reg(REG_FIFO_CTL, SET_FIFO_CTL)
         # Setup samples
         with self.lock:
             self.raw_samples = []
@@ -367,7 +380,7 @@ class ADXL345:
         logging.info("ADXL345 starting '%s' measurements", self.name)
         # Initialize clock tracking
         self.last_sequence = 0
-        self.last_limit_count = 0
+        self.last_limit_count = self.last_error_count = 0
         self.clock_sync.reset(reqclock, 0)
         self.max_query_duration = 1 << 31
         self._update_clock(minclock=reqclock)
@@ -392,7 +405,8 @@ class ADXL345:
         samples = self._extract_samples(raw_samples)
         if not samples:
             return {}
-        return {'data': samples, 'overflows': self.last_limit_count}
+        return {'data': samples, 'errors': self.last_error_count,
+                'overflows': self.last_limit_count}
     def _api_startstop(self, is_start):
         if is_start:
             self._start_measurements()

src/sensor_adxl345.c

@@ -18,7 +18,7 @@ struct adxl345 {
     struct spidev_s *spi;
     uint16_t sequence, limit_count;
     uint8_t flags, data_count;
-    uint8_t data[48];
+    uint8_t data[50];
 };
 
 enum {
@@ -85,17 +85,37 @@ adxl_reschedule_timer(struct adxl345 *ax)
 #define AM_READ  0x80
 #define AM_MULTI 0x40
 
+#define SET_FIFO_CTL 0x90
+
 // Query accelerometer data
 static void
 adxl_query(struct adxl345 *ax, uint8_t oid)
 {
+    // Read data
     uint8_t msg[9] = { AR_DATAX0 | AM_READ | AM_MULTI, 0, 0, 0, 0, 0, 0, 0, 0 };
     spidev_transfer(ax->spi, 1, sizeof(msg), msg);
-    memcpy(&ax->data[ax->data_count], &msg[1], 6);
+    // Extract x, y, z measurements
-    ax->data_count += 6;
-    if (ax->data_count + 6 > ARRAY_SIZE(ax->data))
-        adxl_report(ax, oid);
     uint_fast8_t fifo_status = msg[8] & ~0x80; // Ignore trigger bit
+    uint8_t *d = &ax->data[ax->data_count];
+    if (((msg[2] & 0xf0) && (msg[2] & 0xf0) != 0xf0)
+        || ((msg[4] & 0xf0) && (msg[4] & 0xf0) != 0xf0)
+        || ((msg[6] & 0xf0) && (msg[6] & 0xf0) != 0xf0)
+        || (msg[7] != SET_FIFO_CTL) || (fifo_status > 32)) {
+        // Data error - may be a CS, MISO, MOSI, or SCLK glitch
+        d[0] = d[1] = d[2] = d[3] = d[4] = 0xff;
+        fifo_status = 0;
+    } else {
+        // Copy data
+        d[0] = msg[1]; // x low bits
+        d[1] = msg[3]; // y low bits
+        d[2] = msg[5]; // z low bits
+        d[3] = (msg[2] & 0x1f) | (msg[6] << 5); // x high bits and z high bits
+        d[4] = (msg[4] & 0x1f) | ((msg[6] << 2) & 0x60); // y high and z high
+    }
+    ax->data_count += 5;
+    if (ax->data_count + 5 > ARRAY_SIZE(ax->data))
+        adxl_report(ax, oid);
+    // Check fifo status
     if (fifo_status >= 31)
         ax->limit_count++;
     if (fifo_status > 1 && fifo_status <= 32) {