From 2f9003dbddfdf3d9017fcdfc3fbbaf0fc65d9364 Mon Sep 17 00:00:00 2001
From: Robert Pelnar <robert.pelnar@seznam.cz>
Date: Thu, 26 Jul 2018 19:50:33 +0200
Subject: [PATCH] fsensor - better state handling

---
 Firmware/Marlin_main.cpp |  8 +++++---
 Firmware/fsensor.cpp     | 25 +++++++++++++++++++++----
 2 files changed, 26 insertions(+), 7 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 0141fb4c..1db0a41f 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -536,6 +536,9 @@ static void get_arc_coordinates();
 static bool setTargetedHotend(int code);
 static void print_time_remaining_init();
 
+uint16_t gcode_in_progress = 0;
+uint16_t mcode_in_progress = 0;
+
 void serial_echopair_P(const char *s_P, float v)
     { serialprintPGM(s_P); SERIAL_ECHO(v); }
 void serial_echopair_P(const char *s_P, double v)
@@ -1981,7 +1984,8 @@ void loop()
   checkHitEndstops();
   lcd_update(0);
 #ifdef FILAMENT_SENSOR
-	fsensor_update();
+	if (mcode_in_progress != 600) //M600 not in progress
+		fsensor_update();
 #endif //FILAMENT_SENSOR
 #ifdef TMC2130
 	tmc2130_check_overtemp();
@@ -3193,8 +3197,6 @@ 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()
 {
diff --git a/Firmware/fsensor.cpp b/Firmware/fsensor.cpp
index a280eda1..66008be3 100644
--- a/Firmware/fsensor.cpp
+++ b/Firmware/fsensor.cpp
@@ -33,11 +33,13 @@ extern int8_t FSensorStateMenu;
 
 void fsensor_stop_and_save_print(void)
 {
+	printf_P(PSTR("fsensor_stop_and_save_print\n"));
 	stop_and_save_print_to_ram(0, 0); //XYZE - no change	
 }
 
 void fsensor_restore_print_and_continue(void)
 {
+	printf_P(PSTR("fsensor_restore_print_and_continue\n"));
 	restore_print_from_ram_and_continue(0); //XYZ = orig, E - no change
 }
 
@@ -240,12 +242,12 @@ bool fsensor_check_autoload(void)
 		fsensor_autoload_c--;
 	if (fsensor_autoload_c == 0) fsensor_autoload_sum = 0;
 //	puts_P(_N("fsensor_check_autoload\n"));
-	if (fsensor_autoload_c != fsensor_autoload_c_old)
-		printf_P(PSTR("fsensor_check_autoload dy=%d c=%d sum=%d\n"), dy, fsensor_autoload_c, fsensor_autoload_sum);
+//	if (fsensor_autoload_c != fsensor_autoload_c_old)
+//		printf_P(PSTR("fsensor_check_autoload dy=%d c=%d sum=%d\n"), dy, fsensor_autoload_c, fsensor_autoload_sum);
 //	if ((fsensor_autoload_c >= 15) && (fsensor_autoload_sum > 30))
 	if ((fsensor_autoload_c >= 10) && (fsensor_autoload_sum > 15))
 	{
-		puts_P(_N("fsensor_check_autoload = true !!!\n"));
+//		puts_P(_N("fsensor_check_autoload = true !!!\n"));
 		return true;
 	}
 	return false;
@@ -412,10 +414,10 @@ void fsensor_update(void)
 	{
 		if (fsensor_printing_saved)
 		{
+			fsensor_restore_print_and_continue();
 			fsensor_printing_saved = false;
 			fsensor_watch_runout = true;
 			fsensor_err_cnt = 0;
-			fsensor_restore_print_and_continue();
 		}
 		else if (fsensor_watch_runout && (fsensor_err_cnt > FSENSOR_ERR_MAX))
 		{
@@ -423,6 +425,19 @@ void fsensor_update(void)
 			fsensor_printing_saved = true;
 
 			fsensor_err_cnt = 0;
+/*
+			st_synchronize();
+			for (int axis = X_AXIS; axis <= E_AXIS; axis++)
+				current_position[axis] = st_get_position_mm(axis);
+
+			current_position[E_AXIS] -= 3;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 200 / 60, active_extruder);
+			st_synchronize();
+
+			current_position[E_AXIS] += 3;
+			plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 200 / 60, active_extruder);
+			st_synchronize();
+*/
 
 			enquecommand_front_P((PSTR("G1 E-3 F200")));
 			process_commands();
@@ -437,9 +452,11 @@ void fsensor_update(void)
 			if (fsensor_err_cnt == 0)
 			{
 				fsensor_restore_print_and_continue();
+				fsensor_printing_saved = false;
 			}
 			else
 			{
+//				printf_P(PSTR("fsensor_update - M600\n"));
 				eeprom_update_byte((uint8_t*)EEPROM_FERROR_COUNT, eeprom_read_byte((uint8_t*)EEPROM_FERROR_COUNT) + 1);
 				eeprom_update_word((uint16_t*)EEPROM_FERROR_COUNT_TOT, eeprom_read_word((uint16_t*)EEPROM_FERROR_COUNT_TOT) + 1);
 				enquecommand_front_P((PSTR("M600")));