From 1384e783bfb422afe25bc88b8705203ecc6b26e4 Mon Sep 17 00:00:00 2001
From: vintagepc <53943260+vintagepc@users.noreply.github.com>
Date: Fri, 7 Feb 2020 16:14:33 -0500
Subject: [PATCH 1/4] Force high power mode when running belt test

---
 Firmware/ultralcd.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index cefd9b8c5..4e0057acd 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -7455,6 +7455,11 @@ void lcd_belttest()
 {
     int _progress = 0;
     bool _result = true;
+    
+    #ifdef TMC2130 // Belttest requires high power mode. Enable it.
+	    FORCE_HIGH_POWER_START;
+    #endif
+    
     uint16_t   X = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_X));
     uint16_t   Y = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_Y));
     lcd_belttest_print(_i("Checking X..."), X, Y);
@@ -7482,6 +7487,10 @@ void lcd_belttest()
 
     lcd_belttest_print(_i("Done"), X, Y);
 
+    #ifdef TMC2130
+	    FORCE_HIGH_POWER_END;
+    #endif
+    
     KEEPALIVE_STATE(NOT_BUSY);
     _delay(3000);
 }

From 721b27fcb6984d11f009ea77d21a93923e2397aa Mon Sep 17 00:00:00 2001
From: vintagepc <53943260+vintagepc@users.noreply.github.com>
Date: Sat, 8 Feb 2020 12:33:23 -0500
Subject: [PATCH 2/4] Reorg/cleanup

Removed unused progress variable
reorganized flow to avoid early returns (ensures forced high power mode is disabled regardless of outcome)
---
 Firmware/ultralcd.cpp | 27 ++++++++++-----------------
 1 file changed, 10 insertions(+), 17 deletions(-)

diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 4e0057acd..a4b524772 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -7453,7 +7453,6 @@ void lcd_belttest_print(const char* msg, uint16_t X, uint16_t Y)
 }
 void lcd_belttest()
 {
-    int _progress = 0;
     bool _result = true;
     
     #ifdef TMC2130 // Belttest requires high power mode. Enable it.
@@ -7466,26 +7465,20 @@ void lcd_belttest()
 
     _delay(2000);
     KEEPALIVE_STATE(IN_HANDLER);
-
+    
     _result = lcd_selfcheck_axis_sg(X_AXIS);
     X = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_X));
-    if (!_result){
-        lcd_belttest_print(_i("Error"), X, Y);
-        return;
+    if (_result){
+        lcd_belttest_print(_i("Checking Y..."), X, Y);
+        _result = lcd_selfcheck_axis_sg(Y_AXIS);
+        Y = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_Y));
     }
-
-    lcd_belttest_print(_i("Checking Y..."), X, Y);
-    _result = lcd_selfcheck_axis_sg(Y_AXIS);
-    Y = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_Y));
-
-    if (!_result){
+    
+    if (!_result) {
         lcd_belttest_print(_i("Error"), X, Y);
-        lcd_clear();
-        return;
-    }
-
-
-    lcd_belttest_print(_i("Done"), X, Y);
+    } else {
+        lcd_belttest_print(_i("Done"), X, Y);
+    }   
 
     #ifdef TMC2130
 	    FORCE_HIGH_POWER_END;

From e4b1a1e9c4d265a7b76a74ea172efdfd4f6859f2 Mon Sep 17 00:00:00 2001
From: vintagepc <53943260+vintagepc@users.noreply.github.com>
Date: Sun, 9 Feb 2020 21:39:48 -0500
Subject: [PATCH 3/4] Remove delay for belttest, similar to #2439

There's a separate PR to remove the first delay in the selftest. (#2439). Mirroring that to the belt test function, which I suspect also inherited it.
---
 Firmware/ultralcd.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index a4b524772..af6399f3d 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -7463,7 +7463,6 @@ void lcd_belttest()
     uint16_t   Y = eeprom_read_word((uint16_t*)(EEPROM_BELTSTATUS_Y));
     lcd_belttest_print(_i("Checking X..."), X, Y);
 
-    _delay(2000);
     KEEPALIVE_STATE(IN_HANDLER);
     
     _result = lcd_selfcheck_axis_sg(X_AXIS);

From 1aaefffdb04a94f9ea451f05d2437983f3172c39 Mon Sep 17 00:00:00 2001
From: vintagepc <53943260+vintagepc@users.noreply.github.com>
Date: Tue, 11 Feb 2020 18:37:26 -0500
Subject: [PATCH 4/4] Change to raise_z_above()

---
 Firmware/ultralcd.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index af6399f3d..5c7def256 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -7810,9 +7810,7 @@ static bool lcd_selfcheck_axis_sg(unsigned char axis) {
 	enable_endstops(true);
 
 	if (axis == X_AXIS) { //there is collision between cables and PSU cover in X axis if Z coordinate is too low
-		
-		current_position[Z_AXIS] += 17;
-		plan_buffer_line_curposXYZE(manual_feedrate[0] / 60, active_extruder);
+		raise_z_above(17,true);
 		tmc2130_home_enter(Z_AXIS_MASK);
 		st_synchronize();
 		tmc2130_home_exit();