Merge pull request #4997 from thinkyhead/rc_jerk_from_mk2

Adapt Jerk / Speed code from Prusa MK2

Marlin/planner.cpp

       Planner::axis_steps_per_mm[NUM_AXIS],
       Planner::steps_to_mm[NUM_AXIS];
 
-unsigned long Planner::max_acceleration_steps_per_s2[NUM_AXIS],
-              Planner::max_acceleration_mm_per_s2[NUM_AXIS]; // Use M201 to override by software
+uint32_t Planner::max_acceleration_steps_per_s2[NUM_AXIS],
+         Planner::max_acceleration_mm_per_s2[NUM_AXIS]; // Use M201 to override by software
 
 millis_t Planner::min_segment_time;
 float Planner::min_feedrate_mm_s,
   #endif
 }
 
+#define MINIMAL_STEP_RATE 120
+
 /**
  * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
  * by the provided factors.
  */
-void Planner::calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor) {
+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, 120);
-  NOLESS(final_rate, 120);
+  NOLESS(initial_rate, MINIMAL_STEP_RATE);
+  NOLESS(final_rate, MINIMAL_STEP_RATE);
 
   int32_t accel = block->acceleration_steps_per_s2,
           accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
     plateau_steps = 0;
   }
 
-  #if ENABLED(ADVANCE)
-    volatile int32_t initial_advance = block->advance * sq(entry_factor),
-                       final_advance = block->advance * sq(exit_factor);
-  #endif // ADVANCE
-
   // block->accelerate_until = accelerate_steps;
   // block->decelerate_after = accelerate_steps+plateau_steps;
+
   CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section
   if (!block->busy) { // Don't update variables if block is busy.
     block->accelerate_until = accelerate_steps;
     block->initial_rate = initial_rate;
     block->final_rate = final_rate;
     #if ENABLED(ADVANCE)
-      block->initial_advance = initial_advance;
-      block->final_advance = final_advance;
+      block->initial_advance = block->advance * sq(entry_factor);
+      block->final_advance = block->advance * sq(exit_factor);
     #endif
   }
   CRITICAL_SECTION_END;
 
 
 // The kernel called by recalculate() when scanning the plan from last to first entry.
