diff --git a/klippy/extras/adxl345.py b/klippy/extras/adxl345.py
index 1c2838620..7e21866d2 100644
--- a/klippy/extras/adxl345.py
+++ b/klippy/extras/adxl345.py
@@ -30,53 +30,26 @@ Accel_Measurement = collections.namedtuple(
# Helper class to obtain measurements
class ADXL345QueryHelper:
- def __init__(self, printer, chip, cconn):
+ def __init__(self, printer, cconn):
self.printer = printer
- self.chip = chip
self.cconn = cconn
print_time = printer.lookup_object('toolhead').get_last_move_time()
self.request_start_time = self.request_end_time = print_time
- self.raw_samples = None
self.samples = []
- self.drops = self.overflows = 0
- self.start2_time = 0.
- self.time_per_sample = self.start_range = self.end_range = 0.
def finish_measurements(self):
toolhead = self.printer.lookup_object('toolhead')
self.request_end_time = toolhead.get_last_move_time()
toolhead.wait_moves()
self.cconn.finalize()
- toolhead.dwell(0.200)
- toolhead.wait_moves()
- self._setup_data(*self.chip.final_results) # XXX
- def get_stats(self):
- return ("drops=%d,overflows=%d"
- ",time_per_sample=%.9f,start_range=%.6f,end_range=%.6f"
- % (self.drops, self.overflows,
- self.time_per_sample, self.start_range, self.end_range))
- def _setup_data(self, end_sequence, overflows,
- start1_time, start2_time, end1_time, end2_time):
- raw_samples = self.cconn.get_messages()
- if not raw_samples or not end_sequence:
- return
- self.raw_samples = raw_samples
- self.overflows = overflows
- self.start2_time = start2_time
- self.start_range = start2_time - start1_time
- self.end_range = end2_time - end1_time
- self.total_count = raw_samples[-1]['params']['data'][-1][0] + 1
- total_time = end2_time - start2_time
- self.time_per_sample = total_time / self.total_count
- actual_count = sum([len(m['params']['data']) for m in raw_samples])
- self.drops = self.total_count - actual_count
def decode_samples(self):
- if not self.raw_samples:
+ raw_samples = self.cconn.get_messages()
+ if not raw_samples:
return self.samples
+ total = sum([len(m['params']['data']) for m in raw_samples])
count = 0
- self.samples = samples = [None] * self.total_count
- for msg in self.raw_samples:
- for seq, x, y, z in msg['params']['data']:
- samp_time = self.start2_time + seq * self.time_per_sample
+ self.samples = samples = [None] * total
+ for msg in raw_samples:
+ for samp_time, x, y, z in msg['params']['data']:
if samp_time < self.request_start_time:
continue
if samp_time > self.request_end_time:
@@ -93,8 +66,7 @@ class ADXL345QueryHelper:
except:
pass
f = open(filename, "w")
- f.write("##%s\n#time,accel_x,accel_y,accel_z\n" % (
- self.get_stats(),))
+ f.write("#time,accel_x,accel_y,accel_z\n")
samples = self.samples or self.decode_samples()
for t, accel_x, accel_y, accel_z in samples:
f.write("%.6f,%.6f,%.6f,%.6f\n" % (
@@ -183,6 +155,54 @@ class ADXLCommandHelper:
val = gcmd.get("VAL", minval=0, maxval=255, parser=lambda x: int(x, 0))
self.chip.set_reg(reg, val)
+# Helper class for chip clock synchronization via linear regression
+class ClockSyncRegression:
+ def __init__(self, mcu, chip_clock_smooth, decay = 1. / 20.):
+ self.mcu = mcu
+ self.chip_clock_smooth = chip_clock_smooth
+ self.decay = decay
+ self.last_chip_clock = self.last_exp_mcu_clock = 0.
+ self.mcu_clock_avg = self.mcu_clock_variance = 0.
+ self.chip_clock_avg = self.chip_clock_covariance = 0.
+ def reset(self, mcu_clock, chip_clock):
+ self.mcu_clock_avg = self.last_mcu_clock = mcu_clock
+ self.chip_clock_avg = chip_clock
+ self.mcu_clock_variance = self.chip_clock_covariance = 0.
+ self.last_chip_clock = self.last_exp_mcu_clock = 0.
+ def update(self, mcu_clock, chip_clock):
+ # Update linear regression
+ decay = self.decay
+ diff_mcu_clock = mcu_clock - self.mcu_clock_avg
+ self.mcu_clock_avg += decay * diff_mcu_clock
+ self.mcu_clock_variance = (1. - decay) * (
+ self.mcu_clock_variance + diff_mcu_clock**2 * decay)
+ diff_chip_clock = chip_clock - self.chip_clock_avg
+ self.chip_clock_avg += decay * diff_chip_clock
+ self.chip_clock_covariance = (1. - decay) * (
+ self.chip_clock_covariance + diff_mcu_clock*diff_chip_clock*decay)
+ def set_last_chip_clock(self, chip_clock):
+ base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
+ self.last_chip_clock = chip_clock
+ self.last_exp_mcu_clock = base_mcu + (chip_clock-base_chip) * inv_cfreq
+ def get_clock_translation(self):
+ inv_chip_freq = self.mcu_clock_variance / self.chip_clock_covariance
+ if not self.last_chip_clock:
+ return self.mcu_clock_avg, self.chip_clock_avg, inv_chip_freq
+ # Find mcu clock associated with future chip_clock
+ s_chip_clock = self.last_chip_clock + self.chip_clock_smooth
+ scdiff = s_chip_clock - self.chip_clock_avg
+ s_mcu_clock = self.mcu_clock_avg + scdiff * inv_chip_freq
+ # Calculate frequency to converge at future point
+ mdiff = s_mcu_clock - self.last_exp_mcu_clock
+ s_inv_chip_freq = mdiff / self.chip_clock_smooth
+ return self.last_exp_mcu_clock, self.last_chip_clock, s_inv_chip_freq
+ def get_time_translation(self):
+ base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
+ clock_to_print_time = self.mcu.clock_to_print_time
+ 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
+
MIN_MSG_TIME = 0.100
# Printer class that controls ADXL345 chip
@@ -191,7 +211,6 @@ class ADXL345:
self.printer = config.get_printer()
ADXLCommandHelper(config, self)
self.query_rate = 0
- self.last_sequence = 0
am = {'x': (0, SCALE), 'y': (1, SCALE), 'z': (2, SCALE),
'-x': (0, -SCALE), '-y': (1, -SCALE), '-z': (2, -SCALE)}
axes_map = config.getlist('axes_map', ('x','y','z'), count=3)
@@ -204,19 +223,21 @@ class ADXL345:
# Measurement storage (accessed from background thread)
self.lock = threading.Lock()
self.raw_samples = []
- self.samples_start1 = self.samples_start2 = 0.
# Setup mcu sensor_adxl345 bulk query code
self.spi = bus.MCU_SPI_from_config(config, 3, default_speed=5000000)
self.mcu = mcu = self.spi.get_mcu()
self.oid = oid = mcu.create_oid()
- self.query_adxl345_cmd = self.query_adxl345_end_cmd =None
+ self.query_adxl345_cmd = self.query_adxl345_end_cmd = None
+ self.query_adxl345_status_cmd = None
mcu.add_config_cmd("config_adxl345 oid=%d spi_oid=%d"
% (oid, self.spi.get_oid()))
mcu.add_config_cmd("query_adxl345 oid=%d clock=0 rest_ticks=0"
% (oid,), on_restart=True)
mcu.register_config_callback(self._build_config)
- mcu.register_response(self._handle_adxl345_start, "adxl345_start", oid)
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.clock_sync = ClockSyncRegression(self.mcu, 640)
# API server endpoints
self.api_dump = motion_report.APIDumpHelper(
self.printer, self._api_update, self._api_startstop, 0.100)
@@ -227,21 +248,17 @@ class ADXL345:
wh.register_mux_endpoint("adxl345/dump_adxl345", "sensor", self.name,
self._handle_dump_adxl345)
def _build_config(self):
+ cmdqueue = self.spi.get_command_queue()
self.query_adxl345_cmd = self.mcu.lookup_command(
- "query_adxl345 oid=%c clock=%u rest_ticks=%u",
- cq=self.spi.get_command_queue())
+ "query_adxl345 oid=%c clock=%u rest_ticks=%u", cq=cmdqueue)
self.query_adxl345_end_cmd = self.mcu.lookup_query_command(
"query_adxl345 oid=%c clock=%u rest_ticks=%u",
- "adxl345_end oid=%c end1_clock=%u end2_clock=%u"
- " limit_count=%hu sequence=%hu",
- oid=self.oid, cq=self.spi.get_command_queue())
- def _clock_to_print_time(self, clock):
- return self.mcu.clock_to_print_time(self.mcu.clock32_to_clock64(clock))
- def _convert_sequence(self, sequence):
- sequence = (self.last_sequence & ~0xffff) | sequence
- if sequence < self.last_sequence:
- sequence += 0x10000
- return sequence
+ "adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
+ " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
+ self.query_adxl345_status_cmd = self.mcu.lookup_query_command(
+ "query_adxl345_status oid=%c",
+ "adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
+ " buffered=%c fifo=%c limit_count=%hu", oid=self.oid, cq=cmdqueue)
def read_reg(self, reg):
params = self.spi.spi_transfer([reg | REG_MOD_READ, 0x00])
response = bytearray(params['response'])
@@ -258,9 +275,6 @@ class ADXL345:
# Measurement collection
def is_measuring(self):
return self.query_rate > 0
- def _handle_adxl345_start(self, params):
- self.samples_start1 = self._clock_to_print_time(params['start1_clock'])
- self.samples_start2 = self._clock_to_print_time(params['start2_clock'])
def _handle_adxl345_data(self, params):
with self.lock:
self.raw_samples.append(params)
@@ -268,27 +282,62 @@ class ADXL345:
# Load variables to optimize inner loop below
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
last_sequence = self.last_sequence
+ time_base, chip_base, inv_freq = self.clock_sync.get_time_translation()
# Process every message in raw_samples
- count = 0
+ count = seq = 0
samples = [None] * (len(raw_samples) * 8)
for params in raw_samples:
- seq = (last_sequence & ~0xffff) | params['sequence']
- if seq < last_sequence:
- seq += 0x10000
- last_sequence = seq
+ 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] << 8)) - ((d[i+1] & 0x80) << 9)
+ 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)
- samples[count] = (seq * 8 + i, x, y, z)
+ ptime = round(time_base + (msg_cdiff + i) * inv_freq, 6)
+ samples[count] = (ptime, x, y, z)
count += 1
- self.last_sequence = last_sequence
+ self.clock_sync.set_last_chip_clock(seq * 8 + i)
del samples[count:]
return samples
+ def _update_clock(self, minclock=0):
+ # Query current state
+ for retry in range(5):
+ params = self.query_adxl345_status_cmd.send([self.oid],
+ minclock=minclock)
+ fifo = params['fifo'] & 0x7f
+ if fifo <= 32:
+ break
+ else:
+ raise self.printer.command_error("Unable to query adxl345 fifo")
+ mcu_clock = self.mcu.clock32_to_clock64(params['clock'])
+ sequence = (self.last_sequence & ~0xffff) | params['next_sequence']
+ if sequence < self.last_sequence:
+ sequence += 0x10000
+ self.last_sequence = sequence
+ buffered = params['buffered']
+ limit_count = (self.last_limit_count & ~0xffff) | params['limit_count']
+ if limit_count < self.last_limit_count:
+ limit_count += 0x10000
+ self.last_limit_count = limit_count
+ 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 = sequence * 8 + buffered // 6 + 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.
+ chip_clock = msg_count + 1
+ self.clock_sync.update(mcu_clock + duration // 2, chip_clock)
def _start_measurements(self):
if self.is_measuring():
return
@@ -305,19 +354,24 @@ class ADXL345:
self.set_reg(REG_BW_RATE, QUERY_RATES[self.data_rate])
self.set_reg(REG_FIFO_CTL, 0x80)
# Setup samples
- systime = self.printer.get_reactor().monotonic()
- print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
- self.samples_start1 = self.samples_start2 = print_time
- self.last_sequence = 0
with self.lock:
self.raw_samples = []
# Start bulk reading
+ systime = self.printer.get_reactor().monotonic()
+ print_time = self.mcu.estimated_print_time(systime) + MIN_MSG_TIME
reqclock = self.mcu.print_time_to_clock(print_time)
rest_ticks = self.mcu.seconds_to_clock(4. / self.data_rate)
self.query_rate = self.data_rate
self.query_adxl345_cmd.send([self.oid, reqclock, rest_ticks],
reqclock=reqclock)
logging.info("ADXL345 starting '%s' measurements", self.name)
+ # Initialize clock tracking
+ self.last_sequence = 0
+ self.last_limit_count = 0
+ self.clock_sync.reset(reqclock, 0)
+ self.max_query_duration = 1 << 31
+ self._update_clock(minclock=reqclock)
+ self.max_query_duration = 1 << 31
def _finish_measurements(self):
if not self.is_measuring():
return
@@ -326,17 +380,10 @@ class ADXL345:
self.query_rate = 0
with self.lock:
self.raw_samples = []
- # Generate results
- end1_time = self._clock_to_print_time(params['end1_clock'])
- end2_time = self._clock_to_print_time(params['end2_clock'])
- end_sequence = self._convert_sequence(params['sequence'])
- overflows = params['limit_count']
logging.info("ADXL345 finished '%s' measurements", self.name)
- self.final_results = (end_sequence, overflows,
- self.samples_start1, self.samples_start2,
- end1_time, end2_time) # XXX
# API interface
def _api_update(self, eventtime):
+ self._update_clock()
with self.lock:
raw_samples = self.raw_samples
self.raw_samples = []
@@ -345,7 +392,7 @@ class ADXL345:
samples = self._extract_samples(raw_samples)
if not samples:
return {}
- return {'data': samples}
+ return {'data': samples, 'overflows': self.last_limit_count}
def _api_startstop(self, is_start):
if is_start:
self._start_measurements()
@@ -353,14 +400,14 @@ class ADXL345:
self._finish_measurements()
def _handle_dump_adxl345(self, web_request):
self.api_dump.add_client(web_request)
- hdr = ('sequence', 'x_acceleration', 'y_acceleration', 'z_acceleration')
+ hdr = ('time', 'x_acceleration', 'y_acceleration', 'z_acceleration')
web_request.send({'header': hdr})
def start_internal_client(self):
if self.is_measuring():
raise self.printer.command_error(
"ADXL345 measurement already in progress")
cconn = self.api_dump.add_internal_client()
- return ADXL345QueryHelper(self.printer, self, cconn)
+ return ADXL345QueryHelper(self.printer, cconn)
def load_config(config):
return ADXL345(config)
diff --git a/klippy/extras/resonance_tester.py b/klippy/extras/resonance_tester.py
index eaa49db3a..f66e229c1 100644
--- a/klippy/extras/resonance_tester.py
+++ b/klippy/extras/resonance_tester.py
@@ -181,8 +181,6 @@ class ResonanceTester:
gcmd.respond_info(
"Writing raw accelerometer data to "
"%s file" % (raw_name,))
- gcmd.respond_info("%s-axis accelerometer stats: %s" % (
- chip_axis, aclient.get_stats(),))
if helper is None:
continue
for chip_axis, chip_values in raw_values:
diff --git a/src/sensor_adxl345.c b/src/sensor_adxl345.c
index 1e2cb9e5e..309b3ff16 100644
--- a/src/sensor_adxl345.c
+++ b/src/sensor_adxl345.c
@@ -27,6 +27,7 @@ enum {
static struct task_wake adxl345_wake;
+// Event handler that wakes adxl345_task() periodically
static uint_fast8_t
adxl345_event(struct timer *timer)
{
@@ -56,6 +57,27 @@ adxl_report(struct adxl345 *ax, uint8_t oid)
ax->sequence++;
}
+// Report buffer and fifo status
+static void
+adxl_status(struct adxl345 *ax, uint_fast8_t oid
+ , uint32_t time1, uint32_t time2, uint_fast8_t fifo)
+{
+ sendf("adxl345_status oid=%c clock=%u query_ticks=%u next_sequence=%hu"
+ " buffered=%c fifo=%c limit_count=%hu"
+ , oid, time1, time2-time1, ax->sequence
+ , ax->data_count, fifo, ax->limit_count);
+}
+
+// Helper code to reschedule the adxl345_event() timer
+static void
+adxl_reschedule_timer(struct adxl345 *ax)
+{
+ irq_disable();
+ ax->timer.waketime = timer_read_time() + ax->rest_ticks;
+ sched_add_timer(&ax->timer);
+ irq_enable();
+}
+
// Chip registers
#define AR_POWER_CTL 0x2D
#define AR_DATAX0 0x32
@@ -74,7 +96,7 @@ adxl_query(struct adxl345 *ax, uint8_t oid)
if (ax->data_count + 6 > ARRAY_SIZE(ax->data))
adxl_report(ax, oid);
uint_fast8_t fifo_status = msg[8] & ~0x80; // Ignore trigger bit
- if (fifo_status >= 31 && ax->limit_count != 0xffff)
+ if (fifo_status >= 31)
ax->limit_count++;
if (fifo_status > 1 && fifo_status <= 32) {
// More data in fifo - wake this task again
@@ -83,10 +105,7 @@ adxl_query(struct adxl345 *ax, uint8_t oid)
// Sleep until next check time
sched_del_timer(&ax->timer);
ax->flags &= ~AX_PENDING;
- irq_disable();
- ax->timer.waketime = timer_read_time() + ax->rest_ticks;
- sched_add_timer(&ax->timer);
- irq_enable();
+ adxl_reschedule_timer(ax);
}
}
@@ -97,15 +116,8 @@ adxl_start(struct adxl345 *ax, uint8_t oid)
sched_del_timer(&ax->timer);
ax->flags = AX_RUNNING;
uint8_t msg[2] = { AR_POWER_CTL, 0x08 };
- uint32_t start1_time = timer_read_time();
spidev_transfer(ax->spi, 0, sizeof(msg), msg);
- irq_disable();
- uint32_t start2_time = timer_read_time();
- ax->timer.waketime = start2_time + ax->rest_ticks;
- sched_add_timer(&ax->timer);
- irq_enable();
- sendf("adxl345_start oid=%c start1_clock=%u start2_clock=%u"
- , oid, start1_time, start2_time);
+ adxl_reschedule_timer(ax);
}
// End measurements
@@ -123,18 +135,18 @@ adxl_stop(struct adxl345 *ax, uint8_t oid)
uint_fast8_t i;
for (i=0; i<33; i++) {
msg[0] = AR_FIFO_STATUS | AM_READ;
- msg[1] = 0;
+ msg[1] = 0x00;
spidev_transfer(ax->spi, 1, sizeof(msg), msg);
- if (!(msg[1] & 0x3f))
+ uint_fast8_t fifo_status = msg[1] & ~0x80;
+ if (!fifo_status)
break;
- adxl_query(ax, oid);
+ if (fifo_status <= 32)
+ adxl_query(ax, oid);
}
// Report final data
if (ax->data_count)
adxl_report(ax, oid);
- sendf("adxl345_end oid=%c end1_clock=%u end2_clock=%u"
- " limit_count=%hu sequence=%hu"
- , oid, end1_time, end2_time, ax->limit_count, ax->sequence);
+ adxl_status(ax, oid, end1_time, end2_time, msg[1]);
}
void
@@ -159,6 +171,18 @@ command_query_adxl345(uint32_t *args)
DECL_COMMAND(command_query_adxl345,
"query_adxl345 oid=%c clock=%u rest_ticks=%u");
+void
+command_query_adxl345_status(uint32_t *args)
+{
+ struct adxl345 *ax = oid_lookup(args[0], command_config_adxl345);
+ uint8_t msg[2] = { AR_FIFO_STATUS | AM_READ, 0x00 };
+ uint32_t time1 = timer_read_time();
+ spidev_transfer(ax->spi, 1, sizeof(msg), msg);
+ uint32_t time2 = timer_read_time();
+ adxl_status(ax, args[0], time1, time2, msg[1]);
+}
+DECL_COMMAND(command_query_adxl345_status, "query_adxl345_status oid=%c");
+
void
adxl345_task(void)
{