stepcompress: Implement print time to clock conversion in C code

Implement the conversion from print_time to the local mcu's clock
within the C code.  This simplifies the python code.

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

3 files changed, 61 insertions, 43 deletions

diff --git a/klippy/chelper.py b/klippy/chelper.py
index b7dce015..1f09e72d 100644
--- a/klippy/chelper.py
+++ b/klippy/chelper.py
@@ -36,6 +36,8 @@ defs_stepcompress = """
     struct steppersync *steppersync_alloc(struct serialqueue *sq
         , struct stepcompress **sc_list, int sc_num, int move_num);
     void steppersync_free(struct steppersync *ss);
+    void steppersync_set_time(struct steppersync *ss
+        , double time_offset, double mcu_freq);
     int steppersync_flush(struct steppersync *ss, uint64_t move_clock);
 """
 
diff --git a/klippy/mcu.py b/klippy/mcu.py
index ba3381c1..b48a99f7 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -20,9 +20,8 @@ class MCU_stepper:
         self._dir_pin = self._invert_dir = None
         self._commanded_pos = 0
         self._step_dist = self._inv_step_dist = 1.
-        self._velocity_factor = self._accel_factor = 0.
         self._mcu_position_offset = 0
-        self._mcu_freq = self._min_stop_interval = 0.
+        self._min_stop_interval = 0.
         self._reset_cmd = self._get_position_cmd = None
         self._ffi_lib = self._stepqueue = None
     def get_mcu(self):
@@ -38,9 +37,6 @@ class MCU_stepper:
         self._step_dist = step_dist
         self._inv_step_dist = 1. / step_dist
     def build_config(self):
-        self._mcu_freq = self._mcu.get_mcu_freq()
-        self._velocity_factor = 1. / (self._mcu_freq * self._step_dist)
-        self._accel_factor = 1. / (self._mcu_freq**2 * self._step_dist)
         max_error = self._mcu.get_max_stepper_error()
         min_stop_interval = max(0., self._min_stop_interval - max_error)
         self._mcu.add_config_cmd(
@@ -107,29 +103,27 @@ class MCU_stepper:
         if ret:
             raise error("Internal error in stepcompress")
     def step(self, print_time, sdir):
-        clock = print_time * self._mcu_freq
-        count = self._ffi_lib.stepcompress_push(self._stepqueue, clock, sdir)
+        count = self._ffi_lib.stepcompress_push(
+            self._stepqueue, print_time, sdir)
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self._commanded_pos += count
     def step_const(self, print_time, start_pos, dist, start_v, accel):
-        clock = print_time * self._mcu_freq
         inv_step_dist = self._inv_step_dist
         step_offset = self._commanded_pos - start_pos * inv_step_dist
         count = self._ffi_lib.stepcompress_push_const(
-            self._stepqueue, clock, step_offset, dist * inv_step_dist,
-            start_v * self._velocity_factor, accel * self._accel_factor)
+            self._stepqueue, print_time, step_offset, dist * inv_step_dist,
+            start_v * inv_step_dist, accel * inv_step_dist)
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
         self._commanded_pos += count
     def step_delta(self, print_time, dist, start_v, accel
                    , height_base, startxy_d, arm_d, movez_r):
-        clock = print_time * self._mcu_freq
         inv_step_dist = self._inv_step_dist
         height = self._commanded_pos - height_base * inv_step_dist
         count = self._ffi_lib.stepcompress_push_delta(
-            self._stepqueue, clock, dist * inv_step_dist,
-            start_v * self._velocity_factor, accel * self._accel_factor,
+            self._stepqueue, print_time, dist * inv_step_dist,
+            start_v * inv_step_dist, accel * inv_step_dist,
             height, startxy_d * inv_step_dist, arm_d * inv_step_dist, movez_r)
         if count == STEPCOMPRESS_ERROR_RET:
             raise error("Internal error in stepcompress")
@@ -636,6 +630,7 @@ class MCU:
         self._steppersync = self._ffi_lib.steppersync_alloc(
             self.serial.serialqueue, self._stepqueues, len(self._stepqueues),
             move_count)
+        self._ffi_lib.steppersync_set_time(self._steppersync, 0., self._mcu_freq)
         for c in self._init_cmds:
             self.send(self.create_command(c))
     # Config creation helpers
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c
index 9c815b3a..eef07fdb 100644
--- a/klippy/stepcompress.c
+++ b/klippy/stepcompress.c
@@ -31,6 +31,7 @@ struct stepcompress {
     uint32_t *queue, *queue_end, *queue_pos, *queue_next;
     // Internal tracking
     uint32_t max_error;
+    double mcu_time_offset, mcu_freq;
     // Message generation
     uint64_t last_step_clock, homing_clock;
     struct list_head msg_queue;
@@ -364,6 +365,15 @@ stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
     return 0;
 }
 