-void Planner::reverse_pass_kernel(block_t* current, block_t* next) {
-  if (!current) return;
-
-  if (next) {
-    // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
-    // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
-    // check for maximum allowable speed reductions to ensure maximum possible planned speed.
-    float max_entry_speed = current->max_entry_speed;
-    if (current->entry_speed != max_entry_speed) {
-
-      // If nominal length true, max junction speed is guaranteed to be reached. Only compute
-      // for max allowable speed if block is decelerating and nominal length is false.
-      if (!current->nominal_length_flag && max_entry_speed > next->entry_speed) {
-        current->entry_speed = min(max_entry_speed,
-                                   max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
-      }
-      else {
-        current->entry_speed = max_entry_speed;
-      }
-      current->recalculate_flag = true;
-
-    }
-  } // Skip last block. Already initialized and set for recalculation.
+void Planner::reverse_pass_kernel(block_t* const current, const block_t *next) {
+  if (!current || !next) return;
+  // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
+  // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
+  // check for maximum allowable speed reductions to ensure maximum possible planned speed.
+  float max_entry_speed = current->max_entry_speed;
+  if (current->entry_speed != max_entry_speed) {
+    // If nominal length true, max junction speed is guaranteed to be reached. Only compute
+    // for max allowable speed if block is decelerating and nominal length is false.
+    current->entry_speed = ((current->flag & BLOCK_FLAG_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed)
+      ? max_entry_speed
+      : min(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
+    current->flag |= BLOCK_FLAG_RECALCULATE;
+  }
 }
 
 /**
     block_t* block[3] = { NULL, NULL, NULL };
 
     // Make a local copy of block_buffer_tail, because the interrupt can alter it
-    CRITICAL_SECTION_START;
-      uint8_t tail = block_buffer_tail;
-    CRITICAL_SECTION_END
+    // Is a critical section REALLY needed for a single byte change?
+    //CRITICAL_SECTION_START;
+    uint8_t tail = block_buffer_tail;
+    //CRITICAL_SECTION_END
 
     uint8_t b = BLOCK_MOD(block_buffer_head - 3);
     while (b != tail) {
+      if (block[0] && (block[0]->flag & BLOCK_FLAG_START_FROM_FULL_HALT)) break;
       b = prev_block_index(b);
       block[2] = block[1];
       block[1] = block[0];
 }
 
 // The kernel called by recalculate() when scanning the plan from first to last entry.
-void Planner::forward_pass_kernel(block_t* previous, block_t* current) {
+void Planner::forward_pass_kernel(const block_t* previous, block_t* const current) {
   if (!previous) return;
 
   // If the previous block is an acceleration block, but it is not long enough to complete the
   // full speed change within the block, we need to adjust the entry speed accordingly. Entry
   // speeds have already been reset, maximized, and reverse planned by reverse planner.
   // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
-  if (!previous->nominal_length_flag) {
+  if (!(previous->flag & BLOCK_FLAG_NOMINAL_LENGTH)) {
     if (previous->entry_speed < current->entry_speed) {
       float entry_speed = min(current->entry_speed,
                                max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters));
       // Check for junction speed change
       if (current->entry_speed != entry_speed) {
         current->entry_speed = entry_speed;
-        current->recalculate_flag = true;
+        current->flag |= BLOCK_FLAG_RECALCULATE;
       }
     }
   }
  */
 void Planner::recalculate_trapezoids() {
   int8_t block_index = block_buffer_tail;
-  block_t* current;
-  block_t* next = NULL;
+  block_t *current, *next = NULL;
 
   while (block_index != block_buffer_head) {
     current = next;
     next = &block_buffer[block_index];
     if (current) {
       // Recalculate if current block entry or exit junction speed has changed.
-      if (current->recalculate_flag || next->recalculate_flag) {
+      if ((current->flag & BLOCK_FLAG_RECALCULATE) || (next->flag & BLOCK_FLAG_RECALCULATE)) {
         // NOTE: Entry and exit factors always > 0 by all previous logic operations.
         float nom = current->nominal_speed;
         calculate_trapezoid_for_block(current, current->entry_speed / nom, next->entry_speed / nom);
-        current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
+        current->flag &= ~BLOCK_FLAG_RECALCULATE; // Reset current only to ensure next trapezoid is computed
       }
     }
     block_index = next_block_index(block_index);
   if (next) {
     float nom = next->nominal_speed;
     calculate_trapezoid_for_block(next, next->entry_speed / nom, (MINIMUM_PLANNER_SPEED) / nom);
-    next->recalculate_flag = false;
+    next->flag &= ~BLOCK_FLAG_RECALCULATE;
   }
 }
 
   // Bail if this is a zero-length block
   if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return;
 
+  // Clear the block flags
+  block->flag = 0;
+
   // For a mixing extruder, get a magnified step_event_count for each
   #if ENABLED(MIXING_EXTRUDER)
     for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
 
   // Compute and limit the acceleration rate for the trapezoid generator.
   float steps_per_mm = block->step_event_count / block->millimeters;
+  uint32_t accel;
   if (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS]) {
-    block->acceleration_steps_per_s2 = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
+    // convert to: acceleration steps/sec^2
+    accel = ceil(retract_acceleration * steps_per_mm);
   }
   else {
+    #define LIMIT_ACCEL(AXIS) do{ \
+      const uint32_t comp = max_acceleration_steps_per_s2[AXIS] * block->step_event_count; \
+      if (accel * block->steps[AXIS] > comp) accel = comp / block->steps[AXIS]; \
+    }while(0)
+
+    // Start with print or travel acceleration
+    accel = ceil((block->steps[E_AXIS] ? acceleration : travel_acceleration) * steps_per_mm);
+
     // Limit acceleration per axis
-    block->acceleration_steps_per_s2 = ceil((block->steps[E_AXIS] ? acceleration : travel_acceleration) * steps_per_mm);
-    if (max_acceleration_steps_per_s2[X_AXIS] < (block->acceleration_steps_per_s2 * block->steps[X_AXIS]) / block->step_event_count)
-      block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[X_AXIS] * block->step_event_count) / block->steps[X_AXIS];
-    if (max_acceleration_steps_per_s2[Y_AXIS] < (block->acceleration_steps_per_s2 * block->steps[Y_AXIS]) / block->step_event_count)
-      block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[Y_AXIS] * block->step_event_count) / block->steps[Y_AXIS];
-    if (max_acceleration_steps_per_s2[Z_AXIS] < (block->acceleration_steps_per_s2 * block->steps[Z_AXIS]) / block->step_event_count)
-      block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[Z_AXIS] * block->step_event_count) / block->steps[Z_AXIS];
-    if (max_acceleration_steps_per_s2[E_AXIS] < (block->acceleration_steps_per_s2 * block->steps[E_AXIS]) / block->step_event_count)
-      block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[E_AXIS] * block->step_event_count) / block->steps[E_AXIS];
+    LIMIT_ACCEL(X_AXIS);
+    LIMIT_ACCEL(Y_AXIS);
+    LIMIT_ACCEL(Z_AXIS);
+    LIMIT_ACCEL(E_AXIS);
   }
