From 3ccecc568dbfd505fe3bdc46b4d16bf7a4528996 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Mon, 14 Aug 2017 11:46:35 -0400 Subject: [PATCH] mcu: Initial support for multiple micro-controllers Add initial support for controlling multiple independent micro-controllers from a single Klippy host instance. Add basic support for synchronizing the clocks of the additional mcus to the main mcu's clock. Signed-off-by: Kevin O'Connor --- config/example-extras.cfg | 11 ++++ klippy/clocksync.py | 58 +++++++++++++++++++-- klippy/klippy.py | 32 ++++++++---- klippy/mcu.py | 107 ++++++++++++++++++++++++++------------ klippy/toolhead.py | 17 +++--- 5 files changed, 172 insertions(+), 53 deletions(-) diff --git a/config/example-extras.cfg b/config/example-extras.cfg index b3091e77..935a3d52 100644 --- a/config/example-extras.cfg +++ b/config/example-extras.cfg @@ -42,6 +42,17 @@ # the fan is disabled. The default is 50 Celsius. +# Additional micro-controllers (one may define any number of sections +# with an "mcu" prefix). Additional micro-controllers introduce +# additional pins that may be configured as heaters, steppers, fans, +# etc.. For example, if an "[mcu extra_mcu]" section is introduced, +# then pins such as "extra_mcu:ar9" may then be used elsewhere in the +# config (where "ar9" is a hardware pin name or alias name on the +# given mcu). +#[mcu my_extra_mcu] +# See the "mcu" section in example.cfg for configuration parameters. + + # Statically configured digital output pins (one may define any number # of sections with a "static_digital_output" prefix). Pins configured # here will be setup as a GPIO output during MCU configuration. diff --git a/klippy/clocksync.py b/klippy/clocksync.py index 2007841b..a2c217a0 100644 --- a/klippy/clocksync.py +++ b/klippy/clocksync.py @@ -51,6 +51,8 @@ class ClockSync: self.last_clock, self.last_clock_time) def is_active(self, eventtime): return self.queries_pending <= 4 + def calibrate_clock(self, print_time, eventtime): + return (0., self.mcu_freq) def get_clock(self, eventtime): with self.lock: last_clock = self.last_clock @@ -106,9 +108,8 @@ class ClockSync: new_min_freq = ( clock_delta / (sent_time - self.last_clock_time)) logging.warning( - "High clock drift! Now %.0f:%.0f was %.0f:%.0f" % ( - new_min_freq, new_max_freq, - self.min_freq, self.max_freq)) + "High clock drift! Now %.0f:%.0f was %.0f:%.0f", + new_min_freq, new_max_freq, self.min_freq, self.max_freq) self.min_freq, self.max_freq = new_min_freq, new_max_freq min_time, max_time = sent_time, receive_time # Update variables @@ -116,3 +117,54 @@ class ClockSync: self.last_clock_time = max_time self.last_clock_time_min = min_time self.serial.set_clock_est(self.min_freq, max_time + 0.001, clock) + +# Clock synching code for secondary MCUs (whose clocks are sync'ed to +# a primary MCU) +class SecondarySync(ClockSync): + def __init__(self, reactor, main_sync): + ClockSync.__init__(self, reactor) + self.main_sync = main_sync + self.clock_adj = (0., 0.) + def connect(self, serial): + ClockSync.connect(self, serial) + self.clock_adj = (0., self.mcu_freq) + curtime = self.reactor.monotonic() + main_print_time = self.main_sync.estimated_print_time(curtime) + local_print_time = self.estimated_print_time(curtime) + self.clock_adj = (main_print_time - local_print_time, self.mcu_freq) + self.calibrate_clock(0., curtime) + def connect_file(self, serial, pace=False): + ClockSync.connect_file(self, serial, pace) + self.clock_adj = (0., self.mcu_freq) + def print_time_to_clock(self, print_time): + adjusted_offset, adjusted_freq = self.clock_adj + return int((print_time - adjusted_offset) * adjusted_freq) + def clock_to_print_time(self, clock): + adjusted_offset, adjusted_freq = self.clock_adj + return clock / adjusted_freq + adjusted_offset + def get_adjusted_freq(self): + adjusted_offset, adjusted_freq = self.clock_adj + return adjusted_freq + def calibrate_clock(self, print_time, eventtime): + #logging.debug("calibrate: %.3f: %.6f vs %.6f", + # eventtime, + # self.estimated_print_time(eventtime), + # self.main_sync.estimated_print_time(eventtime)) + with self.main_sync.lock: + ser_clock = self.main_sync.last_clock + ser_clock_time = self.main_sync.last_clock_time + ser_freq = self.main_sync.min_freq + main_mcu_freq = self.main_sync.mcu_freq + + main_clock = (eventtime - ser_clock_time) * ser_freq + ser_clock + print_time = max(print_time, main_clock / main_mcu_freq) + main_sync_clock = (print_time + 2.) * main_mcu_freq + sync_time = ser_clock_time + (main_sync_clock - ser_clock) / ser_freq + + print_clock = self.print_time_to_clock(print_time) + sync_clock = self.get_clock(sync_time) + adjusted_freq = .5 * (sync_clock - print_clock) + adjusted_offset = print_time - print_clock / adjusted_freq + + self.clock_adj = (adjusted_offset, adjusted_freq) + return self.clock_adj diff --git a/klippy/klippy.py b/klippy/klippy.py index 9b899940..a40d6da7 100644 --- a/klippy/klippy.py +++ b/klippy/klippy.py @@ -140,7 +140,7 @@ class Printer: self.state_message = message_startup self.run_result = None self.fileconfig = None - self.mcu = None + self.mcus = [] def get_start_args(self): return self.start_args def _stats(self, eventtime, force_output=False): @@ -149,7 +149,7 @@ class Printer: self.gcode.dump_debug() self.need_dump_debug = False toolhead = self.objects.get('toolhead') - if toolhead is None or self.mcu is None: + if toolhead is None: return eventtime + 1. is_active = toolhead.check_active(eventtime) if not is_active and not force_output: @@ -157,7 +157,8 @@ class Printer: out = [] out.append(self.gcode.stats(eventtime)) out.append(toolhead.stats(eventtime)) - out.append(self.mcu.stats(eventtime)) + for m in self.mcus: + out.append(m.stats(eventtime)) logging.info("Stats %.1f: %s" % (eventtime, ' '.join(out))) return eventtime + 1. def add_object(self, name, obj): @@ -175,7 +176,7 @@ class Printer: config = ConfigWrapper(self, 'printer') for m in [pins, mcu, chipmisc, toolhead, extruder, heater, fan]: m.add_printer_objects(self, config) - self.mcu = self.objects['mcu'] + self.mcus = mcu.get_printer_mcus(self) # Validate that there are no undefined parameters in the config file valid_sections = { s: 1 for s, o in self.all_config_options } for section in self.fileconfig.sections(): @@ -193,7 +194,8 @@ class Printer: self.reactor.unregister_timer(self.connect_timer) try: self._load_config() - self.mcu.connect() + for m in self.mcus: + m.connect() self.gcode.set_printer_ready(True) self.state_message = message_ready if self.start_args.get('debugoutput') is None: @@ -227,10 +229,10 @@ class Printer: run_result = self.run_result try: self._stats(self.reactor.monotonic(), force_output=True) - if self.mcu is not None: + for m in self.mcus: if run_result == 'firmware_restart': - self.mcu.microcontroller_restart() - self.mcu.disconnect() + m.microcontroller_restart() + m.disconnect() except: logging.exception("Unhandled exception during post run") return run_result @@ -254,6 +256,15 @@ class Printer: # Startup ###################################################################### +def arg_dictionary(option, opt_str, value, parser): + key, fname = "dictionary", value + if '=' in value: + mcu_name, fname = value.split('=', 1) + key = "dictionary_" + mcu_name + if parser.values.dictionary is None: + parser.values.dictionary = {} + parser.values.dictionary[key] = fname + def main(): usage = "%prog [options] " opts = optparse.OptionParser(usage) @@ -267,7 +278,8 @@ def main(): help="enable debug messages") opts.add_option("-o", "--debugoutput", dest="debugoutput", help="write output to file instead of to serial port") - opts.add_option("-d", "--dictionary", dest="dictionary", + opts.add_option("-d", "--dictionary", dest="dictionary", type="string", + action="callback", callback=arg_dictionary, help="file to read for mcu protocol dictionary") options, args = opts.parse_args() if len(args) != 1: @@ -287,7 +299,7 @@ def main(): input_fd = util.create_pty(options.inputtty) if options.debugoutput: start_args['debugoutput'] = options.debugoutput - start_args['dictionary'] = options.dictionary + start_args.update(options.dictionary) if options.logfile: bglogger = queuelogger.setup_bg_logging(options.logfile, debuglevel) else: diff --git a/klippy/mcu.py b/klippy/mcu.py index 7a533f46..f0397f19 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -180,7 +180,7 @@ class MCU_endstop: while self._check_busy(eventtime): eventtime = self._mcu.pause(eventtime + 0.1) def _handle_end_stop_state(self, params): - logging.debug("end_stop_state %s" % (params,)) + logging.debug("end_stop_state %s", params) self._last_state = params def _check_busy(self, eventtime): # Check if need to send an end_stop_query command @@ -393,6 +393,9 @@ class MCU: def __init__(self, printer, config, clocksync): self._printer = printer self._clocksync = clocksync + self._name = config.section + if self._name.startswith('mcu '): + self._name = self._name[4:] # Serial port self._serialport = config.get('serial', '/dev/ttyS0') baud = 0 @@ -411,9 +414,9 @@ class MCU: self._is_shutdown = False self._shutdown_msg = "" if printer.bglogger is not None: - printer.bglogger.set_rollover_info("mcu", None) + printer.bglogger.set_rollover_info(self._name, None) # Config building - pins.get_printer_pins(printer).register_chip("mcu", self) + pins.get_printer_pins(printer).register_chip(self._name, self) self._oid_count = 0 self._config_objects = [] self._init_cmds = [] @@ -447,26 +450,32 @@ class MCU: return self._is_shutdown = True self._shutdown_msg = msg = params['#msg'] - logging.info("%s: %s" % (params['#name'], self._shutdown_msg)) + logging.info("%s: %s", params['#name'], self._shutdown_msg) self._serial.dump_debug() - prefix = "MCU shutdown: " + prefix = "MCU '%s' shutdown: " % (self._name,) if params['#name'] == 'is_shutdown': - prefix = "Previous MCU shutdown: " + prefix = "Previous MCU '%s' shutdown: " % (self._name,) self._printer.note_shutdown(prefix + msg + error_help(msg)) # Connection phase def _check_restart(self, reason): start_reason = self._printer.get_start_args().get("start_reason") if start_reason == 'firmware_restart': return - logging.info("Attempting automated firmware restart: %s" % (reason,)) + logging.info("Attempting automated MCU '%s' restart: %s", + self._name, reason) self._printer.request_exit('firmware_restart') self._printer.reactor.pause(self._printer.reactor.monotonic() + 2.000) - raise error("Attempt firmware restart failed") + raise error("Attempt MCU '%s' restart failed" % (self._name,)) def _connect_file(self, pace=False): # In a debugging mode. Open debug output file and read data dictionary - out_fname = self._printer.get_start_args().get('debugoutput') + start_args = self._printer.get_start_args() + if self._name == 'mcu': + out_fname = start_args.get('debugoutput') + dict_fname = start_args.get('dictionary') + else: + out_fname = start_args.get('debugoutput') + "-" + self._name + dict_fname = start_args.get('dictionary_' + self._name) outfile = open(out_fname, 'wb') - dict_fname = self._printer.get_start_args().get('dictionary') dfile = open(dict_fname, 'rb') dict_data = dfile.read() dfile.close() @@ -520,34 +529,36 @@ class MCU: # Only configure mcu after usb power reset self._check_restart("full reset before config") # Send config commands - logging.info("Sending printer configuration...") + logging.info("Sending MCU '%s' printer configuration...", + self._name) for c in self._config_cmds: self.send(self.create_command(c)) if not self.is_fileoutput(): config_params = self.send_with_response(msg, 'config') if not config_params['is_config']: if self._is_shutdown: - raise error("Firmware error during config: %s" % ( - self._shutdown_msg,)) - raise error("Unable to configure printer") + raise error("MCU '%s' error during config: %s" % ( + self._name, self._shutdown_msg)) + raise error("Unable to configure MCU '%s'" % (self._name,)) else: start_reason = self._printer.get_start_args().get("start_reason") if start_reason == 'firmware_restart': - raise error("Failed automated reset of micro-controller") + raise error("Failed automated reset of MCU '%s'" % (self._name,)) if self._config_crc != config_params['crc']: self._check_restart("CRC mismatch") - raise error("Printer CRC does not match config") + raise error("MCU '%s' CRC does not match config" % (self._name,)) move_count = config_params['move_count'] - logging.info("Configured (%d moves)" % (move_count,)) + logging.info("Configured MCU '%s' (%d moves)", self._name, move_count) if self._printer.bglogger is not None: msgparser = self._serial.msgparser info = [ - "Configured (%d moves)" % (move_count,), - "Loaded %d commands (%s)" % ( - len(msgparser.messages_by_id), msgparser.version), - "MCU config: %s" % (" ".join( + "Configured MCU '%s' (%d moves)" % (self._name, move_count), + "Loaded MCU '%s' %d commands (%s)" % ( + self._name, len(msgparser.messages_by_id), + msgparser.version), + "MCU '%s' config: %s" % (self._name, " ".join( ["%s=%s" % (k, v) for k, v in msgparser.config.items()]))] - self._printer.bglogger.set_rollover_info("mcu", "\n".join(info)) + self._printer.bglogger.set_rollover_info(self._name, "\n".join(info)) self._steppersync = self._ffi_lib.steppersync_alloc( self._serial.serialqueue, self._stepqueues, len(self._stepqueues), move_count) @@ -647,25 +658,36 @@ class MCU: if self._steppersync is None: return clock = self.print_time_to_clock(print_time) + if clock < 0: + return ret = self._ffi_lib.steppersync_flush(self._steppersync, clock) if ret: - raise error("Internal error in stepcompress") + raise error("Internal error in MCU '%s' stepcompress" % ( + self._name,)) def check_active(self, print_time, eventtime): + if self._steppersync is None: + return + offset, freq = self._clocksync.calibrate_clock(print_time, eventtime) + self._ffi_lib.steppersync_set_time(self._steppersync, offset, freq) if self._clocksync.is_active(eventtime): return - logging.info("Timeout with firmware (eventtime=%f)", eventtime) - self._printer.note_mcu_error("Lost communication with firmware") + logging.info("Timeout with MCU '%s' (eventtime=%f)", + self._name, eventtime) + self._printer.note_mcu_error("Lost communication with MCU '%s'" % ( + self._name,)) def stats(self, eventtime): - msg = "mcu_awake=%.03f mcu_task_avg=%.06f mcu_task_stddev=%.06f" % ( - self._mcu_tick_awake, self._mcu_tick_avg, self._mcu_tick_stddev) - return ' '.join([self._serial.stats(eventtime), - self._clocksync.stats(eventtime), msg]) + msg = "%s: mcu_awake=%.03f mcu_task_avg=%.06f mcu_task_stddev=%.06f" % ( + self._name, self._mcu_tick_awake, self._mcu_tick_avg, + self._mcu_tick_stddev) + return ' '.join([msg, self._serial.stats(eventtime), + self._clocksync.stats(eventtime)]) def force_shutdown(self): self.send(self._emergency_stop_cmd.encode()) def microcontroller_restart(self): reactor = self._printer.reactor if self._restart_method == 'rpi_usb': - logging.info("Attempting a microcontroller reset via rpi usb power") + logging.info("Attempting MCU '%s' reset via rpi usb power", + self._name) self.disconnect() chelper.run_hub_ctrl(0) reactor.pause(reactor.monotonic() + 2.000) @@ -675,11 +697,13 @@ class MCU: eventtime = reactor.monotonic() if ((self._reset_cmd is None and self._config_reset_cmd is None) or not self._clocksync.is_active(eventtime)): - logging.info("Unable to issue reset command") + logging.info("Unable to issue reset command on MCU '%s'", + self._name) return if self._reset_cmd is None: # Attempt reset via config_reset command - logging.info("Attempting a microcontroller config_reset command") + logging.info("Attempting MCU '%s' config_reset command", + self._name) self._is_shutdown = True self.force_shutdown() reactor.pause(reactor.monotonic() + 0.015) @@ -688,13 +712,13 @@ class MCU: self.disconnect() return # Attempt reset via reset command - logging.info("Attempting a microcontroller reset command") + logging.info("Attempting MCU '%s' reset command", self._name) self.send(self._reset_cmd.encode()) reactor.pause(reactor.monotonic() + 0.015) self.disconnect() return # Attempt reset via arduino mechanism - logging.info("Attempting a microcontroller reset") + logging.info("Attempting MCU '%s' reset", self._name) self.disconnect() serialhdl.arduino_reset(self._serialport, reactor) def disconnect(self): @@ -731,3 +755,18 @@ def error_help(msg): def add_printer_objects(printer, config): mainsync = clocksync.ClockSync(printer.reactor) printer.add_object('mcu', MCU(printer, config.getsection('mcu'), mainsync)) + for s in config.get_prefix_sections('mcu '): + printer.add_object(s.section, MCU( + printer, s, clocksync.SecondarySync(printer.reactor, mainsync))) + +def get_printer_mcus(printer): + return [printer.objects[n] for n in sorted(printer.objects) + if n.startswith('mcu')] + +def get_printer_mcu(printer, name): + mcu_name = name + if name != 'mcu': + mcu_name = 'mcu ' + name + if mcu_name not in printer.objects: + raise printer.config_error("Unknown MCU %s" % (name,)) + return printer.objects[mcu_name] diff --git a/klippy/toolhead.py b/klippy/toolhead.py index fadbbae6..7a1b3e52 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -4,7 +4,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging -import homing, cartesian, corexy, delta, extruder +import mcu, homing, cartesian, corexy, delta, extruder # Common suffixes: _d is distance (in mm), _v is velocity (in # mm/second), _v2 is velocity squared (mm^2/s^2), _t is time (in @@ -184,7 +184,8 @@ class ToolHead: def __init__(self, printer, config): self.printer = printer self.reactor = printer.reactor - self.mcu = printer.objects['mcu'] + self.all_mcus = mcu.get_printer_mcus(printer) + self.mcu = self.all_mcus[0] self.max_velocity = config.getfloat('max_velocity', above=0.) self.max_accel = config.getfloat('max_accel', above=0.) self.max_accel_to_decel = config.getfloat( @@ -227,7 +228,8 @@ class ToolHead: def update_move_time(self, movetime): self.print_time += movetime flush_to_time = self.print_time - self.move_flush_time - self.mcu.flush_moves(flush_to_time) + for m in self.all_mcus: + m.flush_moves(flush_to_time) def get_next_move_time(self): if not self.sync_print_time: return self.print_time @@ -248,9 +250,10 @@ class ToolHead: if sync_print_time or must_sync: self.sync_print_time = True self.move_queue.set_flush_time(self.buffer_time_high) - self.mcu.flush_moves(self.print_time) self.need_check_stall = -1. self.reactor.update_timer(self.flush_timer, self.reactor.NEVER) + for m in self.all_mcus: + m.flush_moves(self.print_time) def get_last_move_time(self): self._flush_lookahead() return self.get_next_move_time() @@ -357,7 +360,8 @@ class ToolHead: self.commanded_pos[3] = extrude_pos # Misc commands def check_active(self, eventtime): - self.mcu.check_active(self.print_time, eventtime) + for m in self.all_mcus: + m.check_active(self.print_time, eventtime) if not self.sync_print_time: return True return self.print_time + 60. > self.mcu.estimated_print_time(eventtime) @@ -368,7 +372,8 @@ class ToolHead: self.print_time, buffer_time, self.print_stall) def force_shutdown(self): try: - self.mcu.force_shutdown() + for m in self.all_mcus: + m.force_shutdown() self.move_queue.reset() self.reset_print_time() except: