Subversion Repositories Tronxy-X3A-Marlin

/**

 * 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.cpp

 *

 * Buffer movement commands and manage the acceleration profile plan

 *

 * Derived from Grbl

 * Copyright (c) 2009-2011 Simen Svale Skogsrud

 *

 * The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis.

 *

 *

 * Reasoning behind the mathematics in this module (in the key of 'Mathematica'):

 *

 * s == speed, a == acceleration, t == time, d == distance

 *

 * Basic definitions:

 *   Speed[s_, a_, t_] := s + (a*t)

 *   Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]

 *

 * 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()

 *

 * Speed after a given distance of travel with constant acceleration:

 *   Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]

 *   m -> Sqrt[2 a d + s^2]

 *

 * 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:

 *   Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]

 *   di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()

 *

 * IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)

 *

 * --

 *

 * The fast inverse function needed for Bézier interpolation for AVR

 * was designed, written and tested by Eduardo José Tagle on April/2018

 */

#include "planner.h"

#include "stepper.h"

#include "temperature.h"

#include "ultralcd.h"

#include "language.h"

#include "parser.h"

#include "Marlin.h"

#if ENABLED(MESH_BED_LEVELING)

  #include "mesh_bed_leveling.h"

#elif ENABLED(AUTO_BED_LEVELING_UBL)

  #include "ubl.h"

#endif

#if ENABLED(AUTO_POWER_CONTROL)

  #include "power.h"

#endif

// Delay for delivery of first block to the stepper ISR, if the queue contains 2 or

// fewer movements. The delay is measured in milliseconds, and must be less than 250ms

#define BLOCK_DELAY_FOR_1ST_MOVE 100

Planner planner;

  // public:

/**

 * A ring buffer of moves described in steps

 */

block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];

volatile uint8_t Planner::block_buffer_head,    // Index of the next block to be pushed

                 Planner::block_buffer_nonbusy, // Index of the first non-busy block

                 Planner::block_buffer_planned, // Index of the optimally planned block

                 Planner::block_buffer_tail;    // Index of the busy block, if any

uint16_t Planner::cleaning_buffer_counter;      // A counter to disable queuing of blocks

uint8_t Planner::delay_before_delivering;       // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks

uint32_t Planner::max_acceleration_mm_per_s2[NUM_AXIS_N],    // (mm/s^2) M201 XYZE

         Planner::max_acceleration_steps_per_s2[NUM_AXIS_N], // (steps/s^2) Derived from mm_per_s2

         Planner::min_segment_time_us;                       // (µs) M205 Q

float Planner::max_feedrate_mm_s[NUM_AXIS_N], // (mm/s) M203 XYZE - Max speeds

      Planner::axis_steps_per_mm[NUM_AXIS_N], // (steps) M92 XYZE - Steps per millimeter

      Planner::steps_to_mm[NUM_AXIS_N],       // (mm) Millimeters per step

      Planner::min_feedrate_mm_s,             // (mm/s) M205 S - Minimum linear feedrate

      Planner::acceleration,                  // (mm/s^2) M204 S - Normal acceleration. DEFAULT ACCELERATION for all printing moves.

      Planner::retract_acceleration,          // (mm/s^2) M204 R - Retract acceleration. Filament pull-back and push-forward while standing still in the other axes

      Planner::travel_acceleration,           // (mm/s^2) M204 T - Travel acceleration. DEFAULT ACCELERATION for all NON printing moves.

      Planner::min_travel_feedrate_mm_s;      // (mm/s) M205 T - Minimum travel feedrate

#if ENABLED(JUNCTION_DEVIATION)

  float Planner::junction_deviation_mm;       // (mm) M205 J

  #if ENABLED(LIN_ADVANCE)

    #if ENABLED(DISTINCT_E_FACTORS)

      float Planner::max_e_jerk[EXTRUDERS];   // Calculated from junction_deviation_mm

    #else

      float Planner::max_e_jerk;

    #endif

  #endif

#else

  float Planner::max_jerk[NUM_AXIS];          // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.

#endif

#if ENABLED(LINE_BUILDUP_COMPENSATION_FEATURE)

  float Planner::k0[MOV_AXIS],

        Planner::k1[MOV_AXIS],

        Planner::k2[MOV_AXIS],

        Planner::sqrtk1[MOV_AXIS];

#endif

#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)

  bool Planner::abort_on_endstop_hit = false;

#endif

#if ENABLED(DISTINCT_E_FACTORS)

  uint8_t Planner::last_extruder = 0;     // Respond to extruder change

  #define _EINDEX (E_AXIS + active_extruder)

#else

  #define _EINDEX E_AXIS

#endif

