delta_calibrate: Support save/restore of delta_calibrate state

Support using SAVE_CONFIG to store the results of DELTA_CALIBRATE to
the printer config file.  Store the low level probe measurements in
the config as well.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 59 insertions, 10 deletions

diff --git a/klippy/extras/delta_calibrate.py b/klippy/extras/delta_calibrate.py
index b065da74..97c75466 100644
--- a/klippy/extras/delta_calibrate.py
+++ b/klippy/extras/delta_calibrate.py
@@ -56,6 +56,17 @@ def get_stable_position(stepper_position, delta_params):
             for sd, ep, sp in zip(
                     dp.stepdists, dp.abs_endstops, stepper_position)]
 
+# Load a stable position from a config entry
+def load_config_stable(config, option):
+    spos = config.get(option)
+    try:
+        sa, sb, sc = map(float, spos.split(','))
+    except:
+        msg = "Unable to parse stable position '%s'" % (spos,)
+        logging.exception(msg)
+        raise config.error(msg)
+    return sa, sb, sc
+
 
 ######################################################################
 # Delta Calibrate class
@@ -76,11 +87,37 @@ class DeltaCalibrate:
             points.append((math.cos(r) * dist, math.sin(r) * dist))
         self.probe_helper = probe.ProbePointsHelper(
             config, self, default_points=points)
+        # Restore probe stable positions
+        self.last_probe_positions = []
+        for i in range(999):
+            height = config.getfloat("height%d" % (i,), None)
+            if height is None:
+                break
+            height_pos = load_config_stable(config, "height%d_pos" % (i,))
+            self.last_probe_positions.append((height, height_pos))
         # Register DELTA_CALIBRATE command
         self.gcode = self.printer.lookup_object('gcode')
         self.gcode.register_command(
             'DELTA_CALIBRATE', self.cmd_DELTA_CALIBRATE,
             desc=self.cmd_DELTA_CALIBRATE_help)
+    def save_state(self, probe_positions, params):
+        # Save main delta parameters
+        configfile = self.printer.lookup_object('configfile')
+        configfile.set('printer', 'delta_radius', "%.6f" % (params['radius']))
+        for axis in 'abc':
+            configfile.set('stepper_'+axis, 'angle',
+                           "%.6f" % (params['angle_'+axis],))
+            configfile.set('stepper_'+axis, 'arm_length',
+                           "%.6f" % (params['arm_'+axis],))
+            configfile.set('stepper_'+axis, 'position_endstop',
+                           "%.6f" % (params['endstop_'+axis],))
+        # Save probe stable positions
+        section = 'delta_calibrate'
+        configfile.remove_section(section)
+        for i, (z_offset, spos) in enumerate(probe_positions):
+            configfile.set(section, "height%d" % (i,), z_offset)
+            configfile.set(section, "height%d_pos" % (i,),
+                           "%d,%d,%d" % tuple(spos))
     cmd_DELTA_CALIBRATE_help = "Delta calibration script"
     def cmd_DELTA_CALIBRATE(self, params):
         self.gcode.run_script_from_command("G28")
@@ -89,43 +126,55 @@ class DeltaCalibrate:
         kin = self.printer.lookup_object('toolhead').get_kinematics()
         return [s.get_commanded_position() for s in kin.get_steppers()]
     def finalize(self, offsets, positions):
+        # Convert positions into (z_offset, stable_position) pairs
         z_offset = offsets[2]
         kin = self.printer.lookup_object('toolhead').get_kinematics()
+        delta_params = build_delta_params(kin.get_calibrate_params())
+        probe_positions = [(z_offset, get_stable_position(p, delta_params))
+                           for p in positions]
+        # Perform analysis
+        self.calculate_params(probe_positions)
+    def calculate_params(self, probe_positions):
+        # Setup for coordinate descent analysis
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
         params = kin.get_calibrate_params()
         orig_delta_params = build_delta_params(params)
-        stable_positions = [get_stable_position(p, orig_delta_params)
-                            for p in positions]
         logging.info("Calculating delta_calibrate with: %s\n"
                      "Initial delta_calibrate parameters: %s",
-                     stable_positions, params)
+                     probe_positions, params)
         adj_params = ('radius', 'angle_a', 'angle_b',
                       'endstop_a', 'endstop_b', 'endstop_c')
+        # Perform coordinate descent
         def delta_errorfunc(params):
             delta_params = build_delta_params(params)
             total_error = 0.
-            for stable_pos in stable_positions:
+            for z_offset, stable_pos in probe_positions:
                 x, y, z = get_position_from_stable(stable_pos, delta_params)
                 total_error += (z - z_offset)**2
             return total_error
         new_params = mathutil.coordinate_descent(
             adj_params, params, delta_errorfunc)
+        # Log and report results
         logging.info("Calculated delta_calibrate parameters: %s", new_params)
         new_delta_params = build_delta_params(new_params)
-        for spos in stable_positions:
-            logging.info("orig: %s new: %s",
-                         get_position_from_stable(spos, orig_delta_params),
-                         get_position_from_stable(spos, new_delta_params))
+        for z_offset, spos in probe_positions:
+            logging.info("height orig: %.6f new: %.6f goal: %.6f",
+                         get_position_from_stable(spos, orig_delta_params)[2],
+                         get_position_from_stable(spos, new_delta_params)[2],
+                         z_offset)
         self.gcode.respond_info(
             "stepper_a: position_endstop: %.6f angle: %.6f\n"
             "stepper_b: position_endstop: %.6f angle: %.6f\n"
             "stepper_c: position_endstop: %.6f angle: %.6f\n"
             "delta_radius: %.6f\n"
-            "To use these parameters, update the printer config file with\n"
-            "the above and then issue a RESTART command" % (
+            "The SAVE_CONFIG command will update the printer config file\n"
+            "with these parameters and restart the printer." % (
                 new_params['endstop_a'], new_params['angle_a'],
                 new_params['endstop_b'], new_params['angle_b'],
                 new_params['endstop_c'], new_params['angle_c'],
                 new_params['radius']))
+        # Store results for SAVE_CONFIG
+        self.save_state(probe_positions, new_params)
 
 def load_config(config):
     return DeltaCalibrate(config)