diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 98e5102c4..c2c5ca933 100755
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -497,6 +497,7 @@ void marlin_wait_for_click();
 void raise_z_above(float target, bool plan=true);
 
 extern "C" void softReset();
+void stack_error();
 
 extern uint32_t IP_address;
 
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 56a90849e..6e0ddcf9b 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1727,6 +1727,20 @@ ISR(WDT_vect)
 }
 #endif
 
+
+void stack_error() {
+    WRITE(BEEPER, HIGH);
+    eeprom_update_byte((uint8_t*)EEPROM_FW_CRASH_FLAG, (uint8_t)dump_crash_reason::stack_error);
+#ifdef EMERGENCY_DUMP
+    xfdump_full_dump_and_reset(dump_crash_reason::stack_error);
+#elif defined(EMERGENCY_SERIAL_DUMP)
+    if (emergency_serial_dump)
+        serial_dump_and_reset(dump_crash_reason::stack_error);
+#endif
+    softReset();
+}
+
+
 void trace();
 
 #define CHUNK_SIZE 64 // bytes
diff --git a/Firmware/temperature.cpp b/Firmware/temperature.cpp
index 1d0dfdb69..3e617a663 100755
--- a/Firmware/temperature.cpp
+++ b/Firmware/temperature.cpp
@@ -38,7 +38,7 @@
 #include "temperature.h"
 #include "cardreader.h"
 
-#include "Sd2PinMap.h"
+#include "SdFatUtil.h"
 
 #include <avr/wdt.h>
 #include "adc.h"
@@ -2065,6 +2065,9 @@ FORCE_INLINE static void temperature_isr()
   }
 #endif //BABYSTEPPING
 
+  // Check if a stack overflow happened
+  if (!SdFatUtil::test_stack_integrity()) stack_error();
+
 #if (defined(FANCHECK) && defined(TACH_0) && (TACH_0 > -1))
   check_fans();
 #endif //(defined(TACH_0))
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index caf9c47c0..b277df4ce 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -29,7 +29,6 @@
 //#include "Configuration.h"
 #include "cmdqueue.h"
 
-#include "SdFatUtil.h"
 #include "xflash_dump.h"
 
 #ifdef FILAMENT_SENSOR
@@ -6721,23 +6720,6 @@ static void lcd_main_menu()
 }
 
 
-#ifdef EMERGENCY_SERIAL_DUMP
-#include "Dcodes.h"
-#endif
-
-void stack_error() {
-    WRITE(BEEPER, HIGH);
-    eeprom_update_byte((uint8_t*)EEPROM_FW_CRASH_FLAG, (uint8_t)dump_crash_reason::stack_error);
-#ifdef EMERGENCY_DUMP
-    xfdump_full_dump_and_reset(dump_crash_reason::stack_error);
-#elif defined(EMERGENCY_SERIAL_DUMP)
-    if (emergency_serial_dump)
-        serial_dump_and_reset(dump_crash_reason::stack_error);
-#endif
-    softReset();
-}
-
-
 #ifdef DEBUG_STEPPER_TIMER_MISSED
 bool stepper_timer_overflow_state = false;
 uint16_t stepper_timer_overflow_max = 0;
@@ -8961,7 +8943,6 @@ void menu_lcd_lcdupdate_func(void)
 		if (lcd_draw_update) lcd_draw_update--;
 		lcd_next_update_millis = _millis() + LCD_UPDATE_INTERVAL;
 	}
-	if (!SdFatUtil::test_stack_integrity()) stack_error();
 	lcd_ping(); //check that we have received ping command if we are in farm mode
 	lcd_send_status();
 	if (lcd_commands_type == LcdCommands::Layer1Cal) lcd_commands();
diff --git a/Firmware/ultralcd.h b/Firmware/ultralcd.h
index d87810011..8c344d120 100755
--- a/Firmware/ultralcd.h
+++ b/Firmware/ultralcd.h
@@ -194,7 +194,6 @@ void mFilamentItemForce();
 void lcd_generic_preheat_menu();
 void unload_filament(bool automatic = false);
 
-void stack_error();
 void lcd_printer_connected();
 void lcd_ping();