diff --git a/Firmware/mmu.cpp b/Firmware/mmu.cpp
index c8ced41b2..ba80a7fd9 100644
--- a/Firmware/mmu.cpp
+++ b/Firmware/mmu.cpp
@@ -450,10 +450,11 @@ void mmu_command(uint8_t cmd)
 	mmu_ready = false;
 }
 
-void mmu_load_step() {
+void mmu_load_step(bool synchronize)
+{
 		current_position[E_AXIS] = current_position[E_AXIS] + MMU_LOAD_FEEDRATE * 0.1;
 		plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMU_LOAD_FEEDRATE, active_extruder);
-		st_synchronize();
+		if (synchronize) st_synchronize();
 }
 
 bool mmu_get_response(uint8_t move)
@@ -554,6 +555,14 @@ bool mmu_get_response(uint8_t move)
 	return response;*/
 }
 
+void mmu_wait_for_heater_blocking()
+{
+    while ((degTargetHotend(active_extruder) - degHotend(active_extruder)) > 5)
+    {
+        delay_keep_alive(1000);
+        lcd_wait_for_heater();
+    }
+}
 
 void manage_response(bool move_axes, bool turn_off_nozzle, uint8_t move)
 {
@@ -648,11 +657,7 @@ void manage_response(bool move_axes, bool turn_off_nozzle, uint8_t move)
 					lcd_display_message_fullscreen_P(_i("MMU OK. Resuming temperature..."));
 					delay_keep_alive(3000);
 				}
-				while ((degTargetHotend(active_extruder) - degHotend(active_extruder)) > 5) 
-				{
-					delay_keep_alive(1000);
-					lcd_wait_for_heater();
-				}
+                mmu_wait_for_heater_blocking();
 			  }			  
 			  if (move_axes) {
 				  lcd_clear();
@@ -955,7 +960,7 @@ static const E_step ramming_sequence[] PROGMEM =
 };
 
 //! @brief Unload sequence to optimize shape of the tip of the unloaded filament
-static void filament_ramming()
+void mmu_filament_ramming()
 {
     for(uint8_t i = 0; i < (sizeof(ramming_sequence)/sizeof(E_step));++i)
     {
@@ -987,7 +992,7 @@ void extr_unload()
 		if (mmu_extruder == MMU_FILAMENT_UNKNOWN) lcd_print(" ");
 		else lcd_print(mmu_extruder + 1);
 
-		filament_ramming();
+		mmu_filament_ramming();
 
 		mmu_command(MMU_CMD_U0);
 		// get response
diff --git a/Firmware/mmu.h b/Firmware/mmu.h
index 1861be6dc..8f7f34a33 100644
--- a/Firmware/mmu.h
+++ b/Firmware/mmu.h
@@ -1,5 +1,8 @@
 //! @file
 
+#ifndef MMU_H
+#define MMU_H
+
 #include <inttypes.h>
 
 
@@ -119,3 +122,8 @@ extern void mmu_eject_fil_2();
 extern void mmu_eject_fil_3();
 extern void mmu_eject_fil_4();
 extern void mmu_continue_loading();
+extern void mmu_filament_ramming();
+extern void mmu_wait_for_heater_blocking();
+extern void mmu_load_step(bool synchronize = true);
+
+#endif //MMU_H
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index b96050649..f9e5c6677 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -163,6 +163,7 @@ static void lcd_selftest_screen_step(int _row, int _col, int _state, const char
 static bool lcd_selftest_manual_fan_check(int _fan, bool check_opposite);
 static bool lcd_selftest_fan_dialog(int _fan);
 static bool lcd_selftest_fsensor();
+static bool selftest_irsensor();
 static void lcd_selftest_error(int _error_no, const char *_error_1, const char *_error_2);
 static void lcd_colorprint_change();
 #ifdef SNMM
@@ -6421,17 +6422,20 @@ bool lcd_selftest()
 	}
 
 #ifdef FILAMENT_SENSOR
-    if (mmu_enabled == false)
+    if (_result)
     {
-        if (_result)
+        _progress = lcd_selftest_screen(9, _progress, 3, true, 2000); //check filaments sensor
+        if (mmu_enabled)
+        {
+            _result = selftest_irsensor();
+        } else
         {
-            _progress = lcd_selftest_screen(9, _progress, 3, true, 2000); //check filaments sensor
             _result = lcd_selftest_fsensor();
         }
-        if (_result)
-        {
-            _progress = lcd_selftest_screen(10, _progress, 3, true, 2000); //fil sensor OK
-        }
+    }
+    if (_result)
+    {
+        _progress = lcd_selftest_screen(10, _progress, 3, true, 2000); //fil sensor OK
     }
 #endif // FILAMENT_SENSOR
 
@@ -6991,6 +6995,49 @@ static bool lcd_selftest_fsensor(void)
 	}
 	return (!fsensor_not_responding);
 }
+
+static bool selftest_irsensor()
+{
+    class TempBackup
+    {
+    public:
+        TempBackup():
+            m_temp(degTargetHotend(active_extruder)),
+            m_extruder(active_extruder){}
+        ~TempBackup(){setTargetHotend(m_temp,m_extruder);}
+    private:
+        float m_temp;
+        uint8_t m_extruder;
+    };
+
+    TempBackup tempBackup;
+    setTargetHotend(PLA_PREHEAT_HOTEND_TEMP,active_extruder);
+    mmu_wait_for_heater_blocking();
+    lcd_selftest_screen(9, 0, 3, true, 0);
+    mmu_filament_ramming();
+
+    for(uint_least8_t i = 0; i < 200; ++i)
+    {
+        mmu_load_step(false);
+        while (blocks_queued())
+        {
+            if (PIN_GET(MMU_IDLER_SENSOR_PIN) == 0) return false;
+#ifdef TMC2130
+            manage_heater();
+            // Vojtech: Don't disable motors inside the planner!
+            if (!tmc2130_update_sg())
+            {
+                manage_inactivity(true);
+            }
+#else //TMC2130
+            manage_heater();
+            // Vojtech: Don't disable motors inside the planner!
+            manage_inactivity(true);
+#endif //TMC2130
+        }
+    }
+    return true;
+}
 #endif //FILAMENT_SENSOR
 
 static bool lcd_selftest_manual_fan_check(int _fan, bool check_opposite)