+// Set the conversion rate of 'print_time' to mcu clock
+static void
+stepcompress_set_time(struct stepcompress *sc
+                      , double time_offset, double mcu_freq)
+{
+    sc->mcu_time_offset = time_offset;
+    sc->mcu_freq = mcu_freq;
+}
+
 
 /****************************************************************
  * Queue management
@@ -380,12 +390,13 @@ struct queue_append {
 
 // Create a cursor for inserting clock times into the queue
 static inline struct queue_append
-queue_append_start(struct stepcompress *sc, double clock_offset, double adjust)
+queue_append_start(struct stepcompress *sc, double print_time, double adjust)
 {
+    double print_clock = (print_time - sc->mcu_time_offset) * sc->mcu_freq;
     return (struct queue_append) {
         .sc = sc, .qnext = sc->queue_next, .qend = sc->queue_end,
         .last_step_clock_32 = sc->last_step_clock,
-        .clock_offset = (clock_offset - (double)sc->last_step_clock) + adjust };
+        .clock_offset = (print_clock - (double)sc->last_step_clock) + adjust };
 }
 
 // Finalize a cursor created with queue_append_start()
@@ -467,11 +478,10 @@ queue_append(struct queue_append *qa, double step_clock)
 
 // Common suffixes: _sd is step distance (a unit length the same
 // distance the stepper moves on each step), _sv is step velocity (in
-// units of step distance per clock tick), _sd2 is step distance
-// squared, _r is ratio (scalar usually between 0.0 and 1.0).  Times
-// are represented as clock ticks (a unit of time determined by a
-// micro-controller tick) and acceleration is in units of step
-// distance per clock ticks squared.
+// units of step distance per time), _sd2 is step distance squared, _r
+// is ratio (scalar usually between 0.0 and 1.0).  Times are in
+// seconds and acceleration is in units of step distance per second
+// squared.
 
 // Wrapper around sqrt() to handle small negative numbers
 static double
@@ -489,12 +499,12 @@ static inline double safe_sqrt(double v) {
 
 // Schedule a step event at the specified step_clock time
 int32_t
-stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
+stepcompress_push(struct stepcompress *sc, double print_time, int32_t sdir)
 {
     int ret = set_next_step_dir(sc, !!sdir);
     if (ret)
         return ret;
-    struct queue_append qa = queue_append_start(sc, step_clock, 0.5);
+    struct queue_append qa = queue_append_start(sc, print_time, 0.5);
     ret = queue_append(&qa, 0.);
     if (ret)
         return ret;
@@ -504,13 +514,13 @@ stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
 
 // Schedule 'steps' number of steps at constant acceleration. If
 // acceleration is zero (ie, constant velocity) it uses the formula:
-//  step_clock = clock_offset + step_num/start_sv
+//  step_time = print_time + step_num/start_sv
 // Otherwise it uses the formula:
-//  step_clock = (clock_offset + sqrt(2*step_num/accel + (start_sv/accel)**2)
-//                - start_sv/accel)
+//  step_time = (print_time + sqrt(2*step_num/accel + (start_sv/accel)**2)
+//               - start_sv/accel)
 int32_t
 stepcompress_push_const(
-    struct stepcompress *sc, double clock_offset
+    struct stepcompress *sc, double print_time
     , double step_offset, double steps, double start_sv, double accel)
 {
     // Calculate number of steps to take
@@ -524,7 +534,7 @@ stepcompress_push_const(
     if (count <= 0 || count > 10000000) {
         if (count && steps) {
             errorf("push_const invalid count %d %f %f %f %f %f"
-                   , sc->oid, clock_offset, step_offset, steps
+                   , sc->oid, print_time, step_offset, steps
                    , start_sv, accel);
             return ERROR_RET;
         }
@@ -538,8 +548,8 @@ stepcompress_push_const(
     // Calculate each step time
     if (!accel) {
         // Move at constant velocity (zero acceleration)
-        struct queue_append qa = queue_append_start(sc, clock_offset, .5);
-        double inv_cruise_sv = 1. / start_sv;
+        struct queue_append qa = queue_append_start(sc, print_time, .5);
+        double inv_cruise_sv = sc->mcu_freq / start_sv;
         double pos = (step_offset + .5) * inv_cruise_sv;
         while (count--) {
             ret = queue_append(&qa, pos);
@@ -551,10 +561,10 @@ stepcompress_push_const(
     } else {
         // Move with constant acceleration
         double inv_accel = 1. / accel;
-        double accel_time = start_sv * inv_accel;
+        double accel_time = start_sv * inv_accel * sc->mcu_freq;
         struct queue_append qa = queue_append_start(
-            sc, clock_offset, 0.5 - accel_time);
-        double accel_multiplier = 2. * inv_accel;
+            sc, print_time, 0.5 - accel_time);
+        double accel_multiplier = 2. * inv_accel * sc->mcu_freq * sc->mcu_freq;
         double pos = (step_offset + .5)*accel_multiplier + accel_time*accel_time;
         while (count--) {
             double v = safe_sqrt(pos);
@@ -572,7 +582,7 @@ stepcompress_push_const(
 static int32_t
 _stepcompress_push_delta(
     struct stepcompress *sc, int sdir
-    , double clock_offset, double move_sd, double start_sv, double accel
+    , double print_time, double move_sd, double start_sv, double accel
     , double height, double startxy_sd, double arm_sd, double movez_r)
 {
     // Calculate number of steps to take
@@ -584,7 +594,7 @@ _stepcompress_push_delta(
     if (count <= 0 || count > 10000000) {
         if (count) {
             errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f"
-                   , sc->oid, count, clock_offset, move_sd, start_sv, accel
+                   , sc->oid, count, print_time, move_sd, start_sv, accel
                    , height, startxy_sd, arm_sd, movez_r);
             return ERROR_RET;
         }
@@ -599,8 +609,8 @@ _stepcompress_push_delta(
     height += (sdir ? .5 : -.5);
     if (!accel) {
         // Move at constant velocity (zero acceleration)
-        struct queue_append qa = queue_append_start(sc, clock_offset, .5);
-        double inv_cruise_sv = 1. / start_sv;
+        struct queue_append qa = queue_append_start(sc, print_time, .5);
+        double inv_cruise_sv = sc->mcu_freq / start_sv;
         if (!movez_r) {
             // Optimized case for common XY only moves (no Z movement)
             while (count--) {
@@ -641,8 +651,8 @@ _stepcompress_push_delta(
         double inv_accel = 1. / accel;
         start_pos += 0.5 * start_sv*start_sv * inv_accel;
         struct queue_append qa = queue_append_start(
-            sc, clock_offset, 0.5 - start_sv * inv_accel);
-        double accel_multiplier = 2. * inv_accel;
+            sc, print_time, 0.5 - start_sv * inv_accel * sc->mcu_freq);
+        double accel_multiplier = 2. * inv_accel * sc->mcu_freq * sc->mcu_freq;
         while (count--) {
             double relheight = movexy_r*height - zoffset;
             double v = safe_sqrt(arm_sd2 - relheight*relheight);
@@ -660,7 +670,7 @@ _stepcompress_push_delta(
 
 int32_t
 stepcompress_push_delta(
-    struct stepcompress *sc, double clock_offset, double move_sd
+    struct stepcompress *sc, double print_time, double move_sd
     , double start_sv, double accel
     , double height, double startxy_sd, double arm_sd, double movez_r)
 {
@@ -668,22 +678,22 @@ stepcompress_push_delta(
     if (reversexy_sd <= 0.)
         // All steps are in down direction
         return _stepcompress_push_delta(
-            sc, 0, clock_offset, move_sd, start_sv, accel
+            sc, 0, print_time, move_sd, start_sv, accel
             , height, startxy_sd, arm_sd, movez_r);
     double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
     if (reversexy_sd >= move_sd * movexy_r)
         // All steps are in up direction
         return _stepcompress_push_delta(
-            sc, 1, clock_offset, move_sd, start_sv, accel
+            sc, 1, print_time, move_sd, start_sv, accel
             , height, startxy_sd, arm_sd, movez_r);
     // Steps in both up and down direction
     int res1 = _stepcompress_push_delta(
-        sc, 1, clock_offset, reversexy_sd / movexy_r, start_sv, accel
+        sc, 1, print_time, reversexy_sd / movexy_r, start_sv, accel
         , height, startxy_sd, arm_sd, movez_r);
     if (res1 == ERROR_RET)
         return res1;
     int res2 = _stepcompress_push_delta(
-        sc, 0, clock_offset, move_sd, start_sv, accel
+        sc, 0, print_time, move_sd, start_sv, accel
         , height + res1, startxy_sd, arm_sd, movez_r);
     if (res2 == ERROR_RET)
         return res2;
@@ -747,6 +757,17 @@ steppersync_free(struct steppersync *ss)
     free(ss);
 }
 
+// Set the conversion rate of 'print_time' to mcu clock
+void
+steppersync_set_time(struct steppersync *ss, double time_offset, double mcu_freq)
+{
+    int i;
+    for (i=0; i<ss->sc_num; i++) {
+        struct stepcompress *sc = ss->sc_list[i];
+        stepcompress_set_time(sc, time_offset, mcu_freq);
+    }
+}
+
 // Implement a binary heap algorithm to track when the next available
 // 'struct move' in the mcu will be available
 static void