retry unload movements (T-codes, M702 C) if filament detected by idler sensor, then turn off stepper and wait, MMU_LOAD_TIME prolonged

Firmware/mmu.cpp

@@ -399,17 +399,35 @@ bool mmu_get_response(uint8_t move)
 				mmu_load_step();
 				break;
 			case MMU_UNLOAD_MOVE:
-				current_position[E_AXIS] = current_position[E_AXIS] - MMU_LOAD_FEEDRATE * MMU_LOAD_TIME;
-				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();
-				disable_e0(); //turn off E-stepper to prevent overheating and alow filament pull-out if necessary
-				move = MMU_NO_MOVE;
+				if (PIN_GET(MMU_IDLER_SENSOR_PIN) == 0) //filament is still detected by idler sensor, printer helps with unlading 
+				{ 
+					printf_P(PSTR("Unload 1\n"));
+					current_position[E_AXIS] = current_position[E_AXIS] - MMU_LOAD_FEEDRATE * MMU_LOAD_TIME_MS*0.001;
+					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();
+				}
+				else //filament was unloaded from idler, no additional movements needed 
+				{ 
+					printf_P(PSTR("Unloading finished 1\n"));
+					disable_e0(); //turn off E-stepper to prevent overheating and alow filament pull-out if necessary
+					move = MMU_NO_MOVE;
+				}
 				break;
-			case MMU_TCODE_MOVE: //first do unload and then continue with infinite loading 
-				current_position[E_AXIS] = current_position[E_AXIS] - MMU_LOAD_FEEDRATE * MMU_LOAD_TIME;
-				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();
-				move = MMU_LOAD_MOVE;
+			case MMU_TCODE_MOVE: //first do unload and then continue with infinite loading movements
+				if (PIN_GET(MMU_IDLER_SENSOR_PIN) == 0) //filament detected by idler sensor, we must unload first 
+				{ 
+					printf_P(PSTR("Unload 2\n"));
+					current_position[E_AXIS] = current_position[E_AXIS] - MMU_LOAD_FEEDRATE * MMU_LOAD_TIME_MS*0.001;
+					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();
+				}
+				else //delay to allow mmu unit to pull out filament from bondtech gears and then start with infinite loading 
+				{ 
+					printf_P(PSTR("Unloading finished 2\n"));
+					disable_e0(); //turn off E-stepper to prevent overheating and alow filament pull-out if necessary
+					delay_keep_alive(MMU_LOAD_TIME_MS);
+					move = MMU_LOAD_MOVE;
+				}
 				break;
 			case MMU_NO_MOVE:
 			default: 

Firmware/mmu.h

@@ -23,7 +23,7 @@ extern int16_t mmu_buildnr;
 #define MMU_TCODE_MOVE 3
 
 #define MMU_LOAD_FEEDRATE 19.02f //mm/s
-#define MMU_LOAD_TIME 1 //1000ms is load time for shortest allowed PTFE tubing and maximum loading speed
+#define MMU_LOAD_TIME_MS 2000 //1000ms is load time for shortest allowed PTFE tubing and maximum loading speed
 
 #define MMU_CMD_NONE 0
 #define MMU_CMD_T0   0x10