Reworked the calculation of jerks in the planner.

Now the confugration values are half the values used before,
and the planner ensures, that the jerks will not be violated.

Firmware/Configuration.h

@@ -405,9 +405,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 // #define EXTRUDER_OFFSET_Y {0.0, 5.00}  // (in mm) for each extruder, offset of the hotend on the Y axis
 
 // The speed change that does not require acceleration (i.e. the software might assume it can be done instantaneously)
-#define DEFAULT_XYJERK                20.0    // (mm/sec)
-#define DEFAULT_ZJERK                 0.4     // (mm/sec)
-#define DEFAULT_EJERK                 5.0    // (mm/sec)
+#define DEFAULT_XJERK                10.0    // (mm/sec)
+#define DEFAULT_YJERK                10.0    // (mm/sec)
+#define DEFAULT_ZJERK                 0.2    // (mm/sec)
+#define DEFAULT_EJERK                 2.5    // (mm/sec)
 
 //===========================================================================
 //=============================Additional Features===========================

Firmware/ConfigurationStore.cpp

@@ -169,9 +169,10 @@ void Config_PrintSettings()
     SERIAL_ECHOPAIR("  M205 S",minimumfeedrate ); 
     SERIAL_ECHOPAIR(" T" ,mintravelfeedrate ); 
     SERIAL_ECHOPAIR(" B" ,minsegmenttime ); 
-    SERIAL_ECHOPAIR(" X" ,max_xy_jerk ); 
-    SERIAL_ECHOPAIR(" Z" ,max_z_jerk);
-    SERIAL_ECHOPAIR(" E" ,max_e_jerk);
+    SERIAL_ECHOPAIR(" X" ,max_jerk[X_AXIS] ); 
+    SERIAL_ECHOPAIR(" Y" ,max_jerk[Y_AXIS] ); 
+    SERIAL_ECHOPAIR(" Z" ,max_jerk[Z_AXIS] ); 
+    SERIAL_ECHOPAIR(" E" ,max_jerk[E_AXIS] ); 
     SERIAL_ECHOLN(""); 
 
     SERIAL_ECHO_START;
@@ -356,9 +357,10 @@ void Config_ResetDefault()
     minimumfeedrate=DEFAULT_MINIMUMFEEDRATE;
     minsegmenttime=DEFAULT_MINSEGMENTTIME;       
     mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE;
-    max_xy_jerk=DEFAULT_XYJERK;
-    max_z_jerk=DEFAULT_ZJERK;
-    max_e_jerk=DEFAULT_EJERK;
+    max_jerk[X_AXIS] = DEFAULT_XJERK;
+    max_jerk[Y_AXIS] = DEFAULT_YJERK;
+    max_jerk[Z_AXIS] = DEFAULT_ZJERK;
+    max_jerk[E_AXIS] = DEFAULT_EJERK;
     add_homing[X_AXIS] = add_homing[Y_AXIS] = add_homing[Z_AXIS] = 0;
 #ifdef ULTIPANEL
     plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;

Firmware/Marlin_main.cpp

@@ -3515,7 +3515,7 @@ Sigma_Exit:
             float value = code_value();
             if(value < 20.0) {
               float factor = axis_steps_per_unit[i] / value; // increase e constants if M92 E14 is given for netfab.
-              max_e_jerk *= factor;
+              max_jerk[E_AXIS] *= factor;
               max_feedrate[i] *= factor;
               axis_steps_per_sqr_second[i] *= factor;
             }
@@ -3718,9 +3718,10 @@ Sigma_Exit:
       if(code_seen('S')) minimumfeedrate = code_value();
       if(code_seen('T')) mintravelfeedrate = code_value();
       if(code_seen('B')) minsegmenttime = code_value() ;
-      if(code_seen('X')) max_xy_jerk = code_value() ;
-      if(code_seen('Z')) max_z_jerk = code_value() ;
-      if(code_seen('E')) max_e_jerk = code_value() ;
+      if(code_seen('X')) max_jerk[X_AXIS] = max_jerk[Y_AXIS] = code_value();
+      if(code_seen('Y')) max_jerk[Y_AXIS] = code_value();
+      if(code_seen('Z')) max_jerk[Z_AXIS] = code_value();
+      if(code_seen('E')) max_jerk[E_AXIS] = code_value();
     }
     break;
     case 206: // M206 additional homing offset

Firmware/planner.cpp

@@ -74,9 +74,8 @@ unsigned long max_acceleration_units_per_sq_second[NUM_AXIS]; // Use M201 to ove
 float minimumfeedrate;
 float acceleration;         // Normal acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX
 float retract_acceleration; //  mm/s^2   filament pull-pack and push-forward  while standing still in the other axis M204 TXXXX
-float max_xy_jerk; //speed than can be stopped at once, if i understand correctly.
-float max_z_jerk;
-float max_e_jerk;
+// Jerk is a maximum immediate velocity change.
+float max_jerk[NUM_AXIS];
 float mintravelfeedrate;
 unsigned long axis_steps_per_sqr_second[NUM_AXIS];
 
