andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

⚡️ Optimize Planner calculations (#24484)

FB4S_WIFI
tombrazier 2 years ago
committed by Scott Lahteine
parent
0a164a88fe
commit
cc4fc28fe0
2 changed files with 80 additions and 110 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 164
      Marlin/src/module/planner.cpp
  2. 26
      Marlin/src/module/planner.h

164
Marlin/src/module/planner.cpp
View File

@ -28,12 +28,14 @@
* Derived from Grbl
* Copyright (c) 2009-2011 Simen Svale Skogsrud
*
* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis.
* Ring buffer gleaned from wiring_serial library by David A. Mellis.
*
* Fast inverse function needed for Bézier interpolation for AVR
* was designed, written and tested by Eduardo José Tagle, April 2018.
*
* Reasoning behind the mathematics in this module (in the key of 'Mathematica'):
* Planner mathematics (Mathematica-style):
*
* s == speed, a == acceleration, t == time, d == distance
* Where: s == speed, a == acceleration, t == time, d == distance
*
* Basic definitions:
* Speed[s_, a_, t_] := s + (a*t)
@ -41,7 +43,7 @@
*
* Distance to reach a specific speed with a constant acceleration:
* Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
* d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
* d -> (m^2 - s^2) / (2 a)
*
* Speed after a given distance of travel with constant acceleration:
* Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
@ -49,17 +51,18 @@
*
* DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
*
* When to start braking (di) to reach a specified destination speed (s2) after accelerating
* from initial speed s1 without ever stopping at a plateau:
* When to start braking (di) to reach a specified destination speed (s2) after
* acceleration from initial speed s1 without ever reaching a plateau:
* Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
* di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
* di -> (2 a d - s1^2 + s2^2)/(4 a)
*
* IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
* We note, as an optimization, that if we have already calculated an
* acceleration distance d1 from s1 to m and a deceration distance d2
* from m to s2 then
*
* --
*
* The fast inverse function needed for Bézier interpolation for AVR
* was designed, written and tested by Eduardo José Tagle on April/2018
* d1 -> (m^2 - s1^2) / (2 a)
* d2 -> (m^2 - s2^2) / (2 a)
* di -> (d + d1 - d2) / 2
*/
#include "planner.h"
@ -211,7 +214,7 @@ xyze_long_t Planner::position{0};
uint32_t Planner::acceleration_long_cutoff;
xyze_float_t Planner::previous_speed;
float Planner::previous_nominal_speed_sqr;
float Planner::previous_nominal_speed;
#if ENABLED(DISABLE_INACTIVE_EXTRUDER)
last_move_t Planner::g_uc_extruder_last_move[E_STEPPERS] = { 0 };
@ -220,7 +223,7 @@ float Planner::previous_nominal_speed_sqr;
#ifdef XY_FREQUENCY_LIMIT
int8_t Planner::xy_freq_limit_hz = XY_FREQUENCY_LIMIT;
float Planner::xy_freq_min_speed_factor = (XY_FREQUENCY_MIN_PERCENT) * 0.01f;
int32_t Planner::xy_freq_min_interval_us = LROUND(1000000.0 / (XY_FREQUENCY_LIMIT));
int32_t Planner::xy_freq_min_interval_us = LROUND(1000000.0f / (XY_FREQUENCY_LIMIT));
#endif
#if ENABLED(LIN_ADVANCE)
@ -250,7 +253,7 @@ void Planner::init() {
TERN_(HAS_POSITION_FLOAT, position_float.reset());
TERN_(IS_KINEMATIC, position_cart.reset());
previous_speed.reset();
previous_nominal_speed_sqr = 0;
previous_nominal_speed = 0;
TERN_(ABL_PLANAR, bed_level_matrix.set_to_identity());
clear_block_buffer();
delay_before_delivering = 0;
@ -786,41 +789,48 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE));
#if ENABLED(S_CURVE_ACCELERATION)
uint32_t cruise_rate = initial_rate;
// If we have some plateau time, the cruise rate will be the nominal rate
uint32_t cruise_rate = block->nominal_rate;
#endif
const int32_t accel = block->acceleration_steps_per_s2;
// Steps required for acceleration, deceleration to/from nominal rate
uint32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel));
// Steps between acceleration and deceleration, if any
int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
// Does accelerate_steps + decelerate_steps exceed step_event_count?
// Then we can't possibly reach the nominal rate, there will be no cruising.
// Use intersection_distance() to calculate accel / braking time in order to
// reach the final_rate exactly at the end of this block.
if (plateau_steps < 0) {
const float accelerate_steps_float = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
accelerate_steps = _MIN(uint32_t(_MAX(accelerate_steps_float, 0)), block->step_event_count);
decelerate_steps = block->step_event_count - accelerate_steps;
plateau_steps = 0;
#if ENABLED(S_CURVE_ACCELERATION)
// We won't reach the cruising rate. Let's calculate the speed we will reach
cruise_rate = final_speed(initial_rate, accel, accelerate_steps);
#endif
// Steps for acceleration, plateau and deceleration
int32_t plateau_steps = block->step_event_count;
uint32_t accelerate_steps = 0,
decelerate_steps = 0;
if (accel != 0) {
// Steps required for acceleration, deceleration to/from nominal rate
const float nominal_rate_sq = sq(float(block->nominal_rate));
float accelerate_steps_float = (nominal_rate_sq - sq(float(initial_rate))) * (0.5f / accel);
accelerate_steps = CEIL(accelerate_steps_float);
const float decelerate_steps_float = (nominal_rate_sq - sq(float(final_rate))) * (0.5f / accel);
decelerate_steps = decelerate_steps_float;
// Steps between acceleration and deceleration, if any
plateau_steps -= accelerate_steps + decelerate_steps;
// Does accelerate_steps + decelerate_steps exceed step_event_count?
// Then we can't possibly reach the nominal rate, there will be no cruising.
// Calculate accel / braking time in order to reach the final_rate exactly
// at the end of this block.
if (plateau_steps < 0) {
accelerate_steps_float = CEIL((block->step_event_count + accelerate_steps_float - decelerate_steps_float) * 0.5f);
accelerate_steps = _MIN(uint32_t(_MAX(accelerate_steps_float, 0)), block->step_event_count);
decelerate_steps = block->step_event_count - accelerate_steps;
#if ENABLED(S_CURVE_ACCELERATION)
// We won't reach the cruising rate. Let's calculate the speed we will reach
cruise_rate = final_speed(initial_rate, accel, accelerate_steps);
#endif
}
}
#if ENABLED(S_CURVE_ACCELERATION)
else // We have some plateau time, so the cruise rate will be the nominal rate
cruise_rate = block->nominal_rate;
#endif
#if ENABLED(S_CURVE_ACCELERATION)
// Jerk controlled speed requires to express speed versus time, NOT steps
uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE),
deceleration_time = ((float)(cruise_rate - final_rate) / accel) * (STEPPER_TIMER_RATE),
uint32_t acceleration_time = (float(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE),
deceleration_time = (float(cruise_rate - final_rate) / accel) * (STEPPER_TIMER_RATE),
// And to offload calculations from the ISR, we also calculate the inverse of those times here
acceleration_time_inverse = get_period_inverse(acceleration_time),
deceleration_time_inverse = get_period_inverse(deceleration_time);
@ -1175,7 +1185,7 @@ void Planner::recalculate_trapezoids(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t
// Go from the tail (currently executed block) to the first block, without including it)
block_t *block = nullptr, *next = nullptr;
float current_entry_speed = 0.0, next_entry_speed = 0.0;
float current_entry_speed = 0.0f, next_entry_speed = 0.0f;
while (block_index != head_block_index) {
next = &block_buffer[block_index];
@ -1199,13 +1209,12 @@ void Planner::recalculate_trapezoids(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t
// Block is not BUSY, we won the race against the Stepper ISR:
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
const float current_nominal_speed = SQRT(block->nominal_speed_sqr),
nomr = 1.0f / current_nominal_speed;
const float nomr = 1.0f / block->nominal_speed;
calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr);
#if ENABLED(LIN_ADVANCE)
if (block->use_advance_lead) {
const float comp = block->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS];
block->max_adv_steps = current_nominal_speed * comp;
block->max_adv_steps = block->nominal_speed * comp;
block->final_adv_steps = next_entry_speed * comp;
}
#endif
@ -1240,13 +1249,12 @@ void Planner::recalculate_trapezoids(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t
if (!stepper.is_block_busy(block)) {
// Block is not BUSY, we won the race against the Stepper ISR:
const float current_nominal_speed = SQRT(block->nominal_speed_sqr),
nomr = 1.0f / current_nominal_speed;
const float nomr = 1.0f / block->nominal_speed;
calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr);
#if ENABLED(LIN_ADVANCE)
if (block->use_advance_lead) {
const float comp = block->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS];
block->max_adv_steps = current_nominal_speed * comp;
block->max_adv_steps = block->nominal_speed * comp;
block->final_adv_steps = next_entry_speed * comp;
}
#endif
@ -1290,14 +1298,10 @@ void Planner::recalculate(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t safe_exit_s
#define FAN_SET(F) do{ kickstart_fan(fan_speed, ms, F); _FAN_SET(F); }while(0)
const millis_t ms = millis();
TERN_(HAS_FAN0, FAN_SET(0));
TERN_(HAS_FAN1, FAN_SET(1));
TERN_(HAS_FAN2, FAN_SET(2));
TERN_(HAS_FAN3, FAN_SET(3));
TERN_(HAS_FAN4, FAN_SET(4));
TERN_(HAS_FAN5, FAN_SET(5));
TERN_(HAS_FAN6, FAN_SET(6));
TERN_(HAS_FAN7, FAN_SET(7));
TERN_(HAS_FAN0, FAN_SET(0)); TERN_(HAS_FAN1, FAN_SET(1));
TERN_(HAS_FAN2, FAN_SET(2)); TERN_(HAS_FAN3, FAN_SET(3));
TERN_(HAS_FAN4, FAN_SET(4)); TERN_(HAS_FAN5, FAN_SET(5));
TERN_(HAS_FAN6, FAN_SET(6)); TERN_(HAS_FAN7, FAN_SET(7));
}
#if FAN_KICKSTART_TIME
@ -1485,7 +1489,7 @@ void Planner::check_axes_activity() {
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
const block_t * const block = &block_buffer[b];
if (NUM_AXIS_GANG(block->steps.x, || block->steps.y, || block->steps.z, || block->steps.i, || block->steps.j, || block->steps.k, || block->steps.u, || block->steps.v, || block->steps.w)) {
const float se = (float)block->steps.e / block->step_event_count * SQRT(block->nominal_speed_sqr); // mm/sec;
const float se = float(block->steps.e) / block->step_event_count * block->nominal_speed; // mm/sec
NOLESS(high, se);
}
}
@ -1936,7 +1940,7 @@ bool Planner::_populate_block(
#if ENABLED(MIXING_EXTRUDER)
bool ignore_e = false;
float collector[MIXING_STEPPERS];
mixer.refresh_collector(1.0, mixer.get_current_vtool(), collector);
mixer.refresh_collector(1.0f, mixer.get_current_vtool(), collector);
MIXER_STEPPER_LOOP(e)
if (e_steps * collector[e] > max_e_steps) { ignore_e = true; break; }
#else
@ -2193,7 +2197,7 @@ bool Planner::_populate_block(
#if SECONDARY_LINEAR_AXES >= 1 && NONE(FOAMCUTTER_XYUV, ARTICULATED_ROBOT_ARM)
if (NEAR_ZERO(distance_sqr)) {
// Move does not involve any primary linear axes (xyz) but might involve secondary linear axes
distance_sqr = (0.0
distance_sqr = (0.0f
SECONDARY_AXIS_GANG(
IF_DISABLED(AXIS4_ROTATES, + sq(steps_dist_mm.i)),
IF_DISABLED(AXIS5_ROTATES, + sq(steps_dist_mm.j)),
@ -2396,7 +2400,7 @@ bool Planner::_populate_block(
if (was_enabled) stepper.wake_up();
#endif
block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0
block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0
block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
#if ENABLED(FILAMENT_WIDTH_SENSOR)
@ -2492,7 +2496,7 @@ bool Planner::_populate_block(
if (speed_factor < 1.0f) {
current_speed *= speed_factor;
block->nominal_rate *= speed_factor;
block->nominal_speed_sqr = block->nominal_speed_sqr * sq(speed_factor);
block->nominal_speed *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
@ -2592,7 +2596,7 @@ bool Planner::_populate_block(
if (block->use_advance_lead) {
block->advance_speed = (STEPPER_TIMER_RATE) / (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * settings.axis_steps_per_mm[E_AXIS_N(extruder)]);
#if ENABLED(LA_DEBUG)
if (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * 2 < SQRT(block->nominal_speed_sqr) * block->e_D_ratio)
if (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * 2 < block->nominal_speed * block->e_D_ratio)
SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed.");
if (block->advance_speed < 200)
SERIAL_ECHOLNPGM("eISR running at > 10kHz.");
@ -2663,7 +2667,7 @@ bool Planner::_populate_block(
unit_vec *= inverse_millimeters; // Use pre-calculated (1 / SQRT(x^2 + y^2 + z^2))
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float junction_cos_theta = LOGICAL_AXIS_GANG(
@ -2792,7 +2796,7 @@ bool Planner::_populate_block(
}
// Get the lowest speed
vmax_junction_sqr = _MIN(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr);
vmax_junction_sqr = _MIN(vmax_junction_sqr, sq(block->nominal_speed), sq(previous_nominal_speed));
}
else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
vmax_junction_sqr = 0;
@ -2801,27 +2805,17 @@ bool Planner::_populate_block(
#endif
#ifdef USE_CACHED_SQRT
#define CACHED_SQRT(N, V) \
static float saved_V, N; \
if (V != saved_V) { N = SQRT(V); saved_V = V; }
#else
#define CACHED_SQRT(N, V) const float N = SQRT(V)
#endif
#if HAS_CLASSIC_JERK
/**
* Adapted from Průša MKS firmware
* https://github.com/prusa3d/Prusa-Firmware
*/
CACHED_SQRT(nominal_speed, block->nominal_speed_sqr);
// Exit speed limited by a jerk to full halt of a previous last segment
static float previous_safe_speed;
// Start with a safe speed (from which the machine may halt to stop immediately).
float safe_speed = nominal_speed;
float safe_speed = block->nominal_speed;
#ifndef TRAVEL_EXTRA_XYJERK
#define TRAVEL_EXTRA_XYJERK 0
@ -2834,7 +2828,7 @@ bool Planner::_populate_block(
maxj = (max_jerk[i] + (i == X_AXIS || i == Y_AXIS ? extra_xyjerk : 0.0f)); // mj : The max jerk setting for this axis
if (jerk > maxj) { // cs > mj : New current speed too fast?
if (limited) { // limited already?
const float mjerk = nominal_speed * maxj; // ns*mj
const float mjerk = block->nominal_speed * maxj; // ns*mj
if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; // ns*mj/cs
}
else {
@ -2845,7 +2839,7 @@ bool Planner::_populate_block(
}
float vmax_junction;
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
// Estimate a maximum velocity allowed at a joint of two successive segments.
// If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
// then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
@ -2856,11 +2850,9 @@ bool Planner::_populate_block(
// The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
// Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
CACHED_SQRT(previous_nominal_speed, previous_nominal_speed_sqr);
float smaller_speed_factor = 1.0f;
if (nominal_speed < previous_nominal_speed) {
vmax_junction = nominal_speed;
if (block->nominal_speed < previous_nominal_speed) {
vmax_junction = block->nominal_speed;
smaller_speed_factor = vmax_junction / previous_nominal_speed;
}
else
@ -2927,11 +2919,11 @@ bool Planner::_populate_block(
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->flag.set_nominal(block->nominal_speed_sqr <= v_allowable_sqr);
block->flag.set_nominal(sq(block->nominal_speed) <= v_allowable_sqr);
// Update previous path unit_vector and nominal speed
previous_speed = current_speed;
previous_nominal_speed_sqr = block->nominal_speed_sqr;
previous_nominal_speed = block->nominal_speed;
position = target; // Update the position
@ -3268,7 +3260,7 @@ void Planner::set_machine_position_mm(const abce_pos_t &abce) {
);
if (has_blocks_queued()) {
//previous_nominal_speed_sqr = 0.0; // Reset planner junction speeds. Assume start from rest.
//previous_nominal_speed = 0.0f; // Reset planner junction speeds. Assume start from rest.
//previous_speed.reset();
buffer_sync_block(BLOCK_BIT_SYNC_POSITION);
}
@ -3344,7 +3336,7 @@ void Planner::refresh_positioning() {
inline void limit_and_warn(float &val, const AxisEnum axis, PGM_P const setting_name, const xyze_float_t &max_limit) {
const uint8_t lim_axis = TERN_(HAS_EXTRUDERS, axis > E_AXIS ? E_AXIS :) axis;
const float before = val;
LIMIT(val, 0.1, max_limit[lim_axis]);
LIMIT(val, 0.1f, max_limit[lim_axis]);
if (before != val) {
SERIAL_CHAR(AXIS_CHAR(lim_axis));
SERIAL_ECHOPGM(" Max ");

26
Marlin/src/module/planner.h
View File

@ -199,7 +199,7 @@ typedef struct PlannerBlock {
volatile bool is_move() { return !(is_sync() || is_page()); }
// Fields used by the motion planner to manage acceleration
float nominal_speed_sqr, // The nominal speed for this block in (mm/sec)^2
float nominal_speed, // The nominal speed for this block in (mm/sec)
entry_speed_sqr, // Entry speed at previous-current junction in (mm/sec)^2
max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2
millimeters, // The total travel of this block in mm
@ -510,7 +510,7 @@ class Planner {
/**
* Nominal speed of previous path line segment (mm/s)^2
*/
static float previous_nominal_speed_sqr;
static float previous_nominal_speed;
/**
* Limit where 64bit math is necessary for acceleration calculation
@ -1009,28 +1009,6 @@ class Planner {
static constexpr uint8_t next_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index + 1); }
static constexpr uint8_t prev_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index - 1); }
/**
* Calculate the distance (not time) it takes to accelerate
* from initial_rate to target_rate using the given acceleration:
*/
static float estimate_acceleration_distance(const_float_t initial_rate, const_float_t target_rate, const_float_t accel) {
if (accel == 0) return 0; // accel was 0, set acceleration distance to 0
return (sq(target_rate) - sq(initial_rate)) / (accel * 2);
}
/**
* Return the point at which you must start braking (at the rate of -'accel') if
* you start at 'initial_rate', accelerate (until reaching the point), and want to end at
* 'final_rate' after traveling 'distance'.
*
* This is used to compute the intersection point between acceleration and deceleration
* in cases where the "trapezoid" has no plateau (i.e., never reaches maximum speed)
*/
static float intersection_distance(const_float_t initial_rate, const_float_t final_rate, const_float_t accel, const_float_t distance) {
if (accel == 0) return 0; // accel was 0, set intersection distance to 0
return (accel * 2 * distance - sq(initial_rate) + sq(final_rate)) / (accel * 4);
}
/**
* Calculate the maximum allowable speed squared at this point, in order
* to reach 'target_velocity_sqr' using 'acceleration' within a given

Write Preview
Loading…
Cancel
Save