int16_t Planner::flow_percentage[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(100); // Extrusion factor for each extruder

float Planner::e_factor[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(1.0f); // The flow percentage and volumetric multiplier combine to scale E movement

#if DISABLED(NO_VOLUMETRICS)

  float Planner::filament_size[EXTRUDERS],          // diameter of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder

        Planner::volumetric_area_nominal = CIRCLE_AREA(float(DEFAULT_NOMINAL_FILAMENT_DIA) * 0.5f), // Nominal cross-sectional area

        Planner::volumetric_multiplier[EXTRUDERS];  // Reciprocal of cross-sectional area of filament (in mm^2). Pre-calculated to reduce computation in the planner

#endif

#if HAS_LEVELING

  bool Planner::leveling_active = false; // Flag that auto bed leveling is enabled

  #if ABL_PLANAR

    matrix_3x3 Planner::bed_level_matrix; // Transform to compensate for bed level

  #endif

  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)

    float Planner::z_fade_height,      // Initialized by settings.load()

          Planner::inverse_z_fade_height,

          Planner::last_fade_z;

  #endif

#else

  constexpr bool Planner::leveling_active;

#endif

#if ENABLED(SKEW_CORRECTION)

  #if ENABLED(SKEW_CORRECTION_GCODE)

    float Planner::xy_skew_factor;

  #else

    constexpr float Planner::xy_skew_factor;

  #endif

  #if ENABLED(SKEW_CORRECTION_FOR_Z) && ENABLED(SKEW_CORRECTION_GCODE)

    float Planner::xz_skew_factor, Planner::yz_skew_factor;

  #else

    constexpr float Planner::xz_skew_factor, Planner::yz_skew_factor;

  #endif

#endif

#if ENABLED(AUTOTEMP)

  float Planner::autotemp_max = 250,

        Planner::autotemp_min = 210,

        Planner::autotemp_factor = 0.1f;

  bool Planner::autotemp_enabled = false;

#endif

// private:

int32_t Planner::position[NUM_AXIS] = { 0 };

uint32_t Planner::cutoff_long;

float Planner::previous_speed[NUM_AXIS],

      Planner::previous_nominal_speed_sqr;

#if ENABLED(DISABLE_INACTIVE_EXTRUDER)

  uint8_t Planner::g_uc_extruder_last_move[EXTRUDERS] = { 0 };

#endif

#ifdef XY_FREQUENCY_LIMIT

  // Old direction bits. Used for speed calculations

  unsigned char Planner::old_direction_bits = 0;

  // Segment times (in µs). Used for speed calculations

  uint32_t Planner::axis_segment_time_us[2][3] = { { MAX_FREQ_TIME_US + 1, 0, 0 }, { MAX_FREQ_TIME_US + 1, 0, 0 } };

#endif

#if ENABLED(LIN_ADVANCE)

  float Planner::extruder_advance_K; // Initialized by settings.load()

#endif

#if HAS_POSITION_FLOAT

  float Planner::position_float[NUM_AXIS]; // Needed for accurate maths. Steps cannot be used!

#endif

#if ENABLED(ULTRA_LCD)

  volatile uint32_t Planner::block_buffer_runtime_us = 0;

#endif

/**

 * Class and Instance Methods

 */

Planner::Planner() { init(); }

void Planner::init() {

  ZERO(position);

  #if HAS_POSITION_FLOAT

    ZERO(position_float);

  #endif

  ZERO(previous_speed);

  previous_nominal_speed_sqr = 0;

  #if ABL_PLANAR

    bed_level_matrix.set_to_identity();

  #endif

  clear_block_buffer();

  delay_before_delivering = 0;

}

#if ENABLED(S_CURVE_ACCELERATION)

  /**

   * This routine returns 0x1000000 / d, getting the inverse as fast as possible.

   * A fast-converging iterative Newton-Raphson method can reach full precision in

   * just 1 iteration, and takes 211 cycles (worst case; the mean case is less, up

   * to 30 cycles for small divisors), instead of the 500 cycles a normal division

   * would take.

   *

   * Inspired by the following page:

   *  https://stackoverflow.com/questions/27801397/newton-raphson-division-with-big-integers

   *

   * Suppose we want to calculate  floor(2 ^ k / B)  where B is a positive integer

   * Then, B must be <= 2^k, otherwise, the quotient is 0.

   *

   * The Newton - Raphson iteration for x = B / 2 ^ k yields:

   *  q[n + 1] = q[n] * (2 - q[n] * B / 2 ^ k)

   *

   * This can be rearranged to:

   *  q[n + 1] = q[n] * (2 ^ (k + 1) - q[n] * B) >> k

   *

   * Each iteration requires only integer multiplications and bit shifts.

   * It doesn't necessarily converge to floor(2 ^ k / B) but in the worst case

   * it eventually alternates between floor(2 ^ k / B) and ceil(2 ^ k / B).

   * So it checks for this case and extracts floor(2 ^ k / B).

   *

   * A simple but important optimization for this approach is to truncate

   * multiplications (i.e., calculate only the higher bits of the product) in the

   * early iterations of the Newton - Raphson method. This is done so the results

   * of the early iterations are far from the quotient. Then it doesn't matter if

   * they are done inaccurately.

   * It's important to pick a good starting value for x. Knowing how many

   * digits the divisor has, it can be estimated:

   *

   *   2^k / x = 2 ^ log2(2^k / x)

   *   2^k / x = 2 ^(log2(2^k)-log2(x))

   *   2^k / x = 2 ^(k*log2(2)-log2(x))

   *   2^k / x = 2 ^ (k-log2(x))

   *   2^k / x >= 2 ^ (k-floor(log2(x)))

   *   floor(log2(x)) is simply the index of the most significant bit set.

   *

   * If this estimation can be improved even further the number of iterations can be

   * reduced a lot, saving valuable execution time.

   * The paper "Software Integer Division" by Thomas L.Rodeheffer, Microsoft

   * Research, Silicon Valley,August 26, 2008, available at

   * https://www.microsoft.com/en-us/research/wp-content/uploads/2008/08/tr-2008-141.pdf

   * suggests, for its integer division algorithm, using a table to supply the first

   * 8 bits of precision, then, due to the quadratic convergence nature of the

   * Newton-Raphon iteration, just 2 iterations should be enough to get maximum

   * precision of the division.

   * By precomputing values of inverses for small denominator values, just one

   * Newton-Raphson iteration is enough to reach full precision.

   * This code uses the top 9 bits of the denominator as index.

   *

   * The AVR assembly function implements this C code using the data below:

   *

   *  // For small divisors, it is best to directly retrieve the results

   *  if (d <= 110) return pgm_read_dword(&small_inv_tab[d]);

   *

   *  // Compute initial estimation of 0x1000000/x -

   *  // Get most significant bit set on divider

   *  uint8_t idx = 0;

   *  uint32_t nr = d;

   *  if (!(nr & 0xFF0000)) {

   *    nr <<= 8; idx += 8;

   *    if (!(nr & 0xFF0000)) { nr <<= 8; idx += 8; }

   *  }

   *  if (!(nr & 0xF00000)) { nr <<= 4; idx += 4; }

   *  if (!(nr & 0xC00000)) { nr <<= 2; idx += 2; }

   *  if (!(nr & 0x800000)) { nr <<= 1; idx += 1; }

   *

   *  // Isolate top 9 bits of the denominator, to be used as index into the initial estimation table

   *  uint32_t tidx = nr >> 15,                                       // top 9 bits. bit8 is always set

   *           ie = inv_tab[tidx & 0xFF] + 256,                       // Get the table value. bit9 is always set

   *           x = idx <= 8 ? (ie >> (8 - idx)) : (ie << (idx - 8));  // Position the estimation at the proper place

   *

   *  x = uint32_t((x * uint64_t(_BV(25) - x * d)) >> 24);            // Refine estimation by newton-raphson. 1 iteration is enough

   *  const uint32_t r = _BV(24) - x * d;                             // Estimate remainder

   *  if (r >= d) x++;                                                // Check whether to adjust result

   *  return uint32_t(x);                                             // x holds the proper estimation

   *

   */

  static uint32_t get_period_inverse(uint32_t d) {

    static const uint8_t inv_tab[256] PROGMEM = {

      255,253,252,250,248,246,244,242,240,238,236,234,233,231,229,227,

      225,224,222,220,218,217,215,213,212,210,208,207,205,203,202,200,

      199,197,195,194,192,191,189,188,186,185,183,182,180,179,178,176,

      175,173,172,170,169,168,166,165,164,162,161,160,158,157,156,154,

      153,152,151,149,148,147,146,144,143,142,141,139,138,137,136,135,

      134,132,131,130,129,128,127,126,125,123,122,121,120,119,118,117,

      116,115,114,113,112,111,110,109,108,107,106,105,104,103,102,101,

      100,99,98,97,96,95,94,93,92,91,90,89,88,88,87,86,

      85,84,83,82,81,80,80,79,78,77,76,75,74,74,73,72,

      71,70,70,69,68,67,66,66,65,64,63,62,62,61,60,59,

      59,58,57,56,56,55,54,53,53,52,51,50,50,49,48,48,

      47,46,46,45,44,43,43,42,41,41,40,39,39,38,37,37,

      36,35,35,34,33,33,32,32,31,30,30,29,28,28,27,27,

      26,25,25,24,24,23,22,22,21,21,20,19,19,18,18,17,

      17,16,15,15,14,14,13,13,12,12,11,10,10,9,9,8,

      8,7,7,6,6,5,5,4,4,3,3,2,2,1,0,0

    };

    // For small denominators, it is cheaper to directly store the result.

    //  For bigger ones, just ONE Newton-Raphson iteration is enough to get

    //  maximum precision we need

    static const uint32_t small_inv_tab[111] PROGMEM = {

      16777216,16777216,8388608,5592405,4194304,3355443,2796202,2396745,2097152,1864135,1677721,1525201,1398101,1290555,1198372,1118481,

      1048576,986895,932067,883011,838860,798915,762600,729444,699050,671088,645277,621378,599186,578524,559240,541200,

      524288,508400,493447,479349,466033,453438,441505,430185,419430,409200,399457,390167,381300,372827,364722,356962,

      349525,342392,335544,328965,322638,316551,310689,305040,299593,294337,289262,284359,279620,275036,270600,266305,

      262144,258111,254200,250406,246723,243148,239674,236298,233016,229824,226719,223696,220752,217885,215092,212369,

      209715,207126,204600,202135,199728,197379,195083,192841,190650,188508,186413,184365,182361,180400,178481,176602,

      174762,172960,171196,169466,167772,166111,164482,162885,161319,159783,158275,156796,155344,153919,152520

    };

    // For small divisors, it is best to directly retrieve the results

    if (d <= 110) return pgm_read_dword(&small_inv_tab[d]);

    register uint8_t r8 = d & 0xFF,

                     r9 = (d >> 8) & 0xFF,

                     r10 = (d >> 16) & 0xFF,

                     r2,r3,r4,r5,r6,r7,r11,r12,r13,r14,r15,r16,r17,r18;

    register const uint8_t* ptab = inv_tab;

    __asm__ __volatile__(

      // %8:%7:%6 = interval

      // r31:r30: MUST be those registers, and they must point to the inv_tab

      A("clr %13")                      // %13 = 0

      // Now we must compute

      // result = 0xFFFFFF / d

      // %8:%7:%6 = interval

      // %16:%15:%14 = nr

      // %13 = 0

      // A plain division of 24x24 bits should take 388 cycles to complete. We will

      // use Newton-Raphson for the calculation, and will strive to get way less cycles

      // for the same result - Using C division, it takes 500cycles to complete .

      A("clr %3")                       // idx = 0

      A("mov %14,%6")

      A("mov %15,%7")

      A("mov %16,%8")                   // nr = interval

      A("tst %16")                      // nr & 0xFF0000 == 0 ?

      A("brne 2f")                      // No, skip this

      A("mov %16,%15")

      A("mov %15,%14")                  // nr <<= 8, %14 not needed

      A("subi %3,-8")                   // idx += 8

      A("tst %16")                      // nr & 0xFF0000 == 0 ?

      A("brne 2f")                      // No, skip this

      A("mov %16,%15")                  // nr <<= 8, %14 not needed

      A("clr %15")                      // We clear %14

      A("subi %3,-8")                   // idx += 8

      // here %16 != 0 and %16:%15 contains at least 9 MSBits, or both %16:%15 are 0

      L("2")

      A("cpi %16,0x10")                 // (nr & 0xF00000) == 0 ?

      A("brcc 3f")                      // No, skip this

      A("swap %15")                     // Swap nibbles

      A("swap %16")                     // Swap nibbles. Low nibble is 0

      A("mov %14, %15")

      A("andi %14,0x0F")                // Isolate low nibble

      A("andi %15,0xF0")                // Keep proper nibble in %15

      A("or %16, %14")                  // %16:%15 <<= 4

      A("subi %3,-4")                   // idx += 4

      L("3")

      A("cpi %16,0x40")                 // (nr & 0xC00000) == 0 ?

      A("brcc 4f")                      // No, skip this

      A("add %15,%15")

      A("adc %16,%16")

      A("add %15,%15")

      A("adc %16,%16")                  // %16:%15 <<= 2

      A("subi %3,-2")                   // idx += 2

      L("4")

      A("cpi %16,0x80")                 // (nr & 0x800000) == 0 ?

      A("brcc 5f")                      // No, skip this

      A("add %15,%15")

      A("adc %16,%16")                  // %16:%15 <<= 1

      A("inc %3")                       // idx += 1

      // Now %16:%15 contains its MSBit set to 1, or %16:%15 is == 0. We are now absolutely sure

      // we have at least 9 MSBits available to enter the initial estimation table

      L("5")

      A("add %15,%15")

      A("adc %16,%16")                  // %16:%15 = tidx = (nr <<= 1), we lose the top MSBit (always set to 1, %16 is the index into the inverse table)

      A("add r30,%16")                  // Only use top 8 bits

      A("adc r31,%13")                  // r31:r30 = inv_tab + (tidx)

      A("lpm %14, Z")                   // %14 = inv_tab[tidx]

      A("ldi %15, 1")                   // %15 = 1  %15:%14 = inv_tab[tidx] + 256

      // We must scale the approximation to the proper place

      A("clr %16")                      // %16 will always be 0 here

      A("subi %3,8")                    // idx == 8 ?

      A("breq 6f")                      // yes, no need to scale

      A("brcs 7f")                      // If C=1, means idx < 8, result was negative!

      // idx > 8, now %3 = idx - 8. We must perform a left shift. idx range:[1-8]

      A("sbrs %3,0")                    // shift by 1bit position?

      A("rjmp 8f")                      // No

      A("add %14,%14")

      A("adc %15,%15")                  // %15:16 <<= 1

      L("8")

      A("sbrs %3,1")                    // shift by 2bit position?

      A("rjmp 9f")                      // No

      A("add %14,%14")

      A("adc %15,%15")

      A("add %14,%14")

      A("adc %15,%15")                  // %15:16 <<= 1

      L("9")

      A("sbrs %3,2")                    // shift by 4bits position?

      A("rjmp 16f")                     // No

      A("swap %15")                     // Swap nibbles. lo nibble of %15 will always be 0

      A("swap %14")                     // Swap nibbles

      A("mov %12,%14")

      A("andi %12,0x0F")                // isolate low nibble

      A("andi %14,0xF0")                // and clear it

      A("or %15,%12")                   // %15:%16 <<= 4

      L("16")

      A("sbrs %3,3")                    // shift by 8bits position?

      A("rjmp 6f")                      // No, we are done

      A("mov %16,%15")

      A("mov %15,%14")

      A("clr %14")

      A("jmp 6f")

      // idx < 8, now %3 = idx - 8. Get the count of bits

      L("7")

      A("neg %3")                       // %3 = -idx = count of bits to move right. idx range:[1...8]

      A("sbrs %3,0")                    // shift by 1 bit position ?

      A("rjmp 10f")                     // No, skip it

      A("asr %15")                      // (bit7 is always 0 here)

      A("ror %14")

      L("10")

      A("sbrs %3,1")                    // shift by 2 bit position ?

      A("rjmp 11f")                     // No, skip it

      A("asr %15")                      // (bit7 is always 0 here)

      A("ror %14")

      A("asr %15")                      // (bit7 is always 0 here)

      A("ror %14")

      L("11")

      A("sbrs %3,2")                    // shift by 4 bit position ?

      A("rjmp 12f")                     // No, skip it

      A("swap %15")                     // Swap nibbles

      A("andi %14, 0xF0")               // Lose the lowest nibble

      A("swap %14")                     // Swap nibbles. Upper nibble is 0

      A("or %14,%15")                   // Pass nibble from upper byte

      A("andi %15, 0x0F")               // And get rid of that nibble

      L("12")

      A("sbrs %3,3")                    // shift by 8 bit position ?

      A("rjmp 6f")                      // No, skip it

      A("mov %14,%15")

      A("clr %15")

      L("6")                            // %16:%15:%14 = initial estimation of 0x1000000 / d

      // Now, we must refine the estimation present on %16:%15:%14 using 1 iteration

      // of Newton-Raphson. As it has a quadratic convergence, 1 iteration is enough

      // to get more than 18bits of precision (the initial table lookup gives 9 bits of

      // precision to start from). 18bits of precision is all what is needed here for result

      // %8:%7:%6 = d = interval

      // %16:%15:%14 = x = initial estimation of 0x1000000 / d

      // %13 = 0

      // %3:%2:%1:%0 = working accumulator

      // Compute 1<<25 - x*d. Result should never exceed 25 bits and should always be positive

      A("clr %0")

      A("clr %1")

      A("clr %2")

      A("ldi %3,2")                     // %3:%2:%1:%0 = 0x2000000

      A("mul %6,%14")                   // r1:r0 = LO(d) * LO(x)

      A("sub %0,r0")

      A("sbc %1,r1")

      A("sbc %2,%13")

      A("sbc %3,%13")                   // %3:%2:%1:%0 -= LO(d) * LO(x)

      A("mul %7,%14")                   // r1:r0 = MI(d) * LO(x)

      A("sub %1,r0")

      A("sbc %2,r1")

      A("sbc %3,%13")                   // %3:%2:%1:%0 -= MI(d) * LO(x) << 8

      A("mul %8,%14")                   // r1:r0 = HI(d) * LO(x)

      A("sub %2,r0")

      A("sbc %3,r1")                    // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16

      A("mul %6,%15")                   // r1:r0 = LO(d) * MI(x)

      A("sub %1,r0")

      A("sbc %2,r1")

      A("sbc %3,%13")                   // %3:%2:%1:%0 -= LO(d) * MI(x) << 8

      A("mul %7,%15")                   // r1:r0 = MI(d) * MI(x)

      A("sub %2,r0")

      A("sbc %3,r1")                    // %3:%2:%1:%0 -= MI(d) * MI(x) << 16

      A("mul %8,%15")                   // r1:r0 = HI(d) * MI(x)

      A("sub %3,r0")                    // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24

      A("mul %6,%16")                   // r1:r0 = LO(d) * HI(x)

      A("sub %2,r0")

      A("sbc %3,r1")                    // %3:%2:%1:%0 -= LO(d) * HI(x) << 16

      A("mul %7,%16")                   // r1:r0 = MI(d) * HI(x)

      A("sub %3,r0")                    // %3:%2:%1:%0 -= MI(d) * HI(x) << 24

      // %3:%2:%1:%0 = (1<<25) - x*d     [169]

      // We need to multiply that result by x, and we are only interested in the top 24bits of that multiply

      // %16:%15:%14 = x = initial estimation of 0x1000000 / d

      // %3:%2:%1:%0 = (1<<25) - x*d = acc

      // %13 = 0

      // result = %11:%10:%9:%5:%4

      A("mul %14,%0")                   // r1:r0 = LO(x) * LO(acc)

      A("mov %4,r1")

      A("clr %5")

      A("clr %9")

      A("clr %10")

      A("clr %11")                      // %11:%10:%9:%5:%4 = LO(x) * LO(acc) >> 8

      A("mul %15,%0")                   // r1:r0 = MI(x) * LO(acc)

      A("add %4,r0")

      A("adc %5,r1")

      A("adc %9,%13")

      A("adc %10,%13")

      A("adc %11,%13")                  // %11:%10:%9:%5:%4 += MI(x) * LO(acc)

      A("mul %16,%0")                   // r1:r0 = HI(x) * LO(acc)

      A("add %5,r0")

      A("adc %9,r1")

      A("adc %10,%13")

      A("adc %11,%13")                  // %11:%10:%9:%5:%4 += MI(x) * LO(acc) << 8

      A("mul %14,%1")                   // r1:r0 = LO(x) * MIL(acc)

      A("add %4,r0")

      A("adc %5,r1")

      A("adc %9,%13")

      A("adc %10,%13")

      A("adc %11,%13")                  // %11:%10:%9:%5:%4 = LO(x) * MIL(acc)

      A("mul %15,%1")                   // r1:r0 = MI(x) * MIL(acc)

      A("add %5,r0")

      A("adc %9,r1")

      A("adc %10,%13")

      A("adc %11,%13")                  // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 8

      A("mul %16,%1")                   // r1:r0 = HI(x) * MIL(acc)

      A("add %9,r0")

      A("adc %10,r1")

      A("adc %11,%13")                  // %11:%10:%9:%5:%4 += MI(x) * MIL(acc) << 16

      A("mul %14,%2")                   // r1:r0 = LO(x) * MIH(acc)

      A("add %5,r0")

      A("adc %9,r1")

      A("adc %10,%13")

      A("adc %11,%13")                  // %11:%10:%9:%5:%4 = LO(x) * MIH(acc) << 8

      A("mul %15,%2")                   // r1:r0 = MI(x) * MIH(acc)

      A("add %9,r0")

      A("adc %10,r1")

      A("adc %11,%13")                  // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 16

      A("mul %16,%2")                   // r1:r0 = HI(x) * MIH(acc)

      A("add %10,r0")

      A("adc %11,r1")                   // %11:%10:%9:%5:%4 += MI(x) * MIH(acc) << 24

      A("mul %14,%3")                   // r1:r0 = LO(x) * HI(acc)

      A("add %9,r0")

      A("adc %10,r1")

      A("adc %11,%13")                  // %11:%10:%9:%5:%4 = LO(x) * HI(acc) << 16

      A("mul %15,%3")                   // r1:r0 = MI(x) * HI(acc)

      A("add %10,r0")

      A("adc %11,r1")                   // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 24

      A("mul %16,%3")                   // r1:r0 = HI(x) * HI(acc)

      A("add %11,r0")                   // %11:%10:%9:%5:%4 += MI(x) * HI(acc) << 32

      // At this point, %11:%10:%9 contains the new estimation of x.

      // Finally, we must correct the result. Estimate remainder as

      // (1<<24) - x*d

      // %11:%10:%9 = x

      // %8:%7:%6 = d = interval" "\n\t"

      A("ldi %3,1")

      A("clr %2")

      A("clr %1")

      A("clr %0")                       // %3:%2:%1:%0 = 0x1000000

      A("mul %6,%9")                    // r1:r0 = LO(d) * LO(x)

      A("sub %0,r0")

      A("sbc %1,r1")

      A("sbc %2,%13")

      A("sbc %3,%13")                   // %3:%2:%1:%0 -= LO(d) * LO(x)

      A("mul %7,%9")                    // r1:r0 = MI(d) * LO(x)

      A("sub %1,r0")

      A("sbc %2,r1")

      A("sbc %3,%13")                   // %3:%2:%1:%0 -= MI(d) * LO(x) << 8

      A("mul %8,%9")                    // r1:r0 = HI(d) * LO(x)

      A("sub %2,r0")

      A("sbc %3,r1")                    // %3:%2:%1:%0 -= MIL(d) * LO(x) << 16

      A("mul %6,%10")                   // r1:r0 = LO(d) * MI(x)

      A("sub %1,r0")

      A("sbc %2,r1")

      A("sbc %3,%13")                   // %3:%2:%1:%0 -= LO(d) * MI(x) << 8

      A("mul %7,%10")                   // r1:r0 = MI(d) * MI(x)

      A("sub %2,r0")

      A("sbc %3,r1")                    // %3:%2:%1:%0 -= MI(d) * MI(x) << 16

      A("mul %8,%10")                   // r1:r0 = HI(d) * MI(x)

      A("sub %3,r0")                    // %3:%2:%1:%0 -= MIL(d) * MI(x) << 24

      A("mul %6,%11")                   // r1:r0 = LO(d) * HI(x)

      A("sub %2,r0")

      A("sbc %3,r1")                    // %3:%2:%1:%0 -= LO(d) * HI(x) << 16

      A("mul %7,%11")                   // r1:r0 = MI(d) * HI(x)

      A("sub %3,r0")                    // %3:%2:%1:%0 -= MI(d) * HI(x) << 24

      // %3:%2:%1:%0 = r = (1<<24) - x*d

      // %8:%7:%6 = d = interval

      // Perform the final correction

      A("sub %0,%6")

      A("sbc %1,%7")

      A("sbc %2,%8")                    // r -= d

      A("brcs 14f")                     // if ( r >= d)

      // %11:%10:%9 = x

      A("ldi %3,1")

      A("add %9,%3")

      A("adc %10,%13")

      A("adc %11,%13")                  // x++

      L("14")

      // Estimation is done. %11:%10:%9 = x

      A("clr __zero_reg__")             // Make C runtime happy

      // [211 cycles total]

      : "=r" (r2),

        "=r" (r3),

        "=r" (r4),

        "=d" (r5),

        "=r" (r6),

        "=r" (r7),

        "+r" (r8),

        "+r" (r9),

        "+r" (r10),

        "=d" (r11),

        "=r" (r12),

        "=r" (r13),

        "=d" (r14),

        "=d" (r15),

        "=d" (r16),

        "=d" (r17),

        "=d" (r18),

        "+z" (ptab)

      :

      : "r0", "r1", "cc"

    );

    // Return the result

    return r11 | (uint16_t(r12) << 8) | (uint32_t(r13) << 16);

  }

#endif // S_CURVE_ACCELERATION

#define MINIMAL_STEP_RATE 120

/**

 * Calculate trapezoid parameters, multiplying the entry- and exit-speeds

 * by the provided factors.

 **

 * ############ VERY IMPORTANT ############

 * NOTE that the PRECONDITION to call this function is that the block is

 * NOT BUSY and it is marked as RECALCULATE. That WARRANTIES the Stepper ISR

 * is not and will not use the block while we modify it, so it is safe to

 * alter its values.

 */

void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {

  uint32_t initial_rate = CEIL(block->nominal_rate * entry_factor),

           final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second)

  // Limit minimal step rate (Otherwise the timer will overflow.)

  NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE));

  NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE));

  #if ENABLED(S_CURVE_ACCELERATION)

    uint32_t cruise_rate = initial_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);

    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

  }

  #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);

    // And to offload calculations from the ISR, we also calculate the inverse of those times here

    uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time);

    uint32_t deceleration_time_inverse = get_period_inverse(deceleration_time);

  #endif

  // Store new block parameters

  block->accelerate_until = accelerate_steps;

  block->decelerate_after = accelerate_steps + plateau_steps;

  block->initial_rate = initial_rate;

  #if ENABLED(S_CURVE_ACCELERATION)

    block->acceleration_time = acceleration_time;

    block->deceleration_time = deceleration_time;

    block->acceleration_time_inverse = acceleration_time_inverse;

    block->deceleration_time_inverse = deceleration_time_inverse;

    block->cruise_rate = cruise_rate;

  #endif

  block->final_rate = final_rate;

}

