diff --git a/Firmware/first_lay_cal.cpp b/Firmware/first_lay_cal.cpp
index 8f06b816b..410462ce3 100644
--- a/Firmware/first_lay_cal.cpp
+++ b/Firmware/first_lay_cal.cpp
@@ -241,3 +241,33 @@ void lay1cal_square(char *cmd_buffer, uint8_t i, float layer_height, float extru
     sprintf_P(cmd_buffer, fmt1, 50, (35 - (i + 1)*short_length * 2), short_extrusion);
     enquecommand(cmd_buffer);
 }
+
+void lay1cal_finish(bool mmu_enabled)
+{
+    static const char cmd_cal_finish_0[] PROGMEM = "M107"; //turn off printer fan
+    static const char cmd_cal_finish_1[] PROGMEM = "G1 E-0.075 F2100"; //retract
+    static const char cmd_cal_finish_2[] PROGMEM = "M104 S0"; // turn off temperature
+    static const char cmd_cal_finish_3[] PROGMEM = "M140 S0"; // turn off heatbed
+    static const char cmd_cal_finish_4[] PROGMEM = "G1 Z10 F1300"; //lift Z
+    static const char cmd_cal_finish_5[] PROGMEM = "G1 X10 Y180 F4000"; //Go to parking position
+    static const char cmd_cal_finish_6[] PROGMEM = "M702 C"; //unload from nozzle
+    static const char cmd_cal_finish_7[] PROGMEM = "M84";// disable motors
+
+    static const char * const cmd_cal_finish[] PROGMEM =
+    {
+            cmd_cal_finish_0,
+            cmd_cal_finish_1,
+            cmd_cal_finish_2,
+            cmd_cal_finish_3,
+            cmd_cal_finish_4,
+            cmd_cal_finish_5
+    };
+
+    for (uint8_t i = 0; i < (sizeof(cmd_cal_finish)/sizeof(cmd_cal_finish[0])); ++i)
+    {
+        enquecommand_P(static_cast<char*>(pgm_read_ptr(&cmd_cal_finish[i])));
+    }
+
+    if (mmu_enabled) enquecommand_P(cmd_cal_finish_6); //unload from nozzle
+    enquecommand_P(cmd_cal_finish_7);// disable motors
+}
diff --git a/Firmware/first_lay_cal.h b/Firmware/first_lay_cal.h
index b17ed289f..3864e19e3 100644
--- a/Firmware/first_lay_cal.h
+++ b/Firmware/first_lay_cal.h
@@ -13,5 +13,6 @@ void lay1cal_before_meander();
 void lay1cal_meander_start(float layer_height, float extrusion_width);
 void lay1cal_meander(float layer_height, float extrusion_width);
 void lay1cal_square(char *cmd_buffer, uint8_t i, float layer_height, float extrusion_width);
+void lay1cal_finish(bool mmu_enabled);
 
 #endif /* FIRMWARE_FIRST_LAY_CAL_H_ */
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 53d4f48e3..e2c8073f8 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -893,14 +893,7 @@ void lcd_commands()
                 lcd_commands_step = 2;
                 break;
             case 2:
-                enquecommand_P(PSTR("M107")); //turn off printer fan
-                enquecommand_P(PSTR("G1 E-0.07500 F2100.00000")); //retract
-                enquecommand_P(PSTR("M104 S0")); // turn off temperature
-                enquecommand_P(PSTR("M140 S0")); // turn off heatbed
-                enquecommand_P(PSTR("G1 Z10 F1300.000")); //lift Z
-                enquecommand_P(PSTR("G1 X10 Y180 F4000")); //Go to parking position
-                if (MMU2::mmu2.Enabled()) enquecommand_P(PSTR("M702")); //unload from nozzle
-                enquecommand_P(PSTR("M84"));// disable motors
+                lay1cal_finish(MMU2::mmu2.Enabled());
                 menu_leaving = 1; //if user dont confirm live adjust Z value by pressing the knob, we are saving last value by timeout to status screen
                 lcd_commands_step = 1;
                 break;