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>

3 files changed, 31 insertions, 15 deletions

diff --git a/src/endstop.c b/src/endstop.c
index 3701f642..913bb466 100644
--- a/src/endstop.c
+++ b/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
diff --git a/src/stepper.c b/src/stepper.c
index 39fdab54..b196dc61 100644
--- a/src/stepper.c
+++ b/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
diff --git a/src/stepper.h b/src/stepper.h
index f4019ae5..8e27b789 100644
--- a/src/stepper.h
+++ b/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