Blame | Last modification | View Log | RSS feed
/*** Marlin 3D Printer Firmware* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]** Based on Sprinter and grbl.* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm** This program is free software: you can redistribute it and/or modify* it under the terms of the GNU General Public License as published by* the Free Software Foundation, either version 3 of the License, or* (at your option) any later version.** This program is distributed in the hope that it will be useful,* but WITHOUT ANY WARRANTY; without even the implied warranty of* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the* GNU General Public License for more details.** You should have received a copy of the GNU General Public License* along with this program. If not, see <http://www.gnu.org/licenses/>.**//*** planner.h** Buffer movement commands and manage the acceleration profile plan** Derived from Grbl* Copyright (c) 2009-2011 Simen Svale Skogsrud*/#ifndef PLANNER_H#define PLANNER_H#include "types.h"#include "enum.h"#include "Marlin.h"#if ABL_PLANAR#include "vector_3.h"#endifenum BlockFlagBit : char {// Recalculate trapezoids on entry junction. For optimization.BLOCK_BIT_RECALCULATE,// Nominal speed always reached.// i.e., The segment is long enough, so the nominal speed is reachable if accelerating// from a safe speed (in consideration of jerking from zero speed).BLOCK_BIT_NOMINAL_LENGTH,// The block is segment 2+ of a longer moveBLOCK_BIT_CONTINUED,// Sync the stepper counts from the blockBLOCK_BIT_SYNC_POSITION};enum BlockFlag : char {BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE),BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH),BLOCK_FLAG_CONTINUED = _BV(BLOCK_BIT_CONTINUED),BLOCK_FLAG_SYNC_POSITION = _BV(BLOCK_BIT_SYNC_POSITION)};/*** struct block_t** A single entry in the planner buffer.* Tracks linear movement over multiple axes.** The "nominal" values are as-specified by gcode, and* may never actually be reached due to acceleration limits.*/typedef struct {volatile uint8_t flag; // Block flags (See BlockFlag enum above) - Modified by ISR and main thread!#if ENABLED(UNREGISTERED_MOVE_SUPPORT)bool count_it;#endif// Fields used by the motion planner to manage accelerationfloat nominal_speed_sqr, // The nominal speed for this block in (mm/sec)^2entry_speed_sqr, // Entry speed at previous-current junction in (mm/sec)^2max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2millimeters, // The total travel of this block in mmacceleration; // acceleration mm/sec^2union {// Data used by all move blocksstruct {// Fields used by the Bresenham algorithm for tracing the lineuint32_t steps[NUM_AXIS]; // Step count along each axis};// Data used by all sync blocksstruct {int32_t position[NUM_AXIS]; // New position to force when this sync block is executed};};uint32_t step_event_count; // The number of step events required to complete this blockuint8_t active_extruder; // The extruder to move (if E move)#if ENABLED(MIXING_EXTRUDER)uint32_t mix_steps[MIXING_STEPPERS]; // Scaled steps[E_AXIS] for the mixing steppers#endif// Settings for the trapezoid generatoruint32_t accelerate_until, // The index of the step event on which to stop accelerationdecelerate_after; // The index of the step event on which to start decelerating#if ENABLED(S_CURVE_ACCELERATION)uint32_t cruise_rate, // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phaseacceleration_time, // Acceleration time and deceleration time in STEP timer countsdeceleration_time,acceleration_time_inverse, // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being useddeceleration_time_inverse;#elseuint32_t acceleration_rate; // The acceleration rate used for acceleration calculation#endifuint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)// Advance extrusion#if ENABLED(LIN_ADVANCE)bool use_advance_lead;uint16_t advance_speed, // STEP timer value for extruder speed offset ISRmax_adv_steps, // max. advance steps to get cruising speed pressure (not always nominal_speed!)final_adv_steps; // advance steps due to exit speedfloat e_D_ratio;#endifuint32_t nominal_rate, // The nominal step rate for this block in step_events/secinitial_rate, // The jerk-adjusted step rate at start of blockfinal_rate, // The minimal rate at exitacceleration_steps_per_s2; // acceleration steps/sec^2#if FAN_COUNT > 0uint16_t fan_speed[FAN_COUNT];#endif#if ENABLED(BARICUDA)uint8_t valve_pressure, e_to_p_pressure;#endifuint32_t segment_time_us;} block_t;#define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || HAS_FEEDRATE_SCALING)#define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))class Planner {public:/*** The move buffer, calculated in stepper steps** block_buffer is a ring buffer...** head,tail : indexes for write,read* head==tail : the buffer is empty* head!=tail : blocks are in the buffer* head==(tail-1)%size : the buffer is full** Writer of head is Planner::buffer_segment().* Reader of tail is Stepper::isr(). Always consider tail busy / read-only*/static block_t block_buffer[BLOCK_BUFFER_SIZE];static volatile uint8_t block_buffer_head, // Index of the next block to be pushedblock_buffer_nonbusy, // Index of the first non busy blockblock_buffer_planned, // Index of the optimally planned blockblock_buffer_tail; // Index of the busy block, if anystatic uint16_t cleaning_buffer_counter; // A counter to disable queuing of blocksstatic uint8_t delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks#if ENABLED(DISTINCT_E_FACTORS)static uint8_t last_extruder; // Respond to extruder change#endifstatic int16_t flow_percentage[EXTRUDERS]; // Extrusion factor for each extruderstatic float e_factor[EXTRUDERS]; // The flow percentage and volumetric multiplier combine to scale E movement#if DISABLED(NO_VOLUMETRICS)static float filament_size[EXTRUDERS], // diameter of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extrudervolumetric_area_nominal, // Nominal cross-sectional areavolumetric_multiplier[EXTRUDERS]; // Reciprocal of cross-sectional area of filament (in mm^2). Pre-calculated to reduce computation in the planner// May be auto-adjusted by a filament width sensor#endifstatic uint32_t max_acceleration_mm_per_s2[NUM_AXIS_N], // (mm/s^2) M201 XYZEmax_acceleration_steps_per_s2[NUM_AXIS_N], // (steps/s^2) Derived from mm_per_s2min_segment_time_us; // (µs) M205 Qstatic float max_feedrate_mm_s[NUM_AXIS_N], // (mm/s) M203 XYZE - Max speedsaxis_steps_per_mm[NUM_AXIS_N], // (steps) M92 XYZE - Steps per millimetersteps_to_mm[NUM_AXIS_N], // (mm) Millimeters per stepmin_feedrate_mm_s, // (mm/s) M205 S - Minimum linear feedrateacceleration, // (mm/s^2) M204 S - Normal acceleration. DEFAULT ACCELERATION for all printing moves.retract_acceleration, // (mm/s^2) M204 R - Retract acceleration. Filament pull-back and push-forward while standing still in the other axestravel_acceleration, // (mm/s^2) M204 T - Travel acceleration. DEFAULT ACCELERATION for all NON printing moves.min_travel_feedrate_mm_s; // (mm/s) M205 T - Minimum travel feedrate#if ENABLED(JUNCTION_DEVIATION)static float junction_deviation_mm; // (mm) M205 J#if ENABLED(LIN_ADVANCE)#if ENABLED(DISTINCT_E_FACTORS)static float max_e_jerk[EXTRUDERS]; // Calculated from junction_deviation_mm#elsestatic float max_e_jerk;#endif#endif#elsestatic float max_jerk[NUM_AXIS]; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.#endif#if ENABLED(LINE_BUILDUP_COMPENSATION_FEATURE)/** Parameters for calculating target[]* See buildup compensation theory:* https://vitana.se/opr3d/tbear/2017.html#hangprinter_project_29*/static float k0[MOV_AXIS],k1[MOV_AXIS],k2[MOV_AXIS],sqrtk1[MOV_AXIS];#endif#if HAS_LEVELINGstatic bool leveling_active; // Flag that bed leveling is enabled#if ABL_PLANARstatic matrix_3x3 bed_level_matrix; // Transform to compensate for bed level#endif#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)static float z_fade_height, inverse_z_fade_height;#endif#elsestatic constexpr bool leveling_active = false;#endif#if ENABLED(LIN_ADVANCE)static float extruder_advance_K;#endif#if HAS_POSITION_FLOATstatic float position_float[NUM_AXIS];#endif#if ENABLED(SKEW_CORRECTION)#if ENABLED(SKEW_CORRECTION_GCODE)static float xy_skew_factor;#elsestatic constexpr float xy_skew_factor = XY_SKEW_FACTOR;#endif#if ENABLED(SKEW_CORRECTION_FOR_Z)#if ENABLED(SKEW_CORRECTION_GCODE)static float xz_skew_factor, yz_skew_factor;#elsestatic constexpr float xz_skew_factor = XZ_SKEW_FACTOR, yz_skew_factor = YZ_SKEW_FACTOR;#endif#elsestatic constexpr float xz_skew_factor = 0, yz_skew_factor = 0;#endif#endif#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)static bool abort_on_endstop_hit;#endifprivate:/*** The current position of the tool in absolute steps* Recalculated if any axis_steps_per_mm are changed by gcode*/static int32_t position[NUM_AXIS];/*** Speed of previous path line segment*/static float previous_speed[NUM_AXIS];/*** Nominal speed of previous path line segment (mm/s)^2*/static float previous_nominal_speed_sqr;/*** Limit where 64bit math is necessary for acceleration calculation*/static uint32_t cutoff_long;#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)static float last_fade_z;#endif#if ENABLED(DISABLE_INACTIVE_EXTRUDER)/*** Counters to manage disabling inactive extruders*/static uint8_t g_uc_extruder_last_move[EXTRUDERS];#endif // DISABLE_INACTIVE_EXTRUDER#ifdef XY_FREQUENCY_LIMIT// Used for the frequency limit#define MAX_FREQ_TIME_US (uint32_t)(1000000.0 / XY_FREQUENCY_LIMIT)// Old direction bits. Used for speed calculationsstatic unsigned char old_direction_bits;// Segment times (in µs). Used for speed calculationsstatic uint32_t axis_segment_time_us[2][3];#endif#if ENABLED(ULTRA_LCD)volatile static uint32_t block_buffer_runtime_us; //Theoretical block buffer runtime in µs#endifpublic:/*** Instance Methods*/Planner();void init();/*** Static (class) Methods*/static void reset_acceleration_rates();static void refresh_positioning();FORCE_INLINE static void refresh_e_factor(const uint8_t e) {e_factor[e] = (flow_percentage[e] * 0.01f#if DISABLED(NO_VOLUMETRICS)* volumetric_multiplier[e]#endif);}// Manage fans, paste pressure, etc.static void check_axes_activity();// Update multipliers based on new diameter measurementsstatic void calculate_volumetric_multipliers();#if ENABLED(FILAMENT_WIDTH_SENSOR)void calculate_volumetric_for_width_sensor(const int8_t encoded_ratio);#endif#if DISABLED(NO_VOLUMETRICS)FORCE_INLINE static void set_filament_size(const uint8_t e, const float &v) {filament_size[e] = v;// make sure all extruders have some sane value for the filament sizefor (uint8_t i = 0; i < COUNT(filament_size); i++)if (!filament_size[i]) filament_size[i] = DEFAULT_NOMINAL_FILAMENT_DIA;}#endif#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)/*** Get the Z leveling fade factor based on the given Z height,* re-calculating only when needed.** Returns 1.0 if planner.z_fade_height is 0.0.* Returns 0.0 if Z is past the specified 'Fade Height'.*/inline static float fade_scaling_factor_for_z(const float &rz) {static float z_fade_factor = 1;if (z_fade_height) {if (rz >= z_fade_height) return 0;if (last_fade_z != rz) {last_fade_z = rz;z_fade_factor = 1 - rz * inverse_z_fade_height;}return z_fade_factor;}return 1;}FORCE_INLINE static void force_fade_recalc() { last_fade_z = -999.999f; }FORCE_INLINE static void set_z_fade_height(const float &zfh) {z_fade_height = zfh > 0 ? zfh : 0;inverse_z_fade_height = RECIPROCAL(z_fade_height);force_fade_recalc();}FORCE_INLINE static bool leveling_active_at_z(const float &rz) {return !z_fade_height || rz < z_fade_height;}#elseFORCE_INLINE static float fade_scaling_factor_for_z(const float &rz) {UNUSED(rz);return 1;}FORCE_INLINE static bool leveling_active_at_z(const float &rz) { UNUSED(rz); return true; }#endif#if ENABLED(SKEW_CORRECTION)FORCE_INLINE static void skew(float &cx, float &cy, const float &cz) {if (WITHIN(cx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(cy, Y_MIN_POS + 1, Y_MAX_POS)) {const float sx = cx - cy * xy_skew_factor - cz * (xz_skew_factor - (xy_skew_factor * yz_skew_factor)),sy = cy - cz * yz_skew_factor;if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {cx = sx; cy = sy;}}}FORCE_INLINE static void unskew(float &cx, float &cy, const float &cz) {if (WITHIN(cx, X_MIN_POS, X_MAX_POS) && WITHIN(cy, Y_MIN_POS, Y_MAX_POS)) {const float sx = cx + cy * xy_skew_factor + cz * xz_skew_factor,sy = cy + cz * yz_skew_factor;if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {cx = sx; cy = sy;}}}#endif // SKEW_CORRECTION#if PLANNER_LEVELING || HAS_UBL_AND_CURVES/*** Apply leveling to transform a cartesian position* as it will be given to the planner and steppers.*/static void apply_leveling(float &rx, float &ry, float &rz);FORCE_INLINE static void apply_leveling(float (&raw)[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }#endif#if PLANNER_LEVELING#define ARG_X float rx#define ARG_Y float ry#define ARG_Z float rz#if ENABLED(HANGPRINTER)#define ARG_E1 float re1#endifstatic void unapply_leveling(float raw[XYZ]);#else#define ARG_X const float &rx#define ARG_Y const float &ry#define ARG_Z const float &rz#if ENABLED(HANGPRINTER)#define ARG_E1 const float &re1#endif#endif// Number of moves currently in the planner including the busy block, if anyFORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }// Number of nonbusy moves currently in the plannerFORCE_INLINE static uint8_t nonbusy_movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_nonbusy); }// Remove all blocks from the bufferFORCE_INLINE static void clear_block_buffer() { block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail = 0; }// Check if movement queue is fullFORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }// Get count of movement slots freeFORCE_INLINE static uint8_t moves_free() { return BLOCK_BUFFER_SIZE - 1 - movesplanned(); }/*** Planner::get_next_free_block** - Get the next head indices (passed by reference)* - Wait for the number of spaces to open up in the planner* - Return the first head block*/FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head, const uint8_t count=1) {// Wait until there are enough slots freewhile (moves_free() < count) { idle(); }// Return the first available blocknext_buffer_head = next_block_index(block_buffer_head);return &block_buffer[block_buffer_head];}/*** Planner::_buffer_steps** Add a new linear movement to the buffer (in terms of steps).** target - target position in steps units* fr_mm_s - (target) speed of the move* extruder - target extruder* millimeters - the length of the movement, if known* count_it - apply this move to the counters (UNREGISTERED_MOVE_SUPPORT)** Returns true if movement was buffered, false otherwise*/static bool _buffer_steps(const int32_t (&target)[NUM_AXIS]#if HAS_POSITION_FLOAT, const float (&target_float)[NUM_AXIS]#endif, float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0#if ENABLED(UNREGISTERED_MOVE_SUPPORT), const bool count_it=true#endif);/*** Planner::_populate_block** Fills a new linear movement in the block (in terms of steps).** target - target position in steps units* fr_mm_s - (target) speed of the move* extruder - target extruder* millimeters - the length of the movement, if known* count_it - apply this move to the counters (UNREGISTERED_MOVE_SUPPORT)** Returns true is movement is acceptable, false otherwise*/static bool _populate_block(block_t * const block, bool split_move,const int32_t (&target)[NUM_AXIS]#if HAS_POSITION_FLOAT, const float (&target_float)[NUM_AXIS]#endif, float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0#if ENABLED(UNREGISTERED_MOVE_SUPPORT), const bool count_it=true#endif);/*** Planner::buffer_sync_block* Add a block to the buffer that just updates the position*/static void buffer_sync_block();/*** Planner::buffer_segment** Add a new linear movement to the buffer in axis units.** Leveling and kinematics should be applied ahead of calling this.** a,b,c,e - target positions in mm and/or degrees* (a, b, c, d, e for Hangprinter)* fr_mm_s - (target) speed of the move* extruder - target extruder* millimeters - the length of the movement, if known* count_it - remember this move in its counters (UNREGISTERED_MOVE_SUPPORT)*/static bool buffer_segment(const float &a, const float &b, const float &c,#if ENABLED(HANGPRINTER)const float &d,#endifconst float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0#if ENABLED(UNREGISTERED_MOVE_SUPPORT), bool count_it=true#endif);static void _set_position_mm(const float &a, const float &b, const float &c,#if ENABLED(HANGPRINTER)const float &d,#endifconst float &e);/*** Add a new linear movement to the buffer.* The target is NOT translated to delta/scara** Leveling will be applied to input on cartesians.* Kinematic machines should call buffer_line_kinematic (for leveled moves).* (Cartesians may also call buffer_line_kinematic.)** rx,ry,rz,e - target position in mm or degrees* (rx, ry, rz, re1 for Hangprinter)* fr_mm_s - (target) speed of the move (mm/s)* extruder - target extruder* millimeters - the length of the movement, if known*/FORCE_INLINE static bool buffer_line(ARG_X, ARG_Y, ARG_Z,#if ENABLED(HANGPRINTER)ARG_E1,#endifconst float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {#if PLANNER_LEVELING && IS_CARTESIANapply_leveling(rx, ry, rz);#endifreturn buffer_segment(rx, ry, rz,#if ENABLED(HANGPRINTER)re1,#endife, fr_mm_s, extruder, millimeters);}/*** Add a new linear movement to the buffer.* The target is cartesian, it's translated to delta/scara if* needed.** cart - x,y,z,e CARTESIAN target in mm* fr_mm_s - (target) speed of the move (mm/s)* extruder - target extruder* millimeters - the length of the movement, if known*/FORCE_INLINE static bool buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {#if PLANNER_LEVELINGfloat raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };apply_leveling(raw);#elseconst float (&raw)[XYZE] = cart;#endif#if IS_KINEMATICinverse_kinematics(raw);return buffer_segment(#if ENABLED(HANGPRINTER)line_lengths[A_AXIS], line_lengths[B_AXIS], line_lengths[C_AXIS], line_lengths[D_AXIS]#elsedelta[A_AXIS], delta[B_AXIS], delta[C_AXIS]#endif, cart[E_CART], fr_mm_s, extruder, millimeters);#elsereturn buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_CART], fr_mm_s, extruder, millimeters);#endif}/*** Set the planner.position and individual stepper positions.* Used by G92, G28, G29, and other procedures.** Multiplies by axis_steps_per_mm[] and does necessary conversion* for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.** Clears previous speed values.*/FORCE_INLINE static void set_position_mm(ARG_X, ARG_Y, ARG_Z,#if ENABLED(HANGPRINTER)ARG_E1,#endifconst float &e) {#if PLANNER_LEVELING && IS_CARTESIANapply_leveling(rx, ry, rz);#endif_set_position_mm(rx, ry, rz,#if ENABLED(HANGPRINTER)re1,#endife);}static void set_position_mm_kinematic(const float (&cart)[XYZE]);static void set_position_mm(const AxisEnum axis, const float &v);FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); }/*** Get an axis position according to stepper position(s)* For CORE machines apply translation from ABC to XYZ.*/static float get_axis_position_mm(const AxisEnum axis);// SCARA AB axes are in degrees, not mm#if IS_SCARAFORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }#endif// Called to force a quick stop of the machine (for example, when an emergency// stop is required, or when endstops are hit)static void quick_stop();// Called when an endstop is triggered. Causes the machine to stop inmediatelystatic void endstop_triggered(const AxisEnum axis);// Triggered position of an axis in mm (not core-savvy)static float triggered_position_mm(const AxisEnum axis);// Block until all buffered steps are executed / cleanedstatic void synchronize();// Wait for moves to finish and disable all steppersstatic void finish_and_disable();// Periodic tick to handle cleaning timeouts// Called from the Temperature ISR at ~1kHzstatic void tick() {if (cleaning_buffer_counter) {--cleaning_buffer_counter;#if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)if (!cleaning_buffer_counter) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));#endif}}/*** Does the buffer have any blocks queued?*/FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }/*** The current block. NULL if the buffer is empty.* This also marks the block as busy.* WARNING: Called from Stepper ISR context!*/static block_t* get_current_block() {// Get the number of moves in the planner queue so farconst uint8_t nr_moves = movesplanned();// If there are any moves queued ...if (nr_moves) {// If there is still delay of delivery of blocks running, decrement itif (delay_before_delivering) {--delay_before_delivering;// If the number of movements queued is less than 3, and there is still time// to wait, do not deliver anythingif (nr_moves < 3 && delay_before_delivering) return NULL;delay_before_delivering = 0;}// If we are here, there is no excuse to deliver the blockblock_t * const block = &block_buffer[block_buffer_tail];// No trapezoid calculated? Don't execute yet.if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return NULL;#if ENABLED(ULTRA_LCD)block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.#endif// As this block is busy, advance the nonbusy block pointerblock_buffer_nonbusy = next_block_index(block_buffer_tail);// Push block_buffer_planned pointer, if encountered.if (block_buffer_tail == block_buffer_planned)block_buffer_planned = block_buffer_nonbusy;// Return the blockreturn block;}// The queue became empty#if ENABLED(ULTRA_LCD)clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.#endifreturn NULL;}/*** "Discard" the block and "release" the memory.* Called when the current block is no longer needed.* NB: There MUST be a current block to call this function!!*/FORCE_INLINE static void discard_current_block() {if (has_blocks_queued())block_buffer_tail = next_block_index(block_buffer_tail);}#if ENABLED(ULTRA_LCD)static uint16_t block_buffer_runtime() {bool was_enabled = STEPPER_ISR_ENABLED();if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();millis_t bbru = block_buffer_runtime_us;if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();// To translate µs to ms a division by 1000 would be required.// We introduce 2.4% error here by dividing by 1024.// Doesn't matter because block_buffer_runtime_us is already too small an estimation.bbru >>= 10;// limit to about a minute.NOMORE(bbru, 0xFFFFul);return bbru;}static void clear_block_buffer_runtime() {bool was_enabled = STEPPER_ISR_ENABLED();if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();block_buffer_runtime_us = 0;if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();}#endif#if ENABLED(AUTOTEMP)static float autotemp_min, autotemp_max, autotemp_factor;static bool autotemp_enabled;static void getHighESpeed();static void autotemp_M104_M109();#endif#if ENABLED(JUNCTION_DEVIATION)FORCE_INLINE static void recalculate_max_e_jerk() {#define GET_MAX_E_JERK(N) SQRT(SQRT(0.5) * junction_deviation_mm * (N) * RECIPROCAL(1.0 - SQRT(0.5)))#if ENABLED(LIN_ADVANCE)#if ENABLED(DISTINCT_E_FACTORS)for (uint8_t i = 0; i < EXTRUDERS; i++)max_e_jerk[i] = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS + i]);#elsemax_e_jerk = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS]);#endif#endif}#endifprivate:/*** Get the index of the next / previous block in the ring buffer*/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 &initial_rate, const float &target_rate, const float &accel) {if (accel == 0) return 0; // accel was 0, set acceleration distance to 0return (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 &initial_rate, const float &final_rate, const float &accel, const float &distance) {if (accel == 0) return 0; // accel was 0, set intersection distance to 0return (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* 'distance'.*/static float max_allowable_speed_sqr(const float &accel, const float &target_velocity_sqr, const float &distance) {return target_velocity_sqr - 2 * accel * distance;}#if ENABLED(S_CURVE_ACCELERATION)/*** Calculate the speed reached given initial speed, acceleration and distance*/static float final_speed(const float &initial_velocity, const float &accel, const float &distance) {return SQRT(sq(initial_velocity) + 2 * accel * distance);}#endifstatic void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);static void reverse_pass_kernel(block_t* const current, const block_t * const next);static void forward_pass_kernel(const block_t * const previous, block_t* const current, uint8_t block_index);static void reverse_pass();static void forward_pass();static void recalculate_trapezoids();static void recalculate();#if ENABLED(JUNCTION_DEVIATION)FORCE_INLINE static void normalize_junction_vector(float (&vector)[XYZE]) {float magnitude_sq = 0;LOOP_XYZE(idx) if (vector[idx]) magnitude_sq += sq(vector[idx]);const float inv_magnitude = RSQRT(magnitude_sq);LOOP_XYZE(idx) vector[idx] *= inv_magnitude;}FORCE_INLINE static float limit_value_by_axis_maximum(const float &max_value, float (&unit_vec)[XYZE]) {float limit_value = max_value;LOOP_XYZE(idx) if (unit_vec[idx]) // Avoid divide by zeroNOMORE(limit_value, ABS(max_acceleration_mm_per_s2[idx] / unit_vec[idx]));return limit_value;}#endif // JUNCTION_DEVIATION};#define PLANNER_XY_FEEDRATE() (MIN(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]))extern Planner planner;#endif // PLANNER_H