support twist compesation

idm.py

@@ -269,15 +269,7 @@ class IDMProbe:
             # correct height and take a new sample.
             self._move_to_probing_height(speed)
             (dist, samples) = self._sample(self.z_settling_time, num_samples)
-        # get z compensation from axis_twist_compensation
         pos = samples[0]["pos"]
-        axis_twist_compensation = self.printer.lookup_object(
-            'axis_twist_compensation', None)
-        z_compensation = 0
-        if axis_twist_compensation is not None:
-            z_compensation = (
-                axis_twist_compensation.get_z_compensation_value(pos))
-        dist+=z_compensation
         self.gcode.respond_info("probe at %.3f,%.3f,%.3f is z=%.6f"
                                 % (pos[0], pos[1], pos[2], dist))
 
@@ -466,11 +458,18 @@ class IDMProbe:
         self._check_hardware(sample)
 
     def _enrich_sample(self, sample):
-        sample["dist"] = self.freq_to_dist(sample["freq"], sample["temp"])
         pos, vel = self._get_trapq_position(sample["time"])
-
+        # get z compensation from axis_twist_compensation        
         if pos is None:
+            sample["dist"] = self.freq_to_dist(sample["freq"], sample["temp"])
             return
+        axis_twist_compensation = self.printer.lookup_object(
+            'axis_twist_compensation', None)
+        z_compensation = 0
+        if axis_twist_compensation is not None:
+            z_compensation = (
+                axis_twist_compensation.get_z_compensation_value(pos))
+        sample["dist"] = self.freq_to_dist(sample["freq"], sample["temp"])+z_compensation
         sample["pos"] = pos
         sample["vel"] = vel