Mirror
/
klipper-dgus
mirror of https://github.com/Desuuuu/klipper.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

adxl345: Implement timing via new adxl345_status messages 

Query the adxl345 message counter every 100ms so that accurate timing
can be obtained during measurements.  This allows the adxl345 data to
be exported with timestamps while captures are running.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2021-08-14 13:41:12 -04:00
parent dd95f80d9d
commit e34137582d
3 changed files with 168 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

203
klippy/extras/adxl345.py
View File

@ -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)

2
klippy/extras/resonance_tester.py
View File

@ -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:

62
src/sensor_adxl345.c
View File

@ -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)
{