diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 3f30832c5b..2ef57068bb 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -2847,7 +2847,7 @@ inline void gcode_G28() { } if (probe_point == 0) { // Set Z to a positive value before recording the first Z. - current_position[Z_AXIS] = MESH_HOME_SEARCH_Z; + current_position[Z_AXIS] = MESH_HOME_SEARCH_Z + home_offset[Z_AXIS]; sync_plan_position(); } else { @@ -2856,7 +2856,7 @@ inline void gcode_G28() { iy = (probe_point - 1) / (MESH_NUM_X_POINTS); if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag mbl.set_z(ix, iy, current_position[Z_AXIS]); - current_position[Z_AXIS] = MESH_HOME_SEARCH_Z; + current_position[Z_AXIS] = MESH_HOME_SEARCH_Z + home_offset[Z_AXIS]; 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(); } @@ -2865,8 +2865,8 @@ inline void gcode_G28() { ix = probe_point % (MESH_NUM_X_POINTS); iy = probe_point / (MESH_NUM_X_POINTS); if (iy & 1) ix = (MESH_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_x(ix) + home_offset[X_AXIS]; + current_position[Y_AXIS] = mbl.get_y(iy) + home_offset[Y_AXIS]; 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(); probe_point++; @@ -3155,7 +3155,7 @@ inline void gcode_G28() { // raise extruder float measured_z, - z_before = probePointCounter ? Z_RAISE_BETWEEN_PROBINGS + current_position[Z_AXIS] : Z_RAISE_BEFORE_PROBING; + z_before = probePointCounter ? Z_RAISE_BETWEEN_PROBINGS + current_position[Z_AXIS] : Z_RAISE_BEFORE_PROBING + home_offset[Z_AXIS]; if (probePointCounter) { #if ENABLED(DEBUG_LEVELING_FEATURE) @@ -3168,7 +3168,7 @@ inline void gcode_G28() { else { #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) { - SERIAL_ECHOPAIR("z_before = (before) ", Z_RAISE_BEFORE_PROBING); + SERIAL_ECHOPAIR("z_before = (before) ", Z_RAISE_BEFORE_PROBING + home_offset[Z_AXIS]); SERIAL_EOL; } #endif @@ -3329,9 +3329,18 @@ inline void gcode_G28() { p1 = ProbeDeploy, p2 = ProbeStay, p3 = ProbeStow; // Probe at 3 arbitrary points - float z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING, p1, verbose_level), - z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, p2, verbose_level), - z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, p3, verbose_level); + float z_at_pt_1 = probe_pt( ABL_PROBE_PT_1_X + home_offset[X_AXIS], + ABL_PROBE_PT_1_Y + home_offset[Y_AXIS], + Z_RAISE_BEFORE_PROBING + home_offset[Z_AXIS], + p1, verbose_level), + z_at_pt_2 = probe_pt( ABL_PROBE_PT_2_X + home_offset[X_AXIS], + ABL_PROBE_PT_2_Y + home_offset[Y_AXIS], + current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, + p2, verbose_level), + z_at_pt_3 = probe_pt( ABL_PROBE_PT_3_X + home_offset[X_AXIS], + ABL_PROBE_PT_3_Y + home_offset[Y_AXIS], + current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, + p3, verbose_level); clean_up_after_endstop_move(); if (!dryrun) set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3); @@ -4986,19 +4995,26 @@ inline void gcode_M205() { if (code_seen('E')) max_e_jerk = code_value(); } +static void set_home_offset(AxisEnum axis, float v) { + min_pos[axis] = base_min_pos(axis) + v; + max_pos[axis] = base_max_pos(axis) + v; + current_position[axis] += v - home_offset[axis]; + home_offset[axis] = v; +} + /** * M206: Set Additional Homing Offset (X Y Z). SCARA aliases T=X, P=Y */ inline void gcode_M206() { - for (int8_t i = X_AXIS; i <= Z_AXIS; i++) { - if (code_seen(axis_codes[i])) { - home_offset[i] = code_value(); - } - } + for (int8_t i = X_AXIS; i <= Z_AXIS; i++) + if (code_seen(axis_codes[i])) + set_home_offset((AxisEnum)i, code_value()); + #if ENABLED(SCARA) - if (code_seen('T')) home_offset[X_AXIS] = code_value(); // Theta - if (code_seen('P')) home_offset[Y_AXIS] = code_value(); // Psi + if (code_seen('T')) set_home_offset(X_AXIS, code_value()); // Theta + if (code_seen('P')) set_home_offset(Y_AXIS, code_value()); // Psi #endif + sync_plan_position(); } #if ENABLED(DELTA) @@ -5685,16 +5701,12 @@ inline void gcode_M410() { quickStop(); } */ inline void gcode_M428() { bool err = false; - float new_offs[3], new_pos[3]; - memcpy(new_pos, current_position, sizeof(new_pos)); - memcpy(new_offs, home_offset, sizeof(new_offs)); for (int8_t i = X_AXIS; i <= Z_AXIS; i++) { if (axis_homed[i]) { - float base = (new_pos[i] > (min_pos[i] + max_pos[i]) / 2) ? base_home_pos(i) : 0, - diff = new_pos[i] - base; + float base = (current_position[i] > (min_pos[i] + max_pos[i]) / 2) ? base_home_pos(i) : 0, + diff = current_position[i] - base; if (diff > -20 && diff < 20) { - new_offs[i] -= diff; - new_pos[i] = base; + set_home_offset((AxisEnum)i, home_offset[i] - diff); } else { SERIAL_ERROR_START; @@ -5710,8 +5722,6 @@ inline void gcode_M428() { } if (!err) { - memcpy(current_position, new_pos, sizeof(new_pos)); - memcpy(home_offset, new_offs, sizeof(new_offs)); sync_plan_position(); LCD_ALERTMESSAGEPGM(MSG_HOME_OFFSETS_APPLIED); #if HAS_BUZZER @@ -6980,10 +6990,10 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_ set_current_to_destination(); return; } - int pix = mbl.select_x_index(current_position[X_AXIS]); - int piy = mbl.select_y_index(current_position[Y_AXIS]); - int ix = mbl.select_x_index(x); - int iy = mbl.select_y_index(y); + int pix = mbl.select_x_index(current_position[X_AXIS] - home_offset[X_AXIS]); + int piy = mbl.select_y_index(current_position[Y_AXIS] - home_offset[Y_AXIS]); + int ix = mbl.select_x_index(x - home_offset[X_AXIS]); + int iy = mbl.select_y_index(y - home_offset[Y_AXIS]); pix = min(pix, MESH_NUM_X_POINTS - 2); piy = min(piy, MESH_NUM_Y_POINTS - 2); ix = min(ix, MESH_NUM_X_POINTS - 2); @@ -6996,7 +7006,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_ } float nx, ny, nz, ne, normalized_dist; if (ix > pix && TEST(x_splits, ix)) { - nx = mbl.get_x(ix); + nx = mbl.get_x(ix) + home_offset[X_AXIS]; normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]); ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist; nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist; @@ -7004,7 +7014,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_ CBI(x_splits, ix); } else if (ix < pix && TEST(x_splits, pix)) { - nx = mbl.get_x(pix); + nx = mbl.get_x(pix) + home_offset[X_AXIS]; normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]); ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist; nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist; @@ -7012,7 +7022,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_ CBI(x_splits, pix); } else if (iy > piy && TEST(y_splits, iy)) { - ny = mbl.get_y(iy); + ny = mbl.get_y(iy) + home_offset[Y_AXIS]; normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]); nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist; nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist; @@ -7020,7 +7030,7 @@ void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_ CBI(y_splits, iy); } else if (iy < piy && TEST(y_splits, piy)) { - ny = mbl.get_y(piy); + ny = mbl.get_y(piy) + home_offset[Y_AXIS]; normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]); nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist; nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist; diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index e9b7c84390..ae2b66d00c 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -568,7 +568,7 @@ float junction_deviation = 0.1; while (block_buffer_tail == next_buffer_head) idle(); #if ENABLED(MESH_BED_LEVELING) - if (mbl.active) z += mbl.get_z(x, y); + if (mbl.active) z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]); #elif ENABLED(AUTO_BED_LEVELING_FEATURE) apply_rotation_xyz(plan_bed_level_matrix, x, y, z); #endif @@ -1123,7 +1123,7 @@ float junction_deviation = 0.1; #endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING { #if ENABLED(MESH_BED_LEVELING) - if (mbl.active) z += mbl.get_z(x, y); + if (mbl.active) z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]); #elif ENABLED(AUTO_BED_LEVELING_FEATURE) apply_rotation_xyz(plan_bed_level_matrix, x, y, z); #endif