mcu: convert stepper, endstop, and digital_out to take mcu_time

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

5 files changed, 79 insertions, 86 deletions

diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index b904d73f..653b2276 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -80,34 +80,33 @@ class CartKinematics:
             if steps < 0:
                 sdir = 1
                 steps = -steps
-            clock_offset, clock_freq, so = self.steppers[i].prep_move(
-                sdir, move_time)
+            mcu_time, so = self.steppers[i].prep_move(move_time, sdir)
 
             step_dist = move.move_d / steps
             step_offset = 0.5
 
             # Acceleration steps
             #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
-            accel_clock_offset = move.start_v * inv_accel * clock_freq
-            accel_sqrt_offset = accel_clock_offset**2
-            accel_multiplier = 2.0 * step_dist * inv_accel * clock_freq**2
+            accel_time_offset = move.start_v * inv_accel
+            accel_sqrt_offset = accel_time_offset**2
+            accel_multiplier = 2.0 * step_dist * inv_accel
             accel_steps = move.accel_r * steps
             step_offset = so.step_sqrt(
-                accel_steps, step_offset, clock_offset - accel_clock_offset
+                mcu_time - accel_time_offset, accel_steps, step_offset
                 , accel_sqrt_offset, accel_multiplier)
-            clock_offset += move.accel_t * clock_freq
+            mcu_time += move.accel_t
             # Cruising steps
             #t = pos/cruise_v
-            cruise_multiplier = step_dist * inv_cruise_v * clock_freq
+            cruise_multiplier = step_dist * inv_cruise_v
             cruise_steps = move.cruise_r * steps
             step_offset = so.step_factor(
-                cruise_steps, step_offset, clock_offset, cruise_multiplier)
-            clock_offset += move.cruise_t * clock_freq
+                mcu_time, cruise_steps, step_offset, cruise_multiplier)
+            mcu_time += move.cruise_t
             # Deceleration steps
             #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
-            decel_clock_offset = move.cruise_v * inv_accel * clock_freq
-            decel_sqrt_offset = decel_clock_offset**2
+            decel_time_offset = move.cruise_v * inv_accel
+            decel_sqrt_offset = decel_time_offset**2
             decel_steps = move.decel_r * steps
             so.step_sqrt(
-                decel_steps, step_offset, clock_offset + decel_clock_offset
+                mcu_time + decel_time_offset, decel_steps, step_offset
                 , decel_sqrt_offset, -accel_multiplier)
diff --git a/klippy/extruder.py b/klippy/extruder.py
index ba12fd64..3aef5a19 100644
--- a/klippy/extruder.py
+++ b/klippy/extruder.py
@@ -77,8 +77,7 @@ class PrinterExtruder:
             if steps < 0:
                 sdir = 1
                 steps = -steps
-            clock_offset, clock_freq, so = self.stepper.prep_move(
-                sdir, move_time)
+            mcu_time, so = self.stepper.prep_move(move_time, sdir)
 
             step_dist = forward_d / steps
             inv_step_dist = 1. / step_dist
@@ -86,28 +85,28 @@ class PrinterExtruder:
 
             # Acceleration steps
             #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
-            accel_clock_offset = start_v * inv_accel * clock_freq
-            accel_sqrt_offset = accel_clock_offset**2
-            accel_multiplier = 2.0 * step_dist * inv_accel * clock_freq**2
+            accel_time_offset = start_v * inv_accel
+            accel_sqrt_offset = accel_time_offset**2
+            accel_multiplier = 2.0 * step_dist * inv_accel
             accel_steps = accel_d * inv_step_dist
             step_offset = so.step_sqrt(
-                accel_steps, step_offset, clock_offset - accel_clock_offset
+                mcu_time - accel_time_offset, accel_steps, step_offset
                 , accel_sqrt_offset, accel_multiplier)
-            clock_offset += accel_t * clock_freq
+            mcu_time += accel_t
             # Cruising steps
             #t = pos/cruise_v
-            cruise_multiplier = step_dist * clock_freq / cruise_v
+            cruise_multiplier = step_dist / cruise_v
             cruise_steps = cruise_d * inv_step_dist
             step_offset = so.step_factor(
-                cruise_steps, step_offset, clock_offset, cruise_multiplier)
-            clock_offset += cruise_t * clock_freq
+                mcu_time, cruise_steps, step_offset, cruise_multiplier)
+            mcu_time += cruise_t
             # Deceleration steps
             #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
