|
@@ -498,6 +498,11 @@ void gcode_M114();
|
498
|
498
|
void print_xyz(const char* prefix, const float xyz[]) {
|
499
|
499
|
print_xyz(prefix, xyz[X_AXIS], xyz[Y_AXIS], xyz[Z_AXIS]);
|
500
|
500
|
}
|
|
501
|
+ #if ENABLED(AUTO_BED_LEVELING_FEATURE)
|
|
502
|
+ void print_xyz(const char* prefix, const vector_3 &xyz) {
|
|
503
|
+ print_xyz(prefix, xyz.x, xyz.y, xyz.z);
|
|
504
|
+ }
|
|
505
|
+ #endif
|
501
|
506
|
#define DEBUG_POS(PREFIX,VAR) do{ SERIAL_ECHOPGM(PREFIX); print_xyz(" > " STRINGIFY(VAR), VAR); }while(0)
|
502
|
507
|
#endif
|
503
|
508
|
|
|
@@ -1408,6 +1413,11 @@ static void setup_for_endstop_move() {
|
1408
|
1413
|
|
1409
|
1414
|
static void set_bed_level_equation_lsq(double* plane_equation_coefficients) {
|
1410
|
1415
|
|
|
1416
|
+ vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
|
|
1417
|
+ plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
|
1418
|
+
|
|
1419
|
+ //plan_bed_level_matrix.debug("bed level before");
|
|
1420
|
+
|
1411
|
1421
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
1412
|
1422
|
plan_bed_level_matrix.set_to_identity();
|
1413
|
1423
|
if (DEBUGGING(LEVELING)) {
|
|
@@ -1417,17 +1427,8 @@ static void setup_for_endstop_move() {
|
1417
|
1427
|
}
|
1418
|
1428
|
#endif
|
1419
|
1429
|
|
1420
|
|
- vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
|
1421
|
|
- // planeNormal.debug("planeNormal");
|
1422
|
|
- plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
1423
|
|
- //bedLevel.debug("bedLevel");
|
1424
|
|
-
|
1425
|
|
- //plan_bed_level_matrix.debug("bed level before");
|
1426
|
|
- //vector_3 uncorrected_position = plan_get_position();
|
1427
|
|
- //uncorrected_position.debug("position before");
|
1428
|
|
-
|
1429
|
1430
|
vector_3 corrected_position = plan_get_position();
|
1430
|
|
- //corrected_position.debug("position after");
|
|
1431
|
+
|
1431
|
1432
|
current_position[X_AXIS] = corrected_position.x;
|
1432
|
1433
|
current_position[Y_AXIS] = corrected_position.y;
|
1433
|
1434
|
current_position[Z_AXIS] = corrected_position.z;
|
|
@@ -1461,12 +1462,20 @@ static void setup_for_endstop_move() {
|
1461
|
1462
|
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
1462
|
1463
|
|
1463
|
1464
|
vector_3 corrected_position = plan_get_position();
|
|
1465
|
+
|
|
1466
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
1467
|
+ if (DEBUGGING(LEVELING)) {
|
|
1468
|
+ vector_3 uncorrected_position = corrected_position;
|
|
1469
|
+ DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position);
|
|
1470
|
+ }
|
|
1471
|
+ #endif
|
|
1472
|
+
|
1464
|
1473
|
current_position[X_AXIS] = corrected_position.x;
|
1465
|
1474
|
current_position[Y_AXIS] = corrected_position.y;
|
1466
|
1475
|
current_position[Z_AXIS] = corrected_position.z;
|
1467
|
1476
|
|
1468
|
1477
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
1469
|
|
- if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", current_position);
|
|
1478
|
+ if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", corrected_position);
|
1470
|
1479
|
#endif
|
1471
|
1480
|
|
1472
|
1481
|
sync_plan_position();
|