diff --git a/Firmware/Configuration_prusa.h b/Firmware/Configuration_prusa.h
index ddd228cf..b5cf2c93 100644
--- a/Firmware/Configuration_prusa.h
+++ b/Firmware/Configuration_prusa.h
@@ -142,8 +142,8 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define TMC2130_SG_HOMING       1     // stallguard homing
 //#define TMC2130_SG_HOMING_SW_XY  1    // stallguard "software" homing for XY axes
 #define TMC2130_SG_HOMING_SW_Z  1     // stallguard "software" homing for Z axis
-#define TMC2130_SG_THRS_X       5     // stallguard sensitivity for X axis
-#define TMC2130_SG_THRS_Y       5     // stallguard sensitivity for Y axis
+#define TMC2130_SG_THRS_X       6     // stallguard sensitivity for X axis
+#define TMC2130_SG_THRS_Y       6     // stallguard sensitivity for Y axis
 #define TMC2130_SG_THRS_Z       3     // stallguard sensitivity for Z axis
 #define TMC2130_SG_DELTA      128    // stallguard delta [usteps] (minimum usteps before stallguard readed - SW homing)
 
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index c3dfe873..edc1db49 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -569,6 +569,27 @@ void stop_and_save_print_to_ram(float z_move, float e_move);
 void restore_print_from_ram_and_continue(float e_move);
 
 
+void crashdet_enable()
+{
+	tmc2130_sg_stop_on_crash = true;
+}
+
+void crashdet_disable()
+{
+	tmc2130_sg_stop_on_crash = false;
+}
+
+void crashdet_stop_and_save_print()
+{
+	stop_and_save_print_to_ram(10, 0); //XY - no change, Z 10mm up, E - no change
+}
+
+void crashdet_restore_print_and_continue()
+{
+	restore_print_from_ram_and_continue(0); //XYZ = orig, E - no change
+}
+
+
 #ifdef PAT9125
 
 void fsensor_stop_and_save_print()
@@ -1206,6 +1227,12 @@ void loop()
 #endif //PAT9125
 #ifdef TMC2130
 	tmc2130_check_overtemp();
+	if (tmc2130_sg_crash)
+	{
+		tmc2130_sg_crash = false;
+		crashdet_stop_and_save_print();
+		enquecommand_P((PSTR("D999")));
+	}
 #endif //TMC2130
 }
 
@@ -5659,6 +5686,45 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 		MYSERIAL.print("selectedSerialPort = ");
 		MYSERIAL.println(selectedSerialPort, DEC);
 		break;
