PulseGen: introduce CurPosition() for testing

CurPosition() returns the live axis position, which in this
implementation is inherently expensive to compute.

This shouldn't be required for the MMU, but it /will/ come in handy to
check for the axis position/s in Motion tests.

src/modules/pulse_gen.cpp

@@ -115,20 +115,34 @@ bool PulseGen::PlanMoveTo(pos_t target, steps_t feed_rate) {
     return true;
 }
 
+pos_t PulseGen::CurPosition() const {
+    pos_t cur_pos = position;
+    circular_index_t iter = block_index;
+
+    // if we have a live block remove the partial offset
+    if (current_block) {
+        cur_pos -= CurBlockShift();
+        iter.pop();
+    }
+
+    // rollback remaining blocks
+    while (!iter.empty()) {
+        cur_pos -= BlockShift(&block_buffer[iter.front()]);
+        iter.pop();
+    }
+
+    return cur_pos;
+}
+
 void PulseGen::AbortPlannedMoves() {
-    if (!current_block)
-        return;
+    // always update to effective position
+    position = CurPosition();
 
-    // update position
-    steps_t steps_missing = (current_block->steps - steps_completed);
-    if (current_block->direction)
-        position -= steps_missing;
-    else
-        position += steps_missing;
-
-    // destroy the block
+    // destroy the current block
+    if (current_block) {
         current_block = nullptr;
         block_index.pop();
+    }
 }
 
 } // namespace motor

src/modules/pulse_gen.h

@@ -14,6 +14,7 @@ using hal::tmc2130::TMC2130;
 using math::mulU24X24toH16;
 using speed_table::calc_timer;
 using speed_table::st_timer_t;
+typedef CircularIndex<uint8_t, blockBufferSize> circular_index_t;
 typedef uint32_t steps_t; ///< Absolute step units
 typedef uint32_t rate_t; ///< Type for step rates
 typedef int32_t pos_t; ///< Axis position (signed)
@@ -35,13 +36,20 @@ public:
     /// @returns true if the move has been enqueued
     bool PlanMoveTo(pos_t pos, steps_t feedrate);
 
-    /// stop whatever moves are being done
+    /// Stop whatever moves are being done
     void AbortPlannedMoves();
 
-    /// @returns the current position of the axis
+    /// @returns the position of the axis at the end of all moves
     pos_t Position() const { return position; }
 
+    /// Fetch the current position of the axis while stepping. This function is expensive!
+    /// It's necessary only in exceptional cases. For regular usage, Position() should
+    /// probably be used instead.
+    /// @returns the current position of the axis
+    pos_t CurPosition() const;
+
     /// Set the position of the axis
+    /// Should only be called when the queue is empty.
     void SetPosition(pos_t x) { position = x; }
 
     /// @returns true if all planned moves have been finished
@@ -150,7 +158,7 @@ private:
 
     // Block buffer parameters
     block_t block_buffer[blockBufferSize];
-    CircularIndex<uint8_t, blockBufferSize> block_index;
+    circular_index_t block_index;
     block_t *current_block;
 
     // Axis data
@@ -168,6 +176,17 @@ private:
 
     /// Calculate the trapezoid parameters for the block
     void CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t exit_speed);
+
+    /// Return the axis shift introduced by the current (live) block
+    inline pos_t CurBlockShift() const {
+        steps_t steps_missing = (current_block->steps - steps_completed);
+        return current_block->direction ? steps_missing : -steps_missing;
+    }
+
+    /// Return the axis shift introduced by the specified full block
+    static inline pos_t BlockShift(const block_t *block) {
+        return block->direction ? block->steps : -block->steps;
+    }
 };
 
 } // namespace pulse_gen

tests/unit/modules/pulse_gen/test_pulse_gen.cpp

@@ -112,10 +112,14 @@ TEST_CASE("pulse_gen::step_count", "[pulse_gen]") {
     pg.PlanMoveTo(10, 10);
     bool st = ReadPin(IDLER_STEP_PIN) == Level::high;
     for (size_t i = 0; i != 10; ++i) {
+        // check current axis position
+        REQUIRE((pos_t)i == pg.CurPosition());
+
+        // perform the step
         REQUIRE(pg.Step(mp) > 0);
         bool newSt = ReadPin(IDLER_STEP_PIN) == Level::high;
 
-        // Assuming DEDGE each step should toggle the pin
+        // assuming DEDGE each step should toggle the pin
         REQUIRE(newSt != st);
         st = newSt;
     }
@@ -129,6 +133,42 @@ TEST_CASE("pulse_gen::step_count", "[pulse_gen]") {
     REQUIRE(pg.Position() == 10);
 }
 
+TEST_CASE("pulse_gen::queue_position", "[pulse_gen]") {
+    MotorParams mp = {
+        .idx = 0,
+        .dirOn = config::idler.dirOn,
+        .csPin = IDLER_CS_PIN,
+        .stepPin = IDLER_STEP_PIN,
+        .sgPin = IDLER_SG_PIN,
+        .uSteps = config::idler.uSteps
+    };
+
+    PulseGen pg(10, 100);
+
+    // enqueue two moves, observing Position and CurPosition.
+    REQUIRE(pg.Position() == 0);
+    REQUIRE(pg.CurPosition() == 0);
+
+    // while enqueuing Position should move but CurPosition should not
+    pg.PlanMoveTo(10, 10);
+    REQUIRE(pg.Position() == 10);
+    REQUIRE(pg.CurPosition() == 0);
+
+    pg.PlanMoveTo(15, 10);
+    REQUIRE(pg.Position() == 15);
+    REQUIRE(pg.CurPosition() == 0);
+
+    // step through the moves manually, cycling through two blocks
+    for (size_t i = 0; i != 15; ++i) {
+        REQUIRE((pos_t)i == pg.CurPosition());
+        REQUIRE(pg.Position() == 15);
+        pg.Step(mp);
+    }
+
+    // the final positions should match
+    REQUIRE(pg.CurPosition() == pg.Position());
+}
+
 TEST_CASE("pulse_gen::queue_size", "[pulse_gen]") {
     PulseGen pg(10, 100);