From ecd996b61b680e9f67923a1a4afe94f5d3cd1530 Mon Sep 17 00:00:00 2001
From: Sgr A* VMT <1611902585@qq.com>
Date: Sun, 28 Jan 2024 22:59:15 +0800
Subject: [PATCH] support twist compesation

---
 idm.py | 19 +++++++++----------
 1 file changed, 9 insertions(+), 10 deletions(-)

diff --git a/idm.py b/idm.py
index 91217ff..61ec483 100644
--- a/idm.py
+++ b/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