mirror of https://github.com/Desuuuu/klipper.git
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>
This commit is contained in:
Kevin O'Connor
parent
e34137582d
commit
22167f9510
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue