Merge pull request #8613 from thinkyhead/bf1_planner_parity

[1.1.x] Fix some planner bugs

Marlin/Marlin_main.cpp

     // setting any extruder filament size disables volumetric on the assumption that
     // slicers either generate in extruder values as cubic mm or as as filament feeds
     // for all extruders
-    if ( (parser.volumetric_enabled = (parser.value_linear_units() != 0.0)) ) {
-      planner.filament_size[target_extruder] = parser.value_linear_units();
-      // make sure all extruders have some sane value for the filament size
-      for (uint8_t i = 0; i < COUNT(planner.filament_size); i++)
-        if (!planner.filament_size[i]) planner.filament_size[i] = DEFAULT_NOMINAL_FILAMENT_DIA;
-    }
+    if ( (parser.volumetric_enabled = (parser.value_linear_units() != 0.0)) )
+      planner.set_filament_size(target_extruder, parser.value_linear_units());
   }
   planner.calculate_volumetric_multipliers();
 }

Marlin/planner.cpp

 
 #if ENABLED(LIN_ADVANCE)
   float Planner::extruder_advance_k, // Initialized by settings.load()
-        Planner::advance_ed_ratio,   // Initialized by settings.load()
-        Planner::position_float[NUM_AXIS] = { 0 };
+        Planner::advance_ed_ratio;   // Initialized by settings.load()
 #endif
 
 #if ENABLED(ULTRA_LCD)
 void Planner::init() {
   block_buffer_head = block_buffer_tail = 0;
   ZERO(position);
-  #if ENABLED(LIN_ADVANCE)
-    ZERO(position_float);
-  #endif
   ZERO(previous_speed);
   previous_nominal_speed = 0.0;
   #if ABL_PLANAR
 
 #if PLANNER_LEVELING
   /**
-   * rx, ry, rz - cartesian position in mm
+   * rx, ry, rz - Cartesian positions in mm
    */
   void Planner::apply_leveling(float &rx, float &ry, float &rz) {
 
     if (!leveling_active) return;
 
-    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-      const float fade_scaling_factor = fade_scaling_factor_for_z(rz);
-      if (!fade_scaling_factor) return;
-    #else
-      constexpr float fade_scaling_factor = 1.0;
-    #endif
-
-    #if ENABLED(AUTO_BED_LEVELING_UBL)
-
-      rz += ubl.get_z_correction(rx, ry) * fade_scaling_factor;
-
-    #elif ENABLED(MESH_BED_LEVELING)
-
-      rz += mbl.get_z(rx, ry
-        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-          , fade_scaling_factor
-        #endif
-      );
-
-    #elif ABL_PLANAR
-
-      UNUSED(fade_scaling_factor);
+    #if ABL_PLANAR
 
       float dx = rx - (X_TILT_FULCRUM),
             dy = ry - (Y_TILT_FULCRUM);
       rx = dx + X_TILT_FULCRUM;
       ry = dy + Y_TILT_FULCRUM;
 
-    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
-
-      float tmp[XYZ] = { rx, ry, 0 };
-      rz += bilinear_z_offset(tmp) * fade_scaling_factor;
-
-    #endif
-  }
-
-  void Planner::unapply_leveling(float raw[XYZ]) {
-
-    #if HAS_LEVELING
-      if (!leveling_active) return;
-    #endif
-
-    #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-      if (!leveling_active_at_z(raw[Z_AXIS])) return;
-    #endif
-
-    #if ENABLED(AUTO_BED_LEVELING_UBL)
-
-      const float z_physical = raw[Z_AXIS],
-                  z_correct = ubl.get_z_correction(raw[X_AXIS], raw[Y_AXIS]),
-                  z_virtual = z_physical - z_correct;
-            float z_raw = z_virtual;
+    #else
 
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+        const float fade_scaling_factor = fade_scaling_factor_for_z(rz);
+        if (!fade_scaling_factor) return;
+      #elif HAS_MESH
+        constexpr float fade_scaling_factor = 1.0;
+      #endif
 
-        // for P=physical_z, L=logical_z, M=mesh_z, H=fade_height,
-        // Given P=L+M(1-L/H) (faded mesh correction formula for L<H)
-        //  then L=P-M(1-L/H)
-        //    so L=P-M+ML/H
-        //    so L-ML/H=P-M
-        //    so L(1-M/H)=P-M
-        //    so L=(P-M)/(1-M/H) for L<H
-
-        if (planner.z_fade_height) {
-          if (z_raw >= planner.z_fade_height)
-            z_raw = z_physical;
-          else
-            z_raw /= 1.0 - z_correct * planner.inverse_z_fade_height;
-        }
-
-      #endif // ENABLE_LEVELING_FADE_HEIGHT
-
-      raw[Z_AXIS] = z_raw;
+      #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
+        const float raw[XYZ] = { rx, ry, 0 };
+      #endif
 
-      return; // don't fall thru to other ENABLE_LEVELING_FADE_HEIGHT logic
+      rz += (
+        #if ENABLED(AUTO_BED_LEVELING_UBL)
+          ubl.get_z_correction(rx, ry) * fade_scaling_factor
+        #elif ENABLED(MESH_BED_LEVELING)
+          mbl.get_z(rx, ry
+            #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+              , fade_scaling_factor
+            #endif
+          )
+        #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+          bilinear_z_offset(raw) * fade_scaling_factor
+        #else
+          0
+        #endif
+      );
 
     #endif
+  }
 
-    #if ENABLED(MESH_BED_LEVELING)
+  void Planner::unapply_leveling(float raw[XYZ]) {
 
-      if (leveling_active) {
-        #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-          const float c = mbl.get_z(raw[X_AXIS], raw[Y_AXIS], 1.0);
-          raw[Z_AXIS] = (z_fade_height * (raw[Z_AXIS]) - c) / (z_fade_height - c);
-        #else
-          raw[Z_AXIS] -= mbl.get_z(raw[X_AXIS], raw[Y_AXIS]);
-        #endif
-      }
+    if (!leveling_active) return;
 
-    #elif ABL_PLANAR
+    #if ABL_PLANAR
 
       matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
 
       raw[X_AXIS] = dx + X_TILT_FULCRUM;
       raw[Y_AXIS] = dy + Y_TILT_FULCRUM;
 
-    #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+    #else
 
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
-        const float c = bilinear_z_offset(raw);
-        raw[Z_AXIS] = (z_fade_height * (raw[Z_AXIS]) - c) / (z_fade_height - c);
-      #else
-        raw[Z_AXIS] -= bilinear_z_offset(raw);
+        const float fade_scaling_factor = fade_scaling_factor_for_z(raw[Z_AXIS]);
+        if (!fade_scaling_factor) return;
+      #elif HAS_MESH
+        constexpr float fade_scaling_factor = 1.0;
       #endif
 
+      raw[Z_AXIS] -= (
+        #if ENABLED(AUTO_BED_LEVELING_UBL)
+          ubl.get_z_correction(raw[X_AXIS], raw[Y_AXIS]) * fade_scaling_factor
+        #elif ENABLED(MESH_BED_LEVELING)
+          mbl.get_z(raw[X_AXIS], raw[Y_AXIS]
+            #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
+              , fade_scaling_factor
+            #endif
+          )
+        #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
+          bilinear_z_offset(raw) * fade_scaling_factor
+        #else
+          0
+        #endif
+      );
+
     #endif
   }
 
     }
   #endif
 
