|
@@ -4332,45 +4332,34 @@ inline void gcode_G28() {
|
4332
|
4332
|
// Correct the current XYZ position based on the tilted plane.
|
4333
|
4333
|
//
|
4334
|
4334
|
|
4335
|
|
- // 1. Get the distance from the current position to the reference point.
|
4336
|
|
- float x_dist = RAW_CURRENT_POSITION(X_AXIS) - X_TILT_FULCRUM,
|
4337
|
|
- y_dist = RAW_CURRENT_POSITION(Y_AXIS) - Y_TILT_FULCRUM,
|
4338
|
|
- z_real = current_position[Z_AXIS],
|
4339
|
|
- z_zero = 0;
|
4340
|
|
-
|
4341
|
4335
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
4342
|
4336
|
if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position);
|
4343
|
4337
|
#endif
|
4344
|
4338
|
|
4345
|
|
- matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix);
|
4346
|
|
-
|
4347
|
|
- // 2. Apply the inverse matrix to the distance
|
4348
|
|
- // from the reference point to X, Y, and zero.
|
4349
|
|
- apply_rotation_xyz(inverse, x_dist, y_dist, z_zero);
|
|
4339
|
+ float converted[XYZ];
|
|
4340
|
+ memcpy(converted, current_position, sizeof(converted));
|
4350
|
4341
|
|
4351
|
|
- // 3. Get the matrix-based corrected Z.
|
4352
|
|
- // (Even if not used, get it for comparison.)
|
4353
|
|
- float new_z = z_real + z_zero;
|
|
4342
|
+ planner.abl_enabled = true;
|
|
4343
|
+ planner.unapply_leveling(converted); // use conversion machinery
|
|
4344
|
+ planner.abl_enabled = false;
|
4354
|
4345
|
|
4355
|
|
- // 4. Use the last measured distance to the bed, if possible
|
|
4346
|
+ // Use the last measured distance to the bed, if possible
|
4356
|
4347
|
if ( NEAR(current_position[X_AXIS], xProbe - (X_PROBE_OFFSET_FROM_EXTRUDER))
|
4357
|
4348
|
&& NEAR(current_position[Y_AXIS], yProbe - (Y_PROBE_OFFSET_FROM_EXTRUDER))
|
4358
|
4349
|
) {
|
4359
|
|
- float simple_z = z_real - (measured_z - (-zprobe_zoffset));
|
|
4350
|
+ float simple_z = current_position[Z_AXIS] - (measured_z - (-zprobe_zoffset));
|
4360
|
4351
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
4361
|
4352
|
if (DEBUGGING(LEVELING)) {
|
4362
|
4353
|
SERIAL_ECHOPAIR("Z from Probe:", simple_z);
|
4363
|
|
- SERIAL_ECHOPAIR(" Matrix:", new_z);
|
4364
|
|
- SERIAL_ECHOLNPAIR(" Discrepancy:", simple_z - new_z);
|
|
4354
|
+ SERIAL_ECHOPAIR(" Matrix:", converted[Z_AXIS]);
|
|
4355
|
+ SERIAL_ECHOLNPAIR(" Discrepancy:", simple_z - converted[Z_AXIS]);
|
4365
|
4356
|
}
|
4366
|
4357
|
#endif
|
4367
|
|
- new_z = simple_z;
|
|
4358
|
+ converted[Z_AXIS] = simple_z;
|
4368
|
4359
|
}
|
4369
|
4360
|
|
4370
|
|
- // 5. The rotated XY and corrected Z are now current_position
|
4371
|
|
- current_position[X_AXIS] = LOGICAL_X_POSITION(x_dist) + X_TILT_FULCRUM;
|
4372
|
|
- current_position[Y_AXIS] = LOGICAL_Y_POSITION(y_dist) + Y_TILT_FULCRUM;
|
4373
|
|
- current_position[Z_AXIS] = new_z;
|
|
4361
|
+ // The rotated XY and corrected Z are now current_position
|
|
4362
|
+ memcpy(current_position, converted, sizeof(converted));
|
4374
|
4363
|
|
4375
|
4364
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
4376
|
4365
|
if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
|