|
@@ -3518,23 +3518,21 @@ inline void gcode_G28() {
|
3518
|
3518
|
#endif
|
3519
|
3519
|
|
3520
|
3520
|
// Probe at 3 arbitrary points
|
3521
|
|
- float z_at_pt_1 = probe_pt( LOGICAL_X_POSITION(ABL_PROBE_PT_1_X),
|
3522
|
|
- LOGICAL_Y_POSITION(ABL_PROBE_PT_1_Y),
|
3523
|
|
- stow_probe_after_each, verbose_level),
|
3524
|
|
- z_at_pt_2 = probe_pt( LOGICAL_X_POSITION(ABL_PROBE_PT_2_X),
|
3525
|
|
- LOGICAL_Y_POSITION(ABL_PROBE_PT_2_Y),
|
3526
|
|
- stow_probe_after_each, verbose_level),
|
3527
|
|
- z_at_pt_3 = probe_pt( LOGICAL_X_POSITION(ABL_PROBE_PT_3_X),
|
3528
|
|
- LOGICAL_Y_POSITION(ABL_PROBE_PT_3_Y),
|
3529
|
|
- stow_probe_after_each, verbose_level);
|
3530
|
|
-
|
3531
|
|
- if (!dryrun) {
|
3532
|
|
- vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1),
|
3533
|
|
- pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2),
|
3534
|
|
- pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
|
|
3521
|
+ vector_3 points[3] = {
|
|
3522
|
+ vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, 0),
|
|
3523
|
+ vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, 0),
|
|
3524
|
+ vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, 0)
|
|
3525
|
+ };
|
3535
|
3526
|
|
3536
|
|
- vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal();
|
|
3527
|
+ for (uint8_t i = 0; i < 3; ++i)
|
|
3528
|
+ points[i].z = probe_pt(
|
|
3529
|
+ LOGICAL_X_POSITION(points[i].x),
|
|
3530
|
+ LOGICAL_Y_POSITION(points[i].y),
|
|
3531
|
+ stow_probe_after_each, verbose_level
|
|
3532
|
+ );
|
3537
|
3533
|
|
|
3534
|
+ if (!dryrun) {
|
|
3535
|
+ vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
|
3538
|
3536
|
if (planeNormal.z < 0) {
|
3539
|
3537
|
planeNormal.x *= -1;
|
3540
|
3538
|
planeNormal.y *= -1;
|