|
@@ -2224,11 +2224,15 @@ static void clean_up_after_endstop_or_probe_move() {
|
2224
|
2224
|
void set_bed_leveling_enabled(bool enable=true) {
|
2225
|
2225
|
#if ENABLED(MESH_BED_LEVELING)
|
2226
|
2226
|
|
2227
|
|
- if (!enable && mbl.active())
|
2228
|
|
- current_position[Z_AXIS] +=
|
2229
|
|
- mbl.get_z(RAW_CURRENT_POSITION(X_AXIS), RAW_CURRENT_POSITION(Y_AXIS)) - (MESH_HOME_SEARCH_Z);
|
|
2227
|
+ if (enable != mbl.active()) {
|
2230
|
2228
|
|
2231
|
|
- mbl.set_active(enable && mbl.has_mesh()); // was set_has_mesh(). Is this not correct?
|
|
2229
|
+ if (!enable)
|
|
2230
|
+ planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
|
|
2231
|
+
|
|
2232
|
+ mbl.set_active(enable && mbl.has_mesh());
|
|
2233
|
+
|
|
2234
|
+ if (enable) planner.unapply_leveling(current_position);
|
|
2235
|
+ }
|
2232
|
2236
|
|
2233
|
2237
|
#elif HAS_ABL
|
2234
|
2238
|
|
|
@@ -3162,8 +3166,10 @@ inline void gcode_G4() {
|
3162
|
3166
|
#elif ENABLED(MESH_BED_LEVELING)
|
3163
|
3167
|
SERIAL_ECHOPGM("Mesh Bed Leveling");
|
3164
|
3168
|
if (mbl.active()) {
|
|
3169
|
+ float lz = current_position[Z_AXIS];
|
|
3170
|
+ planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], lz);
|
3165
|
3171
|
SERIAL_ECHOLNPGM(" (enabled)");
|
3166
|
|
- SERIAL_ECHOPAIR("MBL Adjustment Z", mbl.get_z(RAW_CURRENT_POSITION(X_AXIS), RAW_CURRENT_POSITION(Y_AXIS)));
|
|
3172
|
+ SERIAL_ECHOPAIR("MBL Adjustment Z", lz);
|
3167
|
3173
|
}
|
3168
|
3174
|
SERIAL_EOL;
|
3169
|
3175
|
#endif
|
|
@@ -3321,13 +3327,15 @@ inline void gcode_G28() {
|
3321
|
3327
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
3322
|
3328
|
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("MBL was active");
|
3323
|
3329
|
#endif
|
3324
|
|
- // Save known Z position if already homed
|
|
3330
|
+ // Use known Z position if already homed
|
3325
|
3331
|
if (axis_homed[X_AXIS] && axis_homed[Y_AXIS] && axis_homed[Z_AXIS]) {
|
|
3332
|
+ set_bed_leveling_enabled(false);
|
3326
|
3333
|
pre_home_z = current_position[Z_AXIS];
|
3327
|
|
- pre_home_z += mbl.get_z(RAW_CURRENT_POSITION(X_AXIS), RAW_CURRENT_POSITION(Y_AXIS));
|
3328
|
3334
|
}
|
3329
|
|
- mbl.set_active(false);
|
3330
|
|
- current_position[Z_AXIS] = pre_home_z;
|
|
3335
|
+ else {
|
|
3336
|
+ mbl.set_active(false);
|
|
3337
|
+ current_position[Z_AXIS] = pre_home_z;
|
|
3338
|
+ }
|
3331
|
3339
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
3332
|
3340
|
if (DEBUGGING(LEVELING)) DEBUG_POS("Set Z to pre_home_z", current_position);
|
3333
|
3341
|
#endif
|
|
@@ -3703,8 +3711,8 @@ inline void gcode_G28() {
|
3703
|
3711
|
|
3704
|
3712
|
case MeshReset:
|
3705
|
3713
|
if (mbl.active()) {
|
3706
|
|
- current_position[Z_AXIS] +=
|
3707
|
|
- mbl.get_z(RAW_CURRENT_POSITION(X_AXIS), RAW_CURRENT_POSITION(Y_AXIS)) - MESH_HOME_SEARCH_Z;
|
|
3714
|
+ current_position[Z_AXIS] -= MESH_HOME_SEARCH_Z;
|
|
3715
|
+ planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
|
3708
|
3716
|
mbl.reset();
|
3709
|
3717
|
SYNC_PLAN_POSITION_KINEMATIC();
|
3710
|
3718
|
}
|
|
@@ -7640,9 +7648,12 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
|
7640
|
7648
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
7641
|
7649
|
if (DEBUGGING(LEVELING)) SERIAL_ECHOPAIR("Z before MBL: ", current_position[Z_AXIS]);
|
7642
|
7650
|
#endif
|
7643
|
|
- float xpos = RAW_CURRENT_POSITION(X_AXIS),
|
7644
|
|
- ypos = RAW_CURRENT_POSITION(Y_AXIS);
|
7645
|
|
- current_position[Z_AXIS] += mbl.get_z(xpos + xydiff[X_AXIS], ypos + xydiff[Y_AXIS]) - mbl.get_z(xpos, ypos);
|
|
7651
|
+ float x2 = current_position[X_AXIS] + xydiff[X_AXIS],
|
|
7652
|
+ y2 = current_position[Y_AXIS] + xydiff[Y_AXIS],
|
|
7653
|
+ z1 = current_position[Z_AXIS], z2 = z1;
|
|
7654
|
+ planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], z1);
|
|
7655
|
+ planner.apply_leveling(x2, y2, z2);
|
|
7656
|
+ current_position[Z_AXIS] += z2 - z1;
|
7646
|
7657
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
7647
|
7658
|
if (DEBUGGING(LEVELING))
|
7648
|
7659
|
SERIAL_ECHOLNPAIR(" after: ", current_position[Z_AXIS]);
|