From 86d5e87a091436610d7601f8252d2dc901224bbe Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Mon, 31 Jan 2022 19:46:04 +0100
Subject: [PATCH] motion: Implement Position/CurPosition returning AxisUnit

These come in handy to avoid manual casts and enforce types at runtime.
---
 src/modules/motion.h                      | 16 ++++++++++++++++
 tests/unit/modules/motion/test_motion.cpp |  4 ++++
 2 files changed, 20 insertions(+)

diff --git a/src/modules/motion.h b/src/modules/motion.h
index e37f43a..4fa2f10 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -166,6 +166,14 @@ public:
     /// @param axis axis affected
     pos_t Position(Axis axis) const;
 
+    /// @returns head position of an axis, but in AxisUnit.
+    /// The Axis needs to be supplied as the first template argument: Position<axis>().
+    /// @see Position
+    template <Axis A>
+    constexpr AxisUnit<pos_t, A, Lenght> Position() const {
+        return AxisUnit<pos_t, A, Lenght> { Position(A) };
+    }
+
     /// 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.
@@ -173,6 +181,14 @@ public:
     /// @returns the current position of the axis
     pos_t CurPosition(Axis axis) const { return axisData[axis].ctrl.CurPosition(); }
 
+    /// Fetch the current axis position, but in AxisUnit. This function is expensive!
+    /// The Axis needs to be supplied as the first template argument: CurPosition<axis>().
+    /// @see CurPosition
+    template <Axis A>
+    constexpr AxisUnit<pos_t, A, Lenght> CurPosition() const {
+        return AxisUnit<pos_t, A, Lenght> { CurPosition(A) };
+    }
+
     /// Set the position of an axis. Should only be called when the queue is empty.
     /// @param axis axis affected
     /// @param x position to set
diff --git a/tests/unit/modules/motion/test_motion.cpp b/tests/unit/modules/motion/test_motion.cpp
index 4d2f1dc..60bb1e8 100644
--- a/tests/unit/modules/motion/test_motion.cpp
+++ b/tests/unit/modules/motion/test_motion.cpp
@@ -93,6 +93,8 @@ TEST_CASE("motion::unit", "[motion]") {
     motion.PlanMoveTo<Selector>(10.0_S_mm, 100.0_S_mm_s);
     CHECK(stepUntilDone() != -1);
     REQUIRE(motion.Position(Selector) == target);
+    S_pos_t s_target = motion.Position<Selector>();
+    REQUIRE(s_target.v == target);
 
     // move directly with physical units
     motion.PlanMoveTo<Selector>(10.0_mm, 100.0_mm_s);
@@ -103,6 +105,8 @@ TEST_CASE("motion::unit", "[motion]") {
     motion.PlanMove<Selector>(-5.0_S_mm, 100.0_S_mm_s);
     CHECK(stepUntilDone() != -1);
     REQUIRE(motion.Position(Selector) == target / 2);
+    s_target = motion.Position<Selector>();
+    REQUIRE(s_target.v == target / 2);
 
     // relative move with physical unit
     motion.PlanMove<Selector>(-5.0_mm, 100.0_mm_s);