Fix up bed leveling code

- Init `zprobe_zoffset`
- Remove `current_position[Z_AXIS] = zprobe_zoffset` lines from the
`set_bed_level_equation_*` functions
- Apply standards to `mesh_bed_leveling` files
- Document `MESH_BED_LEVELING`

Marlin/ConfigurationStore.cpp

@@ -11,7 +11,7 @@
  *  max_acceleration_units_per_sq_second (x4)
  *  acceleration
  *  retract_acceleration
- *  travel_aceeleration
+ *  travel_acceleration
  *  minimumfeedrate
  *  mintravelfeedrate
  *  minsegmenttime

Marlin/Marlin_main.cpp

@@ -255,7 +255,7 @@ float home_offset[3] = { 0, 0, 0 };
 float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
 float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
 bool axis_known_position[3] = { false, false, false };
-float zprobe_zoffset;
+float zprobe_zoffset = -Z_PROBE_OFFSET_FROM_EXTRUDER;
 
 // Extruder offset
 #if EXTRUDERS > 1
@@ -1092,9 +1092,6 @@ static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
     current_position[Y_AXIS] = corrected_position.y;
     current_position[Z_AXIS] = corrected_position.z;
 
-    // put the bed at 0 so we don't go below it.
-    current_position[Z_AXIS] = zprobe_zoffset; // in the lsq we reach here after raising the extruder due to the loop structure
-
     plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 }
 #endif
@@ -1121,9 +1118,6 @@ static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float
     current_position[Y_AXIS] = corrected_position.y;
     current_position[Z_AXIS] = corrected_position.z;
 
-    // put the bed at 0 so we don't go below it.
-    current_position[Z_AXIS] = zprobe_zoffset;
-
     plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 
 }
@@ -2010,8 +2004,19 @@ inline void gcode_G28() {
   endstops_hit_on_purpose();
 }
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
 
