Merge pull request #2264 from leptun/MK3_fix_selftest_Z_crash2

PFW-1048: fix selftest Z crash. Use stallGuard when testing Z

Firmware/Marlin.h

@@ -310,9 +310,9 @@ extern int8_t lcd_change_fil_state;
 extern float default_retraction;
 
 #ifdef TMC2130
-void homeaxis(int axis, uint8_t cnt = 1, uint8_t* pstep = 0);
+bool homeaxis(int axis, bool doError = true, uint8_t cnt = 1, uint8_t* pstep = 0);
 #else
-void homeaxis(int axis, uint8_t cnt = 1);
+bool homeaxis(int axis, bool doError = true, uint8_t cnt = 1);
 #endif //TMC2130
 
 

Firmware/Marlin_main.cpp

@@ -1892,10 +1892,6 @@ static void axis_is_at_home(int axis) {
   max_pos[axis] =          base_max_pos(axis) + cs.add_homing[axis];
 }
 
-
-inline void set_current_to_destination() { memcpy(current_position, destination, sizeof(current_position)); }
-inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); }
-
 //! @return original feedmultiply
 static int setup_for_endstop_move(bool enable_endstops_now = true) {
     saved_feedrate = feedrate;
@@ -2191,9 +2187,9 @@ bool calibrate_z_auto()
 #endif //TMC2130
 
 #ifdef TMC2130
-void homeaxis(int axis, uint8_t cnt, uint8_t* pstep)
+bool homeaxis(int axis, bool doError, uint8_t cnt, uint8_t* pstep)
 #else
-void homeaxis(int axis, uint8_t cnt)
+bool homeaxis(int axis, bool doError, uint8_t cnt)
 #endif //TMC2130
 {
 	bool endstops_enabled  = enable_endstops(true); //RP: endstops should be allways enabled durring homing
@@ -2308,8 +2304,10 @@ void homeaxis(int axis, uint8_t cnt)
 #ifdef TMC2130
 		if (READ(Z_TMC2130_DIAG) != 0) { //Z crash
 			FORCE_HIGH_POWER_END;
-			kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
-			return; 
+			if (doError) kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
+            current_position[axis] = -5; //assume that nozzle crashed into bed
+            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+			return 0; 
 		}
 #endif //TMC2130
         current_position[axis] = 0;
@@ -2324,8 +2322,10 @@ void homeaxis(int axis, uint8_t cnt)
 #ifdef TMC2130
 		if (READ(Z_TMC2130_DIAG) != 0) { //Z crash
 			FORCE_HIGH_POWER_END;
-			kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
-			return; 
+			if (doError) kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
+            current_position[axis] = -5; //assume that nozzle crashed into bed
+            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+			return 0; 
 		}
 #endif //TMC2130
         axis_is_at_home(axis);
@@ -2338,6 +2338,7 @@ void homeaxis(int axis, uint8_t cnt)
 #endif	
     }
     enable_endstops(endstops_enabled);
+    return 1;
 }
 
 /**/

Firmware/planner.h

@@ -173,6 +173,9 @@ void plan_set_e_position(const float &e);
 // Reset the E position to zero at the start of the next segment
 void plan_reset_next_e();
 
+inline void set_current_to_destination() { memcpy(current_position, destination, sizeof(current_position)); }
+inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); }
+
 extern bool e_active();
 
 void check_axes_activity();

Firmware/tmc2130.cpp

@@ -994,7 +994,7 @@ bool tmc2130_home_calibrate(uint8_t axis)
 	uint8_t step[16];
 	uint8_t cnt[16];
 	uint8_t val[16];
-	homeaxis(axis, 16, step);
+	homeaxis(axis, true, 16, step);
 	bubblesort_uint8(step, 16, 0);
 	printf_P(PSTR("sorted samples:\n"));
 	for (uint8_t i = 0; i < 16; i++)

Firmware/ultralcd.cpp

@@ -7727,21 +7727,31 @@ bool lcd_selftest()
 #ifdef TMC2130
 		tmc2130_home_exit();
 		enable_endstops(false);
-		current_position[X_AXIS] = current_position[X_AXIS] + 14;
-		current_position[Y_AXIS] = current_position[Y_AXIS] + 12;
 #endif
 
 		//homeaxis(X_AXIS);
 		//homeaxis(Y_AXIS);
+        current_position[X_AXIS] += pgm_read_float(bed_ref_points_4);
+		current_position[Y_AXIS] += pgm_read_float(bed_ref_points_4+1);
+#ifdef TMC2130
+		//current_position[X_AXIS] += 0;
+		current_position[Y_AXIS] += 4;
+#endif //TMC2130
 		current_position[Z_AXIS] = current_position[Z_AXIS] + 10;
 		plan_buffer_line_curposXYZE(manual_feedrate[0] / 60, active_extruder);
 		st_synchronize();
+        set_destination_to_current();
 		_progress = lcd_selftest_screen(TestScreen::AxisZ, _progress, 3, true, 1500);
-		_result = lcd_selfcheck_axis(2, Z_MAX_POS);
-		if (eeprom_read_byte((uint8_t*)EEPROM_WIZARD_ACTIVE) != 1) {
-			enquecommand_P(PSTR("G28 W"));
-			enquecommand_P(PSTR("G1 Z15 F1000"));
-		}
+#ifdef TMC2130
+		_result = homeaxis(Z_AXIS, false);
+#else
+        _result = lcd_selfcheck_axis(Z_AXIS, Z_MAX_POS);
+#endif //TMC2130
+
+		//raise Z to not damage the bed during and hotend testing
+		current_position[Z_AXIS] += 20;
+		plan_buffer_line_curposXYZE(manual_feedrate[0] / 60, active_extruder);
+		st_synchronize();
 	}
 
 #ifdef TMC2130
@@ -7965,7 +7975,7 @@ static bool lcd_selfcheck_axis_sg(unsigned char axis) {
 }
 #endif //TMC2130
 
-//#ifndef TMC2130
+#ifndef TMC2130
 
 static bool lcd_selfcheck_axis(int _axis, int _travel)
 {
@@ -8064,12 +8074,13 @@ static bool lcd_selfcheck_axis(int _axis, int _travel)
 		{
 			lcd_selftest_error(TestError::Motor, _error_1, _error_2);
 		}
-	}
+	}    
+	current_position[_axis] = 0; //simulate axis home to avoid negative numbers for axis position, especially Z.
+	plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 
 	return _stepresult;
 }
 
-#ifndef TMC2130
 static bool lcd_selfcheck_pulleys(int axis)
 {
 	float tmp_motor_loud[3] = DEFAULT_PWM_MOTOR_CURRENT_LOUD;
@@ -8114,9 +8125,6 @@ static bool lcd_selfcheck_pulleys(int axis)
 			((READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING) == 1)) {
 			endstop_triggered = true;
 			if (current_position_init - 1 <= current_position[axis] && current_position_init + 1 >= current_position[axis]) {
-				current_position[axis] += (axis == X_AXIS) ? 13 : 9;
-				plan_buffer_line_curposXYZE(manual_feedrate[0] / 60, active_extruder);
-				st_synchronize();
 				return(true);
 			}
 			else {