From f48673f555169b031e135dd3902cc2cd1cfac2fc Mon Sep 17 00:00:00 2001 From: Sgr A* VMT <1611902585@qq.com> Date: Sun, 31 Mar 2024 23:14:02 +0800 Subject: [PATCH] update --- idm.py | 223 ++++++++++++++++++++++++++--------------------- install.sh | 3 - requirements.txt | 2 +- 3 files changed, 125 insertions(+), 103 deletions(-) diff --git a/idm.py b/idm.py index 76222ee..058a315 100644 --- a/idm.py +++ b/idm.py @@ -566,11 +566,18 @@ class IDMProbe: self._enrich_sample_time(sample) self._data_filter.update(sample["time"], sample["data"]) self._enrich_sample_freq(sample) + self._enrich_sample(sample) if len(self._stream_callbacks) > 0: - self._enrich_sample(sample) for cb in list(self._stream_callbacks.values()): cb(sample) + last = sample + if last is not None: + last = last.copy() + dist = last["dist"] + if dist is None or np.isinf(dist) or np.isnan(dist): + del last["dist"] + self.last_received_sample = last except queue.Empty: return @@ -596,20 +603,12 @@ class IDMProbe: self._stream_flush_schedule() def _get_trapq_position(self, print_time): - move = None - if self._last_trapq_move: - last = self._last_trapq_move[0] - last_end = last.print_time + last.move_t - if last.print_time <= print_time < last_end: - move = last - if move is None: - ffi_main, ffi_lib = chelper.get_ffi() - data = ffi_main.new("struct pull_move[1]") - count = ffi_lib.trapq_extract_old(self.trapq, data, 1, 0.0, print_time) - if not count: - return None, None - self._last_trapq_move = data - move = data[0] + ffi_main, ffi_lib = chelper.get_ffi() + data = ffi_main.new("struct pull_move[1]") + count = ffi_lib.trapq_extract_old(self.trapq, data, 1, 0.0, print_time) + if not count: + return None, None + move = data[0] move_time = max(0.0, min(move.move_t, print_time - move.print_time)) dist = (move.start_v + .5 * move.accel * move_time) * move_time pos = (move.start_x + move.x_r * dist, move.start_y + move.y_r * dist, @@ -618,8 +617,7 @@ class IDMProbe: return pos, velocity def _sample_printtime_sync(self, skip=0, count=1): - toolhead = self.printer.lookup_object("toolhead") - move_time = toolhead.get_last_move_time() + move_time = self.toolhead.get_last_move_time() settle_clock = self._mcu.print_time_to_clock(move_time) samples = [] total = skip + count @@ -681,6 +679,7 @@ class IDMProbe: model = self.model.name return { "last_sample": self.last_sample, + "last_received_sample": self.last_received_sample, "model": model, } @@ -775,7 +774,7 @@ class IDMProbe: "time": sample["time"], "value": last_value, "temp": temp, - "dist": dist, + "dist": None if np.isinf(dist) or np.isnan(dist) else dist, } if dist is None: gcmd.respond_info("Last reading: %.2fHz, %.2fC, no model" % @@ -1410,6 +1409,7 @@ class IDMMeshHelper: def __init__(self, idm, config, mesh_config): self.idm = idm + self.scipy = None self.mesh_config = mesh_config self.bm = self.idm.printer.load_object(mesh_config, "bed_mesh") @@ -1721,10 +1721,20 @@ class IDMMeshHelper: self.toolhead.manual_move([x-xo, y-yo, None], speed) (dist, _samples) = self.idm._sample(50, 10) self.zero_ref_val = dist - + def _is_valid_position(self, x, y): return self.min_x <= x <= self.max_x and self.min_y <= y <= self.min_y + def _is_faulty_coordinate(self, x, y, add_offsets=False): + if add_offsets: + xo, yo = self.idm.x_offset, self.idm.y_offset + x += xo + y += yo + for r in self.faulty_regions: + if r.is_point_within(x, y): + return True + return False + def _sample_mesh(self, gcmd, path, speed, runs): cs = gcmd.get_float("CLUSTER_SIZE", self.cluster_size, minval=0.0) zcs = self.zero_ref_pos_cluster_size @@ -1753,6 +1763,8 @@ class IDMMeshHelper: # Calculate coordinate of the cluster we are in xi = int(round((x - min_x) / self.step_x)) yi = int(round((y - min_y) / self.step_y)) + if xi < 0 or self.res_x <= xi or yi < 0 or self.res_y <= yi: + return # If there's a cluster size limit, apply it here if cs > 0: @@ -1792,10 +1804,11 @@ class IDMMeshHelper: def _process_clusters(self, raw_clusters, gcmd): parent_conn, child_conn = multiprocessing.Pipe() + dump_file = gcmd.get("FILENAME", None) def do(): try: - child_conn.send((False, self._do_process_clusters(raw_clusters))) + child_conn.send((False, self._do_process_clusters(raw_clusters,dump_file))) except: child_conn.send((True, traceback.format_exc())) child_conn.close() @@ -1819,101 +1832,114 @@ class IDMMeshHelper: else: return inner_result - def _do_process_clusters(self, raw_clusters): - clusters = self._interpolate_faulty(raw_clusters) - return self._generate_matrix(clusters) - - def _is_faulty_coordinate(self, x, y, add_offsets=False): - if add_offsets: - xo, yo = self.idm.x_offset, self.idm.y_offset - x += xo - y += yo - for r in self.faulty_regions: - if r.is_point_within(x, y): - return True - return False + def _do_process_clusters(self, raw_clusters, dump_file): + if dump_file: + with open(dump_file, "w") as f: + f.write("x,y,xp,xy,dist\n") + for yi in range(self.res_y): + line = [] + for xi in range(self.res_x): + cluster = raw_clusters.get((xi, yi), []) + xp = xi * self.step_x + self.min_x + yp = yi * self.step_y + self.min_y + for dist in cluster: + f.write("%d,%d,%f,%f,%f\n" % (xi, yi, xp, yp, dist)) - def _interpolate_faulty(self, clusters): - faulty_indexes = [] - position = np.array(list(clusters.keys())) - (xi_max,yi_max) = position.T.max(axis = 1) - pos_temp = (position.T*[[self.step_x],[self.step_y]]+[[self.min_x],[self.min_y]]) - if len(self.faulty_region_.shape) > 1: - length=self.faulty_region_.shape[1] - flag = np.array( - [ - (pos_temp > self.faulty_region_[:2].reshape(1,2,length).T).T.all(axis=1), - (pos_temp < self.faulty_region_[2:].reshape(1,2,length).T).T.all(axis=1) - ] - ).all(axis = 0).any(axis = 1) - for i in range(len(flag)): - if(flag[i]): - clusters[tuple(position[i])] = None - faulty_indexes.append(tuple(position[i])) - del pos_temp + mask = self._generate_fault_mask() + matrix, faulty_regions = self._generate_matrix(raw_clusters, mask) + if len(faulty_regions) > 0: + (error, interpolator_or_msg) = self._load_interpolator() + if error: + return (True, interpolator_or_msg) + matrix = self._interpolate_faulty( + matrix, faulty_regions, interpolator_or_msg + ) + err = self._check_matrix(matrix) + if err is not None: + return (True, err) + return (False, self._finalize_matrix(matrix)) - def get_nearest(start, dx, dy): - inputs = np.array(start) - inputs += [dx,dy] - while ((inputs >= 0).all() and (inputs <= [xi_max,yi_max]).all()): - if clusters.get(tuple(inputs),None) is not None: - return (abs(inputs-np.array(start)).sum(), median(clusters[tuple(inputs)])) - inputs += [dx,dy] + def _generate_fault_mask(self): + if len(self.faulty_regions) == 0: return None + mask = np.full((self.res_y, self.res_x), True) + for r in self.faulty_regions: + r_xmin = int(math.ceil((r.x_min - self.min_x) / self.step_x)) + r_ymin = int(math.ceil((r.y_min - self.min_y) / self.step_y)) + r_xmax = int(math.floor((r.x_max - self.min_x) / self.step_x)) + r_ymax = int(math.floor((r.y_max - self.min_y) / self.step_y)) + for y in range(r_ymin, r_ymax + 1): + for x in range(r_xmin, r_xmax + 1): + mask[(y, x)] = False + return mask - def interp_weighted(lower, higher): - if lower is None and higher is None: - return None - if lower is None and higher is not None: - return higher[1] - elif lower is not None and higher is None: - return lower[1] + def _generate_matrix(self, raw_clusters, mask): + faulty_indexes = [] + matrix = np.empty((self.res_y, self.res_x)) + for (x, y), values in raw_clusters.items(): + if mask is None or mask[(y, x)]: + matrix[(y, x)] = self.idm.trigger_distance - median(values) else: - return ((lower[1] * lower[0] + higher[1] * higher[0]) / - (lower[0] + higher[0])) + matrix[(y, x)] = np.nan + faulty_indexes.append((y, x)) + return matrix, faulty_indexes - for coord in faulty_indexes: - xl = get_nearest(coord, -1, 0) - xh = get_nearest(coord, 1, 0) - xavg = interp_weighted(xl, xh) - yl = get_nearest(coord, 0, -1) - yh = get_nearest(coord, 0, 1) - yavg = interp_weighted(yl, yh) - avg = None - if xavg is not None and yavg is None: - avg = xavg - elif xavg is None and yavg is not None: - avg = yavg - else: - avg = (xavg + yavg) / 2.0 - clusters[coord] = [avg] + def _load_interpolator(self): + if not self.scipy: + try: + self.scipy = importlib.import_module("scipy") + except ImportError: + msg = ( + "Could not load `scipy`. To install it, simply re-run " + "the IDM `install.sh` script. This module is required " + "when using faulty regions when bed meshing." + ) + return (True, msg) + if hasattr(self.scipy.interpolate, "RBFInterpolator"): - return clusters + def rbf_interp(points, values, faulty): + return self.scipy.interpolate.RBFInterpolator(points, values, 64)( + faulty + ) - def _generate_matrix(self, clusters): - matrix = [] - td = self.idm.trigger_distance + return (False, rbf_interp) + else: + + def linear_interp(points, values, faulty): + return self.scipy.interpolate.griddata( + points, values, faulty, method="linear" + ) + + return (False, linear_interp) + + def _interpolate_faulty(self, matrix, faulty_indexes, interpolator): + ys, xs = np.mgrid[0 : matrix.shape[0], 0 : matrix.shape[1]] + points = np.array([ys.flatten(), xs.flatten()]).T + values = matrix.reshape(-1) + good = ~np.isnan(values) + fixed = interpolator(points[good], values[good], faulty_indexes) + matrix[tuple(np.array(faulty_indexes).T)] = fixed + return matrix + + def _check_matrix(self, matrix): empty_clusters = [] for yi in range(self.res_y): - line = [] for xi in range(self.res_x): - cluster = clusters.get((xi, yi), None) - if cluster is None or len(cluster) == 0: + if np.isnan(matrix[(yi, xi)]): xc = xi * self.step_x + self.min_x yc = yi * self.step_y + self.min_y empty_clusters.append(" (%.3f,%.3f)[%d,%d]" % (xc, yc, xi, yi)) - else: - data = [td - d for d in cluster] - line.append(median(data)) - matrix.append(line) if empty_clusters: err = ( "Empty clusters found\n" "Try increasing mesh cluster_size or slowing down.\n" "The following clusters were empty:\n" ) + "\n".join(empty_clusters) - return (True, err) + return err + else: + return None + def _finalize_matrix(self, matrix): z_offset = None if self.zero_ref_mode and self.zero_ref_mode[0] == "rri": rri = self.zero_ref_mode[1] @@ -1924,13 +1950,12 @@ class IDMMeshHelper: rri_y = int(math.floor(rri / self.res_x)) z_offset = matrix[rri_y][rri_x] elif self.zero_ref_mode and self.zero_ref_mode[0] == "pos": - z_offset = td - self.zero_ref_val + z_offset = self.idm.trigger_distance - self.zero_ref_val if z_offset is not None: - for i, line in enumerate(matrix): - matrix[i] = [z - z_offset for z in line] - return (False, matrix) - + matrix = matrix - z_offset + return matrix.tolist() + def _apply_mesh(self, matrix, gcmd): params = self.bm.bmc.mesh_config params["min_x"] = self.min_x diff --git a/install.sh b/install.sh index 89af6f9..8930674 100644 --- a/install.sh +++ b/install.sh @@ -12,9 +12,6 @@ fi # install idm requirements to env echo "idm: installing python requirements to env, this may take 10+ minutes." -sudo apt-get install g++ -sudo apt-get install gfortran -sudo apt-get install libopenblas-dev "${KENV}/bin/pip" install -r "${BKDIR}/requirements.txt" # update link to idm.py diff --git a/requirements.txt b/requirements.txt index 66690cf..447f300 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,4 +1,4 @@ # klipper python virtual environment requirements for IDM numpy>=1.16.6 -scipy>=1.10.0 +scipy>=1.2.3 matplotlib>=3.7.0 \ No newline at end of file