Druk-3D/klipper
1
0
Fork 0
mirror of https://github.com/Klipper3d/klipper.git synced 2026-05-06 17:57:17 +02:00
Code Issues Packages Projects Releases Wiki Activity

probe_eddy_current: Cache data between each "tap" least squares calculation

Browse Source 
Directly calculate the evaluated matrices instead of using
mat_mat_mult() and mat_transp().

Cache the intermediate summations to improve processing when using
similar z guesses.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor
2026-04-06 19:46:35 -04:00
parent c1ed295db2
commit 5680b74b1d
1 changed files with 62 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
klippy/extras/probe_eddy_current.py
View File

@@ -506,6 +506,7 @@ class EddyTap:
self._filter_design = None
self._tap_z_offset = config.getfloat('tap_z_offset', 0.)
self._tap_threshold = config.getfloat('tap_threshold', 0., above=0.)
self._least_squares_cache = {}
if self._tap_threshold:
self._setup_tap()
# Setup for "tap" probe request
@@ -561,8 +562,8 @@ class EddyTap:
s.get_past_mcu_position(pos_time))
for s in kin.get_steppers()}
return kin.calc_position(kin_spos)
def _calc_least_squares(self, samples, est_z_contact):
# XXX - this implementation is not efficient
def _build_ls_matrix(self, samples, est_z_contact):
# The function here is only a reference for the optimized version below
len_samples = len(samples)
eqs = [[0.] * 4 for i in range(len_samples)]
ans = [[0.] for i in range(len_samples)]
@@ -583,6 +584,63 @@ class EddyTap:
eqst = mathutil.mat_transp(eqs)
eqst_eqs = mathutil.mat_mat_mul(eqst, eqs)
eqst_ans = mathutil.mat_mat_mul(eqst, ans)
return eqst_eqs, eqst_ans
def _build_sums(self, samples, num_le):
sum_le_z = sum_le_z2 = sum_le_freq = sum_le_freq_z = 0.
for z, freq in samples[:num_le]:
sum_le_z += z
sum_le_z2 += z**2
sum_le_freq += freq
sum_le_freq_z += freq*z
sum_gt_z = sum_gt_z2 = sum_gt_z3 = sum_gt_z4 = 0.
sum_gt_freq = sum_gt_freq_z = sum_gt_freq_z2 = 0.
for z, freq in samples[num_le:]:
sum_gt_z += z
sum_gt_z2 += z**2
sum_gt_z3 += z**3
sum_gt_z4 += z**4
sum_gt_freq += freq
sum_gt_freq_z += freq*z
sum_gt_freq_z2 += freq * z**2
return (sum_le_z, sum_le_z2, sum_le_freq, sum_le_freq_z,
sum_gt_z, sum_gt_z2, sum_gt_z3, sum_gt_z4,
sum_gt_freq, sum_gt_freq_z, sum_gt_freq_z2)
def _build_ls_matrix_opt(self, samples, est_z_contact):
# This function is an optimized version of _build_ls_matrix()
num_le = bisect.bisect(samples, (est_z_contact, sys.float_info.max))
# Check for previously calculated raw freq/z counters
sums = self._least_squares_cache.get(num_le)
if sums is None:
sums = self._build_sums(samples, num_le)
self._least_squares_cache[num_le] = sums
(sum_le_z, sum_le_z2, sum_le_freq, sum_le_freq_z,
sum_gt_z, sum_gt_z2, sum_gt_z3, sum_gt_z4,
sum_gt_freq, sum_gt_freq_z, sum_gt_freq_z2) = sums
num_samples = len(samples)
ezc = est_z_contact
ezc2 = ezc**2
ezc3 = ezc**3
ezc4 = ezc**4
# Build matrices for least squares evaluation
eqst_eqs = [[0.] * 4 for i in range(4)]
eqst_eqs[0][0] = num_samples
eqst_eqs[1][1] = sum_le_z2 - 2*ezc*sum_le_z + num_le*ezc2
eqst_eqs[2][2] = sum_gt_z2 + num_le*ezc2
eqst_eqs[3][3] = sum_gt_z4 + num_le*ezc4
eqst_eqs[0][1] = eqst_eqs[1][0] = sum_le_z - num_le*ezc
eqst_eqs[0][2] = eqst_eqs[2][0] = sum_gt_z + num_le*ezc
eqst_eqs[0][3] = eqst_eqs[3][0] = sum_gt_z2 + num_le*ezc2
eqst_eqs[2][3] = eqst_eqs[3][2] = sum_gt_z3 + num_le*ezc3
eqst_eqs[2][1] = eqst_eqs[1][2] = ezc * eqst_eqs[0][1]
eqst_eqs[3][1] = eqst_eqs[1][3] = ezc2 * eqst_eqs[0][1]
eqst_ans = [[0.] for i in range(4)]
eqst_ans[0][0] = sum_le_freq + sum_gt_freq
eqst_ans[1][0] = sum_le_freq_z - ezc*sum_le_freq
eqst_ans[2][0] = sum_gt_freq_z + ezc*sum_le_freq
eqst_ans[3][0] = sum_gt_freq_z2 + ezc2 * sum_le_freq
return eqst_eqs, eqst_ans
def _calc_least_squares(self, samples, est_z_contact):
eqst_eqs, eqst_ans = self._build_ls_matrix_opt(samples, est_z_contact)
coeffs = mathutil.gaussian_solve(eqst_eqs, eqst_ans)
if coeffs is not None and coeffs[3][0] < 0.:
# z**2 factor can't be negative - retry using only linear
@@ -598,6 +656,7 @@ class EddyTap:
def _find_least_squares(self, data):
#for d in data:
# logging.info("sample: freq=%.3f z=%.6f", d[0], d[1][2])
self._least_squares_cache.clear()
# Change base of freq/z measurements to improve numerical stability
base_z = .5 * (data[0][1][2] + data[-1][1][2])
base_freq = .5 * (data[0][0] + data[-1][0])
@@ -630,6 +689,7 @@ class EddyTap:
max_z = guess_z
else:
min_z = guess_z
self._least_squares_cache.clear()
# Return to original freq/z measurement base
bc = [v[0] for v in best_coeffs]
final_coeffs = (base_z + best_z,