Automatic filament load when printer idle and preheated

Firmware/Marlin_main.cpp

@@ -5556,21 +5556,17 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
         lcd_wait_interact();
 		//load_filament_time = millis();
 		KEEPALIVE_STATE(PAUSED_FOR_USER);
-		pat9125_update_y(); //update sensor
-		uint16_t y_old = pat9125_y; //save current y value
-		uint8_t change_cnt = 0; //reset number of changes counter
+#ifdef PAT9125
+		if (fsensor_M600) fsensor_autoload_check_start();
+#endif //PAT9125
         while(!lcd_clicked())
 		{
           manage_heater();
           manage_inactivity(true);
-          pat9125_update_y(); //update sensor
-          if (y_old != pat9125_y) //? y value is different
-          {
-			if ((y_old - pat9125_y) > 0)  //? delta-y value is positive (inserting)
-				change_cnt++; //increment change counter
-			y_old = pat9125_y; //save current value
-			if (change_cnt > 20) break; //number of positive changes > 20, start loading
-          }
+#ifdef PAT9125
+		  if (fsensor_M600 && fsensor_check_autoload())
+			  break;
+#endif //PAT9125
 /*#ifdef SNMM
 		  target[E_AXIS] += 0.002;
 		  plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], 500, active_extruder);
@@ -5578,6 +5574,9 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 #endif // SNMM*/
 
         }
+#ifdef PAT9125
+		if (fsensor_M600) fsensor_autoload_check_stop();
+#endif //PAT9125
 		//WRITE(BEEPER, LOW);
 		KEEPALIVE_STATE(IN_HANDLER);
 
@@ -6349,7 +6348,23 @@ void handle_status_leds(void) {
 
 void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument set in Marlin.h
 {
-	
+	if (fsensor_enabled && !fsensor_M600 && !moves_planned() && !IS_SD_PRINTING && !is_usb_printing && (lcd_commands_type != LCD_COMMAND_V2_CAL) && (current_temperature[0] > EXTRUDE_MINTEMP))
+	{
+		if (fsensor_autoload_enabled)
+		{
+			if (fsensor_check_autoload())
+			{
+				fsensor_autoload_check_stop();
+				enquecommand_front_P((PSTR("M701")));
+			}
+		}
+		else
+			fsensor_autoload_check_start();
+	}
+	else
+		if (fsensor_autoload_enabled)
+			fsensor_autoload_check_stop();
+
 #if defined(KILL_PIN) && KILL_PIN > -1
 	static int killCount = 0;   // make the inactivity button a bit less responsive
    const int KILL_DELAY = 10000;

Firmware/fsensor.cpp

@@ -41,6 +41,11 @@ uint8_t fsensor_err_cnt = 0;
 int16_t fsensor_st_cnt = 0;
 uint8_t fsensor_log = 1;
 
+//autoload enable/disable flag
+bool fsensor_autoload_enabled = false;
+uint16_t fsensor_autoload_y = 0;
+uint8_t fsensor_autoload_c = 0;
+uint32_t fsensor_autoload_last_millis = 0;
 
 
 bool fsensor_enable()
@@ -89,6 +94,51 @@ void fsensor_setup_interrupt()
 	pciSetup(FSENSOR_INT_PIN);
 }
 
+void fsensor_autoload_check_start(void)
+{
+	puts_P(PSTR("fsensor_autoload_check_start\n"));
+	pat9125_update_y(); //update sensor
+	fsensor_autoload_y = pat9125_y; //save current y value
+	fsensor_autoload_c = 0; //reset number of changes counter
+	fsensor_autoload_last_millis = millis();
+	fsensor_autoload_enabled = true;
+}
+
+void fsensor_autoload_check_stop(void)
+{
+	puts_P(PSTR("fsensor_autoload_check_stop\n"));
+	fsensor_autoload_enabled = false;
+}
+
+bool fsensor_check_autoload(void)
+{
+	if ((millis() - fsensor_autoload_last_millis) < 50) return false;
+	fsensor_autoload_last_millis = millis();
+	pat9125_update_y(); //update sensor
+	uint16_t dy = fsensor_autoload_y - pat9125_y;
+	if (dy) //? y value is different
+	{
+		if (dy > 0)  //? delta-y value is positive (inserting)
+		{
+			fsensor_autoload_c+=3; //increment change counter
+//			printf_P(PSTR("fsensor_check_autoload dy=%d c=%d\n"), dy, fsensor_autoload_c);
+		}
+		else if (fsensor_autoload_c > 0)
+		{
+			fsensor_autoload_c--;
+//			printf_P(PSTR("fsensor_check_autoload dy=%d c=%d\n"), dy, fsensor_autoload_c);
+		}
+		fsensor_autoload_y = pat9125_y; //save current value
+		if (fsensor_autoload_c > 10) return true; //number of positive changes > 10, start loading
+	}
+	else if (fsensor_autoload_c > 0)
+	{
+		fsensor_autoload_c--;
+//		printf_P(PSTR("fsensor_check_autoload dy=%d c=%d\n"), dy, fsensor_autoload_c);
+	}
+	return false;
+}
+
 ISR(PCINT2_vect)
 {
 //	return;

Firmware/fsensor.h

@@ -17,6 +17,15 @@ extern void fsensor_update();
 //setup pin-change interrupt
 extern void fsensor_setup_interrupt();
 
+//
+extern void fsensor_autoload_check_start(void);
+
+//
+extern void fsensor_autoload_check_stop(void);
+
+//
+extern bool fsensor_check_autoload(void);
+
 //callbacks from stepper
 extern void fsensor_st_block_begin(block_t* bl);
 extern void fsensor_st_block_chunk(block_t* bl, int cnt);
@@ -30,6 +39,8 @@ extern bool fsensor_enabled;
 //not responding flag
 extern bool fsensor_not_responding;
 
+//autoload enable/disable flag
+extern bool fsensor_autoload_enabled;
 
 
 #endif //FSENSOR_H