-            decel_clock_offset = decel_v * inv_accel * clock_freq
-            decel_sqrt_offset = decel_clock_offset**2
+            decel_time_offset = decel_v * inv_accel
+            decel_sqrt_offset = decel_time_offset**2
             decel_steps = decel_d * inv_step_dist
             so.step_sqrt(
-                decel_steps, step_offset, clock_offset + decel_clock_offset
+                mcu_time + decel_time_offset, decel_steps, step_offset
                 , decel_sqrt_offset, -accel_multiplier)
 
         # Determine retract steps
@@ -116,15 +115,15 @@ class PrinterExtruder:
         steps = self.stepper_pos - new_step_pos
         if steps:
             self.stepper_pos = new_step_pos
-            clock_offset, clock_freq, so = self.stepper.prep_move(
-                1, move_time+accel_t+cruise_t+decel_t)
+            mcu_time, so = self.stepper.prep_move(
+                move_time+accel_t+cruise_t+decel_t, 1)
 
             step_dist = retract_d / steps
 
             # Acceleration steps
             #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
-            accel_clock_offset = retract_v * inv_accel * clock_freq
-            accel_sqrt_offset = accel_clock_offset**2
-            accel_multiplier = 2.0 * step_dist * inv_accel * clock_freq**2
-            so.step_sqrt(steps, 0.5, clock_offset - accel_clock_offset
+            accel_time_offset = retract_v * inv_accel
+            accel_sqrt_offset = accel_time_offset**2
+            accel_multiplier = 2.0 * step_dist * inv_accel
+            so.step_sqrt(mcu_time - accel_time_offset, steps, 0.5
                          , accel_sqrt_offset, accel_multiplier)
diff --git a/klippy/homing.py b/klippy/homing.py
index 8bcf4a24..8670c45a 100644
--- a/klippy/homing.py
+++ b/klippy/homing.py
@@ -47,8 +47,7 @@ class Homing:
         # Start homing and issue move
         print_time = self.toolhead.get_last_move_time()
         for s in steppers:
-            hz = speed * s.inv_step_dist
-            es = s.enable_endstop_checking(print_time, hz)
+            es = s.enable_endstop_checking(print_time, s.step_dist / speed)
             self.endstops.append(es)
         self.toolhead.move(self.fill_coord(movepos), speed)
         self.toolhead.reset_print_time()
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 2c7dcbd3..c5e4ee16 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -24,6 +24,9 @@ class MCU_stepper:
         dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin)
         self._sdir = -1
         self._last_move_clock = -2**29
+        self._mcu_freq = mcu.get_mcu_freq()
+        min_stop_interval = int(min_stop_interval * self._mcu_freq)
+        max_error = int(max_error * self._mcu_freq)
         mcu.add_config_cmd(
             "config_stepper oid=%d step_pin=%s dir_pin=%s"
             " min_stop_interval=%d invert_step=%d" % (
@@ -38,6 +41,7 @@ class MCU_stepper:
         ffi_main, self.ffi_lib = chelper.get_ffi()
         self._stepqueue = self.ffi_lib.stepcompress_alloc(
             max_error, self._step_cmd.msgid, self._oid)
+        self.print_to_mcu_time = mcu.print_to_mcu_time
     def get_oid(self):
         return self._oid
     def get_invert_dir(self):
@@ -45,32 +49,35 @@ class MCU_stepper:
     def note_stepper_stop(self):
         self._sdir = -1
         self._last_move_clock = -2**29
-    def reset_step_clock(self, clock):
+    def _reset_step_clock(self, clock):
         self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
         data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff)
         self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
-    def set_next_step_dir(self, sdir, clock):
+    def set_next_step_dir(self, mcu_time, sdir):
+        clock = int(mcu_time * self._mcu_freq)
         if clock - self._last_move_clock >= 2**29:
-            self.reset_step_clock(clock)
+            self._reset_step_clock(clock)
         self._last_move_clock = clock
         if self._sdir == sdir:
             return
         self._sdir = sdir
         data = (self._dir_cmd.msgid, self._oid, sdir ^ self._invert_dir)
         self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
-    def step(self, steptime):
-        self.ffi_lib.stepcompress_push(self._stepqueue, steptime)
-    def step_sqrt(self, steps, step_offset, clock_offset, sqrt_offset, factor):
+    def step(self, mcu_time):
+        clock = mcu_time * self._mcu_freq
+        self.ffi_lib.stepcompress_push(self._stepqueue, clock)
+    def step_sqrt(self, mcu_time, steps, step_offset, sqrt_offset, factor):
+        clock = mcu_time * self._mcu_freq
+        mcu_freq2 = self._mcu_freq**2
         return self.ffi_lib.stepcompress_push_sqrt(
-            self._stepqueue, steps, step_offset, clock_offset
-            , sqrt_offset, factor)
-    def step_factor(self, steps, step_offset, clock_offset, factor):
+            self._stepqueue, steps, step_offset, clock
+            , sqrt_offset * mcu_freq2, factor * mcu_freq2)
+    def step_factor(self, mcu_time, steps, step_offset, factor):
+        clock = mcu_time * self._mcu_freq
         return self.ffi_lib.stepcompress_push_factor(
-            self._stepqueue, steps, step_offset, clock_offset, factor)
+            self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq)
     def get_errors(self):
         return self.ffi_lib.stepcompress_get_errors(self._stepqueue)
