itersolve: Move tracking of commanded position to itersolve code

Track the commanded position in just the itersolve.c code instead of
in mcu.py.

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

klippy/chelper/__init__.py

@@ -49,9 +49,10 @@ defs_itersolve = """
         , double axes_d_x, double axes_d_y, double axes_d_z
         , double start_v, double cruise_v, double accel);
     int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m);
-    void itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos);
     void itersolve_set_stepcompress(struct stepper_kinematics *sk
         , struct stepcompress *sc, double step_dist);
+    void itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos);
+    double itersolve_get_commanded_pos(struct stepper_kinematics *sk);
 """
 
 defs_kin_cartesian = """

klippy/chelper/itersolve.c

@@ -149,7 +149,7 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
     double mcu_freq = stepcompress_get_mcu_freq(sc);
     struct timepos last = { 0., sk->commanded_pos }, low = last, high = last;
     double seek_time_delta = 0.000100;
-    int steps = 0, sdir = stepcompress_get_step_dir(sc);
+    int sdir = stepcompress_get_step_dir(sc);
     struct queue_append qa = queue_append_start(sc, m->print_time, .5);
     for (;;) {
         // Determine if next step is in forward or reverse direction
@@ -192,7 +192,6 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
         int ret = queue_append(&qa, next.time * mcu_freq);
         if (ret)
             return ret;
-        steps += sdir ? 1 : -1;
         seek_time_delta = next.time - last.time;
         if (seek_time_delta < .000000001)
             seek_time_delta = .000000001;
@@ -205,7 +204,7 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
     }
     queue_append_finish(qa);
     sk->commanded_pos = last.position;
-    return steps;
+    return 0;
 }
 
 void __visible
@@ -221,3 +220,9 @@ itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos)
 {
     sk->commanded_pos = pos;
 }
+
+double __visible
+itersolve_get_commanded_pos(struct stepper_kinematics *sk)
+{
+    return sk->commanded_pos;
+}

klippy/mcu.py

@@ -18,8 +18,8 @@ class MCU_stepper:
         self._step_pin = pin_params['pin']
         self._invert_step = pin_params['invert']
         self._dir_pin = self._invert_dir = None
-        self._commanded_pos = self._mcu_position_offset = 0.
+        self._mcu_position_offset = 0.
-        self._step_dist = self._inv_step_dist = 1.
+        self._step_dist = 0.
         self._min_stop_interval = 0.
         self._reset_cmd_id = self._get_position_cmd = None
         ffi_main, self._ffi_lib = chelper.get_ffi()
@@ -39,7 +39,6 @@ class MCU_stepper:
         self._min_stop_interval = min_stop_interval
     def setup_step_distance(self, step_dist):
         self._step_dist = step_dist
-        self._inv_step_dist = 1. / step_dist
     def setup_itersolve(self, sk):
         old_sk = self._stepper_kinematics
         self._stepper_kinematics = sk
@@ -73,16 +72,14 @@ class MCU_stepper:
     def get_step_dist(self):
         return self._step_dist
     def set_position(self, pos):
-        if self._stepper_kinematics is not None:
+        self._mcu_position_offset += self.get_commanded_position() - pos
-            self._ffi_lib.itersolve_set_commanded_pos(
+        self._ffi_lib.itersolve_set_commanded_pos(self._stepper_kinematics, pos)
-                self._stepper_kinematics, pos)
-        steppos = pos * self._inv_step_dist
-        self._mcu_position_offset += self._commanded_pos - steppos
-        self._commanded_pos = steppos
     def get_commanded_position(self):
-        return self._commanded_pos * self._step_dist
+        return self._ffi_lib.itersolve_get_commanded_pos(
+            self._stepper_kinematics)
     def get_mcu_position(self):
-        mcu_pos = self._commanded_pos + self._mcu_position_offset
+        pos_delta = self.get_commanded_position() + self._mcu_position_offset
+        mcu_pos = pos_delta / self._step_dist
         if mcu_pos >= 0.:
             return int(mcu_pos + 0.5)
         return int(mcu_pos - 0.5)
@@ -115,15 +112,15 @@ class MCU_stepper:
             return
         params = self._get_position_cmd.send_with_response(
             [self._oid], response='stepper_position', response_oid=self._oid)
-        pos = params['pos']
+        pos = params['pos'] * self._step_dist
         if self._invert_dir:
             pos = -pos
-        self._commanded_pos = pos - self._mcu_position_offset
+        self._ffi_lib.itersolve_set_commanded_pos(
+            self._stepper_kinematics, pos - self._mcu_position_offset)
     def step_itersolve(self, cmove):
-        count = self._itersolve_gen_steps(self._stepper_kinematics, cmove)
+        ret = self._itersolve_gen_steps(self._stepper_kinematics, cmove)
-        if count == STEPCOMPRESS_ERROR_RET:
+        if ret:
             raise error("Internal error in stepcompress")
-        self._commanded_pos += count
 
 class MCU_endstop:
     class TimeoutError(Exception):