reijii/IDM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

merge

Browse Source
This commit is contained in:
Sgr A* VMT
2023-11-23 23:03:49 +08:00
parent 9e44086903
commit 68d4a2877f
1 changed files with 156 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

184
idm.py
View File

@@ -1,4 +1,3 @@
# Based upon the fantastic Beacon eddy current scanner support
# #
# Copyright (C) 2020-2023 Matt Baker <baker.matt.j@gmail.com> # Copyright (C) 2020-2023 Matt Baker <baker.matt.j@gmail.com>
# Copyright (C) 2020-2023 Lasse Dalegaard <dalegaard@gmail.com> # Copyright (C) 2020-2023 Lasse Dalegaard <dalegaard@gmail.com>
@@ -26,6 +25,7 @@ from mcu import MCU, MCU_trsync
from clocksync import SecondarySync from clocksync import SecondarySync
STREAM_BUFFER_LIMIT_DEFAULT = 100 STREAM_BUFFER_LIMIT_DEFAULT = 100
STREAM_TIMEOUT = 1.0
class IDMProbe: class IDMProbe:
def __init__(self, config): def __init__(self, config):
@@ -44,6 +44,7 @@ class IDMProbe:
self.trigger_dive_threshold = config.getfloat( self.trigger_dive_threshold = config.getfloat(
'trigger_dive_threshold', 1.) 'trigger_dive_threshold', 1.)
self.trigger_hysteresis = config.getfloat('trigger_hysteresis', 0.006) self.trigger_hysteresis = config.getfloat('trigger_hysteresis', 0.006)
self.z_settling_time = config.getint("z_settling_time", 5, minval=0)
# If using paper for calibration, this would be .1mm # If using paper for calibration, this would be .1mm
self.cal_nozzle_z = config.getfloat('cal_nozzle_z', 0.1) self.cal_nozzle_z = config.getfloat('cal_nozzle_z', 0.1)
@@ -57,6 +58,7 @@ class IDMProbe:
self.models = {} self.models = {}
self.model_temp_builder = IDMTempModelBuilder.load(config) self.model_temp_builder = IDMTempModelBuilder.load(config)
self.model_temp = None self.model_temp = None
self.fmin = None
self.default_model_name = config.get('default_model_name', 'default') self.default_model_name = config.get('default_model_name', 'default')
self.model_manager = ModelManager(self) self.model_manager = ModelManager(self)
@@ -66,10 +68,13 @@ class IDMProbe:
self.measured_max = 0. self.measured_max = 0.
self.last_sample = None self.last_sample = None
self.hardware_failure = None
self.mesh_helper = IDMMeshHelper.create(self, config) self.mesh_helper = IDMMeshHelper.create(self, config)
self._stream_en = 0 self._stream_en = 0
self._stream_timeout_timer = self.reactor.register_timer(
self._stream_timeout)
self._stream_callbacks = {} self._stream_callbacks = {}
self._stream_latency_requests = {} self._stream_latency_requests = {}
self._stream_buffer = [] self._stream_buffer = []
@@ -133,6 +138,8 @@ class IDMProbe:
self.idm_stream_cmd.send([0]) self.idm_stream_cmd.send([0])
self.model_temp = self.model_temp_builder.build_with_base(self) self.model_temp = self.model_temp_builder.build_with_base(self)
if self.model_temp:
self.fmin = self.model_temp.fmin
self.model = self.models.get(self.default_model_name, None) self.model = self.models.get(self.default_model_name, None)
if self.model: if self.model:
self._apply_threshold() self._apply_threshold()
@@ -141,6 +148,7 @@ class IDMProbe:
constants = self._mcu.get_constants() constants = self._mcu.get_constants()
self.sensor_freq = self._mcu._mcu_freq if self._mcu._mcu_freq < 20000000 else self._mcu._mcu_freq/2 self.sensor_freq = self._mcu._mcu_freq if self._mcu._mcu_freq < 20000000 else self._mcu._mcu_freq/2
self.inv_adc_max = 1.0 / constants.get("ADC_MAX") self.inv_adc_max = 1.0 / constants.get("ADC_MAX")
self.temp_smooth_count = constants.get('IDM_ADC_SMOOTH_COUNT') self.temp_smooth_count = constants.get('IDM_ADC_SMOOTH_COUNT')
self.thermistor = thermistor.Thermistor(10000., 0.) self.thermistor = thermistor.Thermistor(10000., 0.)
@@ -197,6 +205,7 @@ class IDMProbe:
raise self.printer.command_error("No IDM model loaded") raise self.printer.command_error("No IDM model loaded")
speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.) speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.)
allow_faulty = gcmd.get_int('ALLOW_FAULTY_COORDINATE', 0) != 0
lift_speed = self.get_lift_speed(gcmd) lift_speed = self.get_lift_speed(gcmd)
toolhead = self.printer.lookup_object('toolhead') toolhead = self.printer.lookup_object('toolhead')
curtime = self.reactor.monotonic() curtime = self.reactor.monotonic()
@@ -205,7 +214,7 @@ class IDMProbe:
self._start_streaming() self._start_streaming()
try: try:
return self._probe(speed) return self._probe(speed, allow_faulty=allow_faulty)
finally: finally:
self._stop_streaming() self._stop_streaming()
@@ -225,32 +234,40 @@ class IDMProbe:
pos[2] = status['axis_minimum'][2] pos[2] = status['axis_minimum'][2]
try: try:
self.phoming.probing_move(self.mcu_probe, pos, speed) self.phoming.probing_move(self.mcu_probe, pos, speed)
samples = self._sample_printtime_sync(50) self._sample_printtime_sync(self.z_settling_time)
except self.printer.command_error as e: except self.printer.command_error as e:
reason = str(e) reason = str(e)
if "Timeout during probing move" in reason: if "Timeout during probing move" in reason:
reason += probe.HINT_TIMEOUT reason += probe.HINT_TIMEOUT
raise self.printer.command_error(reason) raise self.printer.command_error(reason)
def _sample(self, num_samples): def _probe(self, speed, num_samples=10, allow_faulty=False):
samples = self._sample_printtime_sync(5, num_samples)
return (median([s['dist'] for s in samples]), samples)
def _probe(self, speed, num_samples=10):
target = self.trigger_distance target = self.trigger_distance
tdt = self.trigger_dive_threshold tdt = self.trigger_dive_threshold
(dist, samples) = self._sample(num_samples) (dist, samples) = self._sample(5, num_samples)
x, y = samples[0]['pos'][0:2]
if self._is_faulty_coordinate(x, y, True):
msg = "Probing within a faulty area"
if not allow_faulty:
raise self.printer.command_error(msg)
else:
self.gcode.respond_raw("!! " + msg + "\n")
if dist > target + tdt: if dist > target + tdt:
# If we are above the dive threshold right now, we'll need to # If we are above the dive threshold right now, we'll need to
# do probing move and then re-measure # do probing move and then re-measure
self._probing_move_to_probing_height(speed) self._probing_move_to_probing_height(speed)
(dist, samples) = self._sample(num_samples) (dist, samples) = self._sample(self.z_settling_time, num_samples)
elif math.isinf(dist) and dist < 0:
# We were below the valid range of the model
msg = "Attempted to probe with Beacon below calibrated model range"
raise self.printer.command_error(msg)
elif self.toolhead.get_position()[2] < target - tdt: elif self.toolhead.get_position()[2] < target - tdt:
# We are below the probing target height, we'll move to the # We are below the probing target height, we'll move to the
# correct height and take a new sample. # correct height and take a new sample.
self._move_to_probing_height(speed) self._move_to_probing_height(speed)
(dist, samples) = self._sample(num_samples) (dist, samples) = self._sample(self.z_settling_time, num_samples)
pos = samples[0]['pos'] pos = samples[0]['pos']
@@ -262,11 +279,19 @@ class IDMProbe:
# Calibration routines # Calibration routines
def _start_calibration(self, gcmd): def _start_calibration(self, gcmd):
allow_faulty = gcmd.get_int('ALLOW_FAULTY_COORDINATE', 0) != 0
if gcmd.get("SKIP_MANUAL_PROBE", None) is not None: if gcmd.get("SKIP_MANUAL_PROBE", None) is not None:
kin = self.toolhead.get_kinematics() kin = self.toolhead.get_kinematics()
kin_spos = {s.get_name(): s.get_commanded_position() kin_spos = {s.get_name(): s.get_commanded_position()
for s in kin.get_steppers()} for s in kin.get_steppers()}
kin_pos = kin.calc_position(kin_spos) kin_pos = kin.calc_position(kin_spos)
if self._is_faulty_coordinate(kin_pos[0], kin_pos[1]):
msg = "Calibrating within a faulty area"
if not allow_faulty:
raise gcmd.error(msg)
else:
gcmd.respond_raw("!! " + msg + "\n")
self._calibrate(gcmd, kin_pos, False) self._calibrate(gcmd, kin_pos, False)
else: else:
curtime = self.printer.get_reactor().monotonic() curtime = self.printer.get_reactor().monotonic()
@@ -275,6 +300,14 @@ class IDMProbe:
raise self.printer.command_error("Must home X and Y " raise self.printer.command_error("Must home X and Y "
"before calibration") "before calibration")
kin_pos = self.toolhead.get_position()
if self._is_faulty_coordinate(kin_pos[0], kin_pos[1]):
msg = "Calibrating within a faulty area"
if not allow_faulty:
raise gcmd.error(msg)
else:
gcmd.respond_raw("!! " + msg + "\n")
forced_z = False forced_z = False
if 'z' not in kin_status['homed_axes']: if 'z' not in kin_status['homed_axes']:
self.toolhead.get_last_move_time() self.toolhead.get_last_move_time()
@@ -309,6 +342,7 @@ class IDMProbe:
# Move over to probe coordinate and pull out backlash # Move over to probe coordinate and pull out backlash
curpos = self.toolhead.get_position() curpos = self.toolhead.get_position()
curpos[2] = cal_max_z + self.backlash_comp curpos[2] = cal_max_z + self.backlash_comp
toolhead.manual_move(curpos, move_speed) # Up toolhead.manual_move(curpos, move_speed) # Up
curpos[0] -= self.x_offset curpos[0] -= self.x_offset
@@ -386,7 +420,30 @@ class IDMProbe:
"name '%s'" % (name,)) "name '%s'" % (name,))
self.models[name] = model self.models[name] = model
# Streaming mode def _is_faulty_coordinate(self, x, y, add_offsets=False):
if not self.mesh_helper:
return False
return self.mesh_helper._is_faulty_coordinate(x, y, add_offsets)
# Streaming mode
def _check_hardware(self, sample):
if not self.hardware_failure:
msg = None
if sample['data'] == 0xFFFFFFF:
msg = "coil is shorted or not connected"
elif self.fmin is not None and sample['freq'] > 1.35 * self.fmin:
msg = "coil expected max frequency exceeded"
if msg:
msg = "IDM hardware issue: " + msg
self.hardware_failure = msg
logging.error(msg)
if self._stream_en:
self.printer.invoke_shutdown(msg)
else:
self.gcode.respond_raw("!! " + msg + "\n")
elif self._stream_en:
self.printer.invoke_shutdown(self.hardware_failure)
def _enrich_sample_time(self, sample): def _enrich_sample_time(self, sample):
clock = sample['clock'] = self._mcu.clock32_to_clock64(sample['clock']) clock = sample['clock'] = self._mcu.clock32_to_clock64(sample['clock'])
@@ -396,10 +453,13 @@ class IDMProbe:
temp_adc = sample['temp'] / self.temp_smooth_count * self.inv_adc_max temp_adc = sample['temp'] / self.temp_smooth_count * self.inv_adc_max
sample['temp'] = self.thermistor.calc_temp(temp_adc) sample['temp'] = self.thermistor.calc_temp(temp_adc)
def _enrich_sample(self, sample): def _enrich_sample_freq(self, sample):
sample['data_smooth'] = self._data_filter.value() sample['data_smooth'] = self._data_filter.value()
freq = sample['freq'] = self.count_to_freq(sample['data_smooth']) sample['freq'] = self.count_to_freq(sample['data_smooth'])
sample['dist'] = self.freq_to_dist(freq, sample['temp']) 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']) pos, vel = self._get_trapq_position(sample['time'])
if pos is None: if pos is None:
@@ -410,15 +470,28 @@ class IDMProbe:
def _start_streaming(self): def _start_streaming(self):
if self._stream_en == 0: if self._stream_en == 0:
self.idm_stream_cmd.send([1]) self.idm_stream_cmd.send([1])
curtime = self.reactor.monotonic()
self.reactor.update_timer(self._stream_timeout_timer,
curtime + STREAM_TIMEOUT)
self._stream_en += 1 self._stream_en += 1
self._data_filter.reset() self._data_filter.reset()
self._stream_flush() self._stream_flush()
def _stop_streaming(self): def _stop_streaming(self):
self._stream_en -= 1 self._stream_en -= 1
if self._stream_en == 0: if self._stream_en == 0:
self.reactor.update_timer(self._stream_timeout_timer,
self.reactor.NEVER)
self.idm_stream_cmd.send([0]) self.idm_stream_cmd.send([0])
self._stream_flush() self._stream_flush()
def _stream_timeout(self, eventtime):
if not self._stream_en:
return self.reactor.NEVER
msg = "Beacon sensor not receiving data"
logging.error(msg)
self.printer.invoke_shutdown(msg)
return self.reactor.NEVER
def request_stream_latency(self, latency): def request_stream_latency(self, latency):
next_key = 0 next_key = 0
if self._stream_latency_requests: if self._stream_latency_requests:
@@ -452,10 +525,23 @@ class IDMProbe:
while True: while True:
try: try:
samples = self._stream_samples_queue.get_nowait() samples = self._stream_samples_queue.get_nowait()
updated_timer = False
for sample in samples: for sample in samples:
self._enrich_sample_temp(sample) if not updated_timer:
curtime = self.reactor.monotonic()
self.reactor.update_timer(self._stream_timeout_timer,
curtime + STREAM_TIMEOUT)
updated_timer = True
self._enrich_sample_temp(sample)
temp = sample['temp'] temp = sample['temp']
if self.model_temp is not None and not (-40 < temp < 180):
msg = ("IDM temperature sensor faulty(read %.2f C),"
" disabling temperaure compensation" % (temp,))
logging.error(msg)
self.gcode.respond_raw("!! " + msg + "\n")
self.model_temp = None
self.last_temp = temp self.last_temp = temp
if temp: if temp:
self.measured_min = min(self.measured_min, temp) self.measured_min = min(self.measured_min, temp)
@@ -463,10 +549,11 @@ class IDMProbe:
self._enrich_sample_time(sample) self._enrich_sample_time(sample)
self._data_filter.update(sample['time'], sample['data']) self._data_filter.update(sample['time'], sample['data'])
self._enrich_sample_freq(sample)
if len(self._stream_callbacks) > 0: if len(self._stream_callbacks) > 0:
self._enrich_sample(sample) self._enrich_sample(sample)
for cb in self._stream_callbacks.values(): for cb in list(self._stream_callbacks.values()):
cb(sample) cb(sample)
except queue.Empty: except queue.Empty:
return return
@@ -529,6 +616,10 @@ class IDMProbe:
else: else:
return samples return samples
def _sample(self, skip, count):
samples = self._sample_printtime_sync(skip, count)
return (median([s['dist'] for s in samples]), samples)
def _sample_async(self, count=1): def _sample_async(self, count=1):
samples = [] samples = []
def cb(sample): def cb(sample):
@@ -538,6 +629,7 @@ class IDMProbe:
with self.streaming_session(cb, latency=count) as ss: with self.streaming_session(cb, latency=count) as ss:
ss.wait() ss.wait()
if count == 1: if count == 1:
return samples[0] return samples[0]
else: else:
@@ -558,11 +650,16 @@ class IDMProbe:
if self.model is None: if self.model is None:
return None return None
return self.model.freq_to_dist(freq, temp) return self.model.freq_to_dist(freq, temp)
def get_status(self, eventtime): def get_status(self, eventtime):
model = None
if self.model is not None:
model = self.model.name
return { return {
'last_sample': self.last_sample 'last_sample': self.last_sample,
'model': model,
} }
# Webhook handlers # Webhook handlers
def _handle_req_status(self, web_request): def _handle_req_status(self, web_request):
@@ -604,6 +701,7 @@ class IDMProbe:
target = gcmd.get_float('Z', self.trigger_distance) target = gcmd.get_float('Z', self.trigger_distance)
num_samples = gcmd.get_int('SAMPLES', 20) num_samples = gcmd.get_int('SAMPLES', 20)
wait = self.z_settling_time
samples_up = [] samples_up = []
samples_down = [] samples_down = []
@@ -613,7 +711,7 @@ class IDMProbe:
try: try:
self._start_streaming() self._start_streaming()
(cur_dist, _samples) = self._sample(10) (cur_dist, _samples) = self._sample(wait, 10)
pos = self.toolhead.get_position() pos = self.toolhead.get_position()
missing = target - cur_dist missing = target - cur_dist
target = pos[2] + missing target = pos[2] + missing
@@ -628,7 +726,7 @@ class IDMProbe:
liftpos = [None, None, target] liftpos = [None, None, target]
self.toolhead.manual_move(liftpos, lift_speed) self.toolhead.manual_move(liftpos, lift_speed)
self.toolhead.wait_moves() self.toolhead.wait_moves()
(dist, _samples) = self._sample(10) (dist, _samples) = self._sample(wait, 10)
{-1: samples_up, 1: samples_down}[next_dir].append(dist) {-1: samples_up, 1: samples_down}[next_dir].append(dist)
next_dir = next_dir * -1 next_dir = next_dir * -1
@@ -703,6 +801,7 @@ class IDMProbe:
lift_speed = self.get_lift_speed(gcmd) lift_speed = self.get_lift_speed(gcmd)
sample_count = gcmd.get_int("SAMPLES", 10, minval=1) sample_count = gcmd.get_int("SAMPLES", 10, minval=1)
sample_retract_dist = gcmd.get_float("SAMPLE_RETRACT_DIST", 0) sample_retract_dist = gcmd.get_float("SAMPLE_RETRACT_DIST", 0)
allow_faulty = gcmd.get_int('ALLOW_FAULTY_COORDINATE', 0) != 0
pos = self.toolhead.get_position() pos = self.toolhead.get_position()
gcmd.respond_info("PROBE_ACCURACY at X:%.3f Y:%.3f Z:%.3f" gcmd.respond_info("PROBE_ACCURACY at X:%.3f Y:%.3f Z:%.3f"
" (samples=%d retract=%.3f" " (samples=%d retract=%.3f"
@@ -718,7 +817,7 @@ class IDMProbe:
self.multi_probe_begin() self.multi_probe_begin()
positions = [] positions = []
while len(positions) < sample_count: while len(positions) < sample_count:
pos = self._probe(speed) pos = self._probe(speed, allow_faulty=allow_faulty)
positions.append(pos) positions.append(pos)
self.toolhead.manual_move(liftpos, lift_speed) self.toolhead.manual_move(liftpos, lift_speed)
self.multi_probe_end() self.multi_probe_end()
@@ -806,7 +905,14 @@ class IDMModel:
"the printer." % (self.name,)) "the printer." % (self.name,))
def freq_to_dist_raw(self, freq): def freq_to_dist_raw(self, freq):
return float(self.poly(1/freq) - self.offset) [begin, end] = self.poly.domain
invfreq = 1/freq
if invfreq > end:
return float('inf')
elif invfreq < begin:
return float('-inf')
else:
return float(self.poly(invfreq) - self.offset)
def freq_to_dist(self, freq, temp): def freq_to_dist(self, freq, temp):
if self.temp is not None and \ if self.temp is not None and \
@@ -816,6 +922,10 @@ class IDMModel:
return self.freq_to_dist_raw(freq) return self.freq_to_dist_raw(freq)
def dist_to_freq_raw(self, dist, max_e=0.00000001): def dist_to_freq_raw(self, dist, max_e=0.00000001):
if dist < self.min_z or dist > self.max_z:
msg = ("Attempted to map out-of-range distance %f, valid range "
"[%.3f, %.3f]" % (dist, self.min_z, self.max_z))
raise self.idm.printer.command_error(msg)
dist += self.offset dist += self.offset
[begin, end] = self.poly.domain [begin, end] = self.poly.domain
for _ in range(0, 50): for _ in range(0, 50):
@@ -1172,8 +1282,11 @@ class IDMEndstopWrapper:
# After homing Z we perform a measurement and adjust the toolhead # After homing Z we perform a measurement and adjust the toolhead
# kinematic position. # kinematic position.
samples = self.idm._sample_printtime_sync(5, 10) (dist, samples) = self.idm._sample(self.idm.z_settling_time, 10)
dist = median([s['dist'] for s in samples]) if math.isinf(dist):
logging.error("Post-homing adjustment measured samples %s", samples)
raise self.idm.printer.command_error(
"Toolhead stopped below model range")
homing_state.set_homed_position([None, None, dist]) homing_state.set_homed_position([None, None, dist])
def get_mcu(self): def get_mcu(self):
@@ -1208,6 +1321,7 @@ class IDMEndstopWrapper:
self.is_homing = True self.is_homing = True
self.idm._apply_threshold() self.idm._apply_threshold()
self.idm._sample_async()
clock = self._mcu.print_time_to_clock(print_time) clock = self._mcu.print_time_to_clock(print_time)
rest_ticks = self._mcu.print_time_to_clock(print_time+rest_time) - clock rest_ticks = self._mcu.print_time_to_clock(print_time+rest_time) - clock
self._rest_ticks = rest_ticks self._rest_ticks = rest_ticks
@@ -1473,6 +1587,9 @@ class IDMMeshHelper:
self.toolhead.manual_move([x, y, None], speed) self.toolhead.manual_move([x, y, None], speed)
self.toolhead.wait_moves() self.toolhead.wait_moves()
def _is_valid_position(self, x, y):
return self.min_x <= x <= self.max_x and self.min_y <= y <= self.min_y
def _sample_mesh(self, gcmd, path, speed, runs): def _sample_mesh(self, gcmd, path, speed, runs):
cs = gcmd.get_float("CLUSTER_SIZE", self.cluster_size, minval=0.) cs = gcmd.get_float("CLUSTER_SIZE", self.cluster_size, minval=0.)
@@ -1481,14 +1598,19 @@ class IDMMeshHelper:
clusters = {} clusters = {}
total_samples = [0] total_samples = [0]
invalid_samples = [0]
def cb(sample): def cb(sample):
total_samples[0] += 1 total_samples[0] += 1
d = sample['dist']
(x, y, z) = sample['pos'] (x, y, z) = sample['pos']
x += xo x += xo
y += yo y += yo
d = sample['dist']
if math.isinf(d):
if self._is_valid_position(x, y):
invalid_samples[0] += 1
return
# Calculate coordinate of the cluster we are in # Calculate coordinate of the cluster we are in
xi = int(round((x - min_x) / self.step_x)) xi = int(round((x - min_x) / self.step_x))
@@ -1515,11 +1637,17 @@ class IDMMeshHelper:
gcmd.respond_info("Sampled %d total points over %d runs" % gcmd.respond_info("Sampled %d total points over %d runs" %
(total_samples[0], runs)) (total_samples[0], runs))
if invalid_samples[0]:
gcmd.respond_info("!! Encountered %d invalid samples!" % (invalid_samples[0],))
gcmd.respond_info("Samples binned in %d clusters" % (len(clusters),)) gcmd.respond_info("Samples binned in %d clusters" % (len(clusters),))
return clusters return clusters
def _is_faulty_coordinate(self, x, 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: for r in self.faulty_regions:
if r.is_point_within(x, y): if r.is_point_within(x, y):
return True return True