diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index 768ae6e90..dd70c8b0d 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -1501,16 +1501,10 @@ uint32_t Stepper::stepper_block_phase_isr() { #if ENABLED(LIN_ADVANCE) if (LA_use_advance_lead) { - // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached - if (step_events_completed == steps_per_isr || (LA_steps && LA_isr_rate != current_block->advance_speed)) { - nextAdvanceISR = 0; - LA_isr_rate = current_block->advance_speed; - } - } - else { - LA_isr_rate = LA_ADV_NEVER; - if (LA_steps) nextAdvanceISR = 0; + // Fire ISR if final adv_rate is reached + if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0; } + else if (LA_steps) nextAdvanceISR = 0; #endif // LIN_ADVANCE } // Are we in Deceleration phase ? @@ -1552,17 +1546,13 @@ uint32_t Stepper::stepper_block_phase_isr() { #if ENABLED(LIN_ADVANCE) if (LA_use_advance_lead) { - if (step_events_completed <= decelerate_after + steps_per_isr || - (LA_steps && LA_isr_rate != current_block->advance_speed) - ) { - nextAdvanceISR = 0; // Wake up eISR on first deceleration loop + // Wake up eISR on first deceleration loop and fire ISR if final adv_rate is reached + if (step_events_completed <= decelerate_after + steps_per_isr || (LA_steps && LA_isr_rate != current_block->advance_speed)) { + nextAdvanceISR = 0; LA_isr_rate = current_block->advance_speed; } } - else { - LA_isr_rate = LA_ADV_NEVER; - if (LA_steps) nextAdvanceISR = 0; - } + else if (LA_steps) nextAdvanceISR = 0; #endif // LIN_ADVANCE } // We must be in cruise phase otherwise @@ -1745,7 +1735,11 @@ uint32_t Stepper::stepper_block_phase_isr() { if ((LA_use_advance_lead = current_block->use_advance_lead)) { LA_final_adv_steps = current_block->final_adv_steps; LA_max_adv_steps = current_block->max_adv_steps; + //Start the ISR + nextAdvanceISR = 0; + LA_isr_rate = current_block->advance_speed; } + else LA_isr_rate = LA_ADV_NEVER; #endif if (current_block->direction_bits != last_direction_bits