From aec742ece4589aa857926c16649a3be358a57c44 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Sat, 21 Aug 2021 22:05:53 -0400 Subject: [PATCH] angle: Initial support for angle sensor calibration Signed-off-by: Kevin O'Connor --- docs/API_Server.md | 3 +- docs/Config_Reference.md | 7 ++ docs/G-Codes.md | 21 ++++ klippy/extras/angle.py | 258 ++++++++++++++++++++++++++++++++++++++- 4 files changed, 286 insertions(+), 3 deletions(-) diff --git a/docs/API_Server.md b/docs/API_Server.md index 8c3e966e..95f8a8d9 100644 --- a/docs/API_Server.md +++ b/docs/API_Server.md @@ -354,7 +354,8 @@ A request may look like: and might return: `{"id": 123,"result":{"header":["time","angle"]}}` and might later produce asynchronous messages such as: -`{"params":{"errors":0,"data":[[1290.951905,-5063],[1290.952321,-5065]]}}` +`{"params":{"position_offset":3.151562,"errors":0, +"data":[[1290.951905,-5063],[1290.952321,-5065]]}}` The "header" field in the initial query response is used to describe the fields found in later "data" responses. diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index 0add9aed..7fca3797 100644 --- a/docs/Config_Reference.md +++ b/docs/Config_Reference.md @@ -4007,6 +4007,7 @@ Magnetic hall angle sensor support for reading stepper motor angle shaft measurements using a1333, as5047d, or tle5012b SPI chips. The measurements are available via the [API Server](API_Server.md) and [motion analysis tool](Debugging.md#motion-analysis-and-data-logging). +See the [G-Code reference](G-Codes.md#angle) for available commands. ``` [angle my_angle_sensor] @@ -4017,6 +4018,12 @@ sensor_type: #sample_period: 0.000400 # The query period (in seconds) to use during measurements. The # default is 0.000400 (which is 2500 samples per second). +#stepper: +# The name of the stepper that the angle sensor is attached to (eg, +# "stepper_x"). Setting this value enables an angle calibration +# tool. To use this feature, the Python "numpy" package must be +# installed. The default is to not enable angle calibration for the +# angle sensor. cs_pin: # The SPI enable pin for the sensor. This parameter must be provided. #spi_speed: diff --git a/docs/G-Codes.md b/docs/G-Codes.md index 34234bfa..131ee6e8 100644 --- a/docs/G-Codes.md +++ b/docs/G-Codes.md @@ -106,6 +106,27 @@ VAL=`: Writes raw "value" into a register "register". Both "value" and "register" can be a decimal or a hexadecimal integer. Use with care, and refer to ADXL345 data sheet for the reference. +### [angle] + +The following commands are available when an +[angle config section](Config_Reference.md#angle) is enabled. + +#### ANGLE_CALIBRATE +`ANGLE_CALIBRATE CHIP=`: Perform angle calibration on the +given sensor (there must be an `[angle chip_name]` config section that +has specified a `stepper` parameter). IMPORTANT - this tool will +command the stepper motor to move without checking the normal +kinematic boundary limits. Ideally the motor should be disconnected +from any printer carriage before performing calibration. If the +stepper can not be disconnected from the printer, make sure the +carriage is near the center of its rail before starting calibration. +(The stepper motor may move forwards or backwards two full rotations +during this test.) After completing this test use the `SAVE_CONFIG` +command to save the calibration data to the config file. In order to +use this tool the Python "numpy" package must be installed (see the +[measuring resonance document](Measuring_Resonances.md#software-installation) +for more information). + ### [bed_mesh] The following commands are available when the diff --git a/klippy/extras/angle.py b/klippy/extras/angle.py index 43d4fd1d..1eec3cf4 100644 --- a/klippy/extras/angle.py +++ b/klippy/extras/angle.py @@ -3,12 +3,261 @@ # Copyright (C) 2021,2022 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. -import logging, threading +import logging, math, threading from . import bus, motion_report MIN_MSG_TIME = 0.100 TCODE_ERROR = 0xff +TRINAMIC_DRIVERS = ["tmc2130", "tmc2208", "tmc2209", "tmc2660", "tmc5160"] + +CALIBRATION_BITS = 6 # 64 entries +ANGLE_BITS = 16 # angles range from 0..65535 + +class AngleCalibration: + def __init__(self, config): + self.printer = config.get_printer() + self.name = config.get_name() + self.stepper_name = config.get('stepper', None) + if self.stepper_name is None: + # No calibration + return + try: + import numpy + except: + raise config.error("Angle calibration requires numpy module") + sconfig = config.getsection(self.stepper_name) + sconfig.getint('microsteps', note_valid=False) + self.tmc_module = self.mcu_stepper = None + # Current calibration data + self.mcu_pos_offset = None + self.angle_phase_offset = 0. + self.calibration_reversed = False + self.calibration = [] + cal = config.get('calibrate', None) + if cal is not None: + data = [d.strip() for d in cal.split(',')] + angles = [float(d) for d in data if d] + self.load_calibration(angles) + # Register commands + self.printer.register_event_handler("stepper:sync_mcu_position", + self.handle_sync_mcu_pos) + self.printer.register_event_handler("klippy:connect", self.connect) + cname = self.name.split()[-1] + gcode = self.printer.lookup_object('gcode') + gcode.register_mux_command("ANGLE_CALIBRATE", "CHIP", + cname, self.cmd_ANGLE_CALIBRATE, + desc=self.cmd_ANGLE_CALIBRATE_help) + def handle_sync_mcu_pos(self, mcu_stepper): + if mcu_stepper.get_name() == self.stepper_name: + self.mcu_pos_offset = None + def calc_mcu_pos_offset(self, sample): + # Lookup phase information + mcu_phase_offset, phases = self.tmc_module.get_phase_offset() + if mcu_phase_offset is None: + return + # Find mcu position at time of sample + angle_time, angle_pos = sample + mcu_pos = self.mcu_stepper.get_past_mcu_position(angle_time) + # Convert angle_pos to mcu_pos units + microsteps, full_steps = self.get_microsteps() + angle_to_mcu_pos = full_steps * microsteps / float(1< phases//2: + phase_diff -= phases + # Store final offset + self.mcu_pos_offset = mcu_pos - (angle_mpos - phase_diff) + def apply_calibration(self, samples): + calibration = self.calibration + if not calibration: + return None + calibration_reversed = self.calibration_reversed + interp_bits = ANGLE_BITS - CALIBRATION_BITS + interp_mask = (1 << interp_bits) - 1 + interp_round = 1 << (interp_bits - 1) + for i, (samp_time, angle) in enumerate(samples): + bucket = (angle & 0xffff) >> interp_bits + cal1 = calibration[bucket] + cal2 = calibration[bucket + 1] + adj = (angle & interp_mask) * (cal2 - cal1) + adj = cal1 + ((adj + interp_round) >> interp_bits) + angle_diff = (angle - adj) & 0xffff + angle_diff -= (angle_diff & 0x8000) << 1 + new_angle = angle - angle_diff + if calibration_reversed: + new_angle = -new_angle + samples[i] = (samp_time, new_angle) + if self.mcu_pos_offset is None: + self.calc_mcu_pos_offset(samples[0]) + if self.mcu_pos_offset is None: + return None + return self.mcu_stepper.mcu_to_commanded_position(self.mcu_pos_offset) + def load_calibration(self, angles): + # Calculate linear intepolation calibration buckets by solving + # linear equations + angle_max = 1 << ANGLE_BITS + calibration_count = 1 << CALIBRATION_BITS + bucket_size = angle_max // calibration_count + full_steps = len(angles) + nominal_step = float(angle_max) / full_steps + self.angle_phase_offset = (angles.index(min(angles)) & 3) * nominal_step + self.calibration_reversed = angles[-2] > angles[-1] + if self.calibration_reversed: + angles = list(reversed(angles)) + first_step = angles.index(min(angles)) + angles = angles[first_step:] + angles[:first_step] + import numpy + eqs = numpy.zeros((full_steps, calibration_count)) + ans = numpy.zeros((full_steps,)) + for step, angle in enumerate(angles): + int_angle = int(angle + .5) % angle_max + bucket = int(int_angle / bucket_size) + bucket_start = bucket * bucket_size + ang_diff = angle - bucket_start + ang_diff_per = ang_diff / bucket_size + eq = eqs[step] + eq[bucket] = 1. - ang_diff_per + eq[(bucket + 1) % calibration_count] = ang_diff_per + ans[step] = float(step * nominal_step) + if bucket + 1 >= calibration_count: + ans[step] -= ang_diff_per * angle_max + sol = numpy.linalg.lstsq(eqs, ans, rcond=None)[0] + isol = [int(s + .5) for s in sol] + self.calibration = isol + [isol[0] + angle_max] + def lookup_tmc(self): + for driver in TRINAMIC_DRIVERS: + driver_name = "%s %s" % (driver, self.stepper_name) + module = self.printer.lookup_object(driver_name, None) + if module is not None: + return module + raise self.printer.command_error("Unable to find TMC driver for %s" + % (self.stepper_name,)) + def connect(self): + self.tmc_module = self.lookup_tmc() + fmove = self.printer.lookup_object('force_move') + self.mcu_stepper = fmove.lookup_stepper(self.stepper_name) + def get_microsteps(self): + configfile = self.printer.lookup_object('configfile') + sconfig = configfile.get_status(None)['settings'] + stconfig = sconfig.get(self.stepper_name, {}) + microsteps = stconfig['microsteps'] + full_steps = stconfig['full_steps_per_rotation'] + return microsteps, full_steps + def get_stepper_phase(self): + mcu_phase_offset, phases = self.tmc_module.get_phase_offset() + if mcu_phase_offset is None: + raise self.printer.command_error("Driver phase not known for %s" + % (self.stepper_name,)) + mcu_pos = self.mcu_stepper.get_mcu_position() + return (mcu_pos + mcu_phase_offset) % phases + def do_calibration_moves(self): + move = self.printer.lookup_object('force_move').manual_move + # Start data collection + angle_sensor = self.printer.lookup_object(self.name) + cconn = angle_sensor.start_internal_client() + # Move stepper several turns (to allow internal sensor calibration) + microsteps, full_steps = self.get_microsteps() + mcu_stepper = self.mcu_stepper + step_dist = mcu_stepper.get_step_dist() + full_step_dist = step_dist * microsteps + rotation_dist = full_steps * full_step_dist + align_dist = step_dist * self.get_stepper_phase() + move_time = 0.010 + move_speed = full_step_dist / move_time + move(mcu_stepper, -(rotation_dist+align_dist), move_speed) + move(mcu_stepper, 2. * rotation_dist, move_speed) + move(mcu_stepper, -2. * rotation_dist, move_speed) + move(mcu_stepper, .5 * rotation_dist - full_step_dist, move_speed) + # Move to each full step position + toolhead = self.printer.lookup_object('toolhead') + times = [] + samp_dist = full_step_dist + for i in range(2 * full_steps): + move(mcu_stepper, samp_dist, move_speed) + start_query_time = toolhead.get_last_move_time() + 0.050 + end_query_time = start_query_time + 0.050 + times.append((start_query_time, end_query_time)) + toolhead.dwell(0.150) + if i == full_steps-1: + # Reverse direction and test each full step again + move(mcu_stepper, .5 * rotation_dist, move_speed) + move(mcu_stepper, -.5 * rotation_dist + samp_dist, move_speed) + samp_dist = -samp_dist + move(mcu_stepper, .5*rotation_dist + align_dist, move_speed) + toolhead.wait_moves() + # Finish data collection + cconn.finalize() + msgs = cconn.get_messages() + # Correlate query responses + cal = {} + step = 0 + for msg in msgs: + for query_time, pos in msg['params']['data']: + # Add to step tracking + while step < len(times) and query_time > times[step][1]: + step += 1 + if step < len(times) and query_time >= times[step][0]: + cal.setdefault(step, []).append(pos) + if len(cal) != len(times): + raise self.printer.command_error( + "Failed calibration - incomplete sensor data") + fcal = { i: cal[i] for i in range(full_steps) } + rcal = { full_steps-i-1: cal[i+full_steps] for i in range(full_steps) } + return fcal, rcal + def calc_angles(self, meas): + total_count = total_variance = 0 + angles = {} + for step, data in meas.items(): + count = len(data) + angle_avg = float(sum(data)) / count + angles[step] = angle_avg + total_count += count + total_variance += sum([(d - angle_avg)**2 for d in data]) + return angles, math.sqrt(total_variance / total_count), total_count + cmd_ANGLE_CALIBRATE_help = "Calibrate angle sensor to stepper motor" + def cmd_ANGLE_CALIBRATE(self, gcmd): + # Perform calibration movement and capture + old_calibration = self.calibration + self.calibration = [] + try: + fcal, rcal = self.do_calibration_moves() + finally: + self.calibration = old_calibration + # Calculate each step position average and variance + microsteps, full_steps = self.get_microsteps() + fangles, fstd, ftotal = self.calc_angles(fcal) + rangles, rstd, rtotal = self.calc_angles(rcal) + if (len({a: i for i, a in fangles.items()}) != len(fangles) + or len({a: i for i, a in rangles.items()}) != len(rangles)): + raise self.printer.command_error( + "Failed calibration - sensor not updating for each step") + merged = { i: fcal[i] + rcal[i] for i in range(full_steps) } + angles, std, total = self.calc_angles(merged) + gcmd.respond_info("angle: stddev=%.3f (%.3f forward / %.3f reverse)" + " in %d queries" % (std, fstd, rstd, total)) + # Order data with lowest/highest magnet position first + anglist = [angles[i] % 0xffff for i in range(full_steps)] + if angles[0] > angles[1]: + first_ang = max(anglist) + else: + first_ang = min(anglist) + first_phase = anglist.index(first_ang) & ~3 + anglist = anglist[first_phase:] + anglist[:first_phase] + # Save results + cal_contents = [] + for i, angle in enumerate(anglist): + if not i % 8: + cal_contents.append('\n') + cal_contents.append("%.1f" % (angle,)) + cal_contents.append(',') + cal_contents.pop() + configfile = self.printer.lookup_object('configfile') + configfile.remove_section(self.name) + configfile.set(self.name, 'calibrate', ''.join(cal_contents)) + class HelperA1333: SPI_MODE = 3 SPI_SPEED = 10000000 @@ -51,6 +300,7 @@ class Angle: self.printer = config.get_printer() self.sample_period = config.getfloat('sample_period', SAMPLE_PERIOD, above=0.) + self.calibration = AngleCalibration(config) # Measurement conversion self.start_clock = self.time_shift = self.sample_ticks = 0 self.last_sequence = self.last_angle = 0 @@ -148,7 +398,9 @@ class Angle: samples, error_count = self._extract_samples(raw_samples) if not samples: return {} - return {'data': samples, 'errors': error_count} + offset = self.calibration.apply_calibration(samples) + return {'data': samples, 'errors': error_count, + 'position_offset': offset} def _start_measurements(self): if self.is_measuring(): return @@ -183,6 +435,8 @@ class Angle: self.api_dump.add_client(web_request) hdr = ('time', 'angle') web_request.send({'header': hdr}) + def start_internal_client(self): + return self.api_dump.add_internal_client() def load_config_prefix(config): return Angle(config)