-  #if ENABLED(LIN_ADVANCE)
-    const float mm_D_float = SQRT(sq(a - position_float[X_AXIS]) + sq(b - position_float[Y_AXIS]));
-  #endif
-
   const long da = target[X_AXIS] - position[X_AXIS],
              db = target[Y_AXIS] - position[Y_AXIS],
              dc = target[Z_AXIS] - position[Z_AXIS];
   //*/
 
   // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
-  if (DEBUGGING(DRYRUN)) {
+  if (DEBUGGING(DRYRUN))
     position[E_AXIS] = target[E_AXIS];
-    #if ENABLED(LIN_ADVANCE)
-      position_float[E_AXIS] = e;
-    #endif
-  }
 
   long de = target[E_AXIS] - position[E_AXIS];
 
-  #if ENABLED(LIN_ADVANCE)
-    float de_float = e - position_float[E_AXIS]; // Should this include e_factor?
-  #endif
-
   #if ENABLED(PREVENT_COLD_EXTRUSION) || ENABLED(PREVENT_LENGTHY_EXTRUDE)
     if (de) {
       #if ENABLED(PREVENT_COLD_EXTRUSION)
         if (thermalManager.tooColdToExtrude(extruder)) {
           position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
           de = 0; // no difference
-          #if ENABLED(LIN_ADVANCE)
-            position_float[E_AXIS] = e;
-            de_float = 0;
-          #endif
           SERIAL_ECHO_START();
           SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
         }
         if (labs(de * e_factor[extruder]) > (int32_t)axis_steps_per_mm[E_AXIS_N] * (EXTRUDE_MAXLENGTH)) { // It's not important to get max. extrusion length in a precision < 1mm, so save some cycles and cast to int
           position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
           de = 0; // no difference
-          #if ENABLED(LIN_ADVANCE)
-            position_float[E_AXIS] = e;
-            de_float = 0;
-          #endif
           SERIAL_ECHO_START();
           SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
         }
     }
   #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
 
+  #if ENABLED(LIN_ADVANCE)
+    float de_float = de * steps_to_mm[E_AXIS_N];
+  #endif
+
   // Compute direction bit-mask for this block
   uint8_t dm = 0;
   #if CORE_IS_XY
 
   #if ENABLED(LIN_ADVANCE)
 