+	case 999:
+	{
+		MYSERIAL.println("D999 - crash");
+
+/*		while (!is_buffer_empty())
+		{
+			process_commands();
+		    cmdqueue_pop_front();
+		}*/
+		st_synchronize();
+
+		lcd_update_enable(true);
+		lcd_implementation_clear();
+		lcd_update(2);
+		bool yesno = lcd_show_fullscreen_message_yes_no_and_wait_P(MSG_CRASH_DETECTED, false);
+		lcd_update_enable(true);
+		lcd_update(2);
+		lcd_setstatuspgm(WELCOME_MSG);
+		if (yesno)
+		{
+			enquecommand_P(PSTR("G28 X"));
+			enquecommand_P(PSTR("G28 Y"));
+			enquecommand_P(PSTR("D1000"));
+		}
+		else
+		{
+			enquecommand_P(PSTR("D1001"));
+		}
+	}
+		break;
+	case 1000:
+		crashdet_restore_print_and_continue();
+		tmc2130_sg_stop_on_crash = true;
+		break;
+	case 1001:
+		card.sdprinting = false;
+		card.closefile();
+		tmc2130_sg_stop_on_crash = true;
+		break;
 /*	case 4:
 		{
 			MYSERIAL.println("D4 - Test");
@@ -6964,26 +7030,6 @@ void restore_print_from_eeprom() {
 
 //extern uint32_t sdpos_atomic;
 
-void crashdet_enable()
-{
-	tmc2130_sg_stop_on_crash = true;
-}
-
-void crashdet_disable()
-{
-	tmc2130_sg_stop_on_crash = false;
-}
-
-void crashdet_stop_and_save_print()
-{
-	stop_and_save_print_to_ram(10, 0); //XY - no change, Z 10mm up, E - no change
-}
-
-void crashdet_restore_print_and_continue()
-{
-	restore_print_from_ram_and_continue(0); //XYZ = orig, E - no change
-}
-
 bool saved_printing = false;
 uint32_t saved_sdpos = 0;
 float saved_pos[4] = {0, 0, 0, 0};
diff --git a/Firmware/language_all.cpp b/Firmware/language_all.cpp
index f062d511..7b88ea68 100644
--- a/Firmware/language_all.cpp
+++ b/Firmware/language_all.cpp
@@ -1030,6 +1030,11 @@ const char * const MSG_RECOVER_PRINT_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_RECOVER_PRINT_EN
 };
 
+const char MSG_CRASH_DETECTED_EN[] PROGMEM = "Crash detected. Continue printing?";
+const char * const MSG_CRASH_DETECTED_LANG_TABLE[LANG_NUM] PROGMEM = {
+	MSG_CRASH_DETECTED_EN
+};
+
 const char MSG_RECTRACT_EN[] PROGMEM = "Rectract";
 const char * const MSG_RECTRACT_LANG_TABLE[LANG_NUM] PROGMEM = {
 	MSG_RECTRACT_EN
diff --git a/Firmware/language_all.h b/Firmware/language_all.h
index 6d10bbaa..2ac9b567 100644
--- a/Firmware/language_all.h
+++ b/Firmware/language_all.h
@@ -433,6 +433,8 @@ extern const char* const MSG_RECOVERING_PRINT_LANG_TABLE[1];
 #define MSG_RECOVERING_PRINT LANG_TABLE_SELECT_EXPLICIT(MSG_RECOVERING_PRINT_LANG_TABLE, 0)
 extern const char* const MSG_RECOVER_PRINT_LANG_TABLE[1];
 #define MSG_RECOVER_PRINT LANG_TABLE_SELECT_EXPLICIT(MSG_RECOVER_PRINT_LANG_TABLE, 0)
+extern const char* const MSG_CRASH_DETECTED_LANG_TABLE[1];
+#define MSG_CRASH_DETECTED LANG_TABLE_SELECT_EXPLICIT(MSG_CRASH_DETECTED_LANG_TABLE, 0)
 extern const char* const MSG_RECTRACT_LANG_TABLE[1];
 #define MSG_RECTRACT LANG_TABLE_SELECT_EXPLICIT(MSG_RECTRACT_LANG_TABLE, 0)
 extern const char* const MSG_REFRESH_LANG_TABLE[1];
diff --git a/Firmware/language_en.h b/Firmware/language_en.h
index 68efb02c..bd9aaddb 100644
--- a/Firmware/language_en.h
+++ b/Firmware/language_en.h
@@ -306,6 +306,7 @@
 #define(length=17, lines=1) MSG_EXTRUDER_4						"Extruder 4"
 #define(length=20, lines=2) MSG_RECOVER_PRINT					"Blackout occurred. Recover print?"
 #define(length=20, lines=1) MSG_RECOVERING_PRINT				"Recovering print    "
+#define(length=20, lines=2) MSG_CRASH_DETECTED					"Crash detected. Continue printing?"
 
 #define(length=15, lines=1) MSG_INFO_EXTRUDER				"Extruder info"
 
diff --git a/Firmware/tmc2130.cpp b/Firmware/tmc2130.cpp
index 734aa084..270394dc 100644
--- a/Firmware/tmc2130.cpp
+++ b/Firmware/tmc2130.cpp
@@ -195,7 +195,7 @@ void tmc2130_st_isr(uint8_t last_step_mask)
 		if (tmc2130_sg_stop_on_crash && error)
 		{
 			tmc2130_sg_crash = true;
-			crashdet_stop_and_save_print();
+			tmc2130_sg_stop_on_crash = false;
 		}
 }