@@ -93,6 +92,7 @@ matrix_3x3 plan_bed_level_matrix = {
 long position[NUM_AXIS];   //rescaled from extern when axis_steps_per_unit are changed by gcode
 static float previous_speed[NUM_AXIS]; // Speed of previous path line segment
 static float previous_nominal_speed; // Nominal speed of previous path line segment
+static float previous_safe_speed; // Exit speed limited by a jerk to full halt of a previous last segment.
 
 #ifdef AUTOTEMP
 float autotemp_max=250;
@@ -279,7 +279,7 @@ void planner_recalculate(const float &safe_final_speed)
                 tail = block_index;
                 // Update the number of blocks to process.
                 n_blocks = (block_buffer_head + BLOCK_BUFFER_SIZE - tail) & (BLOCK_BUFFER_SIZE - 1);
-                SERIAL_ECHOLNPGM("BLOCK_FLAG_START_FROM_FULL_HALT");
+                // SERIAL_ECHOLNPGM("START");
                 break;
             }
             // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
@@ -878,44 +878,40 @@ Having the real displacement of the head, we can calculate the total movement le
   }
 #endif
   // Start with a safe speed.
-  //Vojtech: This code tries to limit the initial jerk to half of the maximum jerk value.
-  //The code is not quite correct. It is pessimistic as it shall limit a projection of the jerk into each axis,
-  //but when the current code clamps, it clamps as if the movement is done in a single axis only.
-  float vmax_junction = max_xy_jerk/2.f;
-  if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2.f)
-    vmax_junction = min(vmax_junction, max_z_jerk/2.f);
-  if(fabs(current_speed[E_AXIS]) > max_e_jerk/2.f)
-    vmax_junction = min(vmax_junction, max_e_jerk/2.f);
-  vmax_junction = min(vmax_junction, block->nominal_speed);
   // Safe speed is the speed, from which the machine may halt to stop immediately.
-  float safe_speed = vmax_junction;
+  float safe_speed = block->nominal_speed;
+  bool  limited = false;
+  for (uint8_t axis = 0; axis < 4; ++ axis) {
+      float jerk = fabs(current_speed[axis]);
+      if (jerk > max_jerk[axis]) {
+          // 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[axis] by the divisor.
+              jerk *= safe_speed;
+              float mjerk = max_jerk[axis] * block->nominal_speed;
+              if (jerk > mjerk) {
+                  safe_speed *= mjerk / jerk;
+                  limited = true;
+              }
+          } else {
+              safe_speed = max_jerk[axis];
+              limited = true;
+          }
+      }
+  }
+
+  // Reset the block flag.
+  block->flag = 0;
+
+  // Initial limit on the segment entry velocity.
+  float vmax_junction;
 
   //FIXME Vojtech: Why only if at least two lines are planned in the queue?
   // Is it because we don't want to tinker with the first buffer line, which
   // is likely to be executed by the stepper interrupt routine soon?
   if (moves_queued > 1 && previous_nominal_speed > 0.0001f) {
-#if 1
-      float jerk;
-      {
-          float dx = current_speed[X_AXIS]-previous_speed[X_AXIS];
-          float dy = current_speed[Y_AXIS]-previous_speed[Y_AXIS];
-          jerk = sqrt(dx*dx+dy*dy);
-      }
-      float vmax_junction_factor = 1.0; 
-      //    if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
-      vmax_junction = block->nominal_speed;
-      //    }
-      if (jerk > max_xy_jerk)
-          vmax_junction_factor = max_xy_jerk/jerk;
-      jerk = fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]);
-      if (jerk > max_z_jerk)
-          vmax_junction_factor = min(vmax_junction_factor, max_z_jerk/jerk);
-      jerk = fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]);
-      if (jerk > max_e_jerk)
-          vmax_junction_factor = min(vmax_junction_factor, max_e_jerk/jerk);
-      //FIXME Vojtech: Why is this asymmetric in regard to the previous nominal speed and the current nominal speed?
-      vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
-#else
       // 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.
@@ -926,72 +922,54 @@ Having the real displacement of the head, we can calculate the total movement le
       // 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_exit  = prev_speed_larger ? smaller_speed_factor : 1.f;