-  block->acceleration = block->acceleration_steps_per_s2 / steps_per_mm;
-  block->acceleration_rate = (long)(block->acceleration_steps_per_s2 * 16777216.0 / ((F_CPU) * 0.125));
+  block->acceleration_steps_per_s2 = accel;
+  block->acceleration = accel / steps_per_mm;
+  block->acceleration_rate = (long)(accel * 16777216.0 / ((F_CPU) * 0.125)); // * 8.388608
+
+  // Initial limit on the segment entry velocity
+  float vmax_junction;
 
   #if 0  // Use old jerk for now
 
     float junction_deviation = 0.1;
 
     // Compute path unit vector
-    double unit_vec[XYZ];
-
-    unit_vec[X_AXIS] = delta_mm[X_AXIS] * inverse_millimeters;
-    unit_vec[Y_AXIS] = delta_mm[Y_AXIS] * inverse_millimeters;
-    unit_vec[Z_AXIS] = delta_mm[Z_AXIS] * inverse_millimeters;
-
-    // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
-    // Let a circle be tangent to both previous and current path line segments, where the junction
-    // deviation is defined as the distance from the junction to the closest edge of the circle,
-    // collinear with the circle center. The circular segment joining the two paths represents the
-    // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
-    // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
-    // path width or max_jerk in the previous grbl version. This approach does not actually deviate
-    // from path, but used as a robust way to compute cornering speeds, as it takes into account the
-    // nonlinearities of both the junction angle and junction velocity.
-    double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
+    double unit_vec[XYZ] = {
+      delta_mm[X_AXIS] * inverse_millimeters,
+      delta_mm[Y_AXIS] * inverse_millimeters,
+      delta_mm[Z_AXIS] * inverse_millimeters
+    };
+
+    /*
+       Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
+
+       Let a circle be tangent to both previous and current path line segments, where the junction
+       deviation is defined as the distance from the junction to the closest edge of the circle,
+       collinear with the circle center.
+
+       The circular segment joining the two paths represents the path of centripetal acceleration.
+       Solve for max velocity based on max acceleration about the radius of the circle, defined
+       indirectly by junction deviation.
+
+       This may be also viewed as path width or max_jerk in the previous grbl version. This approach
+       does not actually deviate from path, but used as a robust way to compute cornering speeds, as
+       it takes into account the nonlinearities of both the junction angle and junction velocity.
+     */
+
+    vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
 
     // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
