From 4e4e2df7394ef0ebac668eb2cc044e6a3f074fc3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Gu=C3=B0ni=20M=C3=A1r=20Gilbert?= <gudni.m.g@gmail.com>
Date: Wed, 25 Dec 2024 14:54:13 +0000
Subject: [PATCH] Improve accuracy in axisUnitToTruncatedUnit

When converting 800mm/s2, it would be truncated to 795mm/s2 for the pulley. This is due to cutting out significant decimal digits.

Instead let's multiply in floating point, this needs quite a bit of resources. So to optimise against this, multiply with the recoprical. Then the cost is not more than 20 bytes.

Testing:
M707 A0x0e; Read Pulley Acceleration (default at boot up is 800mm/s2)
M708 A0x0e X790 ; Set Pulley Acceleration to 790mm/s2
M707 A0x0e; Read Pulley Acceleration (should be 790mm/s2)

The results before this commit:

M707 A0x0e -> returns 805
M708 A0x0e X790 ; Set Pulley Acceleration to 790mm/s2
M707 A0x0e; returns 795

After this commit:

M707 A0x0e -> returns 799
M708 A0x0e X790 ; Set Pulley Acceleration to 790mm/s2
M707 A0x0e; returns 789

NOTE:
axisUnitToTruncatedUnit is used in Idler homing, selector homing, and pulley positioning. I am not sure yet how this improvement will affect those areas.
---
 src/config/axis.h      | 1 +
 src/config/config.h    | 3 +++
 src/modules/axisunit.h | 9 +++++----
 3 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/src/config/axis.h b/src/config/axis.h
index c9d9d69..3e484a4 100644
--- a/src/config/axis.h
+++ b/src/config/axis.h
@@ -28,6 +28,7 @@ struct AxisConfig {
     uint8_t iHold; ///< holding current
     bool stealth; ///< Default to Stealth mode
     long double stepsPerUnit; ///< steps per unit
+    long double stepsPerUnitReciprocal; ///< reciprocal of step per unit (used to avoid divisions)
     int8_t sg_thrs; /// @todo 7bit two's complement for the sg_thrs
 };
 
diff --git a/src/config/config.h b/src/config/config.h
index 313d641..7fd8d26 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -115,6 +115,7 @@ static constexpr AxisConfig pulley = {
     .iHold = 0, /// 17mA in SpreadCycle, freewheel in StealthChop
     .stealth = false,
     .stepsPerUnit = (200 * 8 / 19.147274),
+    .stepsPerUnitReciprocal = 1 / ((200 * 8 / 19.147274)),
     .sg_thrs = 8,
 };
 
@@ -140,6 +141,7 @@ static constexpr AxisConfig selector = {
     .iHold = 0, /// 17mA in SpreadCycle, freewheel in StealthChop
     .stealth = false,
     .stepsPerUnit = (200 * 8 / 8.),
+    .stepsPerUnitReciprocal = 1 / ((200 * 8 / 8.)),
     .sg_thrs = 3,
 };
 
@@ -190,6 +192,7 @@ static constexpr AxisConfig idler = {
     .iHold = 5, /// 99mA - parked current
     .stealth = false,
     .stepsPerUnit = (200 * 16 / 360.),
+    .stepsPerUnitReciprocal = 1 / ((200 * 16 / 360.)),
     .sg_thrs = 7,
 };
 
diff --git a/src/modules/axisunit.h b/src/modules/axisunit.h
index 9dc413c..bbbcf1e 100644
--- a/src/modules/axisunit.h
+++ b/src/modules/axisunit.h
@@ -94,12 +94,13 @@ constexpr AxisUnit<T, A, U> operator*(const long double f, const AxisUnit<T, A,
 struct AxisScale {
     unit::UnitBase base;
     long double stepsPerUnit;
+    long double stepsPerUnitReciprocal;
 };
 
 static constexpr AxisScale axisScale[config::NUM_AXIS] = {
-    { config::pulleyLimits.base, config::pulley.stepsPerUnit },
-    { config::selectorLimits.base, config::selector.stepsPerUnit },
-    { config::idlerLimits.base, config::idler.stepsPerUnit },
+    { config::pulleyLimits.base, config::pulley.stepsPerUnit, config::pulley.stepsPerUnitReciprocal },
+    { config::selectorLimits.base, config::selector.stepsPerUnit, config::selector.stepsPerUnitReciprocal },
+    { config::idlerLimits.base, config::idler.stepsPerUnit, config::idler.stepsPerUnitReciprocal },
 };
 
 /// Convert a unit::Unit to AxisUnit.
@@ -126,7 +127,7 @@ template <typename U, typename AU, typename T = int32_t>
 static constexpr T axisUnitToTruncatedUnit(AU v, long double mul = 1.) {
     static_assert(AU::unit == U::unit, "incorrect unit type conversion");
     static_assert(U::base == axisScale[AU::axis].base, "incorrect unit base conversion");
-    return { ((T)v.v / (T)(axisScale[AU::axis].stepsPerUnit / mul)) };
+    return { ((T)(v.v * (axisScale[AU::axis].stepsPerUnitReciprocal / mul))) };
 }
 
 /// Truncate an Unit type to an integer (normally int32_t)