-      float v_factor_entry = prev_speed_larger ? 1.f : smaller_speed_factor;
-      // First limit the jerk in the XY plane.
-      float jerk;
-      {
-          // Estimate the jerk as if the entry / exit velocity of the two successive segment was limited to the minimum of their nominal velocities.
-          // If coasting, then the segment transition velocity will define the exit / entry velocities of the successive segments
-          // and the jerk defined by the following formula will be always lower.
-          float dx = prev_speed_larger ? (current_speed[X_AXIS] - smaller_speed_factor * previous_speed[X_AXIS]) : (smaller_speed_factor * current_speed[X_AXIS] - previous_speed[X_AXIS]);
-          float dy = prev_speed_larger ? (current_speed[Y_AXIS] - smaller_speed_factor * previous_speed[Y_AXIS]) : (smaller_speed_factor * current_speed[Y_AXIS] - previous_speed[Y_AXIS]);
-          jerk = sqrt(dx*dx+dy*dy);
-      }
-      if (jerk > max_xy_jerk) {
-          // Limit the entry / exit velocities to respect the XY jerk limits.
-          v_factor_exit = v_factor_entry = max_xy_jerk / jerk;
+      float v_factor = 1.f;
+      limited = false;
+      // Now limit the jerk in all axes.
+      for (uint8_t axis = 0; axis < 4; ++ axis) {
+          // Limit an axis. We have to differentiate coasting from the reversal of an axis movement, or a full stop.
+          float v_exit  = previous_speed[axis];
+          float v_entry = current_speed [axis];
           if (prev_speed_larger)
-              v_factor_exit *= smaller_speed_factor;
-          else
-              v_factor_entry *= smaller_speed_factor;
-      }
-      // Now limit the Z and E axes. We have to differentiate coasting from the reversal of an axis movement, or a full stop.
-      float v_exit  = previous_speed[Z_AXIS] * v_factor_exit;
-      float v_entry = current_speed [Z_AXIS] * v_factor_entry;
-      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_z_jerk / 2.f) {
-          float c = (max_z_jerk / 2.f) / jerk;
-          v_factor_exit *= c;
-          v_factor_entry *= c;
+              v_exit *= smaller_speed_factor;
+          if (limited) {
+              v_exit  *= v_factor;
+              v_entry *= v_factor;
+          }
+          // Calculate the jerk depending on whether the axis is coasting in the same direction or reversing a direction.
+          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;
+          }
       }
-      // Limit the E axis.
-      v_exit  = previous_speed[E_AXIS] * v_factor_exit;
-      v_entry = current_speed [E_AXIS] * v_factor_entry;
-      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_e_jerk / 2.f) {
-          float c = (max_e_jerk / 2.f) / jerk;
-          v_factor_exit *= c;
-          v_factor_entry *= c;
+      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;
       }
-
-      // Now the transition velocity is known as nominal * v_factor. Compare the transition velocity against the "safe" velocoties.
-      // If the transition velocity is below the exit / enter safe velocity, the machine is no more cruising, therefore
-      // the safe velocities shall be used.
-#endif
+  } 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;
 
@@ -1008,12 +986,12 @@ Having the real displacement of the head, we can calculate the total movement le
   // 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.
   // Always calculate trapezoid for new block
-  block->flag = (block->nominal_speed <= v_allowable) ? (BLOCK_FLAG_NOMINAL_LENGTH | BLOCK_FLAG_RECALCULATE) : BLOCK_FLAG_RECALCULATE;
+  block->flag |= (block->nominal_speed <= v_allowable) ? (BLOCK_FLAG_NOMINAL_LENGTH | BLOCK_FLAG_RECALCULATE) : BLOCK_FLAG_RECALCULATE;
 
   // Update previous path unit_vector and nominal speed
   memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
   previous_nominal_speed = block->nominal_speed;
-
+  previous_safe_speed = safe_speed;
 
 #ifdef ADVANCE
   // Calculate advance rate
@@ -1056,6 +1034,10 @@ Having the real displacement of the head, we can calculate the total movement le
   // interfere with the process.
   planner_recalculate(safe_speed);
 
+//  SERIAL_ECHOPGM("Q");
+//  SERIAL_ECHO(int(moves_planned()));
+//  SERIAL_ECHOLNPGM("");
+
   st_wake_up();
 }
 

Firmware/planner.h

@@ -37,12 +37,9 @@ enum BlockFlag {
     // Planner flag for nominal speed always reached. That means, the segment is long enough, that the nominal speed
     // may be reached if accelerating from a safe speed (in the regard of jerking from zero speed).
     BLOCK_FLAG_NOMINAL_LENGTH = 2,
-    // If set, the machine will stop to a full halt at the end of this block,
-    // respecting the maximum allowed jerk.
-    BLOCK_FLAG_FULL_HALT_AT_END = 4,
     // If set, the machine will start from a halt at the start of this block,
     // respecting the maximum allowed jerk.
-    BLOCK_FLAG_START_FROM_FULL_HALT = 8,
+    BLOCK_FLAG_START_FROM_FULL_HALT = 4,
 };
 
 // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in 
@@ -135,9 +132,8 @@ extern unsigned long max_acceleration_units_per_sq_second[NUM_AXIS]; // Use M201
 extern float minimumfeedrate;
 extern float acceleration;         // Normal acceleration mm/s^2  THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX
 extern float retract_acceleration; //  mm/s^2   filament pull-pack and push-forward  while standing still in the other axis M204 TXXXX
-extern float max_xy_jerk; //speed than can be stopped at once, if i understand correctly.
-extern float max_z_jerk;
-extern float max_e_jerk;
+// Jerk is a maximum immediate velocity change.
+extern float max_jerk[NUM_AXIS];
 extern float mintravelfeedrate;
 extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];