diff --git a/Firmware/Configuration.h b/Firmware/Configuration.h
index a6c89c88..ef359f39 100644
--- a/Firmware/Configuration.h
+++ b/Firmware/Configuration.h
@@ -551,9 +551,10 @@ enum CalibrationStatus
 
 // Try to maintain a minimum distance from the bed even when Z is
 // unknown when doing the following operations
-#define MIN_Z_FOR_LOAD    50
-#define MIN_Z_FOR_UNLOAD  50
-#define MIN_Z_FOR_PREHEAT 10
+#define MIN_Z_FOR_LOAD    50 // lcd filament loading or autoload
+#define MIN_Z_FOR_UNLOAD  50 // lcd filament unloading
+#define MIN_Z_FOR_SWAP    27 // filament change (including M600)
+#define MIN_Z_FOR_PREHEAT 10 // lcd preheat
 
 #include "Configuration_adv.h"
 #include "thermistortables.h"
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index f391159b..8e685d95 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -3622,12 +3622,12 @@ static T gcode_M600_filament_change_z_shift()
 #ifdef FILAMENTCHANGE_ZADD
 	static_assert(Z_MAX_POS < (255 - FILAMENTCHANGE_ZADD), "Z-range too high, change the T type from uint8_t to uint16_t");
 	// avoid floating point arithmetics when not necessary - results in shorter code
+	T z_shift = T(FILAMENTCHANGE_ZADD); // always move above printout
 	T ztmp = T( current_position[Z_AXIS] );
-	T z_shift = 0;
-	if(ztmp < T(25)){
-		z_shift = T(25) - ztmp; // make sure to be at least 25mm above the heat bed
-	} 
-	return z_shift + T(FILAMENTCHANGE_ZADD); // always move above printout
+	if((ztmp + z_shift) < T(MIN_Z_FOR_SWAP)){
+		z_shift = T(MIN_Z_FOR_SWAP) - ztmp; // make sure to be at least 25mm above the heat bed
+	}
+	return z_shift;
 #else
 	return T(0);
 #endif
@@ -3676,7 +3676,7 @@ static void gcode_M600(bool automatic, float x_position, float y_position, float
 
     // Unload filament
     if (mmu_enabled) extr_unload();	//unload just current filament for multimaterial printers (used also in M702)
-    else unload_filament(); //unload filament for single material (used also in M702)
+    else unload_filament(true); //unload filament for single material (used also in M702)
     //finish moves
     st_synchronize();
 
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 3e765a5c..b0ba7d34 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -2201,15 +2201,16 @@ void mFilamentItem(uint16_t nTemp, uint16_t nTempBed)
     }
     else
     {
-        lcd_set_cursor(0, 0);
-        lcdui_print_temp(LCD_STR_THERMOMETER[0], (int) degHotend(0), (int) degTargetHotend(0));
-
         if (!bFilamentWaitingFlag)
         {
             // First run after the filament preheat selection:
             // setup the fixed LCD parts and raise Z as we wait
             bFilamentWaitingFlag = true;
 
+            lcd_clear();
+            lcd_draw_update = 1;
+            lcd_puts_at_P(0, 3, _i(">Cancel")); ////MSG_ c=20 r=1
+
             lcd_set_cursor(0, 1);
             switch (eFilamentAction)
             {
@@ -2236,9 +2237,11 @@ void mFilamentItem(uint16_t nTemp, uint16_t nTempBed)
                 // handled earlier
                 break;
             }
-            lcd_puts_at_P(0, 3, _i(">Cancel"));                   ////MSG_ c=20 r=1
         }
 
+        lcd_set_cursor(0, 0);
+        lcdui_print_temp(LCD_STR_THERMOMETER[0], (int) degHotend(0), (int) degTargetHotend(0));
+
         if (lcd_clicked())
         {
             bFilamentWaitingFlag = false;
@@ -4885,7 +4888,7 @@ void lcd_wizard(WizState state)
 				lcd_display_message_fullscreen_P(_i("Now I will preheat nozzle for PLA."));
 				wait_preheat();
 				//unload current filament
-				unload_filament();
+				unload_filament(true);
 				//load filament
 				lcd_wizard_load();
 				setTargetHotend(0, 0); //we are finished, cooldown nozzle
@@ -6200,13 +6203,14 @@ static void change_extr_menu(){
 }
 #endif //SNMM
 
-//unload filament for single material printer (used in M702 gcode)
-void unload_filament()
+// unload filament for single material printer (used in M702 gcode)
+// @param automatic: If true, unload_filament is part of a unload+load sequence (M600)
+void unload_filament(bool automatic)
 {
 	custom_message_type = CustomMsg::FilamentLoading;
 	lcd_setstatuspgm(_T(MSG_UNLOADING_FILAMENT));
 
-    raise_z_above(MIN_Z_FOR_UNLOAD);
+    raise_z_above(automatic? MIN_Z_FOR_SWAP: MIN_Z_FOR_UNLOAD);
 
 	//		extr_unload2();
 
diff --git a/Firmware/ultralcd.h b/Firmware/ultralcd.h
index 008a106e..c671326e 100755
--- a/Firmware/ultralcd.h
+++ b/Firmware/ultralcd.h
@@ -195,7 +195,7 @@ extern bool bFilamentAction;
 void mFilamentItem(uint16_t nTemp,uint16_t nTempBed);
 void mFilamentItemForce();
 void lcd_generic_preheat_menu();
-void unload_filament();
+void unload_filament(bool automatic = false);
 
 void stack_error();
 void lcd_printer_connected();