Merge pull request #4601 from leptun/MK3_TMC_restore_currents_post_home

Mk3 TMC restore currents post home
prusa3d · Feb 19, 2024 · c5a036d · c5a036d
2 parents f80fb4c + ccb956e
commit c5a036d
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 41 deletions.
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
@@ -30,18 +30,25 @@ static constexpr uint8_t default_dedge_bit = 0;
 
 //mode
 uint8_t tmc2130_mode = TMC2130_MODE_NORMAL;
-uint8_t tmc2130_current_h[4] = TMC2130_CURRENTS_H;
-//running currents
-uint8_t tmc2130_current_r[4] = TMC2130_CURRENTS_R;
 
+static constexpr uint8_t tmc2130_default_current_h[4] = TMC2130_CURRENTS_H;
+//running currents
+static constexpr uint8_t tmc2130_default_current_r[4] = TMC2130_CURRENTS_R;
 //running currents for homing
-static uint8_t tmc2130_current_r_home[4] = TMC2130_CURRENTS_R_HOME;
+static constexpr uint8_t tmc2130_current_r_home[4] = TMC2130_CURRENTS_R_HOME;
+
+static constexpr MotorCurrents homing_currents_P[NUM_AXIS] PROGMEM = {
+	MotorCurrents(tmc2130_current_r_home[0], tmc2130_current_r_home[0]),
+	MotorCurrents(tmc2130_current_r_home[1], tmc2130_current_r_home[1]),
+	MotorCurrents(tmc2130_current_r_home[2], tmc2130_current_r_home[2]),
+	MotorCurrents(tmc2130_current_r_home[3], tmc2130_current_r_home[3])
+};
 
 MotorCurrents currents[NUM_AXIS] = {
-	MotorCurrents(tmc2130_current_r[0], tmc2130_current_h[0]),
-	MotorCurrents(tmc2130_current_r[1], tmc2130_current_h[1]),
-	MotorCurrents(tmc2130_current_r[2], tmc2130_current_h[2]),
-	MotorCurrents(tmc2130_current_r[3], tmc2130_current_h[3])
+	MotorCurrents(tmc2130_default_current_r[0], tmc2130_default_current_h[0]),
+	MotorCurrents(tmc2130_default_current_r[1], tmc2130_default_current_h[1]),
+	MotorCurrents(tmc2130_default_current_r[2], tmc2130_default_current_h[2]),
+	MotorCurrents(tmc2130_default_current_r[3], tmc2130_default_current_h[3])
 };
 
 union ChopConfU {
@@ -422,8 +429,8 @@ void tmc2130_home_enter(uint8_t axes_mask)
 			tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
 			tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr_home[axis]) << 16));
 			tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, __tcoolthrs(axis));
-			currents[axis].setiRun(tmc2130_current_r_home[axis]);
-			tmc2130_setup_chopper(axis, tmc2130_mres[axis]);
+			MotorCurrents curr(homing_currents_P[axis]);
+			tmc2130_setup_chopper(axis, tmc2130_mres[axis], &curr);
 			tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS); //stallguard output DIAG1, DIAG1 = pushpull
 		}
 	}
@@ -519,17 +526,12 @@ static constexpr bool getIntpolBit([[maybe_unused]]const uint8_t axis, const uin
     return (mres != 0); // intpol to 256 only if microsteps aren't 256
 }
 
-static void SetCurrents(const uint8_t axis) {
-    uint8_t iHold = currents[axis].getiHold();
-    const uint8_t iRun = currents[axis].getiRun();
+static void SetCurrents(const uint8_t axis, const MotorCurrents &curr) {
+    uint8_t iHold = curr.getiHold();
+    const uint8_t iRun = curr.getiRun();
 
     // Make sure iHold never exceeds iRun at runtime
-    if (iHold > iRun) {
-        iHold = iRun;
-
-        // Update global array such that M913 reports correct values
-        currents[axis].setiHold(iRun);
-
+    if (curr.iHoldIsClamped()) {
         // Let user know firmware modified the value
         SERIAL_ECHO_START;
         SERIAL_ECHOLNRPGM(_n("Hold current truncated to Run current"));
@@ -555,7 +557,7 @@ static void SetCurrents(const uint8_t axis) {
     tmc2130_wr(axis, TMC2130_REG_IHOLD_IRUN, ihold_irun.dw);
 }
 
-void tmc2130_setup_chopper(uint8_t axis, uint8_t mres)
+void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, const MotorCurrents *curr /* = nullptr */)
 {
 	// Initialise the chopper configuration
 	ChopConfU chopconf = ChopConfU(currents[axis].getvSense(), mres);
@@ -567,21 +569,10 @@ void tmc2130_setup_chopper(uint8_t axis, uint8_t mres)
 	chopconf.s.tbl = tmc2130_chopper_config[axis].tbl; //blanking time, original value = 2
 
 	tmc2130_wr(axis, TMC2130_REG_CHOPCONF, chopconf.dw);
-	SetCurrents(axis);
-}
-
-void tmc2130_set_current_h(uint8_t axis, uint8_t current)
-{
-//	DBG(_n("tmc2130_set_current_h(axis=%d, current=%d\n"), axis, current);
-	currents[axis].setiHold(current);
-	tmc2130_setup_chopper(axis, tmc2130_mres[axis]);
-}
-
-void tmc2130_set_current_r(uint8_t axis, uint8_t current)
-{
-//	DBG(_n("tmc2130_set_current_r(axis=%d, current=%d\n"), axis, current);
-	currents[axis].setiRun(current);
-	tmc2130_setup_chopper(axis, tmc2130_mres[axis]);
+	if (curr == nullptr) {
+		curr = &currents[axis];
+	}
+	SetCurrents(axis, *curr);
 }
 
 void tmc2130_print_currents()

diff --git a/Firmware/tmc2130.h b/Firmware/tmc2130.h
@@ -95,13 +95,17 @@ struct MotorCurrents {
         }
     }
 
+    // PROGMEM initializer
+    inline __attribute__((always_inline)) MotorCurrents(const MotorCurrents &curr_P) { memcpy_P(this, &curr_P, sizeof(*this)); }
+
     constexpr inline __attribute__((always_inline)) MotorCurrents(uint8_t ir, uint8_t ih)
         : vSense((ir < 32) ? 1 : 0)
         , iRun((ir < 32) ? ir : (ir >> 1))
         , iHold((ir < 32) ? ih : (ih >> 1)) {}
 
     inline uint8_t getiRun() const { return iRun; }
-    inline uint8_t getiHold() const { return iHold; }
+    inline uint8_t getiHold() const { return min(iHold, iRun); }
+    inline bool iHoldIsClamped() const { return iHold > iRun; }
     inline uint8_t getvSense() const { return vSense; }
 
     void __attribute__((noinline)) setiRun(uint8_t ir) {
@@ -165,12 +169,8 @@ extern uint16_t tmc2130_sg_measure_stop();
 // Enable or Disable crash detection according to EEPROM
 void crashdet_use_eeprom_setting();
 
-extern void tmc2130_setup_chopper(uint8_t axis, uint8_t mres);
+extern void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, const MotorCurrents *curr = nullptr);
 
-//set holding current for any axis (M911)
-extern void tmc2130_set_current_h(uint8_t axis, uint8_t current);
-//set running current for any axis (M912)
-extern void tmc2130_set_current_r(uint8_t axis, uint8_t current);
 //print currents (M913)
 extern void tmc2130_print_currents();