From 22167f951022935d0f4eee1f02c3f2635861576e Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sun, 22 Aug 2021 16:24:16 -0400 Subject: [PATCH] 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 --- klippy/extras/adxl345.py | 44 ++++++++++++++++++++++++++-------------- src/sensor_adxl345.c | 30 ++++++++++++++++++++++----- 2 files changed, 54 insertions(+), 20 deletions(-) diff --git a/klippy/extras/adxl345.py b/klippy/extras/adxl345.py index 7e21866d..9d2f8945 100644 --- a/klippy/extras/adxl345.py +++ b/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) - sdata = [(d[i] | ((d[i+1] & 0x1f) << 8)) - ((d[i+1] & 0x10) << 9) - for i in range(0, len_d-1, 2)] - msg_cdiff = seq * 8 - chip_base - for i in range(len_d // 6): - x = round(sdata[i*3 + x_pos] * x_scale, 6) - y = round(sdata[i*3 + y_pos] * y_scale, 6) - z = round(sdata[i*3 + z_pos] * z_scale, 6) + 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 * 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() diff --git a/src/sensor_adxl345.c b/src/sensor_adxl345.c index 309b3ff1..3d80059d 100644 --- a/src/sensor_adxl345.c +++ b/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); - ax->data_count += 6; - if (ax->data_count + 6 > ARRAY_SIZE(ax->data)) - adxl_report(ax, oid); + // Extract x, y, z measurements 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) {