/*                            PLANNER SPEED DEFINITION

                                     +--------+   <- current->nominal_speed

                                    /          \

         current->entry_speed ->   +            \

                                   |             + <- next->entry_speed (aka exit speed)

                                   +-------------+

                                       time -->

  Recalculates the motion plan according to the following basic guidelines:

    1. Go over every feasible block sequentially in reverse order and calculate the junction speeds

        (i.e. current->entry_speed) such that:

      a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of

         neighboring blocks.

      b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed)

         with a maximum allowable deceleration over the block travel distance.

      c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero).

    2. Go over every block in chronological (forward) order and dial down junction speed values if

      a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable

         acceleration over the block travel distance.

  When these stages are complete, the planner will have maximized the velocity profiles throughout the all

  of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In

  other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements

  are possible. If a new block is added to the buffer, the plan is recomputed according to the said

  guidelines for a new optimal plan.

  To increase computational efficiency of these guidelines, a set of planner block pointers have been

  created to indicate stop-compute points for when the planner guidelines cannot logically make any further

  changes or improvements to the plan when in normal operation and new blocks are streamed and added to the

  planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are

  bracketed by junction velocities at their maximums (or by the first planner block as well), no new block

  added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute

  them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute

  point) are all accelerating, they are all optimal and can not be altered by a new block added to the

  planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum

  junction velocity is reached. However, if the operational conditions of the plan changes from infrequently

  used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is

  recomputed as stated in the general guidelines.

  Planner buffer index mapping:

  - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.

  - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether

      the buffer is full or empty. As described for standard ring buffers, this block is always empty.

  - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal

      streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the

      planner buffer that don't change with the addition of a new block, as describe above. In addition,

      this block can never be less than block_buffer_tail and will always be pushed forward and maintain

      this requirement when encountered by the Planner::discard_current_block() routine during a cycle.

  NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short

  line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't

  enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then

  decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and

  becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner

  will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line

  motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use,

  the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance

  for the planner to compute over. It also increases the number of computations the planner has to perform

  to compute an optimal plan, so select carefully.

*/