+  /**
+   * G29: Mesh-based Z-Probe, probes a grid and produces a
+   *      mesh to compensate for variable bed height
+   *
+   * Parameters With MESH_BED_LEVELING:
+   *
+   *  S0 Produce a mesh report
+   *  S1 Start probing mesh points
+   *  S2 Probe the next mesh point
+   *
+   */
   inline void gcode_G29() {
     static int probe_point = -1;
     int state = 0;
@@ -2053,7 +2058,7 @@ inline void gcode_G28() {
     } else if (state == 2) { // Goto next point
 
       if (probe_point < 0) {
-        SERIAL_PROTOCOLPGM("Mesh probing not started.\n");
+        SERIAL_PROTOCOLPGM("Start mesh probing with \"G29 S1\" first.\n");
         return;
       }
       int ix, iy;
@@ -2063,16 +2068,14 @@ inline void gcode_G28() {
       } else {
         ix = (probe_point-1) % MESH_NUM_X_POINTS;
         iy = (probe_point-1) / MESH_NUM_X_POINTS;
-        if (iy&1) { // Zig zag
+        if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
-          ix = (MESH_NUM_X_POINTS - 1) - ix;
-        }
         mbl.set_z(ix, iy, current_position[Z_AXIS]);
         current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
         plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
         st_synchronize();
       }
-      if (probe_point == MESH_NUM_X_POINTS*MESH_NUM_Y_POINTS) {
+      if (probe_point == MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS) {
-        SERIAL_PROTOCOLPGM("Mesh done.\n");
+        SERIAL_PROTOCOLPGM("Mesh probing done.\n");
         probe_point = -1;
         mbl.active = 1;
         enquecommands_P(PSTR("G28"));
@@ -2080,9 +2083,7 @@ inline void gcode_G28() {
       }
       ix = probe_point % MESH_NUM_X_POINTS;
       iy = probe_point / MESH_NUM_X_POINTS;
-      if (iy&1) { // Zig zag
+      if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
-        ix = (MESH_NUM_X_POINTS - 1) - ix;
-      }
       current_position[X_AXIS] = mbl.get_x(ix);
       current_position[Y_AXIS] = mbl.get_y(iy);
       plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
@@ -2091,9 +2092,7 @@ inline void gcode_G28() {
     }
   }
 
-#endif
+#elif defined(ENABLE_AUTO_BED_LEVELING)
-
-#ifdef ENABLE_AUTO_BED_LEVELING
 
   /**
    * G29: Detailed Z-Probe, probes the bed at 3 or more points.
@@ -2210,7 +2209,7 @@ inline void gcode_G28() {
 
     #ifdef Z_PROBE_SLED
       dock_sled(false); // engage (un-dock) the probe
-    #elif defined(Z_PROBE_ALLEN_KEY)
+    #elif defined(Z_PROBE_ALLEN_KEY) //|| defined(SERVO_LEVELING)
       engage_z_probe();
     #endif
 
@@ -2218,9 +2217,8 @@ inline void gcode_G28() {
 
     #ifdef DELTA
       reset_bed_level();
-    #else
+    #else //!DELTA
-
+      // make sure the bed_level_rotation_matrix is identity or the planner will get it wrong
-    // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
       //vector_3 corrected_position = plan_get_position_mm();
       //corrected_position.debug("position before G29");
       plan_bed_level_matrix.set_to_identity();
@@ -2230,7 +2228,7 @@ inline void gcode_G28() {
       current_position[Y_AXIS] = uncorrected_position.y;
       current_position[Z_AXIS] = uncorrected_position.z;
       plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-  #endif
+    #endif //!DELTA
 
     setup_for_endstop_move();
 
@@ -2242,7 +2240,12 @@ inline void gcode_G28() {
       const int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points-1);
       const int yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points-1);
 
-    #ifndef DELTA
+      #ifdef DELTA
+        delta_grid_spacing[0] = xGridSpacing;
+        delta_grid_spacing[1] = yGridSpacing;
+        float z_offset = Z_PROBE_OFFSET_FROM_EXTRUDER;
+        if (code_seen(axis_codes[Z_AXIS])) z_offset += code_value();
+      #else // !DELTA
         // solve the plane equation ax + by + d = z
         // A is the matrix with rows [x y 1] for all the probed points
         // B is the vector of the Z positions
@@ -2254,14 +2257,7 @@ inline void gcode_G28() {
         double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations
                eqnBVector[abl2],     // "B" vector of Z points
                mean = 0.0;
-
+      #endif // !DELTA
-    #else
-      delta_grid_spacing[0] = xGridSpacing;
-      delta_grid_spacing[1] = yGridSpacing;
-
-      float z_offset = Z_PROBE_OFFSET_FROM_EXTRUDER;
-      if (code_seen(axis_codes[Z_AXIS])) z_offset += code_value();
-    #endif
 
       int probePointCounter = 0;
       bool zig = true;
@@ -2333,7 +2329,10 @@ inline void gcode_G28() {
 
       clean_up_after_endstop_move();
 
-    #ifndef DELTA
+      #ifdef DELTA
+        extrapolate_unprobed_bed_level();
+        print_bed_level();
+      #else // !DELTA
         // solve lsq problem
         double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector);
 
@@ -2383,10 +2382,8 @@ inline void gcode_G28() {
 
         set_bed_level_equation_lsq(plane_equation_coefficients);
         free(plane_equation_coefficients);
-    #else
+
-      extrapolate_unprobed_bed_level();
+      #endif // !DELTA
-      print_bed_level();
-    #endif
 
     #else // !AUTO_BED_LEVELING_GRID
 
@@ -2428,7 +2425,7 @@ inline void gcode_G28() {
 
     #ifdef Z_PROBE_SLED
       dock_sled(true, -SLED_DOCKING_OFFSET); // dock the probe, correcting for over-travel
-  #elif defined(Z_PROBE_ALLEN_KEY)
+    #elif defined(Z_PROBE_ALLEN_KEY) //|| defined(SERVO_LEVELING)
       retract_z_probe();
     #endif
 

Marlin/mesh_bed_leveling.cpp

@@ -1,20 +1,16 @@
 #include "mesh_bed_leveling.h"
 
-#if defined(MESH_BED_LEVELING)
+#ifdef MESH_BED_LEVELING
 
-mesh_bed_leveling mbl;
+  mesh_bed_leveling mbl;
 
-mesh_bed_leveling::mesh_bed_leveling() {
+  mesh_bed_leveling::mesh_bed_leveling() { reset(); }
-    reset();
-}
       
-void mesh_bed_leveling::reset() {
+  void mesh_bed_leveling::reset() {
-    for (int y=0; y<MESH_NUM_Y_POINTS; y++) {
+    active = 0;
-        for (int x=0; x<MESH_NUM_X_POINTS; x++) {
+    for (int y = 0; y < MESH_NUM_Y_POINTS; y++)
+      for (int x = 0; x < MESH_NUM_X_POINTS; x++)
         z_values[y][x] = 0;
   }
-    }
-    active = 0;
-}
 
 #endif  // MESH_BED_LEVELING

Marlin/mesh_bed_leveling.h

@@ -2,11 +2,11 @@
 
 #if defined(MESH_BED_LEVELING)
 
-#define MESH_X_DIST ((MESH_MAX_X - MESH_MIN_X)/(MESH_NUM_X_POINTS - 1))
+  #define MESH_X_DIST ((MESH_MAX_X - MESH_MIN_X)/(MESH_NUM_X_POINTS - 1))
-#define MESH_Y_DIST ((MESH_MAX_Y - MESH_MIN_Y)/(MESH_NUM_Y_POINTS - 1))
+  #define MESH_Y_DIST ((MESH_MAX_Y - MESH_MIN_Y)/(MESH_NUM_Y_POINTS - 1))
 
-class mesh_bed_leveling {
+  class mesh_bed_leveling {
-public:
+  public:
     uint8_t active;
     float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
     
@@ -14,24 +14,20 @@ public:
     
     void reset();
     
-    float get_x(int i) { return MESH_MIN_X + MESH_X_DIST*i; }
+    float get_x(int i) { return MESH_MIN_X + MESH_X_DIST * i; }
-    float get_y(int i) { return MESH_MIN_Y + MESH_Y_DIST*i; }
+    float get_y(int i) { return MESH_MIN_Y + MESH_Y_DIST * i; }
     void set_z(int ix, int iy, float z) { z_values[iy][ix] = z; }
     
     int select_x_index(float x) {
       int i = 1;
-        while (x > get_x(i) && i < MESH_NUM_X_POINTS-1) {
+      while (x > get_x(i) && i < MESH_NUM_X_POINTS-1) i++;
-            i++;
+      return i - 1;
-        }
-        return i-1;
     }
     
     int select_y_index(float y) {
       int i = 1;
-        while (y > get_y(i) && i < MESH_NUM_Y_POINTS-1) {
+      while (y > get_y(i) && i < MESH_NUM_Y_POINTS - 1) i++;
-            i++;
+      return i - 1;
-        }
-        return i-1;
     }
     
     float calc_z0(float a0, float a1, float z1, float a2, float z2) {
@@ -54,8 +50,8 @@ public:
                          get_y(y_index+1), z2);
       return z0;
     }
-};
+  };
 
-extern mesh_bed_leveling mbl;
+  extern mesh_bed_leveling mbl;
 
 #endif  // MESH_BED_LEVELING