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 <kevin@koconnor.net>

4 files changed, 161 insertions, 53 deletions

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] <config file>"
     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: