diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index 36b5f7b67..b8c41301c 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -146,9 +146,16 @@ const int dropsegments=5; //everything with this number of steps  will be ignore
 #define WATCHDOG_TIMEOUT 4
 
 
+
+//// Experimental watchdog and minimal temp
+// The watchdog waits for the watchperiod in milliseconds whenever an M104 or M109 increases the target temperature
 // If the temperature has not increased at the end of that period, the target temperature is set to zero. It can be reset with another M104/M109
 //#define WATCHPERIOD 5000 //5 seconds
 
+// Actual temperature must be close to target for this long before M109 returns success
+//#define TEMP_RESIDENCY_TIME 20  // (seconds)
+//#define TEMP_HYSTERESIS 5       // (C°) range of +/- temperatures considered "close" to the target one
+
 //// The minimal temperature defines the temperature below which the heater will not be enabled
 #define MINTEMP 5
 #define BED_MINTEMP 5
diff --git a/Marlin/Marlin.pde b/Marlin/Marlin.pde
index 6922b8a1f..8c8fe4bc2 100644
--- a/Marlin/Marlin.pde
+++ b/Marlin/Marlin.pde
@@ -780,7 +780,7 @@ inline void process_commands()
         #ifdef WATCHPERIOD
             if(target_raw[0] > current_raw[0]){
                 watchmillis = max(1,millis());
-                watch_raw[0] = current_raw[0];
+                watch_raw = current_raw[0];
             }else{
                 watchmillis = 0;
             }
@@ -820,32 +820,54 @@ inline void process_commands()
         #endif
         return;
         //break;
-      case 109: // M109 - Wait for extruder heater to reach target.
-        LCD_MESSAGE("Heating...");
-               if (code_seen('S')) target_raw[0] = temp2analog(code_value());
-#ifdef PIDTEMP
-               pid_setpoint = code_value();
-#endif //PIDTEM
-        #ifdef WATCHPERIOD
-          if(target_raw[0]>current_raw[0]){
+      case 109: {// M109 - Wait for extruder heater to reach target.
+            LCD_MESSAGE("Heating...");
+            if (code_seen('S')) target_raw[0] = temp2analog(code_value());
+            #ifdef PIDTEMP
+            pid_setpoint = code_value();
+            #endif //PIDTEM
+            #ifdef WATCHPERIOD
+            if(target_raw[0]>current_raw[0]) {
               watchmillis = max(1,millis());
-              watch_raw[0] = current_raw[0];
-          }else{
+              watch_raw = current_raw[0];
+            } else {
               watchmillis = 0;
-          }
-        #endif
-          codenum = millis(); 
-          starttime=millis();
-          while(current_raw[0] < target_raw[0]) {
-            if( (millis() - codenum) > 1000 ) { //Print Temp Reading every 1 second while heating up.
-              Serial.print("T:");
-              Serial.println( analog2temp(current_raw[0]) ); 
-              codenum = millis();
             }
-            LCD_STATUS;
-            manage_heater();
+            #endif //WATCHPERIOD
+            codenum = millis(); 
+     
+               /* See if we are heating up or cooling down */
+              bool target_direction = (current_raw[0] < target_raw[0]); // true if heating, false if cooling
+
+            #ifdef TEMP_RESIDENCY_TIME
+            long residencyStart;
+            residencyStart = -1;
+            /* continue to loop until we have reached the target temp   
+              _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
+            while((target_direction ? (current_raw[0] < target_raw[0]) : (current_raw[0] > target_raw[0])) ||
+                    (residencyStart > -1 && (millis() - residencyStart) < TEMP_RESIDENCY_TIME*1000) ) {
+            #else
+            while ( target_direction ? (current_raw[0] < target_raw[0]) : (current_raw[0] > target_raw[0]) ) {
+            #endif //TEMP_RESIDENCY_TIME
+              if( (millis() - codenum) > 1000 ) { //Print Temp Reading every 1 second while heating up/cooling down
+                Serial.print("T:");
+                Serial.println( analog2temp(current_raw[0]) );
+                codenum = millis();
+              }
+              manage_heater();
+              LCD_STATUS;
+              #ifdef TEMP_RESIDENCY_TIME
+               /* start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
+                  or when current temp falls outside the hysteresis after target temp was reached */
+              if ((residencyStart == -1 &&  target_direction && current_raw[0] >= target_raw[0]) ||
+                  (residencyStart == -1 && !target_direction && current_raw[0] <= target_raw[0]) ||
+                  (residencyStart > -1 && labs(analog2temp(current_raw[0]) - analog2temp(target_raw[0])) > TEMP_HYSTERESIS) ) {
+                residencyStart = millis();
+              }
+              #endif //TEMP_RESIDENCY_TIME
+	    }
+            LCD_MESSAGE("Marlin ready.");
           }
-          LCD_MESSAGE("UltiMarlin ready.");
           break;
       case 190: // M190 - Wait bed for heater to reach target.
       #if TEMP_1_PIN > -1