mirror of https://github.com/Desuuuu/klipper.git
angle: Initial support for angle sensor calibration
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
74937326d3
commit
aec742ece4
|
@ -354,7 +354,8 @@ A request may look like:
|
||||||
and might return:
|
and might return:
|
||||||
`{"id": 123,"result":{"header":["time","angle"]}}`
|
`{"id": 123,"result":{"header":["time","angle"]}}`
|
||||||
and might later produce asynchronous messages such as:
|
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 "header" field in the initial query response is used to describe
|
||||||
the fields found in later "data" responses.
|
the fields found in later "data" responses.
|
||||||
|
|
|
@ -4007,6 +4007,7 @@ Magnetic hall angle sensor support for reading stepper motor angle
|
||||||
shaft measurements using a1333, as5047d, or tle5012b SPI chips. The
|
shaft measurements using a1333, as5047d, or tle5012b SPI chips. The
|
||||||
measurements are available via the [API Server](API_Server.md) and
|
measurements are available via the [API Server](API_Server.md) and
|
||||||
[motion analysis tool](Debugging.md#motion-analysis-and-data-logging).
|
[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]
|
[angle my_angle_sensor]
|
||||||
|
@ -4017,6 +4018,12 @@ sensor_type:
|
||||||
#sample_period: 0.000400
|
#sample_period: 0.000400
|
||||||
# The query period (in seconds) to use during measurements. The
|
# The query period (in seconds) to use during measurements. The
|
||||||
# default is 0.000400 (which is 2500 samples per second).
|
# 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:
|
cs_pin:
|
||||||
# The SPI enable pin for the sensor. This parameter must be provided.
|
# The SPI enable pin for the sensor. This parameter must be provided.
|
||||||
#spi_speed:
|
#spi_speed:
|
||||||
|
|
|
@ -106,6 +106,27 @@ VAL=<value>`: Writes raw "value" into a register "register". Both
|
||||||
"value" and "register" can be a decimal or a hexadecimal integer. Use
|
"value" and "register" can be a decimal or a hexadecimal integer. Use
|
||||||
with care, and refer to ADXL345 data sheet for the reference.
|
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=<chip_name>`: 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]
|
### [bed_mesh]
|
||||||
|
|
||||||
The following commands are available when the
|
The following commands are available when the
|
||||||
|
|
|
@ -3,12 +3,261 @@
|
||||||
# Copyright (C) 2021,2022 Kevin O'Connor <kevin@koconnor.net>
|
# Copyright (C) 2021,2022 Kevin O'Connor <kevin@koconnor.net>
|
||||||
#
|
#
|
||||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
# 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
|
from . import bus, motion_report
|
||||||
|
|
||||||
MIN_MSG_TIME = 0.100
|
MIN_MSG_TIME = 0.100
|
||||||
TCODE_ERROR = 0xff
|
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<<ANGLE_BITS)
|
||||||
|
angle_mpos = angle_pos * angle_to_mcu_pos
|
||||||
|
# Calculate adjustment for stepper phases
|
||||||
|
phase_diff = ((angle_mpos + self.angle_phase_offset * angle_to_mcu_pos)
|
||||||
|
- (mcu_pos + mcu_phase_offset)) % phases
|
||||||
|
if phase_diff > 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:
|
class HelperA1333:
|
||||||
SPI_MODE = 3
|
SPI_MODE = 3
|
||||||
SPI_SPEED = 10000000
|
SPI_SPEED = 10000000
|
||||||
|
@ -51,6 +300,7 @@ class Angle:
|
||||||
self.printer = config.get_printer()
|
self.printer = config.get_printer()
|
||||||
self.sample_period = config.getfloat('sample_period', SAMPLE_PERIOD,
|
self.sample_period = config.getfloat('sample_period', SAMPLE_PERIOD,
|
||||||
above=0.)
|
above=0.)
|
||||||
|
self.calibration = AngleCalibration(config)
|
||||||
# Measurement conversion
|
# Measurement conversion
|
||||||
self.start_clock = self.time_shift = self.sample_ticks = 0
|
self.start_clock = self.time_shift = self.sample_ticks = 0
|
||||||
self.last_sequence = self.last_angle = 0
|
self.last_sequence = self.last_angle = 0
|
||||||
|
@ -148,7 +398,9 @@ class Angle:
|
||||||
samples, error_count = self._extract_samples(raw_samples)
|
samples, error_count = self._extract_samples(raw_samples)
|
||||||
if not samples:
|
if not samples:
|
||||||
return {}
|
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):
|
def _start_measurements(self):
|
||||||
if self.is_measuring():
|
if self.is_measuring():
|
||||||
return
|
return
|
||||||
|
@ -183,6 +435,8 @@ class Angle:
|
||||||
self.api_dump.add_client(web_request)
|
self.api_dump.add_client(web_request)
|
||||||
hdr = ('time', 'angle')
|
hdr = ('time', 'angle')
|
||||||
web_request.send({'header': hdr})
|
web_request.send({'header': hdr})
|
||||||
|
def start_internal_client(self):
|
||||||
|
return self.api_dump.add_internal_client()
|
||||||
|
|
||||||
def load_config_prefix(config):
|
def load_config_prefix(config):
|
||||||
return Angle(config)
|
return Angle(config)
|
||||||
|
|
Loading…
Reference in New Issue