stepper: Introduce stepper_get_position command and remove from endstop.c

Move the logic to calculate and report the stepper's current position
from endstop.c to stepper.c.  This localizes the stepper code into
stepper.c.

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

docs/Firmware_Commands.md

@@ -251,6 +251,12 @@ Stepper commands
   so that the next step is relative to the supplied 'clock' time. The
   host usually only sends this command at the start of a print.
 
+* stepper_get_position oid=%c : This command causes the firmware to
+  generate a "stepper_position" response message with the stepper's
+  current position.  The position is the total number of steps
+  generated with dir=1 minus the total number of steps generated with
+  dir=0.
+
 * end_stop_home oid=%c clock=%u rest_ticks=%u pin_value=%c : This
   command is used during stepper "homing" operations. To use this
   command a 'config_end_stop' command with the same 'oid' parameter

klippy/mcu.py

@@ -43,6 +43,8 @@ class MCU_stepper:
             "set_next_step_dir oid=%c dir=%c")
         self._reset_cmd = mcu.lookup_command(
             "reset_step_clock oid=%c clock=%u")
+        self._get_position_cmd = mcu.lookup_command(
+            "stepper_get_position oid=%c")
         ffi_main, self.ffi_lib = chelper.get_ffi()
         self._stepqueue = ffi_main.gc(self.ffi_lib.stepcompress_alloc(
             max_error, self._step_cmd.msgid
@@ -51,13 +53,9 @@ class MCU_stepper:
         self.print_to_mcu_time = mcu.print_to_mcu_time
     def get_oid(self):
         return self._oid
-    def get_invert_dir(self):
-        return self._invert_dir
     def set_position(self, pos):
         self._mcu_position_offset += self.commanded_position - pos
         self.commanded_position = pos
-    def set_mcu_position(self, pos):
-        self._mcu_position_offset = pos - self.commanded_position
     def get_mcu_position(self):
         return self.commanded_position + self._mcu_position_offset
     def note_homing_start(self, homing_clock):
@@ -71,6 +69,14 @@ class MCU_stepper:
         ret = self.ffi_lib.stepcompress_reset(self._stepqueue, 0)
         if ret:
             raise error("Internal error in stepcompress")
+    def note_homing_triggered(self):
+        params = self._mcu.serial.send_with_response(
+            self._get_position_cmd.encode(self._oid),
+            'stepper_position', self._oid)
+        pos = params['pos']
+        if self._invert_dir:
+            pos = -pos
+        self._mcu_position_offset = pos - self.commanded_position
     def reset_step_clock(self, mcu_time):
         clock = int(mcu_time * self._mcu_freq)
         ret = self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
@@ -188,10 +194,7 @@ class MCU_endstop:
             if not self._homing:
                 return False
             if not self._last_state.get('homing', 0):
-                pos = self._last_state.get('pos', 0)
-                if self._stepper.get_invert_dir():
-                    pos = -pos
-                self._stepper.set_mcu_position(pos)
+                self._stepper.note_homing_triggered()
                 self._homing = False
                 return False
             if (self._mcu.serial.get_clock(last_sent_time)

klippy/serialhdl.py

@@ -171,8 +171,8 @@ class SerialReader:
     def encode_and_send(self, data, minclock, reqclock, cq):
         self.ffi_lib.serialqueue_encode_and_send(
             self.serialqueue, cq, data, len(data), minclock, reqclock)
-    def send_with_response(self, cmd, name):
-        src = SerialRetryCommand(self, cmd, name)
+    def send_with_response(self, cmd, name, oid=None):
+        src = SerialRetryCommand(self, cmd, name, oid)
         return src.get_response()
     def alloc_command_queue(self):
         return self.ffi_main.gc(self.ffi_lib.serialqueue_alloc_commandqueue(),
@@ -235,17 +235,18 @@ class SerialReader:
 class SerialRetryCommand:
     TIMEOUT_TIME = 5.0
     RETRY_TIME = 0.500
-    def __init__(self, serial, cmd, name):
+    def __init__(self, serial, cmd, name, oid=None):
         self.serial = serial
         self.cmd = cmd
         self.name = name
+        self.oid = oid
         self.response = None
         self.min_query_time = self.serial.reactor.monotonic()
-        self.serial.register_callback(self.handle_callback, self.name)
+        self.serial.register_callback(self.handle_callback, self.name, self.oid)
         self.send_timer = self.serial.reactor.register_timer(
             self.send_event, self.serial.reactor.NOW)
     def unregister(self):
-        self.serial.unregister_callback(self.name)
+        self.serial.unregister_callback(self.name, self.oid)
         self.serial.reactor.unregister_timer(self.send_timer)
     def send_event(self, eventtime):
         if self.response is not None:

src/endstop.c

@@ -42,7 +42,7 @@ void
 command_config_end_stop(uint32_t *args)
 {
     struct end_stop *e = oid_alloc(args[0], command_config_end_stop, sizeof(*e));
-    struct stepper *s = oid_lookup(args[3], command_config_stepper);
+    struct stepper *s = stepper_oid_lookup(args[3]);
     e->time.func = end_stop_event;
     e->stepper = s;
     e->pin = gpio_in_setup(args[1], args[2]);
@@ -73,14 +73,12 @@ static void
 end_stop_report(uint8_t oid, struct end_stop *e)
 {
     irq_disable();
-    uint32_t position = stepper_get_position(e->stepper);
     uint8_t eflags = e->flags;
     e->flags &= ~ESF_REPORT;
     irq_enable();
 
-    sendf("end_stop_state oid=%c homing=%c pin=%c pos=%i"
-          , oid, !!(eflags & ESF_HOMING), gpio_in_read(e->pin)
-          , position - STEPPER_POSITION_BIAS);
+    sendf("end_stop_state oid=%c homing=%c pin=%c"
+          , oid, !!(eflags & ESF_HOMING), gpio_in_read(e->pin));
 }
 
 void

src/stepper.c

@@ -46,6 +46,8 @@ struct stepper {
     uint8_t flags : 8;
 };
 
+enum { POSITION_BIAS=0x40000000 };
+
 enum { SF_LAST_DIR=1<<0, SF_NEXT_DIR=1<<1, SF_INVERT_STEP=1<<2, SF_HAVE_ADD=1<<3,
        SF_LAST_RESET=1<<4, SF_NO_NEXT_CHECK=1<<5 };
 
@@ -152,18 +154,25 @@ command_config_stepper(uint32_t *args)
     s->step_pin = gpio_out_setup(args[1], s->flags & SF_INVERT_STEP ? 1 : 0);
     s->dir_pin = gpio_out_setup(args[2], 0);
     s->min_stop_interval = args[3];
-    s->position = -STEPPER_POSITION_BIAS;
+    s->position = -POSITION_BIAS;
     move_request_size(sizeof(struct stepper_move));
 }
 DECL_COMMAND(command_config_stepper,
              "config_stepper oid=%c step_pin=%c dir_pin=%c"
              " min_stop_interval=%u invert_step=%c");
 
+// Return the 'struct stepper' for a given stepper oid
+struct stepper *
+stepper_oid_lookup(uint8_t oid)
+{
+    return oid_lookup(oid, command_config_stepper);
+}
+
 // Schedule a set of steps with a given timing
 void
 command_queue_step(uint32_t *args)
 {
-    struct stepper *s = oid_lookup(args[0], command_config_stepper);
+    struct stepper *s = stepper_oid_lookup(args[0]);
     struct stepper_move *m = move_alloc();
     m->interval = args[1];
     m->count = args[2];
@@ -204,7 +213,7 @@ DECL_COMMAND(command_queue_step,
 void
 command_set_next_step_dir(uint32_t *args)
 {
-    struct stepper *s = oid_lookup(args[0], command_config_stepper);
+    struct stepper *s = stepper_oid_lookup(args[0]);
     uint8_t nextdir = args[1] ? SF_NEXT_DIR : 0;
     irq_disable();
     s->flags = (s->flags & ~SF_NEXT_DIR) | nextdir;
@@ -216,7 +225,7 @@ DECL_COMMAND(command_set_next_step_dir, "set_next_step_dir oid=%c dir=%c");
 void
 command_reset_step_clock(uint32_t *args)
 {
-    struct stepper *s = oid_lookup(args[0], command_config_stepper);
+    struct stepper *s = stepper_oid_lookup(args[0]);
     uint32_t waketime = args[1];
     irq_disable();
     if (s->count)
@@ -228,7 +237,7 @@ command_reset_step_clock(uint32_t *args)
 DECL_COMMAND(command_reset_step_clock, "reset_step_clock oid=%c clock=%u");
 
 // Return the current stepper position.  Caller must disable irqs.
-uint32_t
+static uint32_t
 stepper_get_position(struct stepper *s)
 {
     uint32_t position = s->position;
@@ -241,6 +250,19 @@ stepper_get_position(struct stepper *s)
     return position;
 }
 
+// Report the current position of the stepper
+void
+command_stepper_get_position(uint32_t *args)
+{
+    uint8_t oid = args[0];
+    struct stepper *s = stepper_oid_lookup(oid);
+    irq_disable();
+    uint32_t position = stepper_get_position(s);
+    irq_enable();
+    sendf("stepper_position oid=%c pos=%i", oid, position - POSITION_BIAS);
+}
+DECL_COMMAND(command_stepper_get_position, "stepper_get_position oid=%c");
+
 // Stop all moves for a given stepper (used in end stop homing).  IRQs
 // must be off.
 void

src/stepper.h

@@ -3,12 +3,8 @@
 
 #include <stdint.h> // uint8_t
 
-enum { STEPPER_POSITION_BIAS=0x40000000 };
-
 uint_fast8_t stepper_event(struct timer *t);
-void command_config_stepper(uint32_t *args);
-struct stepper;
-uint32_t stepper_get_position(struct stepper *s);
+struct stepper *stepper_oid_lookup(uint8_t oid);
 void stepper_stop(struct stepper *s);
 
 #endif // stepper.h