Merge pull request #7157 from thinkyhead/bf_g33_f_parameter

G33 F and Height bug fix

Marlin/Marlin_main.cpp

 
     float z_dest = LOGICAL_Z_POSITION(z_raise);
     if (zprobe_zoffset < 0) z_dest -= zprobe_zoffset;
+
     #if ENABLED(DELTA)
-      z_dest -= home_offset[Z_AXIS];
+      z_dest -= home_offset[Z_AXIS]; // Account for delta height adjustment
     #endif
 
     if (z_dest > current_position[Z_AXIS])
       // move down quickly before doing the slow probe
       float z = LOGICAL_Z_POSITION(Z_CLEARANCE_BETWEEN_PROBES);
       if (zprobe_zoffset < 0) z -= zprobe_zoffset;
+
       #if ENABLED(DELTA)
-        z -= home_offset[Z_AXIS];
+        z -= home_offset[Z_AXIS]; // Account for delta height adjustment
       #endif
+
       if (z < current_position[Z_AXIS])
         do_blocking_move_to_z(z, MMM_TO_MMS(Z_PROBE_SPEED_FAST));
 
         SERIAL_ECHOLNPAIR(" Discrepancy:", first_probe_z - current_position[Z_AXIS]);
       }
     #endif
-    return RAW_CURRENT_POSITION(Z) + zprobe_zoffset;
+    return RAW_CURRENT_POSITION(Z) + zprobe_zoffset
+      #if ENABLED(DELTA)
+        + home_offset[Z_AXIS] // Account for delta height adjustment
+      #endif
+    ;
   }
 
   /**
      *   T   Don't calibrate tower angle corrections
      *
      *   Cn.nn Calibration precision; when omitted calibrates to maximum precision
+     *   
+     *   Fn  Force to run at least n iterations and takes the best result
      *
      *   Vn  Verbose level:
      *
         return;
       }
 
-      const bool towers_set = !parser.seen('T'),
+      const int8_t force_iterations = parser.seen('F') ? parser.value_int() : 1;
+      if (!WITHIN(force_iterations, 1, 30)) {
+        SERIAL_PROTOCOLLNPGM("?(F)orce iteration is implausible (1-30).");
+        return;
+      }
+
+      const bool towers_set           = !parser.seen('T'),
                  stow_after_each      = parser.seen('E') && parser.value_bool(),
                  _1p_calibration      = probe_points == 1,
                  _4p_calibration      = probe_points == 2,
       float test_precision,
             zero_std_dev = (verbose_level ? 999.0 : 0.0), // 0.0 in dry-run mode : forced end
             zero_std_dev_old = zero_std_dev,
+            zero_std_dev_min = zero_std_dev,
             e_old[XYZ] = {
               endstop_adj[A_AXIS],
               endstop_adj[B_AXIS],
           const uint8_t start = _4p_opposite_points ? 3 : 1,
                          step = _4p_calibration ? 4 : _7p_half_circle ? 2 : 1;
           for (uint8_t axis = start; axis < 13; axis += step) {
-            const float offset_circles = _7p_quadruple_circle ? (zig_zag ? 1.5 : 1.0) :
-                                         _7p_triple_circle    ? (zig_zag ? 1.0 : 0.5) :
-                                         _7p_double_circle    ? (zig_zag ? 0.5 : 0.0) : 0;
+            const float zigadd = (zig_zag ? 0.5 : 0.0),
+                        offset_circles = _7p_quadruple_circle ? zigadd + 1.0 :
+                                         _7p_triple_circle    ? zigadd + 0.5 :
+                                         _7p_double_circle    ? zigadd : 0;
             for (float circles = -offset_circles ; circles <= offset_circles; circles++) {
               const float a = RADIANS(180 + 30 * axis),
                           r = delta_calibration_radius * (1 + circles * (zig_zag ? 0.1 : -0.1));
             N++;
           }
         zero_std_dev_old = zero_std_dev;
-        zero_std_dev = round(SQRT(S2 / N) * 1000.0) / 1000.0 + 0.00001;
-
-        if (iterations == 1) home_offset[Z_AXIS] = zh_old; // reset height after 1st probe change
+        NOMORE(zero_std_dev_min, zero_std_dev);
+        zero_std_dev = round(sqrt(S2 / N) * 1000.0) / 1000.0 + 0.00001;
 
         // Solve matrices
 
-        if (zero_std_dev < test_precision && zero_std_dev > calibration_precision) {
-          COPY(e_old, endstop_adj);
-          dr_old = delta_radius;
-          zh_old = home_offset[Z_AXIS];
-          alpha_old = delta_tower_angle_trim[A_AXIS];
-          beta_old = delta_tower_angle_trim[B_AXIS];
+        if ((zero_std_dev < test_precision && zero_std_dev > calibration_precision) || iterations <= force_iterations) {
+          if (zero_std_dev < zero_std_dev_min) {
+            COPY(e_old, endstop_adj);
+            dr_old = delta_radius;
+            zh_old = home_offset[Z_AXIS];
+            alpha_old = delta_tower_angle_trim[A_AXIS];
+            beta_old = delta_tower_angle_trim[B_AXIS];
+          }
 
           float e_delta[XYZ] = { 0.0 }, r_delta = 0.0, t_alpha = 0.0, t_beta = 0.0;
           const float r_diff = delta_radius - delta_calibration_radius,
           }
         }
         if (test_precision != 0.0) {                                 // !forced end
-          if (zero_std_dev >= test_precision || zero_std_dev <= calibration_precision) {  // end iterations
+          if ((zero_std_dev >= test_precision || zero_std_dev <= calibration_precision) && iterations > force_iterations) {  // end iterations
             SERIAL_PROTOCOLPGM("Calibration OK");
             SERIAL_PROTOCOL_SP(36);
             if (zero_std_dev >= test_precision)
             SERIAL_PROTOCOLPGM("  Tz:+0.00");
             SERIAL_EOL();
           }
-          if (zero_std_dev >= test_precision || zero_std_dev <= calibration_precision)
+          if ((zero_std_dev >= test_precision || zero_std_dev <= calibration_precision) && iterations > force_iterations)
             serialprintPGM(save_message);
             SERIAL_EOL();
         }
         endstops.not_homing();
 
       }
-      while (zero_std_dev < test_precision && zero_std_dev > calibration_precision && iterations < 31);
+      while ((zero_std_dev < test_precision && zero_std_dev > calibration_precision && iterations < 31) || iterations <= force_iterations);
 
       #if ENABLED(DELTA_HOME_TO_SAFE_ZONE)
         do_blocking_move_to_z(delta_clip_start_height);
   inline void gcode_M665() {
     if (parser.seen('H')) {
       home_offset[Z_AXIS] = parser.value_linear_units() - DELTA_HEIGHT;
-      current_position[Z_AXIS] += parser.value_linear_units() - DELTA_HEIGHT - home_offset[Z_AXIS];
       update_software_endstops(Z_AXIS);
     }
     if (parser.seen('L')) delta_diagonal_rod = parser.value_linear_units();