diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 1fcaabd884..b761c9a5ed 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -141,8 +141,8 @@ float Planner::previous_speed[NUM_AXIS], #endif #if ENABLED(LIN_ADVANCE) - float Planner::extruder_advance_k = LIN_ADVANCE_K; - float Planner::position_float[NUM_AXIS] = { 0 }; + float Planner::extruder_advance_k = LIN_ADVANCE_K, + Planner::position_float[NUM_AXIS] = { 0 }; #endif #if ENABLED(ENSURE_SMOOTH_MOVES) @@ -654,7 +654,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const // The target position of the tool in absolute steps // Calculate target position in absolute steps //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow - long target[XYZE] = { + const long target[XYZE] = { lround(a * axis_steps_per_mm[X_AXIS]), lround(b * axis_steps_per_mm[Y_AXIS]), lround(c * axis_steps_per_mm[Z_AXIS]), @@ -670,16 +670,16 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const #endif #if ENABLED(LIN_ADVANCE) - float target_float[XYZE] = {a, b, c, e}; - float de_float = target_float[E_AXIS] - position_float[E_AXIS]; - float mm_D_float = sqrt(sq(target_float[X_AXIS] - position_float[X_AXIS]) + sq(target_float[Y_AXIS] - position_float[Y_AXIS])); + const float target_float[XYZE] = { a, b, c, e }, + de_float = target_float[E_AXIS] - position_float[E_AXIS], + mm_D_float = sqrt(sq(target_float[X_AXIS] - position_float[X_AXIS]) + sq(target_float[Y_AXIS] - position_float[Y_AXIS])); memcpy(position_float, target_float, sizeof(position_float)); #endif - long da = target[X_AXIS] - position[X_AXIS], - db = target[Y_AXIS] - position[Y_AXIS], - dc = target[Z_AXIS] - position[Z_AXIS]; + const long da = target[X_AXIS] - position[X_AXIS], + db = target[Y_AXIS] - position[Y_AXIS], + dc = target[Z_AXIS] - position[Z_AXIS]; /* SERIAL_ECHOPAIR(" Planner FR:", fr_mm_s); @@ -755,11 +755,11 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const #endif if (de < 0) SBI(dm, E_AXIS); - float esteps_float = de * volumetric_multiplier[extruder] * flow_percentage[extruder] * 0.01; - int32_t esteps = abs(esteps_float) + 0.5; + const float esteps_float = de * volumetric_multiplier[extruder] * flow_percentage[extruder] * 0.01; + const int32_t esteps = abs(esteps_float) + 0.5; // Calculate the buffer head after we push this byte - int8_t next_buffer_head = next_block_index(block_buffer_head); + const uint8_t next_buffer_head = next_block_index(block_buffer_head); // If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer. @@ -852,7 +852,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const #if ENABLED(DISABLE_INACTIVE_EXTRUDER) // Enable only the selected extruder - for (int8_t i = 0; i < EXTRUDERS; i++) + for (uint8_t i = 0; i < EXTRUDERS; i++) if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--; switch(extruder) { @@ -980,7 +980,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const // Calculate moves/second for this move. No divide by zero due to previous checks. float inverse_mm_s = fr_mm_s * inverse_millimeters; - int moves_queued = movesplanned(); + const uint8_t moves_queued = movesplanned(); // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill #if ENABLED(SLOWDOWN) @@ -1037,7 +1037,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const // If the index has changed (must have gone forward)... if (filwidth_delay_index[0] != filwidth_delay_index[1]) { filwidth_e_count = 0; // Reset the E movement counter - int8_t meas_sample = thermalManager.widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char + const int8_t meas_sample = thermalManager.widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char do { filwidth_delay_index[1] = (filwidth_delay_index[1] + 1) % MMD_CM; // The next unused slot measurement_delay[filwidth_delay_index[1]] = meas_sample; // Store the measurement @@ -1050,7 +1050,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const // Calculate and limit speed in mm/sec for each axis float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed LOOP_XYZE(i) { - float cs = fabs(current_speed[i] = delta_mm[i] * inverse_mm_s); + const float cs = fabs(current_speed[i] = delta_mm[i] * inverse_mm_s); if (cs > max_feedrate_mm_s[i]) NOMORE(speed_factor, max_feedrate_mm_s[i] / cs); } @@ -1058,7 +1058,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const #ifdef XY_FREQUENCY_LIMIT // Check and limit the xy direction change frequency - unsigned char direction_change = block->direction_bits ^ old_direction_bits; + const unsigned char direction_change = block->direction_bits ^ old_direction_bits; old_direction_bits = block->direction_bits; segment_time = lround((float)segment_time / speed_factor); @@ -1083,11 +1083,11 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const } ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time; - long max_x_segment_time = MAX3(xs0, xs1, xs2), - max_y_segment_time = MAX3(ys0, ys1, ys2), - min_xy_segment_time = min(max_x_segment_time, max_y_segment_time); + const long max_x_segment_time = MAX3(xs0, xs1, xs2), + max_y_segment_time = MAX3(ys0, ys1, ys2), + min_xy_segment_time = min(max_x_segment_time, max_y_segment_time); if (min_xy_segment_time < MAX_FREQ_TIME) { - float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME); + const float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME); NOMORE(speed_factor, low_sf); } #endif // XY_FREQUENCY_LIMIT @@ -1100,7 +1100,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const } // Compute and limit the acceleration rate for the trapezoid generator. - float steps_per_mm = block->step_event_count * inverse_millimeters; + const float steps_per_mm = block->step_event_count * inverse_millimeters; uint32_t accel; if (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS]) { // convert to: acceleration steps/sec^2 @@ -1204,22 +1204,17 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const static float previous_safe_speed; float safe_speed = block->nominal_speed; - bool limited = false; + uint8_t limited = 0; LOOP_XYZE(i) { - float jerk = fabs(current_speed[i]); - if (jerk > max_jerk[i]) { - // The actual jerk is lower if it has been limited by the XY jerk. + const float jerk = fabs(current_speed[i]), maxj = max_jerk[i]; + if (jerk > maxj) { if (limited) { - // Spare one division by a following gymnastics: - // Instead of jerk *= safe_speed / block->nominal_speed, - // multiply max_jerk[i] by the divisor. - jerk *= safe_speed; - float mjerk = max_jerk[i] * block->nominal_speed; - if (jerk > mjerk) safe_speed *= mjerk / jerk; + const float mjerk = maxj * block->nominal_speed; + if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; } else { - safe_speed = max_jerk[i]; - limited = true; + ++limited; + safe_speed = maxj; } } } @@ -1236,7 +1231,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed; // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities. float v_factor = 1.f; - limited = false; + limited = 0; // Now limit the jerk in all axes. LOOP_XYZE(axis) { // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop. @@ -1247,28 +1242,21 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const v_entry *= v_factor; } // Calculate jerk depending on whether the axis is coasting in the same direction or reversing. - float jerk = - (v_exit > v_entry) ? - ((v_entry > 0.f || v_exit < 0.f) ? - // coasting - (v_exit - v_entry) : - // axis reversal - max(v_exit, -v_entry)) : - // v_exit <= v_entry - ((v_entry < 0.f || v_exit > 0.f) ? - // coasting - (v_entry - v_exit) : - // axis reversal - max(-v_exit, v_entry)); + const float jerk = (v_exit > v_entry) + ? // coasting axis reversal + ( (v_entry > 0.f || v_exit < 0.f) ? (v_exit - v_entry) : max(v_exit, -v_entry) ) + : // v_exit <= v_entry coasting axis reversal + ( (v_entry < 0.f || v_exit > 0.f) ? (v_entry - v_exit) : max(-v_exit, v_entry) ); + if (jerk > max_jerk[axis]) { v_factor *= max_jerk[axis] / jerk; - limited = true; + ++limited; } } if (limited) vmax_junction *= v_factor; // Now the transition velocity is known, which maximizes the shared exit / entry velocity while // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints. - float vmax_junction_threshold = vmax_junction * 0.99f; + const float vmax_junction_threshold = vmax_junction * 0.99f; if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) { // Not coasting. The machine will stop and start the movements anyway, // better to start the segment from start. @@ -1285,7 +1273,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. - float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters); + const float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags @@ -1305,36 +1293,41 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const #if ENABLED(LIN_ADVANCE) - // Don't use LIN_ADVANCE for blocks if: - // !block->steps[E_AXIS]: We don't have E steps todo (Travel move) - // !block->steps[X_AXIS] && !block->steps[Y_AXIS]: We don't have a movement in XY direction (Retract / Prime moves) - // extruder_advance_k == 0.0: There is no advance factor set - // block->steps[E_AXIS] == block->step_event_count: A problem occurs when there's a very tiny move before a retract. - // In this case, the retract and the move will be executed together. - // This leads to an enormous number of advance steps due to a huge e_acceleration. - // The math is correct, but you don't want a retract move done with advance! - // de_float <= 0.0: Extruder is running in reverse direction (for example during "Wipe while retracting" (Slic3r) or "Combing" (Cura) movements) - if (!esteps || (!block->steps[X_AXIS] && !block->steps[Y_AXIS]) || extruder_advance_k == 0.0 || (uint32_t)esteps == block->step_event_count || de_float <= 0.0) { - block->use_advance_lead = false; - } - else { - block->use_advance_lead = true; + // + // Use LIN_ADVANCE for blocks if all these are true: + // + // esteps : We have E steps todo (a printing move) + // + // block->steps[X_AXIS] || block->steps[Y_AXIS] : We have a movement in XY direction (i.e., not retract / prime). + // + // extruder_advance_k : There is an advance factor set. + // + // block->steps[E_AXIS] != block->step_event_count : A problem occurs if the move before a retract is too small. + // In that case, the retract and move will be executed together. + // This leads to too many advance steps due to a huge e_acceleration. + // The math is good, but we must avoid retract moves with advance! + // de_float > 0.0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves) + // + block->use_advance_lead = esteps + && (block->steps[X_AXIS] || block->steps[Y_AXIS]) + && extruder_advance_k + && (uint32_t)esteps != block->step_event_count + && de_float > 0.0; + if (block->use_advance_lead) block->abs_adv_steps_multiplier8 = lround(extruder_advance_k * (de_float / mm_D_float) * block->nominal_speed / (float)block->nominal_rate * axis_steps_per_mm[E_AXIS_N] * 256.0); - } #elif ENABLED(ADVANCE) // Calculate advance rate - if (!esteps || (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS])) { - block->advance_rate = 0; - block->advance = 0; - } - else { - long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_steps_per_s2); - float advance = ((STEPS_PER_CUBIC_MM_E) * (EXTRUDER_ADVANCE_K)) * HYPOT(current_speed[E_AXIS], EXTRUSION_AREA) * 256; + if (esteps && (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS])) { + const long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_steps_per_s2); + const float advance = ((STEPS_PER_CUBIC_MM_E) * (EXTRUDER_ADVANCE_K)) * HYPOT(current_speed[E_AXIS], EXTRUSION_AREA) * 256; block->advance = advance; block->advance_rate = acc_dist ? advance / (float)acc_dist : 0; } + else + block->advance_rate = block->advance = 0; + /** SERIAL_ECHO_START; SERIAL_ECHOPGM("advance :");