diff --git a/Firmware/Configuration_prusa.h b/Firmware/Configuration_prusa.h
index 47939e07..63ffa05c 100644
--- a/Firmware/Configuration_prusa.h
+++ b/Firmware/Configuration_prusa.h
@@ -171,7 +171,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #define PAT9125 //!< Filament sensor
 #define FANCHECK
 //#define WATCHDOG
-//#define SAFETYTIMER
+#define SAFETYTIMER
 
 
 /*------------------------------------
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 10248240..a7240bf5 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -54,6 +54,7 @@
 #include "pins_arduino.h"
 #include "math.h"
 #include "util.h"
+#include "Timer.h"
 
 #include <avr/wdt.h>
 
@@ -6720,6 +6721,31 @@ void handle_status_leds(void) {
 }
 #endif
 
+#ifdef SAFETYTIMER
+/**
+ * @brief Turn off heating after 15 minutes of inactivity
+ */
+static void handleSafetyTimer()
+{
+    static_assert(EXTRUDERS == 1,"Implemented only for one extruder.");
+    static Timer safetyTimer;
+    if (IS_SD_PRINTING || is_usb_printing || (custom_message_type == 4) || (lcd_commands_type == LCD_COMMAND_V2_CAL) ||
+            (!degTargetBed() && !degTargetHotend(0)))
+    {
+        safetyTimer.stop();
+    }
+    else if ((degTargetBed() || degTargetHotend(0)) && (!safetyTimer.running()))
+    {
+        safetyTimer.start();
+    }
+    else if (safetyTimer.expired(15*60*1000))
+    {
+        setTargetBed(0);
+        setTargetHotend(0, 0);
+    }
+}
+#endif //SAFETYTIMER
+
 void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument set in Marlin.h
 {
 #ifdef PAT9125
@@ -6763,18 +6789,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument s
 #endif //PAT9125
 
 #ifdef SAFETYTIMER
-		static uint32_t safety_timer = 0;
-		if (degTargetBed() || degTargetHotend(0))
-		{
-			if ((safety_timer == 0) || IS_SD_PRINTING || is_usb_printing || (custom_message_type == 4) || (lcd_commands_type == LCD_COMMAND_V2_CAL))
-				safety_timer = millis();
-			else if ((safety_timer + (15*60*1000)) < millis())
-			{
-				setTargetBed(0);
-				setTargetHotend(0, 0);
-				safety_timer = 0;
-			}
-		}
+	handleSafetyTimer();
 #endif //SAFETYTIMER
 
 
diff --git a/Firmware/Timer.cpp b/Firmware/Timer.cpp
new file mode 100644
index 00000000..78f47a45
--- /dev/null
+++ b/Firmware/Timer.cpp
@@ -0,0 +1,55 @@
+/**
+ * @file
+ * @author Marek Bel
+ */
+
+#include "Timer.h"
+#include "Arduino.h"
+
+Timer::Timer() : m_isRunning(false), m_started()
+{
+}
+
+/**
+ * @brief Start timer
+ */
+void Timer::start()
+{
+    m_started = millis();
+    m_isRunning = true;
+}
+
+/**
+ * @brief Timer has expired
+ *
+ * Timer is considered expired after msPeriod has passed from time the timer was started.
+ * This function must be called at least each (unsigned long maximum value - msPeriod) milliseconds to be sure to
+ * catch first expiration.
+ * This function is expected to handle wrap around of time register well.
+ *
+ * @param msPeriod Time interval in milliseconds.
+ * @retval true Timer has expired
+ * @retval false Timer not expired yet, or is not running, or time window in which is timer considered expired passed.
+ */
+bool Timer::expired(unsigned long msPeriod)
+{
+    if (!m_isRunning) return false;
+    bool expired = false;
+    const unsigned long now = millis();
+    if (m_started <=  m_started + msPeriod)
+    {
+        if ((now >= m_started + msPeriod) || (now < m_started))
+        {
+            expired = true;
+        }
+    }
+    else
+    {
+        if ((now >= m_started + msPeriod) && (now < m_started))
+        {
+            expired = true;
+        }
+    }
+    if (expired) m_isRunning = false;
+    return expired;
+}
diff --git a/Firmware/Timer.h b/Firmware/Timer.h
new file mode 100644
index 00000000..0d3a89dc
--- /dev/null
+++ b/Firmware/Timer.h
@@ -0,0 +1,30 @@
+/*
+ * @file
+ * @author Marek Bel
+ */
+
+#ifndef TIMER_H
+#define TIMER_H
+
+/**
+ * @brief simple timer
+ *
+ * Simple and memory saving implementation. Should handle timer register wrap around well.
+ * Maximum period is at least 49 days. Resolution is one millisecond. To save memory, doesn't store timer period.
+ * If you wish timer which is storing period, derive from this. If you need time intervals smaller than 65 seconds
+ * consider implementing timer with smaller underlying type.
+ */
+class Timer
+{
+public:
+    Timer();
+    void start();
+    void stop(){m_isRunning = false;}
+    bool running(){return m_isRunning;}
+    bool expired(unsigned long msPeriod);
+private:
+    bool m_isRunning;
+    unsigned long m_started;
+};
+
+#endif /* TIMER_H */