-    //
-    // Use LIN_ADVANCE for blocks if all these are true:
-    //
-    // esteps                                          : We have E steps todo (a printing move)
-    //
-    // block->steps[X_AXIS] || block->steps[Y_AXIS]    : We have a movement in XY direction (i.e., not retract / prime).
-    //
-    // extruder_advance_k                              : There is an advance factor set.
-    //
-    // block->steps[E_AXIS] != block->step_event_count : A problem occurs if the move before a retract is too small.
-    //                                                   In that case, the retract and move will be executed together.
-    //                                                   This leads to too many advance steps due to a huge e_acceleration.
-    //                                                   The math is good, but we must avoid retract moves with advance!
-    // de_float > 0.0                                  : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
-    //
-    block->use_advance_lead =  esteps
-                            && (block->steps[X_AXIS] || block->steps[Y_AXIS])
+    /**
+     *
+     * Use LIN_ADVANCE for blocks if all these are true:
+     *
+     * esteps && (block->steps[X_AXIS] || block->steps[Y_AXIS]) : This is a print move
+     *
+     * extruder_advance_k                 : There is an advance factor set.
+     *
+     * esteps != block->step_event_count  : A problem occurs if the move before a retract is too small.
+     *                                      In that case, the retract and move will be executed together.
+     *                                      This leads to too many advance steps due to a huge e_acceleration.
+     *                                      The math is good, but we must avoid retract moves with advance!
+     * de > 0                             : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
+     */
+    block->use_advance_lead =  esteps && (block->steps[X_AXIS] || block->steps[Y_AXIS])
                             && extruder_advance_k
                             && (uint32_t)esteps != block->step_event_count
-                            && de_float > 0.0;
+                            && de > 0;
     if (block->use_advance_lead)
       block->abs_adv_steps_multiplier8 = LROUND(
         extruder_advance_k
-        * (UNEAR_ZERO(advance_ed_ratio) ? de_float / mm_D_float : advance_ed_ratio) // Use the fixed ratio, if set
+        * (UNEAR_ZERO(advance_ed_ratio) ? de * steps_to_mm[E_AXIS_N] / HYPOT(da * steps_to_mm[X_AXIS], db * steps_to_mm[Y_AXIS]) : advance_ed_ratio) // Use the fixed ratio, if set
         * (block->nominal_speed / (float)block->nominal_rate)
         * axis_steps_per_mm[E_AXIS_N] * 256.0
       );
 
   // Update the position (only when a move was queued)
   COPY(position, target);
-  #if ENABLED(LIN_ADVANCE)
-    position_float[X_AXIS] = a;
-    position_float[Y_AXIS] = b;
-    position_float[Z_AXIS] = c;
-    position_float[E_AXIS] = e;
-  #endif
 
   recalculate();
 
              nb = position[Y_AXIS] = LROUND(b * axis_steps_per_mm[Y_AXIS]),
              nc = position[Z_AXIS] = LROUND(c * axis_steps_per_mm[Z_AXIS]),
              ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
-  #if ENABLED(LIN_ADVANCE)
-    position_float[X_AXIS] = a;
-    position_float[Y_AXIS] = b;
-    position_float[Z_AXIS] = c;
-    position_float[E_AXIS] = e;
-  #endif
   stepper.set_position(na, nb, nc, ne);
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
   ZERO(previous_speed);
  * Sync from the stepper positions. (e.g., after an interrupted move)
  */
 void Planner::sync_from_steppers() {
-  LOOP_XYZE(i) {
+  LOOP_XYZE(i)
     position[i] = stepper.position((AxisEnum)i);
-    #if ENABLED(LIN_ADVANCE)
-      position_float[i] = position[i] * steps_to_mm[i
-        #if ENABLED(DISTINCT_E_FACTORS)
-          + (i == E_AXIS ? active_extruder : 0)
-        #endif
-      ];
-    #endif
-  }
 }
 
 /**
     const uint8_t axis_index = axis;
   #endif
   position[axis] = LROUND(v * axis_steps_per_mm[axis_index]);
-  #if ENABLED(LIN_ADVANCE)
-    position_float[axis] = v;
-  #endif
   stepper.set_position(axis, v);
   previous_speed[axis] = 0.0;
 }

Marlin/planner.h

       static uint32_t axis_segment_time_us[2][3];
     #endif
 
-    #if ENABLED(LIN_ADVANCE)
-      static float position_float[NUM_AXIS];
-    #endif
-
     #if ENABLED(ULTRA_LCD)
       volatile static uint32_t block_buffer_runtime_us; //Theoretical block buffer runtime in µs
     #endif
     // Manage fans, paste pressure, etc.
     static void check_axes_activity();
 
-    static void calculate_volumetric_multipliers();
-
     /**
      * Number of moves currently in the planner
      */
 
     static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
 
+    // Update multipliers based on new diameter measurements
+    static void calculate_volumetric_multipliers();
+
+    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 size
+      for (uint8_t i = 0; i < COUNT(filament_size); i++)
+        if (!filament_size[i]) filament_size[i] = DEFAULT_NOMINAL_FILAMENT_DIA;
+    }
+
     #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
 
       /**