improve feedrate for kinematic printers with J.D.

MarlinFirmware · Jul 30, 2022 · c18945c · c18945c
1 parent 8938e4d
commit c18945c
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 4 deletions.
diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
@@ -1084,7 +1084,7 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
     if (!position_is_reachable(destination)) return true;
 
     // Get the linear distance in XYZ
-    float cartesian_mm = diff.magnitude();
+    float cartesian_mm = SQRT(NUM_AXIS_GANG(sq(diff.x), + sq(diff.y), + sq(diff.z), + sq(diff.i), + sq(diff.j), + sq(diff.k), + sq(diff.u), + sq(diff.v), + sq(diff.w)));
 
     // If the move is very short, check the E move distance
     TERN_(HAS_EXTRUDERS, if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = ABS(diff.e));

diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp
@@ -2195,7 +2195,7 @@ bool Planner::_populate_block(
       );
 
       #if SECONDARY_LINEAR_AXES >= 1 && NONE(FOAMCUTTER_XYUV, ARTICULATED_ROBOT_ARM)
-        if (NEAR_ZERO(distance_sqr)) {
+        if (UNEAR_ZERO(distance_sqr)) {
           // Move does not involve any primary linear axes (xyz) but might involve secondary linear axes
           distance_sqr = (0.0f
             SECONDARY_AXIS_GANG(
@@ -2211,7 +2211,7 @@ bool Planner::_populate_block(
       #endif
 
       #if HAS_ROTATIONAL_AXES && NONE(FOAMCUTTER_XYUV, ARTICULATED_ROBOT_ARM)
-        if (NEAR_ZERO(distance_sqr)) {
+        if (UNEAR_ZERO(distance_sqr)) {
           // Move involves only rotational axes. Calculate angular distance in accordance with LinuxCNC
           TERN_(INCH_MODE_SUPPORT, cartesian_move = false);
           distance_sqr = ROTATIONAL_AXIS_GANG(sq(steps_dist_mm.i), + sq(steps_dist_mm.j), + sq(steps_dist_mm.k), + sq(steps_dist_mm.u), + sq(steps_dist_mm.v), + sq(steps_dist_mm.w));
@@ -3154,7 +3154,9 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s
 
     PlannerHints ph = hints;
     if (!hints.millimeters)
-      ph.millimeters = (cart_dist_mm.x || cart_dist_mm.y) ? cart_dist_mm.magnitude() : TERN0(HAS_Z_AXIS, ABS(cart_dist_mm.z));
+      ph.millimeters = (cart_dist_mm.x || cart_dist_mm.y)
+        ? (SQRT(NUM_AXIS_GANG(sq(cart_dist_mm.x), + sq(cart_dist_mm.y), + sq(cart_dist_mm.z), + sq(cart_dist_mm.i), + sq(cart_dist_mm.j), + sq(cart_dist_mm.k), + sq(cart_dist_mm.u), + sq(cart_dist_mm.v), + sq(cart_dist_mm.w))))
+        : TERN0(HAS_Z_AXIS, ABS(cart_dist_mm.z));
 
     #if ENABLED(SCARA_FEEDRATE_SCALING)
       // For SCARA scale the feedrate from mm/s to degrees/s