// The kernel called by recalculate() when scanning the plan from last to first entry.

void Planner::reverse_pass_kernel(block_t* const current, const block_t * const next) {

  if (current) {

    // If entry speed is already at the maximum entry speed, and there was no change of speed

    // in the next block, there is no need to recheck. Block is cruising and there is no need to

    // compute anything for this block,

    // If not, block entry speed needs to be recalculated to ensure maximum possible planned speed.

    const float max_entry_speed_sqr = current->max_entry_speed_sqr;

    // Compute maximum entry speed decelerating over the current block from its exit speed.

    // If not at the maximum entry speed, or the previous block entry speed changed

    if (current->entry_speed_sqr != max_entry_speed_sqr || (next && TEST(next->flag, BLOCK_BIT_RECALCULATE))) {

      // If nominal length true, max junction speed is guaranteed to be reached.

      // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then

      // the current block and next block junction speeds are guaranteed to always be at their maximum

      // junction speeds in deceleration and acceleration, respectively. This is due to how the current

      // 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.

      const float new_entry_speed_sqr = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)

        ? max_entry_speed_sqr

        : MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next ? next->entry_speed_sqr : sq(float(MINIMUM_PLANNER_SPEED)), current->millimeters));

      if (current->entry_speed_sqr != new_entry_speed_sqr) {

        // Need to recalculate the block speed - Mark it now, so the stepper

        // ISR does not consume the block before being recalculated

        SBI(current->flag, BLOCK_BIT_RECALCULATE);

        // But there is an inherent race condition here, as the block may have

        // become BUSY just before being marked RECALCULATE, so check for that!

        if (stepper.is_block_busy(current)) {

          // Block became busy. Clear the RECALCULATE flag (no point in

          // recalculating BUSY blocks). And don't set its speed, as it can't

          // be updated at this time.

          CBI(current->flag, BLOCK_BIT_RECALCULATE);

        }

        else {

          // Block is not BUSY so this is ahead of the Stepper ISR:

          // Just Set the new entry speed.

          current->entry_speed_sqr = new_entry_speed_sqr;

        }

      }

    }

  }

}

