FSensor - M600/autoload fix + optical quality meassurement

Firmware/Marlin.h

@@ -469,6 +469,6 @@ void proc_commands();
 bool mmu_get_reponse();
 void mmu_not_responding();
 void mmu_load_to_nozzle();
-void M600_load_filament(bool fsensor_enabled);
+void M600_load_filament();
 void mmu_M600_load_filament();
 void M600_load_filament_movements();

Firmware/Marlin_main.cpp

@@ -3121,16 +3121,11 @@ void gcode_M701()
 	custom_message = false;
 	custom_message_type = 0;
 
-
 	fsensor_oq_meassure_stop();
 
-
-
-
 	if (!fsensor_oq_result())
 	{
-		bool disable = lcd_show_fullscreen_message_yes_no_and_wait_P(
-			_i("Filament sensor low response, disable it?"), false, true);
+		bool disable = lcd_show_fullscreen_message_yes_no_and_wait_P(_i("Fil. sensor response is poor, disable it?"), false, true);
 		lcd_update_enable(true);
 		lcd_update(2);
 		if (disable)
@@ -3193,6 +3188,9 @@ extern uint8_t st_backlash_x;
 extern uint8_t st_backlash_y;
 #endif //BACKLASH_Y
 
+uint16_t gcode_in_progress = 0;
+uint16_t mcode_in_progress = 0;
+
 void process_commands()
 {
 	if (!buflen) return; //empty command
@@ -3420,7 +3418,8 @@ void process_commands()
     // nothing, this is a version line
   } else if(code_seen('G'))
   {
-    switch((int)code_value())
+	gcode_in_progress = (int)code_value();
+    switch (gcode_in_progress)
     {
     case 0: // G0 -> G1
     case 1: // G1
@@ -4647,10 +4646,12 @@ void process_commands()
 	default:
 		printf_P(PSTR("Unknown G code: %s \n"), cmdbuffer + bufindr + CMDHDRSIZE);
     }
+	gcode_in_progress = 0;
   } // end if(code_seen('G'))
 
   else if(code_seen('M'))
   {
+
 	  int index;
 	  for (index = 1; *(strchr_pointer + index) == ' ' || *(strchr_pointer + index) == '\t'; index++);
 	   
@@ -4659,7 +4660,10 @@ void process_commands()
 		  printf_P(PSTR("Invalid M code: %s \n"), cmdbuffer + bufindr + CMDHDRSIZE);
 
 	  } else
-    switch((int)code_value())
+	  {
+	  mcode_in_progress = (int)code_value();
+
+    switch(mcode_in_progress)
     {
 
     case 0: // M0 - Unconditional stop - Wait for user button press on LCD
@@ -6233,10 +6237,6 @@ Sigma_Exit:
     #ifdef FILAMENTCHANGEENABLE
     case 600: //Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
     {
-#ifdef PAT9125
-		bool old_fsensor_enabled = fsensor_enabled;
-//		fsensor_enabled = false; //temporary solution for unexpected restarting
-#endif //PAT9125
 
 		st_synchronize();
 		float lastpos[4];
@@ -6509,7 +6509,7 @@ Sigma_Exit:
 #ifdef SNMM_V2
 		mmu_M600_load_filament();
 #else		
-		M600_load_filament(old_fsensor_enabled);
+		M600_load_filament();
 #endif    
 
 		//Wait for user to check the state
@@ -6915,10 +6915,6 @@ Sigma_Exit:
 			  extr_unload_all(); //unload all filaments
 		}
 #else
-#ifdef PAT9125
-		bool old_fsensor_enabled = fsensor_enabled;
-//		fsensor_enabled = false;
-#endif //PAT9125
 		custom_message = true;
 		custom_message_type = 2;
 		lcd_setstatuspgm(_T(MSG_UNLOADING_FILAMENT)); 
@@ -6960,9 +6956,6 @@ Sigma_Exit:
 		lcd_setstatuspgm(_T(WELCOME_MSG));
 		custom_message = false;
 		custom_message_type = 0;
-#ifdef PAT9125
-//		fsensor_enabled = old_fsensor_enabled;
-#endif //PAT9125
 #endif	
 	}
 	break;
@@ -6976,7 +6969,8 @@ Sigma_Exit:
 	default: 
 		printf_P(PSTR("Unknown M code: %s \n"), cmdbuffer + bufindr + CMDHDRSIZE);
     }
-	
+	mcode_in_progress = 0;
+	}
   } // end if(code_seen('M')) (end of M codes)
 
   else if(code_seen('T'))
