From 4b1fc690888c52ddf4edf3cefaeb41231b423bf2 Mon Sep 17 00:00:00 2001
From: MRprusa3d <martin.remis@prusa3d.cz>
Date: Tue, 2 Apr 2019 20:04:44 +0200
Subject: [PATCH] "disable_z()" for Delta PSU III

stealthChop set if motor-disable (@ Z) required
---
 Firmware/Marlin.h        |  4 +++
 Firmware/Marlin_main.cpp | 61 +++++++++++++++++++++++++---------------
 Firmware/stepper.cpp     |  5 +++-
 3 files changed, 46 insertions(+), 24 deletions(-)

diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 9ccb81c6f..1516941b9 100644
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -173,9 +173,13 @@ void manage_inactivity(bool ignore_stepper_queue=false);
 #endif
 
 #ifdef PSU_Delta
+void init_force_z();
 #undef disable_z()
 #define disable_z() disable_force_z()
 void disable_force_z();
+#undef enable_disable_z()
+#define enable_z() enable_force_z()
+void enable_force_z();
 #endif // PSU_Delta
 
 
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index d119fbe43..8aa2c6c8d 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -9604,35 +9604,27 @@ if((eSoundMode==e_SOUND_MODE_LOUD)||(eSoundMode==e_SOUND_MODE_ONCE))
 #define FIL_LOAD_LENGTH 60
 
 #ifdef PSU_Delta
+bool bEnableForce_z;
+
+void init_force_z()
+{
+WRITE(Z_ENABLE_PIN,Z_ENABLE_ON);
+bEnableForce_z=true;                              // "true"-value enforce "disable_force_z()" executing
+disable_force_z();
+}
+
 void disable_force_z()
 {
 uint16_t z_microsteps=0;
 
-if(PIN_INQ(Z_ENABLE_PIN)!=Z_ENABLE_ON)
+if(!bEnableForce_z)
      return;                                      // motor already disabled (may be ;-p )
+bEnableForce_z=false;
+
+// alignment to full-step
 #ifdef TMC2130
 z_microsteps=tmc2130_rd_MSCNT(Z_TMC2130_CS);
-#endif //TMC2130
-/*
-//planner_abort_hard();
-//sei();
-plan_buffer_line(
-     current_position[X_AXIS]+10, 
-     current_position[Y_AXIS]+20, 
-//     current_position[Z_AXIS]+float((1024-z_microsteps+7)>>4)/axis_steps_per_unit[Z_AXIS], 
-current_position[Z_AXIS]+0.026, //0.025, 
-     current_position[E_AXIS],
-     40, active_extruder);
-st_synchronize();
- z_microsteps=tmc2130_rd_MSCNT(Z_TMC2130_CS);
- MYSERIAL.println(z_microsteps,DEC);
-
-  MYSERIAL.println(current_position[Z_AXIS]+float((1024-z_microsteps+7)>>4)/cs.axis_steps_per_unit[Z_AXIS],DEC);
-  MYSERIAL.println(current_position[Z_AXIS],DEC);
-  MYSERIAL.println(z_microsteps,DEC);
-  MYSERIAL.println(cs.axis_steps_per_unit[Z_AXIS],DEC);
-  MYSERIAL.println(float((1024-z_microsteps+7)>>4)/cs.axis_steps_per_unit[Z_AXIS],DEC);
-*/
+#endif // TMC2130
 planner_abort_hard();
 sei();
 plan_buffer_line(
@@ -9643,7 +9635,30 @@ plan_buffer_line(
      40, active_extruder);
 st_synchronize();
 
-WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON);
+// switching to silent mode
+#ifdef TMC2130
+tmc2130_mode=TMC2130_MODE_SILENT;
+update_mode_profile();
+tmc2130_init();
+#endif // TMC2130
+
 axis_known_position[Z_AXIS]=false; 
 }
+
+
+void enable_force_z()
+{
+if(bEnableForce_z)
+     return;                                      // motor already enabled (may be ;-p )
+bEnableForce_z=true;
+
+// mode recovering
+#ifdef TMC2130
+tmc2130_mode=eeprom_read_byte((uint8_t*)EEPROM_SILENT)?TMC2130_MODE_SILENT:TMC2130_MODE_NORMAL;
+update_mode_profile();
+tmc2130_init();
+#endif // TMC2130
+
+WRITE(Z_ENABLE_PIN,Z_ENABLE_ON);                  // slightly redundant ;-p
+}
 #endif // PSU_Delta
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index c21a4e9f1..dfb2dbe27 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -1122,7 +1122,7 @@ void clear_current_adv_vars() {
 }
 
 #endif // LIN_ADVANCE
-      
+
 void st_init()
 {
 #ifdef TMC2130
@@ -1306,6 +1306,9 @@ void st_init()
       SET_OUTPUT(Z2_STEP_PIN);
       WRITE(Z2_STEP_PIN,INVERT_Z_STEP_PIN);
     #endif
+    #ifdef PSU_Delta
+      init_force_z();
+    #endif // PSU_Delta
     disable_z();
   #endif
   #if defined(E0_STEP_PIN) && (E0_STEP_PIN > -1)