-    def get_print_clock(self, print_time):
-        return self._mcu.get_print_clock(print_time)
 
 class MCU_endstop:
     RETRY_QUERY = 1.000
@@ -92,9 +99,12 @@ class MCU_endstop:
         self._homing = False
         self._last_position = 0
         self._next_query_clock = 0
-        mcu_freq = self._mcu.get_mcu_freq()
-        self._retry_query_ticks = mcu_freq * self.RETRY_QUERY
-    def home(self, clock, rest_ticks):
+        self._mcu_freq = mcu.get_mcu_freq()
+        self._retry_query_ticks = int(self._mcu_freq * self.RETRY_QUERY)
+        self.print_to_mcu_time = mcu.print_to_mcu_time
+    def home(self, mcu_time, rest_time):
+        clock = int(mcu_time * self._mcu_freq)
+        rest_ticks = int(rest_time * self._mcu_freq)
         self._homing = True
         self._next_query_clock = clock + self._retry_query_ticks
         msg = self._home_cmd.encode(
@@ -124,8 +134,6 @@ class MCU_endstop:
         if self._stepper.get_invert_dir():
             return -self._last_position
         return self._last_position
-    def get_print_clock(self, print_time):
-        return self._mcu.get_print_clock(print_time)
 
 class MCU_digital_out:
     def __init__(self, mcu, pin, max_duration):
@@ -141,7 +149,9 @@ class MCU_digital_out:
             " max_duration=%d" % (self._oid, pin, self._invert, max_duration))
         self._set_cmd = mcu.lookup_command(
             "schedule_digital_out oid=%c clock=%u value=%c")
-    def set_digital(self, clock, value):
+        self.print_to_mcu_time = mcu.print_to_mcu_time
+    def set_digital(self, mcu_time, value):
+        clock = int(mcu_time * self._mcu_freq)
         msg = self._set_cmd.encode(self._oid, clock, value ^ self._invert)
         self._mcu.send(msg, minclock=self._last_clock, reqclock=clock
                       , cq=self._cmd_queue)
@@ -150,13 +160,10 @@ class MCU_digital_out:
     def get_last_setting(self):
         return self._last_value
     def set_pwm(self, mcu_time, value):
-        clock = int(mcu_time * self._mcu_freq)
         dval = 0
         if value > 127:
             dval = 1
-        self.set_digital(clock, dval)
-    def get_print_clock(self, print_time):
-        return self._mcu.get_print_clock(print_time)
+        self.set_digital(mcu_time, dval)
 
 class MCU_pwm:
     def __init__(self, mcu, pin, cycle_ticks, max_duration, hard_pwm=True):
@@ -253,7 +260,7 @@ class MCU:
         self._steppers = []
         self._steppersync = None
         # Print time to clock epoch calculations
-        self._print_start_clock = 0.
+        self._print_start_clock = 0
         self._clock_freq = 0.
         # Stats
         self._mcu_tick_avg = 0.
@@ -420,8 +427,6 @@ class MCU:
         return last_move_end - (float(clock_diff) / self._clock_freq)
     def print_to_mcu_time(self, print_time):
         return print_time + self._print_start_clock / self._clock_freq
-    def get_print_clock(self, print_time):
-        return print_time * self._clock_freq + self._print_start_clock
     def get_mcu_freq(self):
         return self._clock_freq
     def get_last_clock(self):
@@ -430,8 +435,8 @@ class MCU:
     def send(self, cmd, minclock=0, reqclock=0, cq=None):
         self.serial.send(cmd, minclock, reqclock, cq=cq)
     def flush_moves(self, print_time):
-        move_clock = int(self.get_print_clock(print_time))
-        self.ffi_lib.steppersync_flush(self._steppersync, move_clock)
+        clock = int(print_time * self._clock_freq) + self._print_start_clock
+        self.ffi_lib.steppersync_flush(self._steppersync, clock)
 
 
 ######################################################################
@@ -456,10 +461,10 @@ class Dummy_MCU_stepper:
             self._stepid, dirstr, countstr, addstr, interval))
     def set_next_step_dir(self, dir):
         self._sdir = dir