-    if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
+    if (block_buffer_head != block_buffer_tail && previous_nominal_speed > 0.0) {
       // 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.
-      double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
-                         - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
-                         - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
+      float cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
+                        - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
+                        - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
       // Skip and use default max junction speed for 0 degree acute junction.
       if (cos_theta < 0.95) {
         vmax_junction = min(previous_nominal_speed, block->nominal_speed);
         // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
         if (cos_theta > -0.95) {
           // Compute maximum junction velocity based on maximum acceleration and junction deviation
-          double sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
+          float sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
           NOMORE(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
         }
       }
     }
   #endif
 
-  // Start with a safe speed
-  float vmax_junction = max_jerk[X_AXIS] * 0.5, vmax_junction_factor = 1.0;
-  if (max_jerk[Y_AXIS] * 0.5 < fabs(current_speed[Y_AXIS])) NOMORE(vmax_junction, max_jerk[Y_AXIS] * 0.5);
-  if (max_jerk[Z_AXIS] * 0.5 < fabs(current_speed[Z_AXIS])) NOMORE(vmax_junction, max_jerk[Z_AXIS] * 0.5);
-  if (max_jerk[E_AXIS] * 0.5 < fabs(current_speed[E_AXIS])) NOMORE(vmax_junction, max_jerk[E_AXIS] * 0.5);
-  NOMORE(vmax_junction, block->nominal_speed);
-  float safe_speed = vmax_junction;
+  /**
+   * Adapted from Prusa MKS firmware
+   *
+   * Start with a safe speed (from which the machine may halt to stop immediately).
+   */
+
+  // Exit speed limited by a jerk to full halt of a previous last segment
+  static float previous_safe_speed;
+
+  float safe_speed = block->nominal_speed;
+  bool limited = false;
+  LOOP_XYZE(i) {
+    float jerk = fabs(current_speed[i]);
+    if (jerk > max_jerk[i]) {
+      // The actual jerk is lower if it has been limited by the XY jerk.
+      if (limited) {
+        // Spare one division by a following gymnastics:
+        // Instead of jerk *= safe_speed / block->nominal_speed,
+        // multiply max_jerk[i] by the divisor.
+        jerk *= safe_speed;
+        float mjerk = max_jerk[i] * block->nominal_speed;
+        if (jerk > mjerk) safe_speed *= mjerk / jerk;
+      }
+      else {
+        safe_speed = max_jerk[i];
+        limited = true;
+      }
+    }
+  }
 
   if (moves_queued > 1 && previous_nominal_speed > 0.0001) {
-    //if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
-        vmax_junction = block->nominal_speed;
-    //}
-
-    float dsx = fabs(current_speed[X_AXIS] - previous_speed[X_AXIS]),
-          dsy = fabs(current_speed[Y_AXIS] - previous_speed[Y_AXIS]),
-          dsz = fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]),
-          dse = fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]);
-    if (dsx > max_jerk[X_AXIS]) NOMORE(vmax_junction_factor, max_jerk[X_AXIS] / dsx);
-    if (dsy > max_jerk[Y_AXIS]) NOMORE(vmax_junction_factor, max_jerk[Y_AXIS] / dsy);
-    if (dsz > max_jerk[Z_AXIS]) NOMORE(vmax_junction_factor, max_jerk[Z_AXIS] / dsz);
-    if (dse > max_jerk[E_AXIS]) NOMORE(vmax_junction_factor, max_jerk[E_AXIS] / dse);
-
-    vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous 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.
+
+    // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
+    bool prev_speed_larger = previous_nominal_speed > block->nominal_speed;
+    float smaller_speed_factor = prev_speed_larger ? (block->nominal_speed / previous_nominal_speed) : (previous_nominal_speed / block->nominal_speed);
+    // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
+    vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed;
+    // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
+    float v_factor = 1.f;
+    limited = false;
+    // Now limit the jerk in all axes.
+    LOOP_XYZE(axis) {
+      // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
+      float v_exit = previous_speed[axis], v_entry = current_speed[axis];
+      if (prev_speed_larger) v_exit *= smaller_speed_factor;
+      if (limited) {
+        v_exit *= v_factor;
+        v_entry *= v_factor;
+      }
+      // Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
+      float jerk = 
+        (v_exit > v_entry) ?
+          ((v_entry > 0.f || v_exit < 0.f) ?
+            // coasting
+            (v_exit - v_entry) : 
+            // axis reversal
+            max(v_exit, -v_entry)) :
+          // v_exit <= v_entry
+          ((v_entry < 0.f || v_exit > 0.f) ?
+            // coasting
+            (v_entry - v_exit) :
+            // axis reversal
+            max(-v_exit, v_entry));
+      if (jerk > max_jerk[axis]) {
+        v_factor *= max_jerk[axis] / jerk;
+        limited = true;
+      }
+    }
+    if (limited) vmax_junction *= v_factor;
+    // Now the transition velocity is known, which maximizes the shared exit / entry velocity while
+    // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
+    float vmax_junction_threshold = vmax_junction * 0.99f;
+    if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) {
+      // Not coasting. The machine will stop and start the movements anyway,
+      // better to start the segment from start.
+      block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
+      vmax_junction = safe_speed;
+    }
+  }
+  else {
+    block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
+    vmax_junction = safe_speed;
   }
