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

Signed-off-by: Dmitry Butyugin <dmbutyugin@google.com>

4 files changed, 782 insertions, 35 deletions

diff --git a/klippy/extras/adxl345.py b/klippy/extras/adxl345.py
index fb2928c7..c29d5451 100644
--- a/klippy/extras/adxl345.py
+++ b/klippy/extras/adxl345.py
@@ -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)
diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py
index 35e7ff63..ceca89f8 100644
--- a/klippy/extras/input_shaper.py
+++ b/klippy/extras/input_shaper.py
@@ -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(
diff --git a/klippy/extras/resonance_tester.py b/klippy/extras/resonance_tester.py
new file mode 100644
index 00000000..70bea4a1
--- /dev/null
+++ b/klippy/extras/resonance_tester.py
@@ -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)
diff --git a/klippy/extras/shaper_calibrate.py b/klippy/extras/shaper_calibrate.py
new file mode 100644
index 00000000..67160b4f
--- /dev/null
+++ b/klippy/extras/shaper_calibrate.py
@@ -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))