clocksync: Rework multi-mcu adjust to better handle long moves

The multi-mcu clock syncing code relies on the ability to periodically
update the mcu clock adjustments.  If a series of very long moves are
submitted then it is possible the adjustments could become unstable.
For example, if an adjustment is made to reduce a clock error over the
next couple of seconds, but it is applied to a longer period because
the next move lasts many seconds, then this would result in a bigger
adjustment for the following move, which would result in an even
bigger error when that move lasts many seconds.  This can repeat until
the system destabilizes.

Check for cases where the print_time is far in the future of the
current estimated print time and average over a longer period in that
case.  That should reduce the possibility of the adjustment code
becoming unstable.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 19 insertions, 11 deletions

diff --git a/klippy/clocksync.py b/klippy/clocksync.py
index ce5e42a2..65c3face 100644
--- a/klippy/clocksync.py
+++ b/klippy/clocksync.py
@@ -165,6 +165,7 @@ class SecondarySync(ClockSync):
         ClockSync.__init__(self, reactor)
         self.main_sync = main_sync
         self.clock_adj = (0., 1.)
+        self.last_sync_time = 0.
     def connect(self, serial):
         ClockSync.connect(self, serial)
         self.clock_adj = (0., self.mcu_freq)
@@ -195,18 +196,25 @@ class SecondarySync(ClockSync):
         adjusted_offset, adjusted_freq = self.clock_adj
         return "%s adj=%d" % (ClockSync.stats(self, eventtime), adjusted_freq)
     def calibrate_clock(self, print_time, eventtime):
+        # Calculate: est_print_time = main_sync.estimatated_print_time()
         ser_time, ser_clock, ser_freq = self.main_sync.clock_est
         main_mcu_freq = self.main_sync.mcu_freq
-
-        main_clock = (eventtime - ser_time) * ser_freq + ser_clock
-        print_time = max(print_time, main_clock / main_mcu_freq)
-        main_sync_clock = (print_time + 4.) * main_mcu_freq
-        sync_time = ser_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 = .25 * (sync_clock - print_clock)
-        adjusted_offset = print_time - print_clock / adjusted_freq
-
+        est_main_clock = (eventtime - ser_time) * ser_freq + ser_clock
+        est_print_time = est_main_clock / main_mcu_freq
+        # Determine sync1_print_time and sync2_print_time
+        sync1_print_time = max(print_time, est_print_time)
+        sync2_print_time = max(sync1_print_time + 4., self.last_sync_time,
+                               2.5 * (print_time - est_print_time))
+        # Calc sync2_sys_time (inverse of main_sync.estimatated_print_time)
+        sync2_main_clock = sync2_print_time * main_mcu_freq
+        sync2_sys_time = ser_time + (sync2_main_clock - ser_clock) / ser_freq
+        # Adjust freq so estimated print_time will match at sync2_print_time
+        sync1_clock = self.print_time_to_clock(sync1_print_time)
+        sync2_clock = self.get_clock(sync2_sys_time)
+        adjusted_freq = ((sync2_clock - sync1_clock)
+                         / (sync2_print_time - sync1_print_time))
+        adjusted_offset = sync1_print_time - sync1_clock / adjusted_freq
+        # Apply new values
         self.clock_adj = (adjusted_offset, adjusted_freq)
+        self.last_sync_time = sync2_print_time
         return self.clock_adj