/**

 * recalculate() needs to go over the current plan twice.

 * Once in reverse and once forward. This implements the reverse pass.

 */

void Planner::reverse_pass() {

  // Initialize block index to the last block in the planner buffer.

  uint8_t block_index = prev_block_index(block_buffer_head);

  // Read the index of the last buffer planned block.

  // The ISR may change it so get a stable local copy.

  uint8_t planned_block_index = block_buffer_planned;

  // If there was a race condition and block_buffer_planned was incremented

  //  or was pointing at the head (queue empty) break loop now and avoid

  //  planning already consumed blocks

  if (planned_block_index == block_buffer_head) return;

  // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last

  // block in buffer. Cease planning when the last optimal planned or tail pointer is reached.

  // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.

  const block_t *next = NULL;

  while (block_index != planned_block_index) {

    // Perform the reverse pass

    block_t *current = &block_buffer[block_index];

    // Only consider non sync blocks

    if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {

      reverse_pass_kernel(current, next);

      next = current;

    }

    // Advance to the next

    block_index = prev_block_index(block_index);

    // The ISR could advance the block_buffer_planned while we were doing the reverse pass.

    // We must try to avoid using an already consumed block as the last one - So follow

    // changes to the pointer and make sure to limit the loop to the currently busy block

    while (planned_block_index != block_buffer_planned) {

      // If we reached the busy block or an already processed block, break the loop now

      if (block_index == planned_block_index) return;

      // Advance the pointer, following the busy block

      planned_block_index = next_block_index(planned_block_index);

    }

  }

}