@@ -7493,35 +7487,38 @@ static void handleSafetyTimer()
 void manage_inactivity(bool ignore_stepper_queue/*=false*/) //default argument set in Marlin.h
 {
 #ifdef FILAMENT_SENSOR
-	if (!moves_planned() && !IS_SD_PRINTING && !is_usb_printing && (lcd_commands_type != LCD_COMMAND_V2_CAL))
+	if (mcode_in_progress != 600) //M600 not in progress
 	{
-		if (fsensor_check_autoload())
+		if (!moves_planned() && !IS_SD_PRINTING && !is_usb_printing && (lcd_commands_type != LCD_COMMAND_V2_CAL))
 		{
-			fsensor_autoload_check_stop();
-            if (degHotend0() > EXTRUDE_MINTEMP)
-            {
-                tone(BEEPER, 1000);
-                delay_keep_alive(50);
-                noTone(BEEPER);
-                loading_flag = true;
-                enquecommand_front_P((PSTR("M701")));
-            }
-            else
-            {
-                lcd_update_enable(false);
-                lcd_clear();
-                lcd_set_cursor(0, 0);
-                lcd_puts_P(_T(MSG_ERROR));
-                lcd_set_cursor(0, 2);
-                lcd_puts_P(_T(MSG_PREHEAT_NOZZLE));
-                delay(2000);
-                lcd_clear();
-                lcd_update_enable(true);
-            }
+			if (fsensor_check_autoload())
+			{
+				fsensor_autoload_check_stop();
+				if (degHotend0() > EXTRUDE_MINTEMP)
+				{
+					tone(BEEPER, 1000);
+					delay_keep_alive(50);
+					noTone(BEEPER);
+					loading_flag = true;
+					enquecommand_front_P((PSTR("M701")));
+				}
+				else
+				{
+					lcd_update_enable(false);
+					lcd_clear();
+					lcd_set_cursor(0, 0);
+					lcd_puts_P(_T(MSG_ERROR));
+					lcd_set_cursor(0, 2);
+					lcd_puts_P(_T(MSG_PREHEAT_NOZZLE));
+					delay(2000);
+					lcd_clear();
+					lcd_update_enable(true);
+				}
+			}
 		}
+		else
+			fsensor_autoload_check_stop();
 	}
-	else
-		fsensor_autoload_check_stop();
 #endif //FILAMENT_SENSOR
 
 #ifdef SAFETYTIMER
@@ -9104,41 +9101,57 @@ void M600_load_filament_movements() {
                      lcd_loading_filament();
 }
 
-void M600_load_filament(bool fsensor_enabled) {
-
-		lcd_wait_interact();
+void M600_load_filament()
+{
+	lcd_wait_interact();
 
-		//load_filament_time = millis();
-		KEEPALIVE_STATE(PAUSED_FOR_USER);
+	//load_filament_time = millis();
+	KEEPALIVE_STATE(PAUSED_FOR_USER);
 
 #ifdef PAT9125
-		fsensor_autoload_check_start();
+	fsensor_autoload_check_start();
 #endif //PAT9125
-        while(!lcd_clicked())
-		{
-          manage_heater();
-          manage_inactivity(true);
+	while(!lcd_clicked())
+	{
+		manage_heater();
+		manage_inactivity(true);
 #ifdef PAT9125
-		  if (fsensor_check_autoload())
-		  {
+		if (fsensor_check_autoload())
+		{
 			tone(BEEPER, 1000);
 			delay_keep_alive(50);
 			noTone(BEEPER);
-			  break;
-		  }
+			break;
+		}
 #endif //PAT9125
+	}
+#ifdef PAT9125
+	fsensor_autoload_check_stop();
+#endif //PAT9125
+	KEEPALIVE_STATE(IN_HANDLER);
 
-        }
 #ifdef PAT9125
-		fsensor_autoload_check_stop();
+	fsensor_oq_meassure_start();
 #endif //PAT9125
-		KEEPALIVE_STATE(IN_HANDLER);
 
-		M600_load_filament_movements();
+	M600_load_filament_movements();
+
+	tone(BEEPER, 500);
+	delay_keep_alive(50);
+	noTone(BEEPER);
+
+#ifdef PAT9125
+	fsensor_oq_meassure_stop();
 
-		tone(BEEPER, 500);
-		delay_keep_alive(50);
-		noTone(BEEPER);
+	if (!fsensor_oq_result())
+	{
+		bool disable = lcd_show_fullscreen_message_yes_no_and_wait_P(_i("Fil. sensor response is poor, disable it?"), false, true);
+		lcd_update_enable(true);
+		lcd_update(2);
+		if (disable)
+			fsensor_disable();
+	}
+#endif //PAT9125
 
 }
 

Firmware/fsensor.cpp

@@ -1,6 +1,7 @@
 #include "Marlin.h"
 
 #include "fsensor.h"
+#include <avr/pgmspace.h>
 #include "pat9125.h"
 #include "stepper.h"
 #include "planner.h"
@@ -12,11 +13,13 @@
 #define FSENSOR_ERR_MAX         10  //filament sensor maximum error count for runout detection
 
 //Optical quality meassurement params
-#define FSENSOR_OQ_MAX_ER      5    //maximum error count for loading (~150mm)
-#define FSENSOR_OQ_MIN_YD      2    //minimum yd per chunk
-#define FSENSOR_OQ_MAX_YD      200  //maximum yd per chunk
+#define FSENSOR_OQ_MAX_ES      5    //maximum error sum while loading (length 95mm = 144chunks)
+#define FSENSOR_OQ_MAX_EM      1    //maximum error counter value while loading
+#define FSENSOR_OQ_MIN_YD      2    //minimum yd per chunk (applied to avg value)
+#define FSENSOR_OQ_MAX_YD      200  //maximum yd per chunk (applied to avg value)
 #define FSENSOR_OQ_MAX_PD      3    //maximum positive deviation (= yd_max/yd_avg)
 #define FSENSOR_OQ_MAX_ND      5    //maximum negative deviation (= yd_avg/yd_min)
+#define FSENSOR_OQ_MAX_SH      13   //maximum shutter value
 
 
 const char ERRMSG_PAT9125_NOT_RESP[] PROGMEM = "PAT9125 not responding (%d)!\n";
@@ -81,6 +84,8 @@ uint8_t fsensor_autoload_sum;
 bool fsensor_oq_meassure = false;
 //skip-chunk counter, for accurate meassurement is necesary to skip first chunk...
 uint8_t  fsensor_oq_skipchunk;
+//number of samples from start of meassurement
+uint8_t fsensor_oq_cnt;
 //sum of steps in positive direction movements
 uint16_t fsensor_oq_st_sum;
 //sum of deltas in positive direction movements
@@ -93,6 +98,8 @@ uint8_t  fsensor_oq_er_max;
 uint16_t fsensor_oq_yd_min;
 //maximum delta value
 uint16_t fsensor_oq_yd_max;
+//sum of shutter value
+uint16_t fsensor_oq_sh_sum;
 
 
 void fsensor_init(void)
@@ -176,7 +183,7 @@ void fsensor_autoload_check_start(void)
 		printf_P(ERRMSG_PAT9125_NOT_RESP, 3);
 		return;
 	}
-	puts_P(_N(" autoload enabled\n"));
+	puts_P(_N("fsensor_autoload_check_start - autoload ENABLED\n"));
 	fsensor_autoload_y = pat9125_y; //save current y value
 	fsensor_autoload_c = 0; //reset number of changes counter
 	fsensor_autoload_sum = 0;
@@ -194,7 +201,7 @@ void fsensor_autoload_check_stop(void)
 	if (!fsensor_autoload_enabled) return;
 //	puts_P(_N("fsensor_autoload_check_stop 2\n"));
 	if (!fsensor_watch_autoload) return;
-	puts_P(_N(" autoload disabled\n"));
+	puts_P(_N("fsensor_autoload_check_stop - autoload DISABLED\n"));
 	fsensor_autoload_sum = 0;
 	fsensor_watch_autoload = false;
 	fsensor_watch_runout = true;
@@ -249,14 +256,17 @@ bool fsensor_check_autoload(void)
 
 void fsensor_oq_meassure_start(void)
 {
-	fsensor_oq_skipchunk = 1;
+	printf_P(PSTR("fsensor_oq_meassure_start\n"));
+	fsensor_oq_skipchunk = 10;
+	fsensor_oq_cnt = 0;
 	fsensor_oq_st_sum = 0;
 	fsensor_oq_yd_sum = 0;
 	fsensor_oq_er_sum = 0;
 	fsensor_oq_er_max = 0;
 	fsensor_oq_yd_min = FSENSOR_OQ_MAX_YD;
 	fsensor_oq_yd_max = 0;
-	pat9125_update_y();
+	fsensor_oq_sh_sum = 0;
+	pat9125_update();
 	pat9125_y = 0;
 	fsensor_watch_runout = false;
 	fsensor_oq_meassure = true;
@@ -264,27 +274,37 @@ void fsensor_oq_meassure_start(void)
 
 void fsensor_oq_meassure_stop(void)
 {
-	printf_P(PSTR("fsensor_oq_meassure_stop\n"));
+	printf_P(PSTR("fsensor_oq_meassure_stop, %hhu samples\n"), fsensor_oq_cnt);
 	printf_P(_N(" st_sum=%u yd_sum=%u er_sum=%u er_max=%hhu\n"), fsensor_oq_st_sum, fsensor_oq_yd_sum, fsensor_oq_er_sum, fsensor_oq_er_max);
-	printf_P(_N(" yd_min=%u yd_max=%u yd_avg=%u\n"), fsensor_oq_yd_min, fsensor_oq_yd_max, (uint16_t)((uint32_t)fsensor_oq_yd_sum * FSENSOR_CHUNK_LEN / fsensor_oq_st_sum));
+	printf_P(_N(" yd_min=%u yd_max=%u yd_avg=%u sh_avg=%u\n"), fsensor_oq_yd_min, fsensor_oq_yd_max, (uint16_t)((uint32_t)fsensor_oq_yd_sum * FSENSOR_CHUNK_LEN / fsensor_oq_st_sum), (uint16_t)(fsensor_oq_sh_sum / fsensor_oq_cnt));
 	fsensor_oq_meassure = false;
 	fsensor_err_cnt = 0;
 	fsensor_watch_runout = true;
 }
 
+const char _OK[] PROGMEM = "OK";
+const char _NG[] PROGMEM = "NG!";
+
 bool fsensor_oq_result(void)
 {
-	printf(_N("fsensor_oq_result\n"));
-	if (fsensor_oq_er_sum > FSENSOR_OQ_MAX_ER) return false;
-	printf(_N(" er_sum OK\n"));
-	uint8_t yd_avg = (uint16_t)((uint32_t)fsensor_oq_yd_sum * FSENSOR_CHUNK_LEN / fsensor_oq_st_sum);
-	if ((yd_avg < FSENSOR_OQ_MIN_YD) || (yd_avg > FSENSOR_OQ_MAX_YD)) return false;
-	printf(_N(" yd_avg OK\n"));
-	if (fsensor_oq_yd_max > (yd_avg * FSENSOR_OQ_MAX_PD)) return false;
-	printf(_N(" yd_max OK\n"));
-	if (fsensor_oq_yd_min < (yd_avg / FSENSOR_OQ_MAX_ND)) return false;
-	printf(_N(" yd_min OK\n"));
-	return true;
+	printf_P(_N("fsensor_oq_result\n"));
+	bool res_er_sum = (fsensor_oq_er_sum <= FSENSOR_OQ_MAX_ES);
+	printf_P(_N(" er_sum = %u %S\n"), fsensor_oq_er_sum, (res_er_sum?_OK:_NG));
+	bool res_er_max = (fsensor_oq_er_max <= FSENSOR_OQ_MAX_EM);
+	printf_P(_N(" er_max = %hhu %S\n"), fsensor_oq_er_max, (res_er_max?_OK:_NG));
+	uint8_t yd_avg = ((uint32_t)fsensor_oq_yd_sum * FSENSOR_CHUNK_LEN / fsensor_oq_st_sum);
+	bool res_yd_avg = (yd_avg >= FSENSOR_OQ_MIN_YD) && (yd_avg <= FSENSOR_OQ_MAX_YD);
+	printf_P(_N(" yd_avg = %hhu %S\n"), yd_avg, (res_yd_avg?_OK:_NG));
+	bool res_yd_max = (fsensor_oq_yd_max <= (yd_avg * FSENSOR_OQ_MAX_PD));
+	printf_P(_N(" yd_max = %u %S\n"), fsensor_oq_yd_max, (res_yd_max?_OK:_NG));
+	bool res_yd_min = (fsensor_oq_yd_min >= (yd_avg / FSENSOR_OQ_MAX_ND));
+	printf_P(_N(" yd_min = %u %S\n"), fsensor_oq_yd_min, (res_yd_min?_OK:_NG));
+	uint8_t sh_avg = (fsensor_oq_sh_sum / fsensor_oq_cnt);
+	bool res_sh_avg = (sh_avg <= FSENSOR_OQ_MAX_SH);
+	printf_P(_N(" sh_avg = %hhu %S\n"), sh_avg, (res_sh_avg?_OK:_NG));
+	bool res = res_er_sum && res_er_max && res_yd_avg && res_yd_max && res_yd_min && res_sh_avg;
+	printf_P(_N("fsensor_oq_result %S\n"), (res?_OK:_NG));
+	return res;
 }
 
 ISR(PCINT2_vect)
@@ -298,7 +318,8 @@ ISR(PCINT2_vect)
 	fsensor_st_cnt = 0;
 	sei();
 	uint8_t old_err_cnt = fsensor_err_cnt;
-	if (!pat9125_update_y())
+	uint8_t pat9125_res = fsensor_oq_meassure?pat9125_update():pat9125_update_y();
+	if (!pat9125_res)
 	{
 		fsensor_disable();
 		fsensor_not_responding = true;
@@ -320,7 +341,10 @@ ISR(PCINT2_vect)
 			if (fsensor_oq_meassure)
 			{
 				if (fsensor_oq_skipchunk)
+				{
 					fsensor_oq_skipchunk--;
+					fsensor_err_cnt = 0;
+				}
 				else
 				{
 					if (st_cnt == FSENSOR_CHUNK_LEN)
@@ -328,12 +352,14 @@ ISR(PCINT2_vect)
 						if (fsensor_oq_yd_min > pat9125_y) fsensor_oq_yd_min = (fsensor_oq_yd_min + pat9125_y) / 2;
 						if (fsensor_oq_yd_max < pat9125_y) fsensor_oq_yd_max = (fsensor_oq_yd_max + pat9125_y) / 2;
 					}
+					fsensor_oq_cnt++;
 					fsensor_oq_st_sum += st_cnt;
 					fsensor_oq_yd_sum += pat9125_y;
 					if (fsensor_err_cnt > old_err_cnt)
 						fsensor_oq_er_sum += (fsensor_err_cnt - old_err_cnt);
 					if (fsensor_oq_er_max < fsensor_err_cnt)
 						fsensor_oq_er_max = fsensor_err_cnt;
+					fsensor_oq_sh_sum += pat9125_s;
 				}
 			}
 		}