From 5c4235b886b678335146bee4dca85ad7635ff497 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Wed, 5 Feb 2020 15:38:39 +0100
Subject: [PATCH] Correctly reset the PAT9125 filament counters

When the error count is cleared, the cumulative deltas as well as the
segment lenght which is kept inside the stepper isr need to be reset.

Introduce a helper function fsensor_reset_err_cnt to clear all the
required variables in one place which can be used in most cases
(the only exceptions being quality measument).

Introduce a new function st_reset_fsensor to also clear the segment
lenght within the isr.
---
 Firmware/fsensor.cpp | 38 ++++++++++++++++++++++----------------
 Firmware/stepper.cpp | 12 ++++++++++--
 Firmware/stepper.h   |  5 ++++-
 3 files changed, 36 insertions(+), 19 deletions(-)

diff --git a/Firmware/fsensor.cpp b/Firmware/fsensor.cpp
index c1f9f27b..3c42a342 100755
--- a/Firmware/fsensor.cpp
+++ b/Firmware/fsensor.cpp
@@ -138,12 +138,28 @@ void fsensor_stop_and_save_print(void)
     fsensor_watch_runout = false;
 }
 
+#ifdef PAT9125
+// Reset all internal counters to zero, including stepper callbacks
+void fsensor_reset_err_cnt()
+{
+    fsensor_err_cnt = 0;
+    pat9125_y = 0;
+    st_reset_fsensor();
+}
+
+void fsensor_set_axis_steps_per_unit(float u)
+{
+    fsensor_chunk_len = (int16_t)(FSENSOR_CHUNK_LEN * u);
+}
+#endif
+
+
 void fsensor_restore_print_and_continue(void)
 {
     printf_P(PSTR("fsensor_restore_print_and_continue\n"));
     fsensor_watch_runout = true;
 #ifdef PAT9125
-	fsensor_err_cnt = 0;
+    fsensor_reset_err_cnt();
 #endif
     restore_print_from_ram_and_continue(0);
 }
@@ -157,11 +173,6 @@ void fsensor_checkpoint_print(void)
     restore_print_from_ram_and_continue(0);
 }
 
-void fsensor_set_axis_steps_per_unit(float u)
-{
-	fsensor_chunk_len = (int16_t)(FSENSOR_CHUNK_LEN * u);
-}
-
 void fsensor_init(void)
 {
 #ifdef PAT9125
@@ -214,7 +225,7 @@ bool fsensor_enable(bool bUpdateEEPROM)
 		fsensor_enabled = pat9125 ? true : false;
 		fsensor_watch_runout = true;
 		fsensor_oq_meassure = false;
-		fsensor_err_cnt = 0;
+        fsensor_reset_err_cnt();
 		fsensor_dy_old = 0;
 		eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, fsensor_enabled ? 0x01 : 0x00);
 		FSensorStateMenu = fsensor_enabled ? 1 : 0;
@@ -295,12 +306,11 @@ void fsensor_autoload_check_start(void)
 	fsensor_autoload_last_millis = _millis();
 	fsensor_watch_runout = false;
 	fsensor_watch_autoload = true;
-	fsensor_err_cnt = 0;
 }
 
+
 void fsensor_autoload_check_stop(void)
 {
-
 //	puts_P(_N("fsensor_autoload_check_stop\n"));
 	if (!fsensor_enabled) return;
 //	puts_P(_N("fsensor_autoload_check_stop 1\n"));
@@ -311,7 +321,7 @@ void fsensor_autoload_check_stop(void)
 	fsensor_autoload_sum = 0;
 	fsensor_watch_autoload = false;
 	fsensor_watch_runout = true;
-	fsensor_err_cnt = 0;
+    fsensor_reset_err_cnt();
 }
 #endif //PAT9125
 
@@ -410,7 +420,6 @@ void fsensor_oq_meassure_stop(void)
 	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 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_samples));
 	fsensor_oq_meassure = false;
-	fsensor_err_cnt = 0;
 }
 
 const char _OK[] PROGMEM = "OK";
@@ -595,7 +604,7 @@ void fsensor_update(void)
             st_synchronize();
 
             // check the filament in isolation
-			fsensor_err_cnt = 0;
+            fsensor_reset_err_cnt();
 			fsensor_oq_meassure_start(0);
             float e_tmp = current_position[E_AXIS];
             current_position[E_AXIS] -= 3;
@@ -603,13 +612,10 @@ void fsensor_update(void)
             current_position[E_AXIS] = e_tmp;
             plan_buffer_line_curposXYZE(200/60, active_extruder);
             st_synchronize();
-
-			uint8_t err_cnt = fsensor_err_cnt;
 			fsensor_oq_meassure_stop();
 
 			bool err = false;
-			err |= (err_cnt > 1);
-
+			err |= (fsensor_err_cnt > 1);
 			err |= (fsensor_oq_er_sum > 2);
 			err |= (fsensor_oq_yd_sum < (4 * FSENSOR_OQ_MIN_YD));
 
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index 93b3d039..93e4ad8f 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -1357,8 +1357,6 @@ void quickStop()
 }
 
 #ifdef BABYSTEPPING
-
-
 void babystep(const uint8_t axis,const bool direction)
 {
   //MUST ONLY BE CALLED BY A ISR, it depends on that no other ISR interrupts this
@@ -1594,3 +1592,13 @@ void microstep_readings()
       #endif
 }
 #endif //TMC2130
+
+
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
+void st_reset_fsensor()
+{
+    CRITICAL_SECTION_START;
+    fsensor_counter = 0;
+    CRITICAL_SECTION_END;
+}
+#endif //FILAMENT_SENSOR
diff --git a/Firmware/stepper.h b/Firmware/stepper.h
index 7c41743c..7fdf426f 100644
--- a/Firmware/stepper.h
+++ b/Firmware/stepper.h
@@ -92,7 +92,10 @@ void microstep_readings();
 #ifdef BABYSTEPPING
   void babystep(const uint8_t axis,const bool direction); // perform a short step with a single stepper motor, outside of any convention
 #endif
-     
 
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
+// reset the internal filament sensor state
+void st_reset_fsensor();
+#endif
 
 #endif