|
@@ -496,6 +496,11 @@ void gcode_M114();
|
496
|
496
|
void print_xyz(const char* prefix, const float xyz[]) {
|
497
|
497
|
print_xyz(prefix, xyz[X_AXIS], xyz[Y_AXIS], xyz[Z_AXIS]);
|
498
|
498
|
}
|
|
499
|
+ #if ENABLED(AUTO_BED_LEVELING_FEATURE)
|
|
500
|
+ void print_xyz(const char* prefix, const vector_3 &xyz) {
|
|
501
|
+ print_xyz(prefix, xyz.x, xyz.y, xyz.z);
|
|
502
|
+ }
|
|
503
|
+ #endif
|
499
|
504
|
#define DEBUG_POS(PREFIX,VAR) do{ SERIAL_ECHOPGM(PREFIX); print_xyz(" > " STRINGIFY(VAR), VAR); }while(0)
|
500
|
505
|
#endif
|
501
|
506
|
|
|
@@ -1373,6 +1378,11 @@ static void setup_for_endstop_move() {
|
1373
|
1378
|
|
1374
|
1379
|
static void set_bed_level_equation_lsq(double* plane_equation_coefficients) {
|
1375
|
1380
|
|
|
1381
|
+ vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
|
|
1382
|
+ plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
|
1383
|
+
|
|
1384
|
+ //plan_bed_level_matrix.debug("bed level before");
|
|
1385
|
+
|
1376
|
1386
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
1377
|
1387
|
plan_bed_level_matrix.set_to_identity();
|
1378
|
1388
|
if (DEBUGGING(LEVELING)) {
|
|
@@ -1382,17 +1392,8 @@ static void setup_for_endstop_move() {
|
1382
|
1392
|
}
|
1383
|
1393
|
#endif
|
1384
|
1394
|
|
1385
|
|
- vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
|
1386
|
|
- // planeNormal.debug("planeNormal");
|
1387
|
|
- plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
1388
|
|
- //bedLevel.debug("bedLevel");
|
1389
|
|
-
|
1390
|
|
- //plan_bed_level_matrix.debug("bed level before");
|
1391
|
|
- //vector_3 uncorrected_position = plan_get_position();
|
1392
|
|
- //uncorrected_position.debug("position before");
|
1393
|
|
-
|
1394
|
1395
|
vector_3 corrected_position = plan_get_position();
|
1395
|
|
- //corrected_position.debug("position after");
|
|
1396
|
+
|
1396
|
1397
|
current_position[X_AXIS] = corrected_position.x;
|
1397
|
1398
|
current_position[Y_AXIS] = corrected_position.y;
|
1398
|
1399
|
current_position[Z_AXIS] = corrected_position.z;
|
|
@@ -1426,12 +1427,20 @@ static void setup_for_endstop_move() {
|
1426
|
1427
|
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
1427
|
1428
|
|
1428
|
1429
|
vector_3 corrected_position = plan_get_position();
|
|
1430
|
+
|
|
1431
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
1432
|
+ if (DEBUGGING(LEVELING)) {
|
|
1433
|
+ vector_3 uncorrected_position = corrected_position;
|
|
1434
|
+ DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position);
|
|
1435
|
+ }
|
|
1436
|
+ #endif
|
|
1437
|
+
|
1429
|
1438
|
current_position[X_AXIS] = corrected_position.x;
|
1430
|
1439
|
current_position[Y_AXIS] = corrected_position.y;
|
1431
|
1440
|
current_position[Z_AXIS] = corrected_position.z;
|
1432
|
1441
|
|
1433
|
1442
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
1434
|
|
- if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", current_position);
|
|
1443
|
+ if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", corrected_position);
|
1435
|
1444
|
#endif
|
1436
|
1445
|
|
1437
|
1446
|
sync_plan_position();
|