probe: Add PROBE_ACCURACY command

Implementation of "PROBE_ACCURACY" to measure the maximum, minimum,
average and standard deviation of a probe.

Signed-off-by: Rui Caridade <rui.mcbc@gmail.com>
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

2 files changed, 78 insertions, 1 deletions

diff --git a/docs/G-Codes.md b/docs/G-Codes.md
index b9260b43..a39f2c30 100644
--- a/docs/G-Codes.md
+++ b/docs/G-Codes.md
@@ -187,6 +187,10 @@ enabled:
 - `PROBE`: Move the nozzle downwards until the probe triggers.
 - `QUERY_PROBE`: Report the current status of the probe ("triggered"
   or "open").
+- `PROBE_ACCURACY [REPEAT=<times>] [SPEED=<speed mm/s>] [X=<x pos>]
+  [Y=<y pos>] [Z=<z height>]`: Calculate the maximum, minimum, average,
+  median and standard deviation. The default values are: REPEAT=10,
+  SPEED=probe config speed, X=current X, Y=current Y and Z=10.
 - `PROBE_CALIBRATE [SPEED=<speed>]`: Run a helper script useful for
   calibrating the probe's z_offset. See the MANUAL_PROBE command for
   details on the parameters and the additional commands available
diff --git a/klippy/extras/probe.py b/klippy/extras/probe.py
index 3d0bf775..9b44ca04 100644
--- a/klippy/extras/probe.py
+++ b/klippy/extras/probe.py
@@ -38,6 +38,8 @@ class PrinterProbe:
                                     desc=self.cmd_QUERY_PROBE_help)
         self.gcode.register_command('PROBE_CALIBRATE', self.cmd_PROBE_CALIBRATE,
                                     desc=self.cmd_PROBE_CALIBRATE_help)
+        self.gcode.register_command('PROBE_ACCURACY', self.cmd_PROBE_ACCURACY,
+                                    desc=self.cmd_PROBE_ACCURACY_help)
     def setup_pin(self, pin_type, pin_params):
         if pin_type != 'endstop' or pin_params['pin'] != 'z_virtual_endstop':
             raise pins.error("Probe virtual endstop only useful as endstop pin")
@@ -48,6 +50,8 @@ class PrinterProbe:
         return self.x_offset, self.y_offset, self.z_offset
     cmd_PROBE_help = "Probe Z-height at current XY position"
     def cmd_PROBE(self, params):
+        self._probe(self.speed)
+    def _probe(self, speed):
         toolhead = self.printer.lookup_object('toolhead')
         homing_state = homing.Homing(self.printer)
         pos = toolhead.get_position()
@@ -55,7 +59,7 @@ class PrinterProbe:
         endstops = [(self.mcu_probe, "probe")]
         verify = self.printer.get_start_args().get('debugoutput') is None
         try:
-            homing_state.homing_move(pos, endstops, self.speed,
+            homing_state.homing_move(pos, endstops, speed,
                                      probe_pos=True, verify_movement=verify)
         except homing.EndstopError as e:
             reason = str(e)
@@ -74,6 +78,75 @@ class PrinterProbe:
         res = self.mcu_probe.query_endstop_wait()
         self.gcode.respond_info(
             "probe: %s" % (["open", "TRIGGERED"][not not res],))
+    cmd_PROBE_ACCURACY_help = "Probe Z-height accuracy at current XY position"
+    def cmd_PROBE_ACCURACY(self, params):
+        toolhead = self.printer.lookup_object('toolhead')
+        probes = []
+        pos = toolhead.get_position()
+        number_of_reads = self.gcode.get_int('REPEAT', params, default=10,
+                                                       minval=4, maxval=50)
+        speed = self.gcode.get_int('SPEED', params, default=self.speed,
+                                            minval=1, maxval=30)
+        z_start_position = self.gcode.get_float('Z', params, default=10.,
+                                                     minval=10., maxval=70.)
+        x_start_position = self.gcode.get_float('X', params, default=pos[0])
+        y_start_position = self.gcode.get_float('Y', params, default=pos[1])
+        self.gcode.respond_info("probe accuracy: at X:%.3f Y:%.3f Z:%.3f\n"
+                                "                "
+                                "and read %d times with speed of %d mm/s" % (
+                                x_start_position, y_start_position,
+                                z_start_position, number_of_reads, speed))
+        # Probe bed "number_of_reads" times
+        sum_reads = 0
+        for i in range(number_of_reads):
+            # Move Z to start reading position
+            self._move_position(x_start_position, y_start_position,
+                                z_start_position, speed)
+            # Probe
+            self._probe(speed)
+            # Get Z value, accumulate value to calculate average
+            # and save it to calculate standard deviation
+            pos = toolhead.get_position()
+            sum_reads += pos[2]
+            probes.append(pos[2])
+        # Move Z to start reading position
+        self._move_position(x_start_position, y_start_position,
+                            z_start_position, speed)
+        # Calculate maximum, minimum and average values
+        max_value = max(probes)
+        min_value = min(probes)
+        avg_value = sum(probes) / number_of_reads
+        # calculate the standard deviation
+        deviation_sum = 0
+        for i in range(number_of_reads):
+            deviation_sum += pow(probes[i] - avg_value, 2)
+        sigma = (deviation_sum / number_of_reads) ** 0.5
+        # Median
+        sorted_probes = sorted(probes)
+        middle = number_of_reads//2
+        if (number_of_reads & 1) == 1:
+            # odd number of reads
+            median = sorted_probes[middle]
+        else:
+            # even number of reads
+            median = (sorted_probes[middle]+sorted_probes[middle-1])/2
+        # Show information
+        self.gcode.respond_info(
+            "probe accuracy results: maximum %.6f, minimum %.6f, "
+            "average %.6f, median %.6f, standard deviation %.6f" % (
+            max_value, min_value, avg_value, median, sigma))
+    def _move_position(self, x, y, z, speed):
+        toolhead = self.printer.lookup_object('toolhead')
+        pos = toolhead.get_position()
+        # set new position
+        pos[0] = x
+        pos[1] = y
+        pos[2] = z
+        # Move to position
+        try:
+            toolhead.move(pos, speed)
+        except homing.EndstopError as e:
+            raise self.gcode.error(str(e))
     def probe_calibrate_finalize(self, kin_pos):
         if kin_pos is None:
             return