Mesh Bed Leveling rewritten, upcaling of tiles, better bed finding method and minor other tweaks.

Firmware/Marlin_main.cpp

@@ -486,7 +486,9 @@ void serial_echopair_P(const char *s_P, unsigned long v)
   }
 #endif //!SDSUPPORT
 
-
+#ifdef MESH_BED_LEVELING
+static void find_bed();
+#endif
 
 
 //adds an command to the main command buffer
@@ -1076,6 +1078,28 @@ static void axis_is_at_home(int axis) {
 inline void set_current_to_destination() { memcpy(current_position, destination, sizeof(current_position)); }
 inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); }
 
+
+static void setup_for_endstop_move() {
+    saved_feedrate = feedrate;
+    saved_feedmultiply = feedmultiply;
+    feedmultiply = 100;
+    previous_millis_cmd = millis();
+    
+    enable_endstops(true);
+}
+
+static void clean_up_after_endstop_move() {
+#ifdef ENDSTOPS_ONLY_FOR_HOMING
+    enable_endstops(false);
+#endif
+    
+    feedrate = saved_feedrate;
+    feedmultiply = saved_feedmultiply;
+    previous_millis_cmd = millis();
+}
+
+
+
 #ifdef ENABLE_AUTO_BED_LEVELING
 #ifdef AUTO_BED_LEVELING_GRID
 static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
@@ -1184,24 +1208,6 @@ static void do_blocking_move_relative(float offset_x, float offset_y, float offs
     do_blocking_move_to(current_position[X_AXIS] + offset_x, current_position[Y_AXIS] + offset_y, current_position[Z_AXIS] + offset_z);
 }
 
-static void setup_for_endstop_move() {
-    saved_feedrate = feedrate;
-    saved_feedmultiply = feedmultiply;
-    feedmultiply = 100;
-    previous_millis_cmd = millis();
-
-    enable_endstops(true);
-}
-
-static void clean_up_after_endstop_move() {
-#ifdef ENDSTOPS_ONLY_FOR_HOMING
-    enable_endstops(false);
-#endif
-
-    feedrate = saved_feedrate;
-    feedmultiply = saved_feedmultiply;
-    previous_millis_cmd = millis();
-}
 
 static void engage_z_probe() {
     // Engage Z Servo endstop if enabled
@@ -2174,18 +2180,18 @@ void process_commands()
 #endif // ENABLE_AUTO_BED_LEVELING
             
 #ifdef MESH_BED_LEVELING
-            /**
-             * G80: Mesh-based Z probe, probes a grid and produces a
-             *      mesh to compensate for variable bed height
-             *
-             * The S0 report the points as below
-             *
-             *  +----> X-axis
-             *  |
-             *  |
-             *  v Y-axis
-             *
-             */
+    /**
+     * G80: Mesh-based Z probe, probes a grid and produces a
+     *      mesh to compensate for variable bed height
+     *
+     * The S0 report the points as below
+     *
+     *  +----> X-axis
+     *  |
+     *  |
+     *  v Y-axis
+     *
+     */
     case 80:
         {
             // Firstly check if we know where we are
@@ -2217,8 +2223,8 @@ void process_commands()
             int XY_AXIS_FEEDRATE = homing_feedrate[X_AXIS]/20;
             int Z_PROBE_FEEDRATE = homing_feedrate[Z_AXIS]/60;
             int Z_LIFT_FEEDRATE = homing_feedrate[Z_AXIS]/40;
-            
-            while (!(mesh_point == ((MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS)) )) {
+            setup_for_endstop_move();
+            while (!(mesh_point == ((MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) ))) {
             
                 // Move Z to proper distance
                 current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
@@ -2227,12 +2233,12 @@ void process_commands()
                 st_synchronize();
                 
                 // Get cords of measuring point
-                ix = mesh_point % MESH_NUM_X_POINTS;
-                iy = mesh_point / MESH_NUM_X_POINTS;
-                if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // Zig zag
+                ix = mesh_point % MESH_MEAS_NUM_X_POINTS;
+                iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
+                if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag
                 
-                current_position[X_AXIS] = mbl.get_x(ix);
-                current_position[Y_AXIS] = mbl.get_y(iy);
+                current_position[X_AXIS] = mbl.get_meas_x(ix);
+                current_position[Y_AXIS] = mbl.get_meas_y(iy);
                 
                 current_position[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER;
                 current_position[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER;
@@ -2241,14 +2247,9 @@ void process_commands()
                 st_synchronize();
                 
                 // Go down until endstop is hit
-            while ((READ(Z_MIN_PIN)^Z_MIN_ENDSTOP_INVERTING ) == 0) {
-
-                current_position[Z_AXIS] -= MBL_Z_STEP;
                 
-                plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], Z_PROBE_FEEDRATE, active_extruder);
-                st_synchronize();
-                delay(1);
-            }
+                find_bed();
+                
                 
                 
                 mbl.set_z(ix, iy, current_position[Z_AXIS]);
@@ -2256,8 +2257,10 @@ void process_commands()
                 mesh_point++;
                 
             }
+            clean_up_after_endstop_move();
             current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
             plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], Z_LIFT_FEEDRATE, active_extruder);
+            mbl.upsample_3x3();
             mbl.active = 1;
             current_position[X_AXIS] = X_MIN_POS+0.2;
             current_position[Y_AXIS] = Y_MIN_POS+0.2;
@@ -2267,7 +2270,10 @@ void process_commands()
             
         }
         break;
