From 88a489e3cb9f833ebdab562efbc4458406075e3c Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Thu, 29 Dec 2022 09:23:43 +0100
Subject: [PATCH] Add runtime iHold <= iRun check

+ refactor TMC register compilation (saves 26B)
+ update currents registers range documentation
+ update currents even in MovableBase::InitMovementNoReinitAxis
---
 src/hal/tmc2130.cpp          | 51 +++++++++++++++++++++++++++++++++---
 src/modules/movable_base.cpp |  3 ++-
 src/registers.cpp            |  6 ++---
 3 files changed, 52 insertions(+), 8 deletions(-)

diff --git a/src/hal/tmc2130.cpp b/src/hal/tmc2130.cpp
index 98d3939..3811d95 100644
--- a/src/hal/tmc2130.cpp
+++ b/src/hal/tmc2130.cpp
@@ -161,10 +161,53 @@ void TMC2130::SetBridgeOutput(const MotorParams &params, bool bOn) {
 }
 
 void TMC2130::SetCurrents(const MotorParams &params, const MotorCurrents &currents) {
-    uint32_t ihold_irun = (uint32_t)(currents.iHold & 0x1F) << 0 // ihold
-        | (uint32_t)(currents.iRun & 0x1F) << 8 // irun
-        | (uint32_t)(15 & 0x0F) << 16; // IHOLDDELAY
-    WriteRegister(params, Registers::IHOLD_IRUN, ihold_irun);
+    uint8_t iHold = currents.iHold;
+    const uint8_t iRun = currents.iRun;
+
+    //    uint32_t ihold_irun = (uint32_t)(iHold & 0x1F) << 0 // ihold
+    //        | (uint32_t)(iRun & 0x1F) << 8 // irun
+    //        | (uint32_t)(15 & 0x0F) << 16; // IHOLDDELAY
+    // Rewriting the above code into a union makes it 18B shorter
+    // Obviously, those bit shifts and ORs were not understood correctly by the compiler...
+    // Now it looks nice:
+    // 15f6: push r16
+    // 15f8: push r17
+    // 15fa: movw r30, r20
+    // 15fc: ldd r16, Z+2
+    // 15fe: ldd r18, Z+1
+    // 1600: mov r17, r18
+    // 1602: andi r17, 0x1F
+    // 1604: cp r18, r16
+    // 1606: brcs .+18      ; 0x161a
+    // 1608: andi r16, 0x1F
+    // 160a: ldi r18, 0x0F
+    // 160c: ldi r19, 0x00
+    // 160e: ldi r20, 0x10
+    // 1610: call 0x1452    ; 0x1452 <hal::tmc2130::TMC2130::WriteRegister(...
+    // 1614: pop r17
+    // 1616: pop r16
+    // 1618: ret
+    // 161a: mov r16, r17
+    // 161c: rjmp .-20      ; 0x160a
+    union IHoldRun {
+        struct S {
+            uint8_t iHold;
+            uint8_t iRun;
+            uint16_t iHoldDelay;
+            constexpr S(uint8_t ih, uint8_t ir)
+                : iHold(ih)
+                , iRun(ir)
+                , iHoldDelay(15 & 0x0F) {}
+        } s;
+        uint32_t dw;
+        constexpr IHoldRun(uint8_t ih, uint8_t ir)
+            : s(ih, ir) {}
+    };
+
+    // also, make sure iHold never exceeds iRun at runtime
+    IHoldRun ihold_irun((iHold > iRun ? iRun : iHold) & 0x1f, iRun & 0x1f);
+
+    WriteRegister(params, Registers::IHOLD_IRUN, ihold_irun.dw);
 }
 
 void TMC2130::SetSGTHRS(const MotorParams &params) {
diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp
index 33eda21..44f4798 100644
--- a/src/modules/movable_base.cpp
+++ b/src/modules/movable_base.cpp
@@ -38,8 +38,9 @@ MovableBase::OperationResult MovableBase::InitMovement() {
 }
 
 MovableBase::OperationResult __attribute__((noinline)) MovableBase::InitMovementNoReinitAxis() {
+    hal::tmc2130::MotorCurrents c = mm::motion.CurrentsForAxis(axis);
+    SetCurrents(c.iRun, c.iHold);
     PrepareMoveToPlannedSlot();
-    // @@TODO update axis currents at this spot?
     state = Moving;
     return OperationResult::Accepted;
 }
diff --git a/src/registers.cpp b/src/registers.cpp
index 0446539..838cf21 100644
--- a/src/registers.cpp
+++ b/src/registers.cpp
@@ -160,9 +160,9 @@
 | 0x1bh 27 | uint16   | Set/Get_Selector_slot      | 0000h 0      | ffffh 65535 | unit slot [0-4/5] 5=park pos             | Read / Write | M707 A0x1b | M708 A0x1b Xn
 | 0x1ch 28 | uint16   | Set/Get_Idler_slot         | 0000h 0      | ffffh 65535 | unit slot [0-4/5] 5=disengaged           | Read / Write | M707 A0x1c | M708 A0x1c Xn
 | 0x1dh 29 | uint8    | Set/Get Selector cut iRun current | 0 to 63 (aka 0-1024mA)| 31 (530mA) |                | Read / Write | M707 A0x1d | M708 A0x1d Xn
-| 0x1eh 30 | uint16   | Set/Get Pulley iRun current| 0-63         | 14h 20      | 20->350mA: see TMC2130 current conversion| Read / Write | M707 A0x1e | M708 A0x1e Xn
-| 0x1fh 31 | uint16  |Set/Get Selector iRun current| 0-63         | 1fh 31      | 31->530mA: see TMC2130 current conversion| Read / Write | M707 A0x1f | M708 A0x1f Xn
-| 0x20h 32 | uint16   | Set/Get Idler iRun current | 0-63         | 1fh 31      | 31->530mA: see TMC2130 current conversion| Read / Write | M707 A0x20 | M708 A0x20 Xn
+| 0x1eh 30 | uint16   | Set/Get Pulley iRun current| 0-31         | 14h 20      | 20->350mA: see TMC2130 current conversion| Read / Write | M707 A0x1e | M708 A0x1e Xn
+| 0x1fh 31 | uint16  |Set/Get Selector iRun current| 0-31         | 1fh 31      | 31->530mA: see TMC2130 current conversion| Read / Write | M707 A0x1f | M708 A0x1f Xn
+| 0x20h 32 | uint16   | Set/Get Idler iRun current | 0-31         | 1fh 31      | 31->530mA: see TMC2130 current conversion| Read / Write | M707 A0x20 | M708 A0x20 Xn
 */
 
 struct RegisterFlags {