shinys000114/klipper
1
0
Fork 0
mirror of git://soft.sys114.com/klipper synced 2026-02-10 22:58:07 +09:00
Code Issues Packages Projects Releases Wiki Activity

resonance_tester: Resonance testing and input shaper auto-calibration (#3381)

Browse Source 
Signed-off-by: Dmitry Butyugin <dmbutyugin@google.com>
This commit is contained in:
Dmitry Butyugin
2020-10-15 02:08:10 +02:00
committed by GitHub
parent fac4e53e86
commit f8c4f90c04
15 changed files with 1583 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
klippy/extras/adxl345.py
View File

@@ -3,7 +3,7 @@
# Copyright (C) 2020 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging, time, collections
import logging, time, collections, multiprocessing, os
from . import bus
# ADXL345 registers
@@ -28,11 +28,10 @@ Accel_Measurement = collections.namedtuple(
# Sample results
class ADXL345Results:
def __init__(self):
self.raw_samples = None
self.samples = []
self.drops = self.overflows = 0
self.time_per_sample = self.start_range = self.end_range = 0.
def get_samples(self):
return self.samples
def get_stats(self):
return ("drops=%d,overflows=%d"
",time_per_sample=%.9f,start_range=%.6f,end_range=%.6f"
@@ -42,31 +41,57 @@ class ADXL345Results:
start1_time, start2_time, end1_time, end2_time):
if not raw_samples or not end_sequence:
return
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = axes_map
self.axes_map = axes_map
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
total_count = (end_sequence - 1) * 8 + len(raw_samples[-1][1]) // 6
self.total_count = (end_sequence - 1) * 8 + len(raw_samples[-1][1]) // 6
total_time = end2_time - start2_time
self.time_per_sample = time_per_sample = total_time / total_count
seq_to_time = time_per_sample * 8.
self.samples = samples = [None] * total_count
self.time_per_sample = time_per_sample = total_time / self.total_count
self.seq_to_time = time_per_sample * 8.
actual_count = sum([len(data)//6 for _, data in raw_samples])
self.drops = self.total_count - actual_count
def decode_samples(self):
if not self.raw_samples:
return self.samples
(x_pos, x_scale), (y_pos, y_scale), (z_pos, z_scale) = self.axes_map
actual_count = 0
for seq, data in raw_samples:
self.samples = samples = [None] * self.total_count
for seq, data in self.raw_samples:
d = bytearray(data)
count = len(data)
sdata = [(d[i] | (d[i+1] << 8)) - ((d[i+1] & 0x80) << 9)
for i in range(0, count-1, 2)]
seq_time = start2_time + seq * seq_to_time
seq_time = self.start2_time + seq * self.seq_to_time
for i in range(count//6):
samp_time = seq_time + i * time_per_sample
samp_time = seq_time + i * self.time_per_sample
x = sdata[i*3 + x_pos] * x_scale
y = sdata[i*3 + y_pos] * y_scale
z = sdata[i*3 + z_pos] * z_scale
samples[actual_count] = Accel_Measurement(samp_time, x, y, z)
actual_count += 1
del samples[actual_count:]
self.drops = total_count - actual_count
return self.samples
def write_to_file(self, filename):
def write_impl():
try:
# Try to re-nice writing process
os.nice(20)
except:
pass
f = open(filename, "w")
f.write("##%s\n#time,accel_x,accel_y,accel_z\n" % (
self.get_stats(),))
samples = self.samples or self.decode_samples()
for t, accel_x, accel_y, accel_z in samples:
f.write("%.6f,%.6f,%.6f,%.6f\n" % (
t, accel_x, accel_y, accel_z))
f.close()
write_proc = multiprocessing.Process(target=write_impl)
write_proc.daemon = True
write_proc.start()
# Printer class that controls measurments
class ADXL345:
@@ -80,6 +105,9 @@ class ADXL345:
if len(axes_map) != 3 or any([a.strip() not in am for a in axes_map]):
raise config.error("Invalid adxl345 axes_map parameter")
self.axes_map = [am[a.strip()] for a in axes_map]
self.data_rate = config.getint('rate', 3200)
if self.data_rate not in QUERY_RATES:
raise config.error("Invalid rate parameter: %d" % (self.data_rate,))
# Measurement storage (accessed from background thread)
self.raw_samples = []
self.last_sequence = 0
@@ -104,9 +132,14 @@ class ADXL345:
gcode.register_mux_command("ACCELEROMETER_MEASURE", "CHIP", name,
self.cmd_ACCELEROMETER_MEASURE,
desc=self.cmd_ACCELEROMETER_MEASURE_help)
gcode.register_mux_command("ACCELEROMETER_QUERY", "CHIP", name,
self.cmd_ACCELEROMETER_QUERY,
desc=self.cmd_ACCELEROMETER_QUERY_help)
if name == "default":
gcode.register_mux_command("ACCELEROMETER_MEASURE", "CHIP", None,
self.cmd_ACCELEROMETER_MEASURE)
gcode.register_mux_command("ACCELEROMETER_QUERY", "CHIP", None,
self.cmd_ACCELEROMETER_QUERY)
def _build_config(self):
self.query_adxl345_cmd = self.mcu.lookup_command(
"query_adxl345 oid=%c clock=%u rest_ticks=%u",
@@ -128,7 +161,7 @@ class ADXL345:
sequence += 0x10000
self.last_sequence = sequence
raw_samples = self.raw_samples
if len(raw_samples) >= 200000:
if len(raw_samples) >= 300000:
# Avoid filling up memory with too many samples
return
raw_samples.append((sequence, params['data']))
@@ -138,8 +171,7 @@ class ADXL345:
sequence += 0x10000
return sequence
def start_measurements(self, rate=None):
if rate is None:
rate = 3200
rate = rate or self.data_rate
# Verify chip connectivity
params = self.spi.spi_transfer([REG_DEVID | REG_MOD_READ, 0x00])
response = bytearray(params['response'])
@@ -190,33 +222,47 @@ class ADXL345:
self.samples_start1, self.samples_start2,
end1_time, end2_time)
logging.info("ADXL345 finished %d measurements: %s",
len(res.get_samples()), res.get_stats())
res.total_count, res.get_stats())
return res
def end_query(self, name):
if not self.query_rate:
return
res = self.finish_measurements()
# Write data to file
f = open("/tmp/adxl345-%s.csv" % (name,), "w")
f.write("##%s\n#time,accel_x,accel_y,accel_z\n" % (res.get_stats(),))
for t, accel_x, accel_y, accel_z in res.get_samples():
f.write("%.6f,%.6f,%.6f,%.6f\n" % (t, accel_x, accel_y, accel_z))
f.close()
filename = "/tmp/adxl345-%s.csv" % (name,)
res.write_to_file(filename)
cmd_ACCELEROMETER_MEASURE_help = "Start/stop accelerometer"
def cmd_ACCELEROMETER_MEASURE(self, gcmd):
rate = gcmd.get_int("RATE", 0)
if not rate:
if self.query_rate:
name = gcmd.get("NAME", time.strftime("%Y%m%d_%H%M%S"))
if not name.replace('-', '').replace('_', '').isalnum():
raise gcmd.error("Invalid adxl345 NAME parameter")
self.end_query(name)
gcmd.respond_info("adxl345 measurements stopped")
elif self.query_rate:
raise gcmd.error("adxl345 already running")
elif rate not in QUERY_RATES:
raise gcmd.error("Not a valid adxl345 query rate")
else:
rate = gcmd.get_int("RATE", self.data_rate)
if rate not in QUERY_RATES:
raise gcmd.error("Not a valid adxl345 query rate: %d" % (rate,))
self.start_measurements(rate)
gcmd.respond_info("adxl345 measurements started")
cmd_ACCELEROMETER_QUERY_help = "Query accelerometer for the current values"
def cmd_ACCELEROMETER_QUERY(self, gcmd):
if self.query_rate:
raise gcmd.error("adxl345 measurements in progress")
self.start_measurements()
reactor = self.printer.get_reactor()
eventtime = starttime = reactor.monotonic()
while not self.raw_samples:
eventtime = reactor.pause(eventtime + .1)
if eventtime > starttime + 3.:
# Try to shutdown the measurements
self.finish_measurements()
raise gcmd.error("Timeout reading adxl345 data")
result = self.finish_measurements()
values = result.decode_samples()
_, accel_x, accel_y, accel_z = values[-1]
gcmd.respond_info("adxl345 values (x, y, z): %.6f, %.6f, %.6f" % (
accel_x, accel_y, accel_z))
def load_config(config):
return ADXL345(config)

32
klippy/extras/input_shaper.py
View File

@@ -25,14 +25,12 @@ class InputShaper:
, 'ei': ffi_lib.INPUT_SHAPER_EI
, '2hump_ei': ffi_lib.INPUT_SHAPER_2HUMP_EI
, '3hump_ei': ffi_lib.INPUT_SHAPER_3HUMP_EI}
shaper_type = config.getchoice('shaper_type', self.shapers, None)
if shaper_type is None:
self.shaper_type_x = config.getchoice(
'shaper_type_x', self.shapers, 'mzv')
self.shaper_type_y = config.getchoice(
'shaper_type_y', self.shapers, 'mzv')
else:
self.shaper_type_x = self.shaper_type_y = shaper_type
shaper_type = config.get('shaper_type', 'mzv')
self.shaper_type_x = config.getchoice(
'shaper_type_x', self.shapers, shaper_type)
self.shaper_type_y = config.getchoice(
'shaper_type_y', self.shapers, shaper_type)
self.saved_shaper_freq_x = self.saved_shaper_freq_y = 0.
self.stepper_kinematics = []
self.orig_stepper_kinematics = []
# Register gcode commands
@@ -86,6 +84,24 @@ class InputShaper:
, shaper_type_x, shaper_type_y
, shaper_freq_x, shaper_freq_y
, damping_ratio_x, damping_ratio_y)
def disable_shaping(self):
if (self.saved_shaper_freq_x or self.saved_shaper_freq_y) and not (
self.shaper_freq_x or self.shaper_freq_y):
# Input shaper is already disabled
return
self.saved_shaper_freq_x = self.shaper_freq_x
self.saved_shaper_freq_y = self.shaper_freq_y
self._set_input_shaper(self.shaper_type_x, self.shaper_type_y, 0., 0.,
self.damping_ratio_x, self.damping_ratio_y)
def enable_shaping(self):
saved = self.saved_shaper_freq_x or self.saved_shaper_freq_y
if saved:
self._set_input_shaper(self.shaper_type_x, self.shaper_type_y,
self.saved_shaper_freq_x,
self.saved_shaper_freq_y,
self.damping_ratio_x, self.damping_ratio_y)
self.saved_shaper_freq_x = self.saved_shaper_freq_y = 0.
return saved
cmd_SET_INPUT_SHAPER_help = "Set cartesian parameters for input shaper"
def cmd_SET_INPUT_SHAPER(self, gcmd):
damping_ratio_x = gcmd.get_float(

303
klippy/extras/resonance_tester.py Normal file
View File

@@ -0,0 +1,303 @@
# A utility class to test resonances of the printer
#
# Copyright (C) 2020 Dmitry Butyugin <dmbutyugin@google.com>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging, math, os, time
from . import shaper_calibrate
def _parse_probe_points(config):
points = config.get('probe_points').split('\n')
try:
points = [line.split(',', 2) for line in points if line.strip()]
return [[float(coord.strip()) for coord in p] for p in points]
except:
raise config.error("Unable to parse probe_points in %s" % (
config.get_name()))
class VibrationPulseTest:
def __init__(self, config):
printer = config.get_printer()
self.gcode = printer.lookup_object('gcode')
self.min_freq = config.getfloat('min_freq', 5., minval=1.)
self.max_freq = config.getfloat('max_freq', 120.,
minval=self.min_freq, maxval=200.)
self.accel_per_hz = config.getfloat('accel_per_hz', 75.0, above=0.)
self.hz_per_sec = config.getfloat('hz_per_sec', 1.,
minval=0.1, maxval=2.)
self.probe_points = _parse_probe_points(config)
def get_supported_axes(self):
return ['x', 'y']
def get_start_test_points(self):
return self.probe_points
def prepare_test(self, toolhead, gcmd):
self.freq_start = gcmd.get_float("FREQ_START", self.min_freq, minval=1.)
self.freq_end = gcmd.get_float("FREQ_END", self.max_freq,
minval=self.freq_start, maxval=200.)
self.hz_per_sec = gcmd.get_float("HZ_PER_SEC", self.hz_per_sec,
above=0., maxval=2.)
# Attempt to adjust maximum acceleration and acceleration to
# deceleration based on the maximum test frequency.
max_accel = self.freq_end * self.accel_per_hz
toolhead.cmd_SET_VELOCITY_LIMIT(self.gcode.create_gcode_command(
"SET_VELOCITY_LIMIT", "SET_VELOCITY_LIMIT",
{"ACCEL": max_accel, "ACCEL_TO_DECEL": max_accel}))
def run_test(self, toolhead, axis, gcmd):
X, Y, Z, E = toolhead.get_position()
if axis not in self.get_supported_axes():
raise gcmd.error("Test axis '%s' is not supported", axis)
vib_dir = (1, 0) if axis == 'x' else (0., 1.)
sign = 1.
freq = self.freq_start
gcmd.respond_info("Testing frequency %.0f Hz" % (freq,))
_, max_accel = toolhead.get_max_velocity()
while freq <= self.freq_end + 0.000001:
t_seg = .25 / freq
accel = min(self.accel_per_hz * freq, max_accel)
V = accel * t_seg
toolhead.cmd_M204(self.gcode.create_gcode_command(
"M204", "M204", {"S": accel}))
L = .5 * accel * t_seg**2
nX = X + sign * vib_dir[0] * L
nY = Y + sign * vib_dir[1] * L
toolhead.move([nX, nY, Z, E], V)
toolhead.move([X, Y, Z, E], V)
sign = -sign
old_freq = freq
freq += 2. * t_seg * self.hz_per_sec
if math.floor(freq) > math.floor(old_freq):
gcmd.respond_info("Testing frequency %.0f Hz" % (freq,))
class ResonanceTester:
def __init__(self, config):
self.printer = config.get_printer()
self.move_speed = config.getfloat('move_speed', 50., above=0.)
self.test = VibrationPulseTest(config)
if not config.get('accel_chip_x', None):
self.accel_chip_names = [('xy', config.get('accel_chip').strip())]
else:
self.accel_chip_names = [
('x', config.get('accel_chip_x').strip()),
('y', config.get('accel_chip_y').strip())]
if self.accel_chip_names[0][1] == self.accel_chip_names[1][1]:
self.accel_chip_names = [('xy', self.accel_chip_names[0][1])]
self.gcode = self.printer.lookup_object('gcode')
self.gcode.register_command("MEASURE_AXES_NOISE",
self.cmd_MEASURE_AXES_NOISE)
self.gcode.register_command("TEST_RESONANCES",
self.cmd_TEST_RESONANCES)
self.gcode.register_command("SHAPER_CALIBRATE",
self.cmd_SHAPER_CALIBRATE)
self.printer.register_event_handler("klippy:connect", self.connect)
def connect(self):
self.accel_chips = [
(axis, self.printer.lookup_object(chip_name))
for axis, chip_name in self.accel_chip_names]
def cmd_TEST_RESONANCES(self, gcmd):
toolhead = self.printer.lookup_object('toolhead')
# Parse parameters
self.test.prepare_test(toolhead, gcmd)
if len(self.test.get_supported_axes()) > 1:
axis = gcmd.get("AXIS").lower()
else:
axis = gcmd.get("AXIS", self.test.get_supported_axes()[0]).lower()
if axis not in self.test.get_supported_axes():
raise gcmd.error("Unsupported axis '%s'" % (axis,))
outputs = gcmd.get("OUTPUT", "resonances").lower().split(',')
for output in outputs:
if output not in ['resonances', 'raw_data']:
raise gcmd.error("Unsupported output '%s', only 'resonances'"
" and 'raw_data' are supported" % (output,))
if not outputs:
raise gcmd.error("No output specified, at least one of 'resonances'"
" or 'raw_data' must be set in OUTPUT parameter")
name_suffix = gcmd.get("NAME", time.strftime("%Y%m%d_%H%M%S"))
if not self.is_valid_name_suffix(name_suffix):
raise gcmd.error("Invalid NAME parameter")
csv_output = 'resonances' in outputs
raw_output = 'raw_data' in outputs
# Setup calculation of resonances
if csv_output:
helper = shaper_calibrate.ShaperCalibrate(self.printer)
currentPos = toolhead.get_position()
Z = currentPos[2]
E = currentPos[3]
calibration_points = self.test.get_start_test_points()
data = None
for point in calibration_points:
toolhead.manual_move(point, self.move_speed)
if len(calibration_points) > 1:
gcmd.respond_info(
"Probing point (%.3f, %.3f, %.3f)" % tuple(point))
toolhead.wait_moves()
toolhead.dwell(0.500)
gcmd.respond_info("Testing axis %s" % axis.upper())
for chip_axis, chip in self.accel_chips:
if axis in chip_axis or chip_axis in axis:
chip.start_measurements()
# Generate moves
self.test.run_test(toolhead, axis, gcmd)
raw_values = []
for chip_axis, chip in self.accel_chips:
if axis in chip_axis or chip_axis in axis:
results = chip.finish_measurements()
if raw_output:
raw_name = self.get_filename(
'raw_data', name_suffix, axis,
point if len(calibration_points) > 1 else None)
results.write_to_file(raw_name)
gcmd.respond_info(
"Writing raw accelerometer data to %s file" % (
raw_name,))
raw_values.append((chip_axis, results))
if not csv_output:
continue
for chip_axis, chip_values in raw_values:
gcmd.respond_info("%s-axis accelerometer stats: %s" % (
chip_axis, chip_values.get_stats(),))
if not chip_values:
raise gcmd.error(
"%s-axis accelerometer measured no data" % (
chip_axis,))
new_data = helper.process_accelerometer_data(chip_values)
data = data.join(new_data) if data else new_data
if csv_output:
csv_name = self.save_calibration_data('resonances', name_suffix,
helper, axis, data)
gcmd.respond_info(
"Resonances data written to %s file" % (csv_name,))
def cmd_SHAPER_CALIBRATE(self, gcmd):
toolhead = self.printer.lookup_object('toolhead')
# Parse parameters
self.test.prepare_test(toolhead, gcmd)
axis = gcmd.get("AXIS", None)
if not axis:
calibrate_axes = self.test.get_supported_axes()
elif axis.lower() not in self.test.get_supported_axes():
raise gcmd.error("Unsupported axis '%s'" % (axis,))
else:
calibrate_axes = [axis.lower()]
name_suffix = gcmd.get("NAME", time.strftime("%Y%m%d_%H%M%S"))
if not self.is_valid_name_suffix(name_suffix):
raise gcmd.error("Invalid NAME parameter")
# Setup shaper calibration
helper = shaper_calibrate.ShaperCalibrate(self.printer)
input_shaper = self.printer.lookup_object('input_shaper', None)
if input_shaper is not None:
input_shaper.disable_shaping()
gcmd.respond_info("Disabled [input_shaper] for calibration")
currentPos = toolhead.get_position()
Z = currentPos[2]
E = currentPos[3]
calibration_data = {axis: None for axis in calibrate_axes}
calibration_points = self.test.get_start_test_points()
for point in calibration_points:
toolhead.manual_move(point, self.move_speed)
if len(calibration_points) > 1:
gcmd.respond_info(
"Probing point (%.3f, %.3f, %.3f)" % tuple(point))
for axis in calibrate_axes:
toolhead.wait_moves()
toolhead.dwell(0.500)
gcmd.respond_info("Testing axis %s" % axis.upper())
for chip_axis, chip in self.accel_chips:
if axis in chip_axis or chip_axis in axis:
chip.start_measurements()
# Generate moves
self.test.run_test(toolhead, axis, gcmd)
raw_values = [(chip_axis, chip.finish_measurements())
for chip_axis, chip in self.accel_chips
if axis in chip_axis or chip_axis in axis]
for chip_axis, chip_values in raw_values:
gcmd.respond_info("%s-axis accelerometer stats: %s" % (
chip_axis, chip_values.get_stats(),))
if not chip_values:
raise gcmd.error(
"%s-axis accelerometer measured no data" % (
chip_axis,))
new_data = helper.process_accelerometer_data(chip_values)
if calibration_data[axis] is None:
calibration_data[axis] = new_data
else:
calibration_data[axis].join(new_data)
configfile = self.printer.lookup_object('configfile')
for axis in calibrate_axes:
gcmd.respond_info(
"Calculating the best input shaper parameters for %s axis"
% (axis,))
calibration_data[axis].normalize_to_frequencies()
shaper_name, shaper_freq, shapers_vals = helper.find_best_shaper(
calibration_data[axis], gcmd.respond_info)
gcmd.respond_info(
"Recommended shaper_type_%s = %s, shaper_freq_%s = %.1f Hz"
% (axis, shaper_name, axis, shaper_freq))
helper.save_params(configfile, axis, shaper_name, shaper_freq)
csv_name = self.save_calibration_data(
'calibration_data', name_suffix, helper, axis,
calibration_data[axis], shapers_vals)
gcmd.respond_info(
"Shaper calibration data written to %s file" % (csv_name,))
gcmd.respond_info(
"The SAVE_CONFIG command will update the printer config file\n"
"with these parameters and restart the printer.")
if input_shaper is not None:
input_shaper.enable_shaping()
gcmd.respond_info("Re-enabled [input_shaper] after calibration")
def cmd_MEASURE_AXES_NOISE(self, gcmd):
meas_time = gcmd.get_float("MEAS_TIME", 2.)
for _, chip in self.accel_chips:
chip.start_measurements()
self.printer.lookup_object('toolhead').dwell(meas_time)
raw_values = [(axis, chip.finish_measurements())
for axis, chip in self.accel_chips]
helper = shaper_calibrate.ShaperCalibrate(self.printer)
for axis, raw_data in raw_values:
data = helper.process_accelerometer_data(raw_data)
vx = data.psd_x.mean()
vy = data.psd_y.mean()
vz = data.psd_z.mean()
gcmd.respond_info("Axes noise for %s-axis accelerometer: "
"%.6f (x), %.6f (y), %.6f (z)" % (
axis, vx, vy, vz))
def is_valid_name_suffix(self, name_suffix):
return name_suffix.replace('-', '').replace('_', '').isalnum()
def get_filename(self, base, name_suffix, axis=None, point=None):
name = base
if axis:
name += '_' + axis
if point:
name += "_%.3f_%.3f_%.3f" % (point[0], point[1], point[2])
name += '_' + name_suffix
return os.path.join("/tmp", name + ".csv")
def save_calibration_data(self, base_name, name_suffix, shaper_calibrate,
axis, calibration_data, shapers_vals=None):
output = self.get_filename(base_name, name_suffix, axis)
shaper_calibrate.save_calibration_data(output, calibration_data,
shapers_vals)
return output
def load_config(config):
return ResonanceTester(config)

382
klippy/extras/shaper_calibrate.py Normal file
View File

@@ -0,0 +1,382 @@
# Automatic calibration of input shapers
#
# Copyright (C) 2020 Dmitry Butyugin <dmbutyugin@google.com>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import importlib, logging, math, multiprocessing
MIN_FREQ = 5.
MAX_FREQ = 200.
WINDOW_T_SEC = 0.5
MAX_SHAPER_FREQ = 150.
TEST_DAMPING_RATIOS=[0.075, 0.1, 0.15]
SHAPER_DAMPING_RATIO = 0.1
######################################################################
# Input shapers
######################################################################
class InputShaperCfg:
def __init__(self, name, init_func, min_freq):
self.name = name
self.init_func = init_func
self.min_freq = min_freq
def get_zv_shaper(shaper_freq, damping_ratio):
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
A = [1., K]
T = [0., .5*t_d]
return (A, T)
def get_zvd_shaper(shaper_freq, damping_ratio):
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
A = [1., 2.*K, K**2]
T = [0., .5*t_d, t_d]
return (A, T)
def get_mzv_shaper(shaper_freq, damping_ratio):
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-.75 * damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
a1 = 1. - 1. / math.sqrt(2.)
a2 = (math.sqrt(2.) - 1.) * K
a3 = a1 * K * K
A = [a1, a2, a3]
T = [0., .375*t_d, .75*t_d]
return (A, T)
def get_ei_shaper(shaper_freq, damping_ratio):
v_tol = 0.05 # vibration tolerance
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
a1 = .25 * (1. + v_tol)
a2 = .5 * (1. - v_tol) * K
a3 = a1 * K * K
A = [a1, a2, a3]
T = [0., .5*t_d, t_d]
return (A, T)
def get_2hump_ei_shaper(shaper_freq, damping_ratio):
v_tol = 0.05 # vibration tolerance
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
V2 = v_tol**2
X = pow(V2 * (math.sqrt(1. - V2) + 1.), 1./3.)
a1 = (3.*X*X + 2.*X + 3.*V2) / (16.*X)
a2 = (.5 - a1) * K
a3 = a2 * K
a4 = a1 * K * K * K
A = [a1, a2, a3, a4]
T = [0., .5*t_d, t_d, 1.5*t_d]
return (A, T)
def get_3hump_ei_shaper(shaper_freq, damping_ratio):
v_tol = 0.05 # vibration tolerance
df = math.sqrt(1. - damping_ratio**2)
K = math.exp(-damping_ratio * math.pi / df)
t_d = 1. / (shaper_freq * df)
K2 = K*K
a1 = 0.0625 * (1. + 3. * v_tol + 2. * math.sqrt(2. * (v_tol + 1.) * v_tol))
a2 = 0.25 * (1. - v_tol) * K
a3 = (0.5 * (1. + v_tol) - 2. * a1) * K2
a4 = a2 * K2
a5 = a1 * K2 * K2
A = [a1, a2, a3, a4, a5]
T = [0., .5*t_d, t_d, 1.5*t_d, 2.*t_d]
return (A, T)
INPUT_SHAPERS = [
InputShaperCfg('zv', get_zv_shaper, 15.),
InputShaperCfg('mzv', get_mzv_shaper, 25.),
InputShaperCfg('ei', get_ei_shaper, 30.),
InputShaperCfg('2hump_ei', get_2hump_ei_shaper, 37.5),
InputShaperCfg('3hump_ei', get_3hump_ei_shaper, 50.),
]
######################################################################
# Frequency response calculation and shaper auto-tuning
######################################################################
class CalibrationData:
def __init__(self, freq_bins, psd_sum, psd_x, psd_y, psd_z):
self.freq_bins = freq_bins
self.psd_sum = psd_sum
self.psd_x = psd_x
self.psd_y = psd_y
self.psd_z = psd_z
self._psd_list = [self.psd_sum, self.psd_x, self.psd_y, self.psd_z]
self.data_sets = 1
def join(self, other):
np = self.numpy
joined_data_sets = self.data_sets + other.data_sets
for psd, other_psd in zip(self._psd_list, other._psd_list):
# `other` data may be defined at different frequency bins,
# interpolating to fix that.
other_normalized = other.data_sets * np.interp(
self.freq_bins, other.freq_bins, other_psd)
psd *= self.data_sets
psd[:] = (psd + other_normalized) * (1. / joined_data_sets)
self.data_sets = joined_data_sets
def set_numpy(self, numpy):
self.numpy = numpy
def normalize_to_frequencies(self):
for psd in self._psd_list:
# Avoid division by zero errors
psd /= self.freq_bins + .1
# Remove low-frequency noise
psd[self.freq_bins < MIN_FREQ] = 0.
class ShaperCalibrate:
def __init__(self, printer):
self.printer = printer
self.error = printer.command_error if printer else Exception
try:
self.numpy = importlib.import_module('numpy')
except ImportError:
raise self.error(
"Failed to import `numpy` module, make sure it was "
"installed via `~/klippy-env/bin/pip install` (refer to "
"docs/Measuring_Resonances.md for more details).")
def background_process_exec(self, method, args):
if self.printer is None:
return method(*args)
import queuelogger
parent_conn, child_conn = multiprocessing.Pipe()
def wrapper():
queuelogger.clear_bg_logging()
try:
res = method(*args)
except:
child_conn.send((True, traceback.format_exc()))
child_conn.close()
return
child_conn.send((False, res))
child_conn.close()
# Start a process to perform the calculation
calc_proc = multiprocessing.Process(target=wrapper)
calc_proc.daemon = True
calc_proc.start()
# Wait for the process to finish
reactor = self.printer.get_reactor()
gcode = self.printer.lookup_object("gcode")
eventtime = last_report_time = reactor.monotonic()
while calc_proc.is_alive():
if eventtime > last_report_time + 5.:
last_report_time = eventtime
gcode.respond_info("Wait for calculations..", log=False)
eventtime = reactor.pause(eventtime + .1)
# Return results
is_err, res = parent_conn.recv()
if is_err:
raise self.error("Error in remote calculation: %s" % (res,))
calc_proc.join()
parent_conn.close()
return res
def _split_into_windows(self, x, window_size, overlap):
# Memory-efficient algorithm to split an input 'x' into a series
# of overlapping windows
step_between_windows = window_size - overlap
n_windows = (x.shape[-1] - overlap) // step_between_windows
shape = (window_size, n_windows)
strides = (x.strides[-1], step_between_windows * x.strides[-1])
return self.numpy.lib.stride_tricks.as_strided(
x, shape=shape, strides=strides, writeable=False)
def _psd(self, x, fs, nfft):
# Calculate power spectral density (PSD) using Welch's algorithm
np = self.numpy
window = np.kaiser(nfft, 6.)
# Compensation for windowing loss
scale = 1.0 / (window**2).sum()
# Split into overlapping windows of size nfft
overlap = nfft // 2
x = self._split_into_windows(x, nfft, overlap)
# First detrend, then apply windowing function
x = window[:, None] * (x - np.mean(x, axis=0))
# Calculate frequency response for each window using FFT
result = np.fft.rfft(x, n=nfft, axis=0)
result = np.conjugate(result) * result
result *= scale / fs
# For one-sided FFT output the response must be doubled, except
# the last point for unpaired Nyquist frequency (assuming even nfft)
# and the 'DC' term (0 Hz)
result[1:-1,:] *= 2.
# Welch's algorithm: average response over windows
psd = result.real.mean(axis=-1)
# Calculate the frequency bins
freqs = np.fft.rfftfreq(nfft, 1. / fs)
return freqs, psd
def calc_freq_response(self, raw_values):
np = self.numpy
if raw_values is None:
return None
if isinstance(raw_values, np.ndarray):
data = raw_values
else:
data = np.array(raw_values.decode_samples())
N = data.shape[0]
T = data[-1,0] - data[0,0]
SAMPLING_FREQ = N / T
# Round up to the nearest power of 2 for faster FFT
M = 1 << int(SAMPLING_FREQ * WINDOW_T_SEC - 1).bit_length()
if N <= M:
return None
# Calculate PSD (power spectral density) of vibrations per
# frequency bins (the same bins for X, Y, and Z)
fx, px = self._psd(data[:,1], SAMPLING_FREQ, M)
fy, py = self._psd(data[:,2], SAMPLING_FREQ, M)
fz, pz = self._psd(data[:,3], SAMPLING_FREQ, M)
return CalibrationData(fx, px+py+pz, px, py, pz)
def process_accelerometer_data(self, data):
calibration_data = self.background_process_exec(
self.calc_freq_response, (data,))
if calibration_data is None:
raise self.error(
"Internal error processing accelerometer data %s" % (data,))
calibration_data.set_numpy(self.numpy)
return calibration_data
def _estimate_shaper(self, shaper, test_damping_ratio, test_freqs):
np = self.numpy
A, T = np.array(shaper[0]), np.array(shaper[1])
inv_D = 1. / A.sum()
omega = 2. * math.pi * test_freqs
damping = test_damping_ratio * omega
omega_d = omega * math.sqrt(1. - test_damping_ratio**2)
W = A * np.exp(np.outer(-damping, (T[-1] - T)))
S = W * np.sin(np.outer(omega_d, T))
C = W * np.cos(np.outer(omega_d, T))
return np.sqrt(S.sum(axis=1)**2 + C.sum(axis=1)**2) * inv_D
def _estimate_remaining_vibrations(self, shaper, test_damping_ratio,
freq_bins, psd):
vals = self._estimate_shaper(shaper, test_damping_ratio, freq_bins)
remaining_vibrations = (vals * psd).sum() / psd.sum()
return (remaining_vibrations, vals)
def fit_shaper(self, shaper_cfg, calibration_data):
np = self.numpy
test_freqs = np.arange(shaper_cfg.min_freq, MAX_SHAPER_FREQ, .2)
freq_bins = calibration_data.freq_bins
psd = calibration_data.psd_sum[freq_bins <= MAX_FREQ]
freq_bins = freq_bins[freq_bins <= MAX_FREQ]
best_freq = None
best_remaining_vibrations = 0
best_shaper_vals = []
for test_freq in test_freqs[::-1]:
cur_remaining_vibrations = 0.
shaper_vals = np.zeros(shape=freq_bins.shape)
shaper = shaper_cfg.init_func(test_freq, SHAPER_DAMPING_RATIO)
# Exact damping ratio of the printer is unknown, pessimizing
# remaining vibrations over possible damping values.
for dr in TEST_DAMPING_RATIOS:
vibrations, vals = self._estimate_remaining_vibrations(
shaper, dr, freq_bins, psd)
shaper_vals = np.maximum(shaper_vals, vals)
if vibrations > cur_remaining_vibrations:
cur_remaining_vibrations = vibrations
if (best_freq is None or
best_remaining_vibrations > cur_remaining_vibrations):
# The current frequency is better for the shaper.
best_freq = test_freq
best_remaining_vibrations = cur_remaining_vibrations
best_shaper_vals = shaper_vals
return (best_freq, best_remaining_vibrations, best_shaper_vals)
def find_best_shaper(self, calibration_data, logger=None):
best_shaper = prev_shaper = None
best_freq = prev_freq = 0.
best_vibrations = prev_vibrations = 0.
all_shaper_vals = []
for shaper in INPUT_SHAPERS:
shaper_freq, vibrations, shaper_vals = self.background_process_exec(
self.fit_shaper, (shaper, calibration_data))
if logger is not None:
logger("Fitted shaper '%s' frequency = %.1f Hz "
"(vibrations = %.1f%%)" % (
shaper.name, shaper_freq, vibrations * 100.))
if best_shaper is None or 1.75 * vibrations < best_vibrations:
if 1.25 * vibrations < prev_vibrations:
best_shaper = shaper.name
best_freq = shaper_freq
best_vibrations = vibrations
else:
# The current shaper is good, but not sufficiently better
# than the previous one, using previous shaper instead.
best_shaper = prev_shaper
best_freq = prev_freq
best_vibrations = prev_vibrations
prev_shaper = shaper.name
prev_shaper_vals = shaper_vals
prev_freq = shaper_freq
prev_vibrations = vibrations
all_shaper_vals.append((shaper.name, shaper_freq, shaper_vals))
return (best_shaper, best_freq, all_shaper_vals)
def save_params(self, configfile, axis, shaper_name, shaper_freq):
if axis == 'xy':
self.save_params(configfile, 'x', shaper_name, shaper_freq)
self.save_params(configfile, 'y', shaper_name, shaper_freq)
else:
configfile.set('input_shaper', 'shaper_type_'+axis, shaper_name)
configfile.set('input_shaper', 'shaper_freq_'+axis,
'%.1f' % (shaper_freq,))
def save_calibration_data(self, output, calibration_data,
shapers_vals=None):
try:
with open(output, "w") as csvfile:
csvfile.write("freq,psd_x,psd_y,psd_z,psd_xyz")
if shapers_vals:
for name, freq, _ in shapers_vals:
csvfile.write(",%s(%.1f)" % (name, freq))
csvfile.write("\n")
num_freqs = calibration_data.freq_bins.shape[0]
for i in range(num_freqs):
if calibration_data.freq_bins[i] >= MAX_FREQ:
break
csvfile.write("%.1f,%.3e,%.3e,%.3e,%.3e" % (
calibration_data.freq_bins[i],
calibration_data.psd_x[i],
calibration_data.psd_y[i],
calibration_data.psd_z[i],
calibration_data.psd_sum[i]))
if shapers_vals:
for _, _, vals in shapers_vals:
csvfile.write(",%.3f" % (vals[i],))
csvfile.write("\n")
except IOError as e:
raise self.error("Error writing to file '%s': %s", output, str(e))