-    def reset_step_clock(self, clock):
+    def _reset_step_clock(self, clock):
         self._mcu.outfile.write("G6S%dT%d\n" % (self._stepid, clock))
-    def get_print_clock(self, print_time):
-        return self._mcu.get_print_clock(print_time)
+    def print_to_mcu_time(self, print_time):
+        return self._mcu.print_to_mcu_time(print_time)
 
 class Dummy_MCU_obj:
     def __init__(self, mcu):
@@ -480,14 +485,12 @@ class Dummy_MCU_obj:
         pass
     def print_to_mcu_time(self, print_time):
         return self._mcu.print_to_mcu_time(print_time)
-    def get_print_clock(self, print_time):
-        return self._mcu.get_print_clock(print_time)
 
 class DummyMCU:
     def __init__(self, outfile):
         self.outfile = outfile
         self._stepid = -1
-        self._print_start_clock = 0.
+        self._print_start_clock = 0
         self._clock_freq = 16000000.
         logging.debug('Translated by klippy')
     def connect(self):
@@ -515,8 +518,6 @@ class DummyMCU:
         return 0.250
     def print_to_mcu_time(self, print_time):
         return print_time + self._print_start_clock / self._clock_freq
-    def get_print_clock(self, print_time):
-        return print_time * self._clock_freq + self._print_start_clock
     def get_mcu_freq(self):
         return self._clock_freq
     def flush_moves(self, print_time):
diff --git a/klippy/stepper.py b/klippy/stepper.py
index c15d194c..084f0131 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -42,23 +42,18 @@ class PrinterStepper:
         self.position_endstop = config.getfloat('position_endstop')
         self.position_max = config.getfloat('position_max')
 
-        self.clock_ticks = None
         self.need_motor_enable = True
     def set_max_jerk(self, max_jerk):
         self.max_jerk = max_jerk
     def build_config(self):
-        self.clock_ticks = self.printer.mcu.get_mcu_freq()
         max_error = self.config.getfloat('max_error', 0.000050)
-        max_error = int(max_error * self.clock_ticks)
-
         step_pin = self.config.get('step_pin')
         dir_pin = self.config.get('dir_pin')
         jc = self.max_jerk / self.max_accel
         inv_max_step_accel = self.step_dist / self.max_accel
-        min_stop_interval = int((math.sqrt(3.*inv_max_step_accel + jc**2)
-                                 - math.sqrt(inv_max_step_accel + jc**2))
-                                * self.clock_ticks) - max_error
-        min_stop_interval = max(0, min_stop_interval)
+        min_stop_interval = (math.sqrt(3.*inv_max_step_accel + jc**2)
+                             - math.sqrt(inv_max_step_accel + jc**2)) - max_error
+        min_stop_interval = max(0., min_stop_interval)
         mcu = self.printer.mcu
         self.mcu_stepper = mcu.create_stepper(
             step_pin, dir_pin, min_stop_interval, max_error)
@@ -71,19 +66,19 @@ class PrinterStepper:
     def motor_enable(self, move_time, enable=0):
         if (self.mcu_enable is not None
             and self.mcu_enable.get_last_setting() != enable):
-            mc = int(self.mcu_enable.get_print_clock(move_time))
-            self.mcu_enable.set_digital(mc + 1, enable)
+            mcu_time = self.mcu_enable.print_to_mcu_time(move_time)
+            self.mcu_enable.set_digital(mcu_time, enable)
         self.need_motor_enable = True
-    def prep_move(self, sdir, move_time):
-        move_clock = self.mcu_stepper.get_print_clock(move_time)
-        self.mcu_stepper.set_next_step_dir(sdir, int(move_clock))
+    def prep_move(self, move_time, sdir):
+        mcu_time = self.mcu_stepper.print_to_mcu_time(move_time)
+        self.mcu_stepper.set_next_step_dir(mcu_time, sdir)
         if self.need_motor_enable:
             self.motor_enable(move_time, 1)
             self.need_motor_enable = False
-        return (move_clock, self.clock_ticks, self.mcu_stepper)
-    def enable_endstop_checking(self, move_time, hz):
-        move_clock = int(self.mcu_endstop.get_print_clock(move_time))
-        self.mcu_endstop.home(move_clock, int(self.clock_ticks / hz))
+        return (mcu_time, self.mcu_stepper)
+    def enable_endstop_checking(self, move_time, step_time):
+        mcu_time = self.mcu_endstop.print_to_mcu_time(move_time)
+        self.mcu_endstop.home(mcu_time, step_time)
         return self.mcu_endstop
     def get_homed_position(self):
         if not self.homing_stepper_phases: