From 1d8c24e8a9877eaf97240b73f53d69650f1093af Mon Sep 17 00:00:00 2001
From: MRprusa3d <martin.remis@prusa3d.cz>
Date: Wed, 5 Jun 2019 19:22:04 +0200
Subject: [PATCH] "disable_z()" for Delta PSU IV

correction for (motor) mode switching
---
 Firmware/Marlin.h        |  1 +
 Firmware/Marlin_main.cpp | 13 +++++++++++--
 Firmware/tmc2130.cpp     |  7 ++++++-
 Firmware/tmc2130.h       |  2 +-
 4 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 1516941b..149d3298 100644
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -174,6 +174,7 @@ void manage_inactivity(bool ignore_stepper_queue=false);
 
 #ifdef PSU_Delta
 void init_force_z();
+void check_force_z();
 #undef disable_z()
 #define disable_z() disable_force_z()
 void disable_force_z();
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 8aa2c6c8..2b873354 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1279,6 +1279,9 @@ void setup()
 	update_mode_profile();
 	tmc2130_init();
 #endif //TMC2130
+#ifdef PSU_Delta
+     init_force_z();                              // ! important for correct Z-axis initialization
+#endif // PSU_Delta
     
 	setup_photpin();
 
@@ -9613,6 +9616,12 @@ bEnableForce_z=true;                              // "true"-value enforce "disab
 disable_force_z();
 }
 
+void check_force_z()
+{
+if(!(bEnableForce_z||eeprom_read_byte((uint8_t*)EEPROM_SILENT)))
+     init_force_z();                              // causes enforced switching into disable-state
+}
+
 void disable_force_z()
 {
 uint16_t z_microsteps=0;
@@ -9639,7 +9648,7 @@ st_synchronize();
 #ifdef TMC2130
 tmc2130_mode=TMC2130_MODE_SILENT;
 update_mode_profile();
-tmc2130_init();
+tmc2130_init(true);
 #endif // TMC2130
 
 axis_known_position[Z_AXIS]=false; 
@@ -9656,7 +9665,7 @@ bEnableForce_z=true;
 #ifdef TMC2130
 tmc2130_mode=eeprom_read_byte((uint8_t*)EEPROM_SILENT)?TMC2130_MODE_SILENT:TMC2130_MODE_NORMAL;
 update_mode_profile();
-tmc2130_init();
+tmc2130_init(true);
 #endif // TMC2130
 
 WRITE(Z_ENABLE_PIN,Z_ENABLE_ON);                  // slightly redundant ;-p
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index 8cd3422a..7c35815c 100644
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -143,7 +143,7 @@ uint16_t __tcoolthrs(uint8_t axis)
 	return 0;
 }
 
-void tmc2130_init()
+void tmc2130_init(bool bSupressFlag=false)
 {
 //	DBG(_n("tmc2130_init(), mode=%S\n"), tmc2130_mode?_n("STEALTH"):_n("NORMAL"));
 	WRITE(X_TMC2130_CS, HIGH);
@@ -216,6 +216,11 @@ void tmc2130_init()
 	tmc2130_set_wave(E_AXIS, 247, tmc2130_wave_fac[E_AXIS]);
 #endif //TMC2130_LINEARITY_CORRECTION
 
+#ifdef PSU_Delta
+     if(!bSupressFlag)
+          check_force_z();
+#endif // PSU_Delta
+
 }
 
 uint8_t tmc2130_sample_diag()
diff --git a/Firmware/tmc2130.h b/Firmware/tmc2130.h
index 1c854f79..a1d160f0 100644
--- a/Firmware/tmc2130.h
+++ b/Firmware/tmc2130.h
@@ -51,7 +51,7 @@ typedef struct
 extern tmc2130_chopper_config_t tmc2130_chopper_config[4];
 
 //initialize tmc2130
-extern void tmc2130_init();
+extern void tmc2130_init(bool bSupressFlag=false);
 //check diag pins (called from stepper isr)
 extern void tmc2130_st_isr();
 //update stall guard (called from st_synchronize inside the loop)