+
+  // Max entry speed of this block equals the max exit speed of the previous block.
   block->max_entry_speed = vmax_junction;
 
   // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
   // 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->nominal_length_flag = (block->nominal_speed <= v_allowable);
-  block->recalculate_flag = true; // Always calculate trapezoid for new block
+  block->flag |= BLOCK_FLAG_RECALCULATE | (block->nominal_speed <= v_allowable ? BLOCK_FLAG_NOMINAL_LENGTH : 0);
 
   // Update previous path unit_vector and nominal speed
   memcpy(previous_speed, current_speed, sizeof(previous_speed));
   previous_nominal_speed = block->nominal_speed;
+  previous_safe_speed = safe_speed;
 
   #if ENABLED(LIN_ADVANCE)
 

Marlin/planner.h

   #include "vector_3.h"
 #endif
 
+enum BlockFlag {
+    // Recalculate trapezoids on entry junction. For optimization.
+    BLOCK_FLAG_RECALCULATE          = _BV(0),
+
+    // 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_FLAG_NOMINAL_LENGTH       = _BV(1),
+
+    // Start from a halt at the start of this block, respecting the maximum allowed jerk.
+    BLOCK_FLAG_START_FROM_FULL_HALT = _BV(2)
+};
+
 /**
  * struct block_t
  *
   #endif
 
   // Fields used by the motion planner to manage acceleration
-  float nominal_speed,                               // The nominal speed for this block in mm/sec
-        entry_speed,                                 // Entry speed at previous-current junction in mm/sec
-        max_entry_speed,                             // Maximum allowable junction entry speed in mm/sec
-        millimeters,                                 // The total travel of this block in mm
-        acceleration;                                // acceleration mm/sec^2
-  unsigned char recalculate_flag,                    // Planner flag to recalculate trapezoids on entry junction
-                nominal_length_flag;                 // Planner flag for nominal speed always reached
+  float nominal_speed,                          // The nominal speed for this block in mm/sec
+        entry_speed,                            // Entry speed at previous-current junction in mm/sec
+        max_entry_speed,                        // Maximum allowable junction entry speed in mm/sec
+        millimeters,                            // The total travel of this block in mm
+        acceleration;                           // acceleration mm/sec^2
+  uint8_t flag;                                 // Block flags (See BlockFlag enum above)
 
   // Settings for the trapezoid generator
-  unsigned long nominal_rate,                        // The nominal step rate for this block in step_events/sec
-                initial_rate,                        // The jerk-adjusted step rate at start of block
-                final_rate,                          // The minimal rate at exit
-                acceleration_steps_per_s2;           // acceleration steps/sec^2
+  uint32_t nominal_rate,                        // The nominal step rate for this block in step_events/sec
+           initial_rate,                        // The jerk-adjusted step rate at start of block
+           final_rate,                          // The minimal rate at exit
+           acceleration_steps_per_s2;           // acceleration steps/sec^2
 
   #if FAN_COUNT > 0
     unsigned long fan_speed[FAN_COUNT];
       return sqrt(sq(target_velocity) - 2 * accel * distance);
     }
 
-    static void calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor);
+    static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);
 
-    static void reverse_pass_kernel(block_t* current, block_t* next);
-    static void forward_pass_kernel(block_t* previous, block_t* current);
+    static void reverse_pass_kernel(block_t* const current, const block_t *next);
+    static void forward_pass_kernel(const block_t *previous, block_t* const current);
 
     static void reverse_pass();
     static void forward_pass();