|
@@ -2222,7 +2222,14 @@ static void clean_up_after_endstop_or_probe_move() {
|
2222
|
2222
|
return false;
|
2223
|
2223
|
}
|
2224
|
2224
|
|
2225
|
|
- static bool do_probe_move(float z, float fr_mm_m) {
|
|
2225
|
+ /**
|
|
2226
|
+ * @brief Used by run_z_probe to do a single Z probe move.
|
|
2227
|
+ *
|
|
2228
|
+ * @param z Z destination
|
|
2229
|
+ * @param fr_mm_s Feedrate in mm/s
|
|
2230
|
+ * @return true to indicate an error
|
|
2231
|
+ */
|
|
2232
|
+ static bool do_probe_move(const float z, const float fr_mm_m) {
|
2226
|
2233
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
2227
|
2234
|
if (DEBUGGING(LEVELING)) DEBUG_POS(">>> do_probe_move", current_position);
|
2228
|
2235
|
#endif
|
|
@@ -2241,7 +2248,7 @@ static void clean_up_after_endstop_or_probe_move() {
|
2241
|
2248
|
|
2242
|
2249
|
// Check to see if the probe was triggered
|
2243
|
2250
|
const bool probe_triggered = TEST(Endstops::endstop_hit_bits,
|
2244
|
|
- #ifdef Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN
|
|
2251
|
+ #if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
|
2245
|
2252
|
Z_MIN
|
2246
|
2253
|
#else
|
2247
|
2254
|
Z_MIN_PROBE
|
|
@@ -2273,9 +2280,14 @@ static void clean_up_after_endstop_or_probe_move() {
|
2273
|
2280
|
return !probe_triggered;
|
2274
|
2281
|
}
|
2275
|
2282
|
|
2276
|
|
- // Do a single Z probe and return with current_position[Z_AXIS]
|
2277
|
|
- // at the height where the probe triggered.
|
2278
|
|
- static float run_z_probe(bool printable=true) {
|
|
2283
|
+ /**
|
|
2284
|
+ * @details Used by probe_pt to do a single Z probe.
|
|
2285
|
+ * Leaves current_position[Z_AXIS] at the height where the probe triggered.
|
|
2286
|
+ *
|
|
2287
|
+ * @param short_move Flag for a shorter probe move towards the bed
|
|
2288
|
+ * @return The raw Z position where the probe was triggered
|
|
2289
|
+ */
|
|
2290
|
+ static float run_z_probe(const bool short_move=true) {
|
2279
|
2291
|
|
2280
|
2292
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
2281
|
2293
|
if (DEBUGGING(LEVELING)) DEBUG_POS(">>> run_z_probe", current_position);
|
|
@@ -2313,7 +2325,7 @@ static void clean_up_after_endstop_or_probe_move() {
|
2313
|
2325
|
#endif
|
2314
|
2326
|
|
2315
|
2327
|
// move down slowly to find bed
|
2316
|
|
- if (do_probe_move(-10 + (printable ? 0 : -(Z_MAX_LENGTH)), Z_PROBE_SPEED_SLOW)) return NAN;
|
|
2328
|
+ if (do_probe_move(-10 + (short_move ? 0 : -(Z_MAX_LENGTH)), Z_PROBE_SPEED_SLOW)) return NAN;
|
2317
|
2329
|
|
2318
|
2330
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
2319
|
2331
|
if (DEBUGGING(LEVELING)) DEBUG_POS("<<< run_z_probe", current_position);
|
|
@@ -2410,6 +2422,12 @@ static void clean_up_after_endstop_or_probe_move() {
|
2410
|
2422
|
|
2411
|
2423
|
feedrate_mm_s = old_feedrate_mm_s;
|
2412
|
2424
|
|
|
2425
|
+ if (isnan(measured_z)) {
|
|
2426
|
+ LCD_MESSAGEPGM(MSG_ERR_PROBING_FAILED);
|
|
2427
|
+ SERIAL_ERROR_START();
|
|
2428
|
+ SERIAL_ERRORLNPGM(MSG_ERR_PROBING_FAILED);
|
|
2429
|
+ }
|
|
2430
|
+
|
2413
|
2431
|
return measured_z;
|
2414
|
2432
|
}
|
2415
|
2433
|
|
|
@@ -3775,9 +3793,7 @@ inline void gcode_G4() {
|
3775
|
3793
|
// If an endstop was not hit, then damage can occur if homing is continued.
|
3776
|
3794
|
// This can occur if the delta height (DELTA_HEIGHT + home_offset[Z_AXIS]) is
|
3777
|
3795
|
// not set correctly.
|
3778
|
|
- if (!(TEST(Endstops::endstop_hit_bits, X_MAX) ||
|
3779
|
|
- TEST(Endstops::endstop_hit_bits, Y_MAX) ||
|
3780
|
|
- TEST(Endstops::endstop_hit_bits, Z_MAX))) {
|
|
3796
|
+ if (!(Endstops::endstop_hit_bits & (_BV(X_MAX) | _BV(Y_MAX) | _BV(Z_MAX)))) {
|
3781
|
3797
|
LCD_MESSAGEPGM(MSG_ERR_HOMING_FAILED);
|
3782
|
3798
|
SERIAL_ERROR_START();
|
3783
|
3799
|
SERIAL_ERRORLNPGM(MSG_ERR_HOMING_FAILED);
|
|
@@ -4126,20 +4142,6 @@ void home_all_axes() { gcode_G28(true); }
|
4126
|
4142
|
|
4127
|
4143
|
#endif
|
4128
|
4144
|
|
4129
|
|
-#if HAS_BED_PROBE
|
4130
|
|
-
|
4131
|
|
- static bool nan_error(const float v) {
|
4132
|
|
- const bool is_nan = isnan(v);
|
4133
|
|
- if (is_nan) {
|
4134
|
|
- LCD_MESSAGEPGM(MSG_ERR_PROBING_FAILED);
|
4135
|
|
- SERIAL_ERROR_START();
|
4136
|
|
- SERIAL_ERRORLNPGM(MSG_ERR_PROBING_FAILED);
|
4137
|
|
- }
|
4138
|
|
- return is_nan;
|
4139
|
|
- }
|
4140
|
|
-
|
4141
|
|
-#endif // HAS_BED_PROBE
|
4142
|
|
-
|
4143
|
4145
|
#if ENABLED(MESH_BED_LEVELING)
|
4144
|
4146
|
|
4145
|
4147
|
// Save 130 bytes with non-duplication of PSTR
|
|
@@ -4675,18 +4677,16 @@ void home_all_axes() { gcode_G28(true); }
|
4675
|
4677
|
SYNC_PLAN_POSITION_KINEMATIC();
|
4676
|
4678
|
}
|
4677
|
4679
|
|
4678
|
|
- if (!faux) setup_for_endstop_or_probe_move();
|
4679
|
|
-
|
4680
|
|
- //xProbe = yProbe = measured_z = 0;
|
4681
|
|
-
|
4682
|
4680
|
#if HAS_BED_PROBE
|
4683
|
4681
|
// Deploy the probe. Probe will raise if needed.
|
4684
|
4682
|
if (DEPLOY_PROBE()) {
|
4685
|
4683
|
planner.abl_enabled = abl_should_enable;
|
4686
|
|
- goto FAIL;
|
|
4684
|
+ return;
|
4687
|
4685
|
}
|
4688
|
4686
|
#endif
|
4689
|
4687
|
|
|
4688
|
+ if (!faux) setup_for_endstop_or_probe_move();
|
|
4689
|
+
|
4690
|
4690
|
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
4691
|
4691
|
|
4692
|
4692
|
#if ENABLED(PROBE_MANUALLY)
|
|
@@ -4907,7 +4907,7 @@ void home_all_axes() { gcode_G28(true); }
|
4907
|
4907
|
bool zig = PR_OUTER_END & 1; // Always end at RIGHT and BACK_PROBE_BED_POSITION
|
4908
|
4908
|
|
4909
|
4909
|
// Outer loop is Y with PROBE_Y_FIRST disabled
|
4910
|
|
- for (uint8_t PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END; PR_OUTER_VAR++) {
|
|
4910
|
+ for (uint8_t PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END && !isnan(measured_z); PR_OUTER_VAR++) {
|
4911
|
4911
|
|
4912
|
4912
|
int8_t inStart, inStop, inInc;
|
4913
|
4913
|
|
|
@@ -4944,9 +4944,9 @@ void home_all_axes() { gcode_G28(true); }
|
4944
|
4944
|
|
4945
|
4945
|
measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
|
4946
|
4946
|
|
4947
|
|
- if (nan_error(measured_z)) {
|
|
4947
|
+ if (isnan(measured_z)) {
|
4948
|
4948
|
planner.abl_enabled = abl_should_enable;
|
4949
|
|
- goto FAIL;
|
|
4949
|
+ break;
|
4950
|
4950
|
}
|
4951
|
4951
|
|
4952
|
4952
|
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
@@ -4980,14 +4980,14 @@ void home_all_axes() { gcode_G28(true); }
|
4980
|
4980
|
xProbe = LOGICAL_X_POSITION(points[i].x);
|
4981
|
4981
|
yProbe = LOGICAL_Y_POSITION(points[i].y);
|
4982
|
4982
|
measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
|
4983
|
|
- if (nan_error(measured_z)) {
|
|
4983
|
+ if (isnan(measured_z)) {
|
4984
|
4984
|
planner.abl_enabled = abl_should_enable;
|
4985
|
|
- goto FAIL;
|
|
4985
|
+ break;
|
4986
|
4986
|
}
|
4987
|
4987
|
points[i].z = measured_z;
|
4988
|
4988
|
}
|
4989
|
4989
|
|
4990
|
|
- if (!dryrun) {
|
|
4990
|
+ if (!dryrun && !isnan(measured_z)) {
|
4991
|
4991
|
vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
|
4992
|
4992
|
if (planeNormal.z < 0) {
|
4993
|
4993
|
planeNormal.x *= -1;
|
|
@@ -5005,7 +5005,7 @@ void home_all_axes() { gcode_G28(true); }
|
5005
|
5005
|
// Raise to _Z_CLEARANCE_DEPLOY_PROBE. Stow the probe.
|
5006
|
5006
|
if (STOW_PROBE()) {
|
5007
|
5007
|
planner.abl_enabled = abl_should_enable;
|
5008
|
|
- goto FAIL;
|
|
5008
|
+ measured_z = NAN;
|
5009
|
5009
|
}
|
5010
|
5010
|
}
|
5011
|
5011
|
#endif // !PROBE_MANUALLY
|
|
@@ -5032,114 +5032,91 @@ void home_all_axes() { gcode_G28(true); }
|
5032
|
5032
|
#endif
|
5033
|
5033
|
|
5034
|
5034
|
// Calculate leveling, print reports, correct the position
|
5035
|
|
- #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
|
5035
|
+ if (!isnan(measured_z)) {
|
|
5036
|
+ #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
5036
|
5037
|
|
5037
|
|
- if (!dryrun) extrapolate_unprobed_bed_level();
|
5038
|
|
- print_bilinear_leveling_grid();
|
|
5038
|
+ if (!dryrun) extrapolate_unprobed_bed_level();
|
|
5039
|
+ print_bilinear_leveling_grid();
|
5039
|
5040
|
|
5040
|
|
- refresh_bed_level();
|
|
5041
|
+ refresh_bed_level();
|
5041
|
5042
|
|
5042
|
|
- #if ENABLED(ABL_BILINEAR_SUBDIVISION)
|
5043
|
|
- bed_level_virt_print();
|
5044
|
|
- #endif
|
|
5043
|
+ #if ENABLED(ABL_BILINEAR_SUBDIVISION)
|
|
5044
|
+ bed_level_virt_print();
|
|
5045
|
+ #endif
|
5045
|
5046
|
|
5046
|
|
- #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
5047
|
+ #elif ENABLED(AUTO_BED_LEVELING_LINEAR)
|
5047
|
5048
|
|
5048
|
|
- // For LINEAR leveling calculate matrix, print reports, correct the position
|
|
5049
|
+ // For LINEAR leveling calculate matrix, print reports, correct the position
|
5049
|
5050
|
|
5050
|
|
- /**
|
5051
|
|
- * solve the plane equation ax + by + d = z
|
5052
|
|
- * A is the matrix with rows [x y 1] for all the probed points
|
5053
|
|
- * B is the vector of the Z positions
|
5054
|
|
- * the normal vector to the plane is formed by the coefficients of the
|
5055
|
|
- * plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
|
5056
|
|
- * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
|
5057
|
|
- */
|
5058
|
|
- float plane_equation_coefficients[3];
|
5059
|
|
-
|
5060
|
|
- finish_incremental_LSF(&lsf_results);
|
5061
|
|
- plane_equation_coefficients[0] = -lsf_results.A; // We should be able to eliminate the '-' on these three lines and down below
|
5062
|
|
- plane_equation_coefficients[1] = -lsf_results.B; // but that is not yet tested.
|
5063
|
|
- plane_equation_coefficients[2] = -lsf_results.D;
|
5064
|
|
-
|
5065
|
|
- mean /= abl2;
|
5066
|
|
-
|
5067
|
|
- if (verbose_level) {
|
5068
|
|
- SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
|
5069
|
|
- SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8);
|
5070
|
|
- SERIAL_PROTOCOLPGM(" b: ");
|
5071
|
|
- SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8);
|
5072
|
|
- SERIAL_PROTOCOLPGM(" d: ");
|
5073
|
|
- SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8);
|
5074
|
|
- SERIAL_EOL();
|
5075
|
|
- if (verbose_level > 2) {
|
5076
|
|
- SERIAL_PROTOCOLPGM("Mean of sampled points: ");
|
5077
|
|
- SERIAL_PROTOCOL_F(mean, 8);
|
|
5051
|
+ /**
|
|
5052
|
+ * solve the plane equation ax + by + d = z
|
|
5053
|
+ * A is the matrix with rows [x y 1] for all the probed points
|
|
5054
|
+ * B is the vector of the Z positions
|
|
5055
|
+ * the normal vector to the plane is formed by the coefficients of the
|
|
5056
|
+ * plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
|
|
5057
|
+ * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
|
|
5058
|
+ */
|
|
5059
|
+ float plane_equation_coefficients[3];
|
|
5060
|
+
|
|
5061
|
+ finish_incremental_LSF(&lsf_results);
|
|
5062
|
+ plane_equation_coefficients[0] = -lsf_results.A; // We should be able to eliminate the '-' on these three lines and down below
|
|
5063
|
+ plane_equation_coefficients[1] = -lsf_results.B; // but that is not yet tested.
|
|
5064
|
+ plane_equation_coefficients[2] = -lsf_results.D;
|
|
5065
|
+
|
|
5066
|
+ mean /= abl2;
|
|
5067
|
+
|
|
5068
|
+ if (verbose_level) {
|
|
5069
|
+ SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
|
|
5070
|
+ SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8);
|
|
5071
|
+ SERIAL_PROTOCOLPGM(" b: ");
|
|
5072
|
+ SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8);
|
|
5073
|
+ SERIAL_PROTOCOLPGM(" d: ");
|
|
5074
|
+ SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8);
|
5078
|
5075
|
SERIAL_EOL();
|
|
5076
|
+ if (verbose_level > 2) {
|
|
5077
|
+ SERIAL_PROTOCOLPGM("Mean of sampled points: ");
|
|
5078
|
+ SERIAL_PROTOCOL_F(mean, 8);
|
|
5079
|
+ SERIAL_EOL();
|
|
5080
|
+ }
|
5079
|
5081
|
}
|
5080
|
|
- }
|
5081
|
5082
|
|
5082
|
|
- // Create the matrix but don't correct the position yet
|
5083
|
|
- if (!dryrun)
|
5084
|
|
- planner.bed_level_matrix = matrix_3x3::create_look_at(
|
5085
|
|
- vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1) // We can eliminate the '-' here and up above
|
5086
|
|
- );
|
|
5083
|
+ // Create the matrix but don't correct the position yet
|
|
5084
|
+ if (!dryrun)
|
|
5085
|
+ planner.bed_level_matrix = matrix_3x3::create_look_at(
|
|
5086
|
+ vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1) // We can eliminate the '-' here and up above
|
|
5087
|
+ );
|
5087
|
5088
|
|
5088
|
|
- // Show the Topography map if enabled
|
5089
|
|
- if (do_topography_map) {
|
5090
|
|
-
|
5091
|
|
- SERIAL_PROTOCOLLNPGM("\nBed Height Topography:\n"
|
5092
|
|
- " +--- BACK --+\n"
|
5093
|
|
- " | |\n"
|
5094
|
|
- " L | (+) | R\n"
|
5095
|
|
- " E | | I\n"
|
5096
|
|
- " F | (-) N (+) | G\n"
|
5097
|
|
- " T | | H\n"
|
5098
|
|
- " | (-) | T\n"
|
5099
|
|
- " | |\n"
|
5100
|
|
- " O-- FRONT --+\n"
|
5101
|
|
- " (0,0)");
|
5102
|
|
-
|
5103
|
|
- float min_diff = 999;
|
5104
|
|
-
|
5105
|
|
- for (int8_t yy = abl_grid_points_y - 1; yy >= 0; yy--) {
|
5106
|
|
- for (uint8_t xx = 0; xx < abl_grid_points_x; xx++) {
|
5107
|
|
- int ind = indexIntoAB[xx][yy];
|
5108
|
|
- float diff = eqnBVector[ind] - mean,
|
5109
|
|
- x_tmp = eqnAMatrix[ind + 0 * abl2],
|
5110
|
|
- y_tmp = eqnAMatrix[ind + 1 * abl2],
|
5111
|
|
- z_tmp = 0;
|
5112
|
|
-
|
5113
|
|
- apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
|
5114
|
|
-
|
5115
|
|
- NOMORE(min_diff, eqnBVector[ind] - z_tmp);
|
5116
|
|
-
|
5117
|
|
- if (diff >= 0.0)
|
5118
|
|
- SERIAL_PROTOCOLPGM(" +"); // Include + for column alignment
|
5119
|
|
- else
|
5120
|
|
- SERIAL_PROTOCOLCHAR(' ');
|
5121
|
|
- SERIAL_PROTOCOL_F(diff, 5);
|
5122
|
|
- } // xx
|
5123
|
|
- SERIAL_EOL();
|
5124
|
|
- } // yy
|
5125
|
|
- SERIAL_EOL();
|
|
5089
|
+ // Show the Topography map if enabled
|
|
5090
|
+ if (do_topography_map) {
|
5126
|
5091
|
|
5127
|
|
- if (verbose_level > 3) {
|
5128
|
|
- SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:");
|
|
5092
|
+ SERIAL_PROTOCOLLNPGM("\nBed Height Topography:\n"
|
|
5093
|
+ " +--- BACK --+\n"
|
|
5094
|
+ " | |\n"
|
|
5095
|
+ " L | (+) | R\n"
|
|
5096
|
+ " E | | I\n"
|
|
5097
|
+ " F | (-) N (+) | G\n"
|
|
5098
|
+ " T | | H\n"
|
|
5099
|
+ " | (-) | T\n"
|
|
5100
|
+ " | |\n"
|
|
5101
|
+ " O-- FRONT --+\n"
|
|
5102
|
+ " (0,0)");
|
|
5103
|
+
|
|
5104
|
+ float min_diff = 999;
|
5129
|
5105
|
|
5130
|
5106
|
for (int8_t yy = abl_grid_points_y - 1; yy >= 0; yy--) {
|
5131
|
5107
|
for (uint8_t xx = 0; xx < abl_grid_points_x; xx++) {
|
5132
|
5108
|
int ind = indexIntoAB[xx][yy];
|
5133
|
|
- float x_tmp = eqnAMatrix[ind + 0 * abl2],
|
|
5109
|
+ float diff = eqnBVector[ind] - mean,
|
|
5110
|
+ x_tmp = eqnAMatrix[ind + 0 * abl2],
|
5134
|
5111
|
y_tmp = eqnAMatrix[ind + 1 * abl2],
|
5135
|
5112
|
z_tmp = 0;
|
5136
|
5113
|
|
5137
|
5114
|
apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
|
5138
|
5115
|
|
5139
|
|
- float diff = eqnBVector[ind] - z_tmp - min_diff;
|
|
5116
|
+ NOMORE(min_diff, eqnBVector[ind] - z_tmp);
|
|
5117
|
+
|
5140
|
5118
|
if (diff >= 0.0)
|
5141
|
|
- SERIAL_PROTOCOLPGM(" +");
|
5142
|
|
- // Include + for column alignment
|
|
5119
|
+ SERIAL_PROTOCOLPGM(" +"); // Include + for column alignment
|
5143
|
5120
|
else
|
5144
|
5121
|
SERIAL_PROTOCOLCHAR(' ');
|
5145
|
5122
|
SERIAL_PROTOCOL_F(diff, 5);
|
|
@@ -5147,87 +5124,110 @@ void home_all_axes() { gcode_G28(true); }
|
5147
|
5124
|
SERIAL_EOL();
|
5148
|
5125
|
} // yy
|
5149
|
5126
|
SERIAL_EOL();
|
5150
|
|
- }
|
5151
|
|
- } //do_topography_map
|
5152
|
5127
|
|
5153
|
|
- #endif // AUTO_BED_LEVELING_LINEAR
|
|
5128
|
+ if (verbose_level > 3) {
|
|
5129
|
+ SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:");
|
|
5130
|
+
|
|
5131
|
+ for (int8_t yy = abl_grid_points_y - 1; yy >= 0; yy--) {
|
|
5132
|
+ for (uint8_t xx = 0; xx < abl_grid_points_x; xx++) {
|
|
5133
|
+ int ind = indexIntoAB[xx][yy];
|
|
5134
|
+ float x_tmp = eqnAMatrix[ind + 0 * abl2],
|
|
5135
|
+ y_tmp = eqnAMatrix[ind + 1 * abl2],
|
|
5136
|
+ z_tmp = 0;
|
|
5137
|
+
|
|
5138
|
+ apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
|
|
5139
|
+
|
|
5140
|
+ float diff = eqnBVector[ind] - z_tmp - min_diff;
|
|
5141
|
+ if (diff >= 0.0)
|
|
5142
|
+ SERIAL_PROTOCOLPGM(" +");
|
|
5143
|
+ // Include + for column alignment
|
|
5144
|
+ else
|
|
5145
|
+ SERIAL_PROTOCOLCHAR(' ');
|
|
5146
|
+ SERIAL_PROTOCOL_F(diff, 5);
|
|
5147
|
+ } // xx
|
|
5148
|
+ SERIAL_EOL();
|
|
5149
|
+ } // yy
|
|
5150
|
+ SERIAL_EOL();
|
|
5151
|
+ }
|
|
5152
|
+ } //do_topography_map
|
5154
|
5153
|
|
5155
|
|
- #if ABL_PLANAR
|
|
5154
|
+ #endif // AUTO_BED_LEVELING_LINEAR
|
5156
|
5155
|
|
5157
|
|
- // For LINEAR and 3POINT leveling correct the current position
|
|
5156
|
+ #if ABL_PLANAR
|
5158
|
5157
|
|
5159
|
|
- if (verbose_level > 0)
|
5160
|
|
- planner.bed_level_matrix.debug(PSTR("\n\nBed Level Correction Matrix:"));
|
|
5158
|
+ // For LINEAR and 3POINT leveling correct the current position
|
5161
|
5159
|
|
5162
|
|
- if (!dryrun) {
|
5163
|
|
- //
|
5164
|
|
- // Correct the current XYZ position based on the tilted plane.
|
5165
|
|
- //
|
|
5160
|
+ if (verbose_level > 0)
|
|
5161
|
+ planner.bed_level_matrix.debug(PSTR("\n\nBed Level Correction Matrix:"));
|
5166
|
5162
|
|
5167
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
5168
|
|
- if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position);
|
5169
|
|
- #endif
|
|
5163
|
+ if (!dryrun) {
|
|
5164
|
+ //
|
|
5165
|
+ // Correct the current XYZ position based on the tilted plane.
|
|
5166
|
+ //
|
5170
|
5167
|
|
5171
|
|
- float converted[XYZ];
|
5172
|
|
- COPY(converted, current_position);
|
|
5168
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
5169
|
+ if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position);
|
|
5170
|
+ #endif
|
5173
|
5171
|
|
5174
|
|
- planner.abl_enabled = true;
|
5175
|
|
- planner.unapply_leveling(converted); // use conversion machinery
|
5176
|
|
- planner.abl_enabled = false;
|
|
5172
|
+ float converted[XYZ];
|
|
5173
|
+ COPY(converted, current_position);
|
|
5174
|
+
|
|
5175
|
+ planner.abl_enabled = true;
|
|
5176
|
+ planner.unapply_leveling(converted); // use conversion machinery
|
|
5177
|
+ planner.abl_enabled = false;
|
|
5178
|
+
|
|
5179
|
+ // Use the last measured distance to the bed, if possible
|
|
5180
|
+ if ( NEAR(current_position[X_AXIS], xProbe - (X_PROBE_OFFSET_FROM_EXTRUDER))
|
|
5181
|
+ && NEAR(current_position[Y_AXIS], yProbe - (Y_PROBE_OFFSET_FROM_EXTRUDER))
|
|
5182
|
+ ) {
|
|
5183
|
+ const float simple_z = current_position[Z_AXIS] - measured_z;
|
|
5184
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
5185
|
+ if (DEBUGGING(LEVELING)) {
|
|
5186
|
+ SERIAL_ECHOPAIR("Z from Probe:", simple_z);
|
|
5187
|
+ SERIAL_ECHOPAIR(" Matrix:", converted[Z_AXIS]);
|
|
5188
|
+ SERIAL_ECHOLNPAIR(" Discrepancy:", simple_z - converted[Z_AXIS]);
|
|
5189
|
+ }
|
|
5190
|
+ #endif
|
|
5191
|
+ converted[Z_AXIS] = simple_z;
|
|
5192
|
+ }
|
|
5193
|
+
|
|
5194
|
+ // The rotated XY and corrected Z are now current_position
|
|
5195
|
+ COPY(current_position, converted);
|
5177
|
5196
|
|
5178
|
|
- // Use the last measured distance to the bed, if possible
|
5179
|
|
- if ( NEAR(current_position[X_AXIS], xProbe - (X_PROBE_OFFSET_FROM_EXTRUDER))
|
5180
|
|
- && NEAR(current_position[Y_AXIS], yProbe - (Y_PROBE_OFFSET_FROM_EXTRUDER))
|
5181
|
|
- ) {
|
5182
|
|
- const float simple_z = current_position[Z_AXIS] - measured_z;
|
5183
|
5197
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
5184
|
|
- if (DEBUGGING(LEVELING)) {
|
5185
|
|
- SERIAL_ECHOPAIR("Z from Probe:", simple_z);
|
5186
|
|
- SERIAL_ECHOPAIR(" Matrix:", converted[Z_AXIS]);
|
5187
|
|
- SERIAL_ECHOLNPAIR(" Discrepancy:", simple_z - converted[Z_AXIS]);
|
5188
|
|
- }
|
|
5198
|
+ if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
|
5189
|
5199
|
#endif
|
5190
|
|
- converted[Z_AXIS] = simple_z;
|
5191
|
5200
|
}
|
5192
|
5201
|
|
5193
|
|
- // The rotated XY and corrected Z are now current_position
|
5194
|
|
- COPY(current_position, converted);
|
|
5202
|
+ #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
5195
|
5203
|
|
5196
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
5197
|
|
- if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
|
5198
|
|
- #endif
|
5199
|
|
- }
|
|
5204
|
+ if (!dryrun) {
|
|
5205
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
5206
|
+ if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("G29 uncorrected Z:", current_position[Z_AXIS]);
|
|
5207
|
+ #endif
|
5200
|
5208
|
|
5201
|
|
- #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
|
5209
|
+ // Unapply the offset because it is going to be immediately applied
|
|
5210
|
+ // and cause compensation movement in Z
|
|
5211
|
+ current_position[Z_AXIS] -= bilinear_z_offset(current_position);
|
5202
|
5212
|
|
5203
|
|
- if (!dryrun) {
|
5204
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
5205
|
|
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("G29 uncorrected Z:", current_position[Z_AXIS]);
|
5206
|
|
- #endif
|
|
5213
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
5214
|
+ if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR(" corrected Z:", current_position[Z_AXIS]);
|
|
5215
|
+ #endif
|
|
5216
|
+ }
|
5207
|
5217
|
|
5208
|
|
- // Unapply the offset because it is going to be immediately applied
|
5209
|
|
- // and cause compensation movement in Z
|
5210
|
|
- current_position[Z_AXIS] -= bilinear_z_offset(current_position);
|
|
5218
|
+ #endif // ABL_PLANAR
|
5211
|
5219
|
|
|
5220
|
+ #ifdef Z_PROBE_END_SCRIPT
|
5212
|
5221
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
5213
|
|
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR(" corrected Z:", current_position[Z_AXIS]);
|
|
5222
|
+ if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
|
5214
|
5223
|
#endif
|
5215
|
|
- }
|
5216
|
|
-
|
5217
|
|
- #endif // ABL_PLANAR
|
5218
|
|
-
|
5219
|
|
- #ifdef Z_PROBE_END_SCRIPT
|
5220
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
5221
|
|
- if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
|
|
5224
|
+ enqueue_and_echo_commands_P(PSTR(Z_PROBE_END_SCRIPT));
|
|
5225
|
+ stepper.synchronize();
|
5222
|
5226
|
#endif
|
5223
|
|
- enqueue_and_echo_commands_P(PSTR(Z_PROBE_END_SCRIPT));
|
5224
|
|
- stepper.synchronize();
|
5225
|
|
- #endif
|
5226
|
|
-
|
5227
|
|
- // Auto Bed Leveling is complete! Enable if possible.
|
5228
|
|
- planner.abl_enabled = dryrun ? abl_should_enable : true;
|
5229
|
5227
|
|
5230
|
|
- FAIL:
|
|
5228
|
+ // Auto Bed Leveling is complete! Enable if possible.
|
|
5229
|
+ planner.abl_enabled = dryrun ? abl_should_enable : true;
|
|
5230
|
+ } // !isnan(measured_z)
|
5231
|
5231
|
|
5232
|
5232
|
// Restore state after probing
|
5233
|
5233
|
if (!faux) clean_up_after_endstop_or_probe_move();
|
|
@@ -5272,7 +5272,7 @@ void home_all_axes() { gcode_G28(true); }
|
5272
|
5272
|
|
5273
|
5273
|
const float measured_z = probe_pt(xpos, ypos, parser.boolval('S', true), 1);
|
5274
|
5274
|
|
5275
|
|
- if (!nan_error(measured_z)) {
|
|
5275
|
+ if (!isnan(measured_z)) {
|
5276
|
5276
|
SERIAL_PROTOCOLPAIR("Bed X: ", FIXFLOAT(xpos));
|
5277
|
5277
|
SERIAL_PROTOCOLPAIR(" Y: ", FIXFLOAT(ypos));
|
5278
|
5278
|
SERIAL_PROTOCOLLNPAIR(" Z: ", FIXFLOAT(measured_z));
|