// The kernel called by recalculate() when scanning the plan from first to last entry.

void Planner::forward_pass_kernel(const block_t* const previous, block_t* const current, const uint8_t block_index) {

  if (previous) {

    // If the previous block is an acceleration block, too short to complete the full speed

    // change, adjust the entry speed accordingly. Entry speeds have already been reset,

    // maximized, and reverse-planned. If nominal length is set, max junction speed is

    // guaranteed to be reached. No need to recheck.

    if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH) &&

      previous->entry_speed_sqr < current->entry_speed_sqr) {

      // Compute the maximum allowable speed

      const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);

      // If true, current block is full-acceleration and we can move the planned pointer forward.

      if (new_entry_speed_sqr < current->entry_speed_sqr) {

        // Mark we need to recompute the trapezoidal shape, and do it now,

        // so the stepper ISR does not consume the block before being recalculated

        SBI(current->flag, BLOCK_BIT_RECALCULATE);

        // But there is an inherent race condition here, as the block maybe

        // became BUSY, just before it was marked as RECALCULATE, so check

        // if that is the case!

        if (stepper.is_block_busy(current)) {

          // Block became busy. Clear the RECALCULATE flag (no point in

          //  recalculating BUSY blocks and don't set its speed, as it can't

          //  be updated at this time.

          CBI(current->flag, BLOCK_BIT_RECALCULATE);

        }

        else {

          // Block is not BUSY, we won the race against the Stepper ISR:

          // Always <= max_entry_speed_sqr. Backward pass sets this.

          current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.

          // Set optimal plan pointer.

          block_buffer_planned = block_index;

        }

      }

    }

    // Any block set at its maximum entry speed also creates an optimal plan up to this

    // point in the buffer. When the plan is bracketed by either the beginning of the

    // buffer and a maximum entry speed or two maximum entry speeds, every block in between

    // cannot logically be further improved. Hence, we don't have to recompute them anymore.

    if (current->entry_speed_sqr == current->max_entry_speed_sqr)

      block_buffer_planned = block_index;

  }

}

/**

 * recalculate() needs to go over the current plan twice.

 * Once in reverse and once forward. This implements the forward pass.

 */

void Planner::forward_pass() {

  // Forward Pass: Forward plan the acceleration curve from the planned pointer onward.

  // Also scans for optimal plan breakpoints and appropriately updates the planned pointer.

  // Begin at buffer planned pointer. Note that block_buffer_planned can be modified

  //  by the stepper ISR,  so read it ONCE. It it guaranteed that block_buffer_planned

  //  will never lead head, so the loop is safe to execute. Also note that the forward

  //  pass will never modify the values at the tail.

  uint8_t block_index = block_buffer_planned;

  block_t *current;

  const block_t * previous = NULL;

  while (block_index != block_buffer_head) {

    // Perform the forward pass

    current = &block_buffer[block_index];

    // Skip SYNC blocks

    if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {

      // If there's no previous block or the previous block is not

      // BUSY (thus, modifiable) run the forward_pass_kernel. Otherwise,

      // the previous block became BUSY, so assume the current block's

      // entry speed can't be altered (since that would also require