diff --git a/Firmware/fsensor.cpp b/Firmware/fsensor.cpp
index d520ba533..d296a6e74 100644
--- a/Firmware/fsensor.cpp
+++ b/Firmware/fsensor.cpp
@@ -492,11 +492,10 @@ void fsensor_setup_interrupt(void)
 
 #endif //PAT9125
 
-void fsensor_st_block_begin(block_t* bl)
+void fsensor_st_block_begin(bool rev)
 {
 	if (!fsensor_enabled) return;
-	if (((fsensor_st_cnt > 0) && (bl->direction_bits & 0x8)) || 
-		((fsensor_st_cnt < 0) && !(bl->direction_bits & 0x8)))
+	if (fsensor_st_cnt && ((fsensor_st_cnt > 0) ^ rev))
 	{
 // !!! bit toggling (PINxn <- 1) (for PinChangeInterrupt) does not work for some MCU pins
 		if (PIN_GET(FSENSOR_INT_PIN)) {PIN_VAL(FSENSOR_INT_PIN, LOW);}
@@ -504,11 +503,11 @@ void fsensor_st_block_begin(block_t* bl)
 	}
 }
 
-void fsensor_st_block_chunk(block_t* bl, int cnt)
+void fsensor_st_block_chunk(int cnt)
 {
 	if (!fsensor_enabled) return;
-	fsensor_st_cnt += (bl->direction_bits & 0x8)?-cnt:cnt;
-	if ((fsensor_st_cnt >= fsensor_chunk_len) || (fsensor_st_cnt <= -fsensor_chunk_len))
+	fsensor_st_cnt += cnt;
+	if (abs(fsensor_st_cnt) >= fsensor_chunk_len)
 	{
 // !!! bit toggling (PINxn <- 1) (for PinChangeInterrupt) does not work for some MCU pins
 		if (PIN_GET(FSENSOR_INT_PIN)) {PIN_VAL(FSENSOR_INT_PIN, LOW);}
diff --git a/Firmware/fsensor.h b/Firmware/fsensor.h
index 71922fbfc..85992d39d 100644
--- a/Firmware/fsensor.h
+++ b/Firmware/fsensor.h
@@ -60,8 +60,8 @@ extern bool fsensor_oq_result(void);
 #include "planner.h"
 //! @name callbacks from stepper
 //! @{
-extern void fsensor_st_block_begin(block_t* bl);
-extern void fsensor_st_block_chunk(block_t* bl, int cnt);
+extern void fsensor_st_block_begin(bool rev);
+extern void fsensor_st_block_chunk(int cnt);
 //! @}
 
 #endif //FSENSOR_H
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index fda9f11c7..111d1fa7a 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -339,7 +339,7 @@ FORCE_INLINE void stepper_next_block()
 
 #ifdef FILAMENT_SENSOR
 	fsensor_counter = 0;
-	fsensor_st_block_begin(current_block);
+	fsensor_st_block_begin(count_direction[E_AXIS] < 0);
 #endif //FILAMENT_SENSOR
     // The busy flag is set by the plan_get_current_block() call.
     // current_block->busy = true;
@@ -638,7 +638,7 @@ FORCE_INLINE void stepper_tick_lowres()
       e_steps += count_direction[E_AXIS];
 #else
 	#ifdef FILAMENT_SENSOR
-	  ++ fsensor_counter;
+	  fsensor_counter += count_direction[E_AXIS];
 	#endif //FILAMENT_SENSOR
       WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
 #endif
@@ -700,7 +700,7 @@ FORCE_INLINE void stepper_tick_highres()
       e_steps += count_direction[E_AXIS];
 #else
     #ifdef FILAMENT_SENSOR
-      ++ fsensor_counter;
+      fsensor_counter += count_direction[E_AXIS];
     #endif //FILAMENT_SENSOR
       WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
 #endif
@@ -857,18 +857,18 @@ FORCE_INLINE void isr() {
 
     // If current block is finished, reset pointer
     if (step_events_completed.wide >= current_block->step_event_count.wide) {
-#ifdef FILAMENT_SENSOR
-		fsensor_st_block_chunk(current_block, fsensor_counter);
+#if !defined(LIN_ADVANCE) && defined(FILAMENT_SENSOR)
+		fsensor_st_block_chunk(fsensor_counter);
 		fsensor_counter = 0;
 #endif //FILAMENT_SENSOR
 
       current_block = NULL;
       plan_discard_current_block();
     }
-#ifdef FILAMENT_SENSOR
-  	else if ((fsensor_counter >= fsensor_chunk_len))
+#if !defined(LIN_ADVANCE) && defined(FILAMENT_SENSOR)
+	else if ((abs(fsensor_counter) >= fsensor_chunk_len))
   	{
-      fsensor_st_block_chunk(current_block, fsensor_counter);
+      fsensor_st_block_chunk(fsensor_counter);
   	  fsensor_counter = 0;
   	}
 #endif //FILAMENT_SENSOR
@@ -952,20 +952,25 @@ FORCE_INLINE void advance_isr_scheduler() {
         uint8_t max_ticks = (eisr? e_step_loops: step_loops);
         max_ticks = min(abs(e_steps), max_ticks);
         bool rev = (e_steps < 0);
-#ifdef FILAMENT_SENSOR
-        if (count_direction[E_AXIS] == 1)
-            fsensor_counter += (rev? -max_ticks: max_ticks);
-        else
-            fsensor_counter -= (rev? -max_ticks: max_ticks);
-#endif
         WRITE_NC(E0_DIR_PIN, rev? INVERT_E0_DIR: !INVERT_E0_DIR);
         do
         {
             WRITE_NC(E0_STEP_PIN, !INVERT_E_STEP_PIN);
             e_steps += (rev? 1: -1);
             WRITE_NC(E0_STEP_PIN, INVERT_E_STEP_PIN);
+#ifdef FILAMENT_SENSOR
+            fsensor_counter += (rev? -1: 1);
+#endif
         }
         while(--max_ticks);
+
+#ifdef FILAMENT_SENSOR
+        if (!current_block || (abs(fsensor_counter) >= fsensor_chunk_len))
+        {
+            fsensor_st_block_chunk(fsensor_counter);
+            fsensor_counter = 0;
+        }
+#endif
     }
 
     // Schedule the next closest tick, ignoring advance if scheduled too