-            
+        
+        /**
+         * G81: Print mesh bed leveling status and bed profile if activated
+         */
         case 81:
             if (mbl.active) {
                 SERIAL_PROTOCOLPGM("Num X,Y: ");
@@ -2288,6 +2294,21 @@ void process_commands()
             else
                 SERIAL_PROTOCOLLNPGM("Mesh bed leveling not active.");
             break;
+        /**
+         * G82: Single Z probe at current location
+         *
+         * WARNING! USE WITH CAUTION! If you'll try to probe where is no leveling pad, nasty things can happen!
+         *
+         */
+        case 82:
+            SERIAL_PROTOCOLLNPGM("Finding bed ");
+            setup_for_endstop_move();
+            find_bed();
+            clean_up_after_endstop_move();
+            SERIAL_PROTOCOLPGM("Bed found at: ");
+            SERIAL_PROTOCOL_F(current_position[Z_AXIS], 5);
+            SERIAL_PROTOCOLPGM("\n");
+            break;
 #endif  // ENABLE_MESH_BED_LEVELING
 
     case 90: // G90
@@ -4623,58 +4644,58 @@ void calculate_delta(float cartesian[3])
 
 #ifdef MESH_BED_LEVELING
     
-// This function is used to split lines on mesh borders so each segment is only part of one mesh area
-#define X_MID_POS (0.5f*(X_MIN_POS+X_MAX_POS))
-#define Y_MID_POS (0.5f*(Y_MIN_POS+Y_MAX_POS))
+    static void find_bed() {
+        feedrate = homing_feedrate[Z_AXIS];
+        
+        // move down until you find the bed
+        float zPosition = -10;
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
+        st_synchronize();
+        
+        // we have to let the planner know where we are right now as it is not where we said to go.
+        zPosition = st_get_position_mm(Z_AXIS);
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
+        
+        // move up the retract distance
+        zPosition += home_retract_mm(Z_AXIS);
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
+        st_synchronize();
+        
+        // move back down slowly to find bed
+        feedrate = homing_feedrate[Z_AXIS]/4;
+        zPosition -= home_retract_mm(Z_AXIS) * 2;
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
+        st_synchronize();
+        
+        current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
+        // make sure the planner knows where we are as it may be a bit different than we last said to move to
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+    }
+    
     
-    void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_rate, const uint8_t& extruder) {
-        int tileDiff = 0;
+    void mesh_plan_buffer_line(const float &x, const float &y, const float &z, const float &e, const float &feed_rate, const uint8_t extruder) {
+        float dx = x - current_position[X_AXIS];
+        float dy = y - current_position[Y_AXIS];
+        float dz = z - current_position[Z_AXIS];
+        int n_segments = 0;
         if (mbl.active) {
-            // In which of the four tiles the start and the end points fall?
-            // tileDiff is a bitmask,
-            // 1st bit indicates a crossing of the tile boundary in the X axis,
-            // 2nd bit indicates a crossing of the tile boundary in the Y axis.
-            tileDiff =
-            ((current_position[X_AXIS] > X_MID_POS) | ((current_position[Y_AXIS] > Y_MID_POS) << 1)) ^
-            ((x > X_MID_POS) | ((y > Y_MID_POS) << 1));
+            float len = abs(dx) + abs(dy) + abs(dz);
+            if (len > 0)
+                n_segments = int(floor(len / 30.f));
         }
         
-        // Normalized parameters for the crossing of the tile boundary.
-        float s1, s2;
-        // Interpolate a linear movement at the tile boundary, in X, Y and E axes.
-#define INRPL_X(u) (current_position[X_AXIS] + (x - current_position[X_AXIS]) * u)
-#define INRPL_Y(u) (current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * u)
-#define INRPL_E(u) (current_position[E_AXIS] + (e - current_position[E_AXIS]) * u)
-        switch (tileDiff) {
-            case 0:
-                // The start and end points on same tile, or the mesh bed leveling is not active.
-                break;
-            case 1:
-                // The start and end tiles differ in X only.
-                s1 = (X_MID_POS - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
-                plan_buffer_line(X_MID_POS, INRPL_Y(s1), z, INRPL_E(s1), feed_rate, extruder);
-                break;
-            case 2:
-                // The start and end tiles differ in Y only.
-                s1 = (Y_MID_POS - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
-                plan_buffer_line(INRPL_X(s1), Y_MID_POS, z, INRPL_E(s1), feed_rate, extruder);
-                break;
-            default:
-                // The start and end tiles differ in both coordinates.
-                // assert(tileDiff == 3);
-                s1 = (X_MID_POS - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
-                s2 = (Y_MID_POS - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
-                if (s1 < s2) {
-                    plan_buffer_line(X_MID_POS, INRPL_Y(s1), z, INRPL_E(s1), feed_rate, extruder);
-                    plan_buffer_line(INRPL_X(s2), Y_MID_POS, z, INRPL_E(s2), feed_rate, extruder);
-                } else {
-                    plan_buffer_line(INRPL_X(s1), Y_MID_POS, z, INRPL_E(s1), feed_rate, extruder);
-                    plan_buffer_line(X_MID_POS, INRPL_Y(s2), z, INRPL_E(s2), feed_rate, extruder);
-                }
+        if (n_segments > 1) {
+            float de = e - current_position[E_AXIS];
+            for (int i = 1; i < n_segments; ++ i) {
+                float t = float(i) / float(n_segments);
+                plan_buffer_line(
+                                 current_position[X_AXIS] + t * dx,
+                                 current_position[Y_AXIS] + t * dy,
+                                 current_position[Z_AXIS] + t * dz,
+                                 current_position[E_AXIS] + t * de,
+                                 feed_rate, extruder);
+            }
         }
-#undef INRPL_X
-#undef INRPL_Y
-#undef INRPL_E
         // The rest of the path.
         plan_buffer_line(x, y, z, e, feed_rate, extruder);
         set_current_to_destination();

Firmware/mesh_bed_leveling.cpp

@@ -2,15 +2,65 @@
 
 #ifdef MESH_BED_LEVELING
 
-  mesh_bed_leveling mbl;
+mesh_bed_leveling mbl;
 
-  mesh_bed_leveling::mesh_bed_leveling() { reset(); }
+mesh_bed_leveling::mesh_bed_leveling() { reset(); }
 
-  void mesh_bed_leveling::reset() {
+void mesh_bed_leveling::reset() {
     active = 0;
     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;
-  }
+        for (int x = 0; x < MESH_NUM_X_POINTS; x++)
+            z_values[y][x] = 0;
+}
+
+#if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1
+// Works for an odd number of MESH_NUM_X_POINTS and MESH_NUM_Y_POINTS
+void mesh_bed_leveling::upsample_3x3()
+{
+    int idx0 = 0;
+    int idx1 = MESH_NUM_X_POINTS / 2;
+    int idx2 = MESH_NUM_X_POINTS - 1;
+    {
+        // First interpolate the points in X axis.
+        static const float x0 = MESH_MIN_X;
+        static const float x1 = 0.5f * float(MESH_MIN_X + MESH_MAX_X);
+        static const float x2 = MESH_MAX_X;
+        for (int j = 0; j < 3; ++ j) {
+            // 1) Copy the source points to their new destination.
+            z_values[j][idx2] = z_values[j][2];
+            z_values[j][idx1] = z_values[j][1];
+            // 2) Interpolate the remaining values by Largrangian polynomials.
+            for (int i = idx0 + 1; i < idx2; ++ i) {
+                if (i == idx1)
+                    continue;
+                float x = get_x(i);
+                z_values[j][i] = z_values[j][idx0] * (x - x1) * (x - x2) / ((x0 - x1) * (x0 - x2)) +
+                z_values[j][idx1] * (x - x0) * (x - x2) / ((x1 - x0) * (x1 - x2)) +
+                z_values[j][idx2] * (x - x0) * (x - x1) / ((x2 - x0) * (x2 - x1));
+            }
+        }
+    }
+    {
+        // Second interpolate the points in Y axis.
+        static const float y0 = MESH_MIN_Y;
+        static const float y1 = 0.5f * float(MESH_MIN_Y + MESH_MAX_Y);
+        static const float y2 = MESH_MAX_Y;
+        for (int i = 0; i < MESH_NUM_X_POINTS; ++ i) {
+            // 1) Copy the intermediate points to their new destination.
+            z_values[idx2][i] = z_values[2][i];
+            z_values[idx1][i] = z_values[1][i];
+            // 2) Interpolate the remaining values by Largrangian polynomials.
+            for (int j = 1; j + 1 < MESH_NUM_Y_POINTS; ++ j) {
+                if (j == idx1)
+                    continue;
+                float y = get_y(j);
+                z_values[j][i] = z_values[idx0][i] * (y - y1) * (y - y2) / ((y0 - y1) * (y0 - y2)) +
+                z_values[idx1][i] * (y - y0) * (y - y2) / ((y1 - y0) * (y1 - y2)) +
+                z_values[idx2][i] * (y - y0) * (y - y1) / ((y2 - y0) * (y2 - y1));
+            }
+        }
+    }
+}
+#endif
 
 #endif  // MESH_BED_LEVELING

Firmware/mesh_bed_leveling.h

@@ -2,56 +2,122 @@
 
 #ifdef MESH_BED_LEVELING
 
-  #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 MEAS_NUM_X_DIST (float(MESH_MAX_X - MESH_MIN_X)/float(MESH_MEAS_NUM_X_POINTS - 1))
+#define MEAS_NUM_Y_DIST (float(MESH_MAX_Y - MESH_MIN_Y)/float(MESH_MEAS_NUM_Y_POINTS - 1))
 
-  class mesh_bed_leveling {
-  public:
+#define MESH_X_DIST (float(MESH_MAX_X - MESH_MIN_X)/float(MESH_NUM_X_POINTS - 1))
+#define MESH_Y_DIST (float(MESH_MAX_Y - MESH_MIN_Y)/float(MESH_NUM_Y_POINTS - 1))
+
+class mesh_bed_leveling {
+public:
     uint8_t active;
     float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
-
+    
     mesh_bed_leveling();
-
+    
     void reset();
-
-    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; }
+    
+#if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1
+    void upsample_3x3();
+#endif
+    
+    float get_x(int i) { return float(MESH_MIN_X) + float(MESH_X_DIST) * float(i); }
+    float get_y(int i) { return float(MESH_MIN_Y) + float(MESH_Y_DIST) * float(i); }
+    
+    float get_meas_x(int i) { return float(MESH_MIN_X) + float(MEAS_NUM_X_DIST) * float(i); }
+    float get_meas_y(int i) { return float(MESH_MIN_Y) + float(MEAS_NUM_Y_DIST) * float(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) i++;
-      return i - 1;
+        int i = 1;
+        while (x > get_x(i) && i < MESH_NUM_X_POINTS - 1) i++;
+        return i - 1;
     }
-
+    
     int select_y_index(float y) {
-      int i = 1;
-      while (y > get_y(i) && i < MESH_NUM_Y_POINTS - 1) i++;
-      return i - 1;
-    }
-
-    float calc_z0(float a0, float a1, float z1, float a2, float z2) {
-      float delta_z = (z2 - z1) / (a2 - a1);
-      float delta_a = a0 - a1;
-      return z1 + delta_a * delta_z;
+        int i = 1;
+        while (y > get_y(i) && i < MESH_NUM_Y_POINTS - 1) i++;
+        return i - 1;
     }
-
-    float get_z(float x0, float y0) {
-      int x_index = select_x_index(x0);
-      int y_index = select_y_index(y0);
-      float z1 = calc_z0(x0,
-                         get_x(x_index), z_values[y_index][x_index],
-                         get_x(x_index + 1), z_values[y_index][x_index + 1]);
-      float z2 = calc_z0(x0,
-                         get_x(x_index), z_values[y_index + 1][x_index],
-                         get_x(x_index + 1), z_values[y_index + 1][x_index + 1]);
-      float z0 = calc_z0(y0,
-                         get_y(y_index), z1,
-                         get_y(y_index + 1), z2);
-      return z0;
+    
+    float get_z(float x, float y) {
+        int   i, j;
+        float s, t;
+        
+#if MESH_NUM_X_POINTS==3 && MESH_NUM_Y_POINTS==3
+#define MESH_MID_X (0.5f*(MESH_MIN_X+MESH_MAX_X))
+#define MESH_MID_Y (0.5f*(MESH_MIN_Y+MESH_MAX_Y))
+        if (x < MESH_MID_X) {
+            i = 0;
+            s = (x - MESH_MIN_X) / MESH_X_DIST;
+            if (s > 1.f)
+                s = 1.f;
+        } else {
+            i = 1;
+            s = (x - MESH_MID_X) / MESH_X_DIST;
+            if (s < 0)
+                s = 0;
+        }
+        if (y < MESH_MID_Y) {
+            j = 0;
+            t = (y - MESH_MIN_Y) / MESH_Y_DIST;
+            if (t > 1.f)
+                t = 1.f;
+        } else {
+            j = 1;
+            t = (y - MESH_MID_Y) / MESH_Y_DIST;
+            if (t < 0)
+                t = 0;
+        }
+#else
+        i = int(floor((x - MESH_MIN_X) / MESH_X_DIST));
+        if (i < 0) {
+            i = 0;
+            s = (x - MESH_MIN_X) / MESH_X_DIST;
+            if (s > 1.f)
+                s = 1.f;
+        }
+        else if (i > MESH_NUM_X_POINTS - 2) {
+            i = MESH_NUM_X_POINTS - 2;
+            s = (x - get_x(i)) / MESH_X_DIST;
+            if (s < 0)
+                s = 0;
+        } else {
+            s = (x - get_x(i)) / MESH_X_DIST;
+            if (s < 0)
+                s = 0;
+            else if (s > 1.f)
+                s = 1.f;
+        }
+        j = int(floor((y - MESH_MIN_Y) / MESH_Y_DIST));
+        if (j < 0) {
+            j = 0;
+            t = (y - MESH_MIN_Y) / MESH_Y_DIST;
+            if (t > 1.f)
+                t = 1.f;
+        } else if (j > MESH_NUM_Y_POINTS - 2) {
+            j = MESH_NUM_Y_POINTS - 2;
+            t = (y - get_y(j)) / MESH_Y_DIST;
+            if (t < 0)
+                t = 0;
+        } else {
+            t = (y - get_y(j)) / MESH_Y_DIST;
+            if (t < 0)
+                t = 0;
+            else if (t > 1.f)
+                t = 1.f;
+        }
+#endif /* MESH_NUM_X_POINTS==3 && MESH_NUM_Y_POINTS==3 */
+        
+        float si = 1.f-s;
+        float z0 = si * z_values[j  ][i] + s * z_values[j  ][i+1];
+        float z1 = si * z_values[j+1][i] + s * z_values[j+1][i+1];
+        return (1.f-t) * z0 + t * z1;
     }
-  };
+    
+};
 
-  extern mesh_bed_leveling mbl;
+extern mesh_bed_leveling mbl;
 
 #endif  // MESH_BED_LEVELING

Firmware/planner.cpp

@@ -538,20 +538,6 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
 void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
 #endif  //ENABLE_AUTO_BED_LEVELING
 {
-
-#ifdef DEVELOPER
-    SERIAL_PROTOCOLPGM("MESHING TO:");
-    SERIAL_PROTOCOLPGM(" X:");
-    SERIAL_PROTOCOL(x);
-    SERIAL_PROTOCOLPGM(" Y:");
-    SERIAL_PROTOCOL(y);
-    SERIAL_PROTOCOLPGM(" Z:");
-    SERIAL_PROTOCOL(z);
-    SERIAL_PROTOCOLPGM(" E:");
-    SERIAL_PROTOCOL(e);
-    SERIAL_PROTOCOLPGM("\n");
-#endif
-    
     // Calculate the buffer head after we push this byte
   int next_buffer_head = next_block_index(block_buffer_head);
 
@@ -576,7 +562,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
   target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
 #ifdef MESH_BED_LEVELING
     if (mbl.active){
-        target[Z_AXIS] += lround((z+mbl.get_z(x, y))*axis_steps_per_unit[Z_AXIS]);
+        target[Z_AXIS] = lround((z+mbl.get_z(x, y))*axis_steps_per_unit[Z_AXIS]);
     }else{
         target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
     }
@@ -584,7 +570,6 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
     target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
 #endif // ENABLE_MESH_BED_LEVELING
   target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
-
   #ifdef PREVENT_DANGEROUS_EXTRUDE
   if(target[E_AXIS]!=position[E_AXIS])
   {
@@ -1090,7 +1075,7 @@ void plan_set_position(const float &x, const float &y, const float &z, const flo
   position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
 #ifdef MESH_BED_LEVELING
     if (mbl.active){
-      position[Z_AXIS] += lround((z+mbl.get_z(x, y))*axis_steps_per_unit[Z_AXIS]);
+      position[Z_AXIS] = lround((z+mbl.get_z(x, y))*axis_steps_per_unit[Z_AXIS]);
     }else{
         position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
     }

Firmware/stepper.cpp

@@ -1061,13 +1061,13 @@ long st_get_position(uint8_t axis)
   return count_pos;
 }
 
-#ifdef ENABLE_AUTO_BED_LEVELING
+
 float st_get_position_mm(uint8_t axis)
 {
   float steper_position_in_steps = st_get_position(axis);
   return steper_position_in_steps / axis_steps_per_unit[axis];
 }
-#endif  // ENABLE_AUTO_BED_LEVELING
+
 
 void finishAndDisableSteppers()
 {

Firmware/stepper.h

@@ -61,10 +61,10 @@ void st_set_e_position(const long &e);
 // Get current position in steps
 long st_get_position(uint8_t axis);
 
-#ifdef ENABLE_AUTO_BED_LEVELING
+
 // Get current position in mm
 float st_get_position_mm(uint8_t axis);
-#endif  //ENABLE_AUTO_BED_LEVELING
+
 
 // The stepper subsystem goes to sleep when it runs out of things to execute. Call this
 // to notify the subsystem that it is time to go to work.

Firmware/variants/1_7dev-RAMBo13a-E3Dv6lite.h

@@ -151,7 +151,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 #if MOTHERBOARD == 102 || MOTHERBOARD == 302
   #define MOTOR_CURRENT_PWM_RANGE 2000
   #define DEFAULT_PWM_MOTOR_CURRENT  {270, 450, 450} // {XY,Z,E}
-  #define DEFAULT_PWM_MOTOR_CURRENT_LOUD  {540, 530, 500} // {XY,Z,E}
+  #define DEFAULT_PWM_MOTOR_CURRENT_LOUD  {540, 830, 500} // {XY,Z,E}
 #endif
 
 /*------------------------------------
@@ -170,8 +170,13 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
     #define MESH_MIN_Y 7
     #define MESH_MAX_Y 203
 
-    #define MESH_NUM_X_POINTS 3  // Don't use more than 7 points per axis, implementation limited.
-    #define MESH_NUM_Y_POINTS 3
+    // Mesh upsample definition
+    #define MESH_NUM_X_POINTS 7
+    #define MESH_NUM_Y_POINTS 7
+    // Mesh measure definition
+    #define MESH_MEAS_NUM_X_POINTS 3
+    #define MESH_MEAS_NUM_Y_POINTS 3
+
     #define MESH_HOME_Z_CALIB 0.2
     #define MESH_HOME_Z_SEARCH 5