Skip to content

Commit 507c122

Browse files
thinkyheadthinkyhead
committed
♻️ Refactor UBL 'G29 J'
1 parent 51d3eab commit 507c122

File tree

1 file changed

+66
-104
lines changed

1 file changed

+66
-104
lines changed

‎Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp

+66-104
Original file line numberDiff line numberDiff line change
@@ -1479,82 +1479,33 @@ void unified_bed_leveling::smart_fill_mesh() {
14791479

14801480
void unified_bed_leveling::tilt_mesh_based_on_probed_grid(const bool do_3_pt_leveling) {
14811481

1482-
#ifdef G29J_MESH_TILT_MARGIN
1483-
const float x_min = _MAX(probe.min_x() + (G29J_MESH_TILT_MARGIN), X_MIN_POS),
1484-
x_max = _MIN(probe.max_x() - (G29J_MESH_TILT_MARGIN), X_MAX_POS),
1485-
y_min = _MAX(probe.min_y() + (G29J_MESH_TILT_MARGIN), Y_MIN_POS),
1486-
y_max = _MIN(probe.max_y() - (G29J_MESH_TILT_MARGIN), Y_MAX_POS);
1487-
#else
1488-
const float x_min = probe.min_x(), x_max = probe.max_x(),
1489-
y_min = probe.min_y(), y_max = probe.max_y();
1490-
#endif
1491-
const float dx = (x_max - x_min) / (param.J_grid_size - 1),
1492-
dy = (y_max - y_min) / (param.J_grid_size - 1);
1493-
1494-
xy_float_t points[3];
1495-
probe.get_three_points(points);
1496-
14971482
float measured_z;
14981483
bool abort_flag = false;
14991484

1500-
#if ENABLED(VALIDATE_MESH_TILT)
1501-
float z1, z2, z3; // Needed for algorithm validation below
1502-
#endif
1503-
15041485
struct linear_fit_data lsf_results;
15051486
incremental_LSF_reset(&lsf_results);
15061487

15071488
if (do_3_pt_leveling) {
1508-
SERIAL_ECHOLNPGM("Tilting mesh (1/3)");
1509-
TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " 1/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
1489+
xy_float_t points[3];
1490+
probe.get_three_points(points);
15101491

1511-
measured_z = probe.probe_at_point(points[0], PROBE_PT_RAISE, param.V_verbosity);
1512-
if (isnan(measured_z))
1513-
abort_flag = true;
1514-
else {
1515-
measured_z -= get_z_correction(points[0]);
1516-
TERN_(VALIDATE_MESH_TILT, z1 = measured_z);
1517-
if (param.V_verbosity > 3) {
1518-
serial_spaces(16);
1519-
SERIAL_ECHOLNPGM("Corrected_Z=", measured_z);
1520-
}
1521-
incremental_LSF(&lsf_results, points[0], measured_z);
1522-
}
1492+
#if ENABLED(VALIDATE_MESH_TILT)
1493+
float gotz[3]; // Used for algorithm validation below
1494+
#endif
15231495

1524-
if (!abort_flag) {
1525-
SERIAL_ECHOLNPGM("Tilting mesh (2/3)");
1526-
TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " 2/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
1496+
LOOP_L_N(i, 3) {
1497+
SERIAL_ECHOLNPGM("Tilting mesh (", i + 1, "/3)");
1498+
TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/3"), i + 1, GET_TEXT(MSG_LCD_TILTING_MESH)));
15271499

1528-
measured_z = probe.probe_at_point(points[1], PROBE_PT_RAISE, param.V_verbosity);
1529-
TERN_(VALIDATE_MESH_TILT, z2 = measured_z);
1530-
if (isnan(measured_z))
1531-
abort_flag = true;
1532-
else {
1533-
measured_z -= get_z_correction(points[1]);
1534-
if (param.V_verbosity > 3) {
1535-
serial_spaces(16);
1536-
SERIAL_ECHOLNPGM("Corrected_Z=", measured_z);
1537-
}
1538-
incremental_LSF(&lsf_results, points[1], measured_z);
1539-
}
1540-
}
1500+
measured_z = probe.probe_at_point(points[i], i < 2 ? PROBE_PT_RAISE : PROBE_PT_LAST_STOW, param.V_verbosity);
1501+
if ((abort_flag = isnan(measured_z))) break;
15411502

1542-
if (!abort_flag) {
1543-
SERIAL_ECHOLNPGM("Tilting mesh (3/3)");
1544-
TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " 3/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
1503+
measured_z -= get_z_correction(points[i]);
1504+
TERN_(VALIDATE_MESH_TILT, gotz[i] = measured_z);
15451505

1546-
measured_z = probe.probe_at_point(points[2], PROBE_PT_LAST_STOW, param.V_verbosity);
1547-
TERN_(VALIDATE_MESH_TILT, z3 = measured_z);
1548-
if (isnan(measured_z))
1549-
abort_flag = true;
1550-
else {
1551-
measured_z -= get_z_correction(points[2]);
1552-
if (param.V_verbosity > 3) {
1553-
serial_spaces(16);
1554-
SERIAL_ECHOLNPGM("Corrected_Z=", measured_z);
1555-
}
1556-
incremental_LSF(&lsf_results, points[2], measured_z);
1557-
}
1506+
if (param.V_verbosity > 3) { serial_spaces(16); SERIAL_ECHOLNPGM("Corrected_Z=", measured_z); }
1507+
1508+
incremental_LSF(&lsf_results, points[i], measured_z);
15581509
}
15591510

15601511
probe.stow();
@@ -1567,54 +1518,62 @@ void unified_bed_leveling::smart_fill_mesh() {
15671518
}
15681519
else { // !do_3_pt_leveling
15691520

1521+
#ifdef G29J_MESH_TILT_MARGIN
1522+
const float x_min = _MAX(probe.min_x() + (G29J_MESH_TILT_MARGIN), X_MIN_POS),
1523+
x_max = _MIN(probe.max_x() - (G29J_MESH_TILT_MARGIN), X_MAX_POS),
1524+
y_min = _MAX(probe.min_y() + (G29J_MESH_TILT_MARGIN), Y_MIN_POS),
1525+
y_max = _MIN(probe.max_y() - (G29J_MESH_TILT_MARGIN), Y_MAX_POS);
1526+
#else
1527+
const float x_min = probe.min_x(), x_max = probe.max_x(),
1528+
y_min = probe.min_y(), y_max = probe.max_y();
1529+
#endif
1530+
const float dx = (x_max - x_min) / (param.J_grid_size - 1),
1531+
dy = (y_max - y_min) / (param.J_grid_size - 1);
1532+
15701533
bool zig_zag = false;
15711534

15721535
const uint16_t total_points = sq(param.J_grid_size);
15731536
uint16_t point_num = 1;
15741537

1575-
xy_pos_t rpos;
15761538
LOOP_L_N(ix, param.J_grid_size) {
1539+
xy_pos_t rpos;
15771540
rpos.x = x_min + ix * dx;
15781541
LOOP_L_N(iy, param.J_grid_size) {
15791542
rpos.y = y_min + dy * (zig_zag ? param.J_grid_size - 1 - iy : iy);
15801543

1581-
if (!abort_flag) {
1582-
SERIAL_ECHOLNPGM("Tilting mesh point ", point_num, "/", total_points, "\n");
1583-
TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/%i"), GET_TEXT(MSG_LCD_TILTING_MESH), point_num, total_points));
1584-
1585-
measured_z = probe.probe_at_point(rpos, parser.seen_test('E') ? PROBE_PT_STOW : PROBE_PT_RAISE, param.V_verbosity); // TODO: Needs error handling
1586-
1587-
abort_flag = isnan(measured_z);
1588-
1589-
#if ENABLED(DEBUG_LEVELING_FEATURE)
1590-
if (DEBUGGING(LEVELING)) {
1591-
const xy_pos_t lpos = rpos.asLogical();
1592-
DEBUG_CHAR('(');
1593-
DEBUG_ECHO_F(rpos.x, 7);
1594-
DEBUG_CHAR(',');
1595-
DEBUG_ECHO_F(rpos.y, 7);
1596-
DEBUG_ECHOPAIR_F(") logical: (", lpos.x, 7);
1597-
DEBUG_CHAR(',');
1598-
DEBUG_ECHO_F(lpos.y, 7);
1599-
DEBUG_ECHOPAIR_F(") measured: ", measured_z, 7);
1600-
DEBUG_ECHOPAIR_F(" correction: ", get_z_correction(rpos), 7);
1601-
}
1602-
#endif
1544+
SERIAL_ECHOLNPGM("Tilting mesh point ", point_num, "/", total_points, "\n");
1545+
TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/%i"), GET_TEXT(MSG_LCD_TILTING_MESH), point_num, total_points));
16031546

1604-
measured_z -= get_z_correction(rpos) /* + probe.offset.z */ ;
1547+
measured_z = probe.probe_at_point(rpos, parser.seen_test('E') ? PROBE_PT_STOW : PROBE_PT_RAISE, param.V_verbosity); // TODO: Needs error handling
16051548

1606-
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR_F(" final >>>---> ", measured_z, 7);
1549+
if ((abort_flag = isnan(measured_z))) break;
16071550

1608-
if (param.V_verbosity > 3) {
1609-
serial_spaces(16);
1610-
SERIAL_ECHOLNPGM("Corrected_Z=", measured_z);
1551+
const float zcorr = get_z_correction(rpos);
1552+
1553+
#if ENABLED(DEBUG_LEVELING_FEATURE)
1554+
if (DEBUGGING(LEVELING)) {
1555+
const xy_pos_t lpos = rpos.asLogical();
1556+
DEBUG_CHAR('('); DEBUG_ECHO_F(rpos.x, 7); DEBUG_CHAR(','); DEBUG_ECHO_F(rpos.y, 7);
1557+
DEBUG_ECHOPAIR_F(") logical: (", lpos.x, 7); DEBUG_CHAR(','); DEBUG_ECHO_F(lpos.y, 7);
1558+
DEBUG_ECHOPAIR_F(") measured: ", measured_z, 7);
1559+
DEBUG_ECHOPAIR_F(" correction: ", zcorr, 7);
16111560
}
1612-
incremental_LSF(&lsf_results, rpos, measured_z);
1561+
#endif
1562+
1563+
measured_z -= zcorr;
1564+
1565+
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR_F(" final >>>---> ", measured_z, 7);
1566+
1567+
if (param.V_verbosity > 3) {
1568+
serial_spaces(16);
1569+
SERIAL_ECHOLNPGM("Corrected_Z=", measured_z);
16131570
}
1571+
incremental_LSF(&lsf_results, rpos, measured_z);
16141572

16151573
point_num++;
16161574
}
16171575

1576+
if (abort_flag) break;
16181577
zig_zag ^= true;
16191578
}
16201579
}
@@ -1696,20 +1655,23 @@ void unified_bed_leveling::smart_fill_mesh() {
16961655
auto normed = [&](const xy_pos_t &pos, const_float_t zadd) {
16971656
return normal.x * pos.x + normal.y * pos.y + zadd;
16981657
};
1699-
auto debug_pt = [](FSTR_P const pre, const xy_pos_t &pos, const_float_t zadd) {
1700-
d_from(); SERIAL_ECHOF(pre);
1658+
auto debug_pt = [](const int num, const xy_pos_t &pos, const_float_t zadd) {
1659+
d_from(); DEBUG_ECHOPGM("Point ", num, ":");
17011660
DEBUG_ECHO_F(normed(pos, zadd), 6);
17021661
DEBUG_ECHOLNPAIR_F(" Z error = ", zadd - get_z_correction(pos), 6);
17031662
};
1704-
debug_pt(F("1st point: "), probe_pt[0], normal.z * z1);
1705-
debug_pt(F("2nd point: "), probe_pt[1], normal.z * z2);
1706-
debug_pt(F("3rd point: "), probe_pt[2], normal.z * z3);
1707-
d_from(); DEBUG_ECHOPGM("safe home with Z=");
1708-
DEBUG_ECHOLNPAIR_F("0 : ", normed(safe_homing_xy, 0), 6);
1709-
d_from(); DEBUG_ECHOPGM("safe home with Z=");
1710-
DEBUG_ECHOLNPAIR_F("mesh value ", normed(safe_homing_xy, get_z_correction(safe_homing_xy)), 6);
1711-
DEBUG_ECHOPGM(" Z error = (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT);
1712-
DEBUG_ECHOLNPAIR_F(") = ", get_z_correction(safe_homing_xy), 6);
1663+
debug_pt(1, probe_pt[0], normal.z * gotz[0]);
1664+
debug_pt(2, probe_pt[1], normal.z * gotz[1]);
1665+
debug_pt(3, probe_pt[2], normal.z * gotz[2]);
1666+
#if ENABLED(Z_SAFE_HOMING)
1667+
constexpr xy_float_t safe_xy = { Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT };
1668+
d_from(); DEBUG_ECHOPGM("safe home with Z=");
1669+
DEBUG_ECHOLNPAIR_F("0 : ", normed(safe_xy, 0), 6);
1670+
d_from(); DEBUG_ECHOPGM("safe home with Z=");
1671+
DEBUG_ECHOLNPAIR_F("mesh value ", normed(safe_xy, get_z_correction(safe_xy)), 6);
1672+
DEBUG_ECHOPGM(" Z error = (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT);
1673+
DEBUG_ECHOLNPAIR_F(") = ", get_z_correction(safe_xy), 6);
1674+
#endif
17131675
#endif
17141676
} // DEBUGGING(LEVELING)
17151677

0 commit comments

Comments
 (0)