|
|
|
@ -365,25 +365,23 @@
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
case 0:
|
|
|
|
case 0:
|
|
|
|
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) { // Create a bowl shape - similar to
|
|
|
|
GRID_LOOP(x, y) { // Create a bowl shape similar to a poorly-calibrated Delta
|
|
|
|
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) { // a poorly calibrated Delta.
|
|
|
|
const float p1 = 0.5f * (GRID_MAX_POINTS_X) - x,
|
|
|
|
const float p1 = 0.5f * (GRID_MAX_POINTS_X) - x,
|
|
|
|
p2 = 0.5f * (GRID_MAX_POINTS_Y) - y;
|
|
|
|
p2 = 0.5f * (GRID_MAX_POINTS_Y) - y;
|
|
|
|
z_values[x][y] += 2.0f * HYPOT(p1, p2);
|
|
|
|
z_values[x][y] += 2.0f * HYPOT(p1, p2);
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
ExtUI::onMeshUpdate(x, y, z_values[x][y]);
|
|
|
|
ExtUI::onMeshUpdate(x, y, z_values[x][y]);
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
|
|
case 1:
|
|
|
|
case 1:
|
|
|
|
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) { // Create a diagonal line several Mesh cells thick that is raised
|
|
|
|
LOOP_L_N(x, GRID_MAX_POINTS_X) { // Create a diagonal line several Mesh cells thick that is raised
|
|
|
|
z_values[x][x] += 9.999f;
|
|
|
|
z_values[x][x] += 9.999f;
|
|
|
|
z_values[x][x + (x < GRID_MAX_POINTS_Y - 1) ? 1 : -1] += 9.999f; // We want the altered line several mesh points thick
|
|
|
|
z_values[x][x + (x < (GRID_MAX_POINTS_Y) - 1) ? 1 : -1] += 9.999f; // We want the altered line several mesh points thick
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
ExtUI::onMeshUpdate(x, x, z_values[x][x]);
|
|
|
|
ExtUI::onMeshUpdate(x, x, z_values[x][x]);
|
|
|
|
ExtUI::onMeshUpdate(x, (x + (x < GRID_MAX_POINTS_Y - 1) ? 1 : -1), z_values[x][x + (x < GRID_MAX_POINTS_Y - 1) ? 1 : -1]);
|
|
|
|
ExtUI::onMeshUpdate(x, (x + (x < (GRID_MAX_POINTS_Y) - 1) ? 1 : -1), z_values[x][x + (x < (GRID_MAX_POINTS_Y) - 1) ? 1 : -1]);
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
@ -467,7 +465,7 @@
|
|
|
|
//
|
|
|
|
//
|
|
|
|
// Manually Probe Mesh in areas that can't be reached by the probe
|
|
|
|
// Manually Probe Mesh in areas that can't be reached by the probe
|
|
|
|
//
|
|
|
|
//
|
|
|
|
SERIAL_ECHOLNPGM("Manually probing unreachable mesh locations.");
|
|
|
|
SERIAL_ECHOLNPGM("Manually probing unreachable points.");
|
|
|
|
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
|
|
|
|
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
|
|
|
|
|
|
|
|
|
|
|
|
if (parser.seen('C') && !xy_seen) {
|
|
|
|
if (parser.seen('C') && !xy_seen) {
|
|
|
|
@ -537,9 +535,7 @@
|
|
|
|
if (cpos.x < 0) {
|
|
|
|
if (cpos.x < 0) {
|
|
|
|
// No more REAL INVALID mesh points to populate, so we ASSUME
|
|
|
|
// No more REAL INVALID mesh points to populate, so we ASSUME
|
|
|
|
// user meant to populate ALL INVALID mesh points to value
|
|
|
|
// user meant to populate ALL INVALID mesh points to value
|
|
|
|
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
|
|
|
|
GRID_LOOP(x, y) if (isnan(z_values[x][y])) z_values[x][y] = g29_constant;
|
|
|
|
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
|
|
|
|
|
|
|
|
if (isnan(z_values[x][y])) z_values[x][y] = g29_constant;
|
|
|
|
|
|
|
|
break; // No more invalid Mesh Points to populate
|
|
|
|
break; // No more invalid Mesh Points to populate
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
else {
|
|
|
|
@ -696,12 +692,11 @@
|
|
|
|
void unified_bed_leveling::adjust_mesh_to_mean(const bool cflag, const float value) {
|
|
|
|
void unified_bed_leveling::adjust_mesh_to_mean(const bool cflag, const float value) {
|
|
|
|
float sum = 0;
|
|
|
|
float sum = 0;
|
|
|
|
int n = 0;
|
|
|
|
int n = 0;
|
|
|
|
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
|
|
|
|
GRID_LOOP(x, y)
|
|
|
|
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
|
|
|
|
if (!isnan(z_values[x][y])) {
|
|
|
|
if (!isnan(z_values[x][y])) {
|
|
|
|
sum += z_values[x][y];
|
|
|
|
sum += z_values[x][y];
|
|
|
|
n++;
|
|
|
|
n++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const float mean = sum / n;
|
|
|
|
const float mean = sum / n;
|
|
|
|
|
|
|
|
|
|
|
|
@ -709,10 +704,9 @@
|
|
|
|
// Sum the squares of difference from mean
|
|
|
|
// Sum the squares of difference from mean
|
|
|
|
//
|
|
|
|
//
|
|
|
|
float sum_of_diff_squared = 0;
|
|
|
|
float sum_of_diff_squared = 0;
|
|
|
|
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
|
|
|
|
GRID_LOOP(x, y)
|
|
|
|
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
|
|
|
|
if (!isnan(z_values[x][y]))
|
|
|
|
if (!isnan(z_values[x][y]))
|
|
|
|
sum_of_diff_squared += sq(z_values[x][y] - mean);
|
|
|
|
sum_of_diff_squared += sq(z_values[x][y] - mean);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
SERIAL_ECHOLNPAIR("# of samples: ", n);
|
|
|
|
SERIAL_ECHOLNPAIR("# of samples: ", n);
|
|
|
|
SERIAL_ECHOLNPAIR_F("Mean Mesh Height: ", mean, 6);
|
|
|
|
SERIAL_ECHOLNPAIR_F("Mean Mesh Height: ", mean, 6);
|
|
|
|
@ -721,27 +715,25 @@
|
|
|
|
SERIAL_ECHOLNPAIR_F("Standard Deviation: ", sigma, 6);
|
|
|
|
SERIAL_ECHOLNPAIR_F("Standard Deviation: ", sigma, 6);
|
|
|
|
|
|
|
|
|
|
|
|
if (cflag)
|
|
|
|
if (cflag)
|
|
|
|
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
|
|
|
|
GRID_LOOP(x, y)
|
|
|
|
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
|
|
|
|
|
|
|
|
if (!isnan(z_values[x][y])) {
|
|
|
|
|
|
|
|
z_values[x][y] -= mean + value;
|
|
|
|
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
|
|
|
|
ExtUI::onMeshUpdate(x, y, z_values[x][y]);
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void unified_bed_leveling::shift_mesh_height() {
|
|
|
|
|
|
|
|
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
|
|
|
|
|
|
|
|
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
|
|
|
|
|
|
|
|
if (!isnan(z_values[x][y])) {
|
|
|
|
if (!isnan(z_values[x][y])) {
|
|
|
|
z_values[x][y] += g29_constant;
|
|
|
|
z_values[x][y] -= mean + value;
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
ExtUI::onMeshUpdate(x, y, z_values[x][y]);
|
|
|
|
ExtUI::onMeshUpdate(x, y, z_values[x][y]);
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void unified_bed_leveling::shift_mesh_height() {
|
|
|
|
|
|
|
|
GRID_LOOP(x, y)
|
|
|
|
|
|
|
|
if (!isnan(z_values[x][y])) {
|
|
|
|
|
|
|
|
z_values[x][y] += g29_constant;
|
|
|
|
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
|
|
|
|
ExtUI::onMeshUpdate(x, y, z_values[x][y]);
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
#if HAS_BED_PROBE
|
|
|
|
#if HAS_BED_PROBE
|
|
|
|
/**
|
|
|
|
/**
|
|
|
|
* Probe all invalidated locations of the mesh that can be reached by the probe.
|
|
|
|
* Probe all invalidated locations of the mesh that can be reached by the probe.
|
|
|
|
@ -1243,52 +1235,46 @@
|
|
|
|
|
|
|
|
|
|
|
|
mesh_index_pair farthest { -1, -1, -99999.99 };
|
|
|
|
mesh_index_pair farthest { -1, -1, -99999.99 };
|
|
|
|
|
|
|
|
|
|
|
|
for (int8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
|
|
|
|
GRID_LOOP(i, j) {
|
|
|
|
for (int8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
|
|
|
|
if (!isnan(z_values[i][j])) continue; // Skip valid mesh points
|
|
|
|
|
|
|
|
|
|
|
|
if (isnan(z_values[i][j])) { // Invalid mesh point?
|
|
|
|
// Skip unreachable points
|
|
|
|
|
|
|
|
if (!probe.can_reach(mesh_index_to_xpos(i), mesh_index_to_ypos(j)))
|
|
|
|
|
|
|
|
continue;
|
|
|
|
|
|
|
|
|
|
|
|
// Skip points the probe can't reach
|
|
|
|
found_a_NAN = true;
|
|
|
|
if (!probe.can_reach(mesh_index_to_xpos(i), mesh_index_to_ypos(j)))
|
|
|
|
|
|
|
|
continue;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
found_a_NAN = true;
|
|
|
|
xy_int8_t near { -1, -1 };
|
|
|
|
|
|
|
|
float d1, d2 = 99999.9f;
|
|
|
|
|
|
|
|
GRID_LOOP(k, l) {
|
|
|
|
|
|
|
|
if (isnan(z_values[k][l])) continue;
|
|
|
|
|
|
|
|
|
|
|
|
xy_int8_t near { -1, -1 };
|
|
|
|
found_a_real = true;
|
|
|
|
float d1, d2 = 99999.9f;
|
|
|
|
|
|
|
|
for (int8_t k = 0; k < GRID_MAX_POINTS_X; k++) {
|
|
|
|
|
|
|
|
for (int8_t l = 0; l < GRID_MAX_POINTS_Y; l++) {
|
|
|
|
|
|
|
|
if (!isnan(z_values[k][l])) {
|
|
|
|
|
|
|
|
found_a_real = true;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Add in a random weighting factor that scrambles the probing of the
|
|
|
|
// Add in a random weighting factor that scrambles the probing of the
|
|
|
|
// last half of the mesh (when every unprobed mesh point is one index
|
|
|
|
// last half of the mesh (when every unprobed mesh point is one index
|
|
|
|
// from a probed location).
|
|
|
|
// from a probed location).
|
|
|
|
|
|
|
|
|
|
|
|
d1 = HYPOT(i - k, j - l) + (1.0f / ((millis() % 47) + 13));
|
|
|
|
d1 = HYPOT(i - k, j - l) + (1.0f / ((millis() % 47) + 13));
|
|
|
|
|
|
|
|
|
|
|
|
if (d1 < d2) { // Invalid mesh point (i,j) is closer to the defined point (k,l)
|
|
|
|
if (d1 < d2) { // Invalid mesh point (i,j) is closer to the defined point (k,l)
|
|
|
|
d2 = d1;
|
|
|
|
d2 = d1;
|
|
|
|
near.set(i, j);
|
|
|
|
near.set(i, j);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//
|
|
|
|
//
|
|
|
|
// At this point d2 should have the near defined mesh point to invalid mesh point (i,j)
|
|
|
|
// At this point d2 should have the near defined mesh point to invalid mesh point (i,j)
|
|
|
|
//
|
|
|
|
//
|
|
|
|
|
|
|
|
|
|
|
|
if (found_a_real && near.x >= 0 && d2 > farthest.distance) {
|
|
|
|
if (found_a_real && near.x >= 0 && d2 > farthest.distance) {
|
|
|
|
farthest.pos = near; // Found an invalid location farther from the defined mesh point
|
|
|
|
farthest.pos = near; // Found an invalid location farther from the defined mesh point
|
|
|
|
farthest.distance = d2;
|
|
|
|
farthest.distance = d2;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
} // GRID_LOOP
|
|
|
|
} // for j
|
|
|
|
|
|
|
|
} // for i
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (!found_a_real && found_a_NAN) { // if the mesh is totally unpopulated, start the probing
|
|
|
|
if (!found_a_real && found_a_NAN) { // if the mesh is totally unpopulated, start the probing
|
|
|
|
farthest.pos.set(GRID_MAX_POINTS_X / 2, GRID_MAX_POINTS_Y / 2);
|
|
|
|
farthest.pos.set((GRID_MAX_POINTS_X) / 2, (GRID_MAX_POINTS_Y) / 2);
|
|
|
|
farthest.distance = 1;
|
|
|
|
farthest.distance = 1;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return farthest;
|
|
|
|
return farthest;
|
|
|
|
@ -1304,36 +1290,34 @@
|
|
|
|
|
|
|
|
|
|
|
|
float best_so_far = 99999.99f;
|
|
|
|
float best_so_far = 99999.99f;
|
|
|
|
|
|
|
|
|
|
|
|
for (int8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
|
|
|
|
GRID_LOOP(i, j) {
|
|
|
|
for (int8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
|
|
|
|
if ( (type == (isnan(z_values[i][j]) ? INVALID : REAL))
|
|
|
|
if ( (type == (isnan(z_values[i][j]) ? INVALID : REAL))
|
|
|
|
|| (type == SET_IN_BITMAP && !done_flags->marked(i, j))
|
|
|
|
|| (type == SET_IN_BITMAP && !done_flags->marked(i, j))
|
|
|
|
) {
|
|
|
|
) {
|
|
|
|
// Found a Mesh Point of the specified type!
|
|
|
|
// Found a Mesh Point of the specified type!
|
|
|
|
const xy_pos_t mpos = { mesh_index_to_xpos(i), mesh_index_to_ypos(j) };
|
|
|
|
const xy_pos_t mpos = { mesh_index_to_xpos(i), mesh_index_to_ypos(j) };
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// If using the probe as the reference there are some unreachable locations.
|
|
|
|
// If using the probe as the reference there are some unreachable locations.
|
|
|
|
// Also for round beds, there are grid points outside the bed the nozzle can't reach.
|
|
|
|
// Also for round beds, there are grid points outside the bed the nozzle can't reach.
|
|
|
|
// Prune them from the list and ignore them till the next Phase (manual nozzle probing).
|
|
|
|
// Prune them from the list and ignore them till the next Phase (manual nozzle probing).
|
|
|
|
|
|
|
|
|
|
|
|
if (!(probe_relative ? probe.can_reach(mpos) : position_is_reachable(mpos)))
|
|
|
|
if (!(probe_relative ? probe.can_reach(mpos) : position_is_reachable(mpos)))
|
|
|
|
continue;
|
|
|
|
continue;
|
|
|
|
|
|
|
|
|
|
|
|
// Reachable. Check if it's the best_so_far location to the nozzle.
|
|
|
|
// Reachable. Check if it's the best_so_far location to the nozzle.
|
|
|
|
|
|
|
|
|
|
|
|
const xy_pos_t diff = current_position - mpos;
|
|
|
|
const xy_pos_t diff = current_position - mpos;
|
|
|
|
const float distance = (ref - mpos).magnitude() + diff.magnitude() * 0.1f;
|
|
|
|
const float distance = (ref - mpos).magnitude() + diff.magnitude() * 0.1f;
|
|
|
|
|
|
|
|
|
|
|
|
// factor in the distance from the current location for the normal case
|
|
|
|
// factor in the distance from the current location for the normal case
|
|
|
|
// so the nozzle isn't running all over the bed.
|
|
|
|
// so the nozzle isn't running all over the bed.
|
|
|
|
if (distance < best_so_far) {
|
|
|
|
if (distance < best_so_far) {
|
|
|
|
best_so_far = distance; // Found a closer location with the desired value type.
|
|
|
|
best_so_far = distance; // Found a closer location with the desired value type.
|
|
|
|
closest.pos.set(i, j);
|
|
|
|
closest.pos.set(i, j);
|
|
|
|
closest.distance = best_so_far;
|
|
|
|
closest.distance = best_so_far;
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
} // for j
|
|
|
|
}
|
|
|
|
} // for i
|
|
|
|
} // GRID_LOOP
|
|
|
|
|
|
|
|
|
|
|
|
return closest;
|
|
|
|
return closest;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
@ -1373,7 +1357,7 @@
|
|
|
|
info3 PROGMEM = { GRID_MAX_POINTS_X - 1, 0, 0, GRID_MAX_POINTS_Y, true }; // Right side of the mesh looking left
|
|
|
|
info3 PROGMEM = { GRID_MAX_POINTS_X - 1, 0, 0, GRID_MAX_POINTS_Y, true }; // Right side of the mesh looking left
|
|
|
|
static const smart_fill_info * const info[] PROGMEM = { &info0, &info1, &info2, &info3 };
|
|
|
|
static const smart_fill_info * const info[] PROGMEM = { &info0, &info1, &info2, &info3 };
|
|
|
|
|
|
|
|
|
|
|
|
for (uint8_t i = 0; i < COUNT(info); ++i) {
|
|
|
|
LOOP_L_N(i, COUNT(info)) {
|
|
|
|
const smart_fill_info *f = (smart_fill_info*)pgm_read_ptr(&info[i]);
|
|
|
|
const smart_fill_info *f = (smart_fill_info*)pgm_read_ptr(&info[i]);
|
|
|
|
const int8_t sx = pgm_read_byte(&f->sx), sy = pgm_read_byte(&f->sy),
|
|
|
|
const int8_t sx = pgm_read_byte(&f->sx), sy = pgm_read_byte(&f->sy),
|
|
|
|
ex = pgm_read_byte(&f->ex), ey = pgm_read_byte(&f->ey);
|
|
|
|
ex = pgm_read_byte(&f->ex), ey = pgm_read_byte(&f->ey);
|
|
|
|
@ -1496,12 +1480,13 @@
|
|
|
|
|
|
|
|
|
|
|
|
bool zig_zag = false;
|
|
|
|
bool zig_zag = false;
|
|
|
|
|
|
|
|
|
|
|
|
uint16_t total_points = g29_grid_size * g29_grid_size, point_num = 1;
|
|
|
|
const uint16_t total_points = sq(g29_grid_size);
|
|
|
|
|
|
|
|
uint16_t point_num = 1;
|
|
|
|
|
|
|
|
|
|
|
|
xy_pos_t rpos;
|
|
|
|
xy_pos_t rpos;
|
|
|
|
for (uint8_t ix = 0; ix < g29_grid_size; ix++) {
|
|
|
|
LOOP_L_N(ix, g29_grid_size) {
|
|
|
|
rpos.x = x_min + ix * dx;
|
|
|
|
rpos.x = x_min + ix * dx;
|
|
|
|
for (int8_t iy = 0; iy < g29_grid_size; iy++) {
|
|
|
|
LOOP_L_N(iy, g29_grid_size) {
|
|
|
|
rpos.y = y_min + dy * (zig_zag ? g29_grid_size - 1 - iy : iy);
|
|
|
|
rpos.y = y_min + dy * (zig_zag ? g29_grid_size - 1 - iy : iy);
|
|
|
|
|
|
|
|
|
|
|
|
if (!abort_flag) {
|
|
|
|
if (!abort_flag) {
|
|
|
|
@ -1569,39 +1554,37 @@
|
|
|
|
|
|
|
|
|
|
|
|
matrix_3x3 rotation = matrix_3x3::create_look_at(vector_3(lsf_results.A, lsf_results.B, 1));
|
|
|
|
matrix_3x3 rotation = matrix_3x3::create_look_at(vector_3(lsf_results.A, lsf_results.B, 1));
|
|
|
|
|
|
|
|
|
|
|
|
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
|
|
|
|
GRID_LOOP(i, j) {
|
|
|
|
for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
|
|
|
|
float mx = mesh_index_to_xpos(i),
|
|
|
|
float mx = mesh_index_to_xpos(i),
|
|
|
|
my = mesh_index_to_ypos(j),
|
|
|
|
my = mesh_index_to_ypos(j),
|
|
|
|
mz = z_values[i][j];
|
|
|
|
mz = z_values[i][j];
|
|
|
|
|
|
|
|
|
|
|
|
if (DEBUGGING(LEVELING)) {
|
|
|
|
if (DEBUGGING(LEVELING)) {
|
|
|
|
DEBUG_ECHOPAIR_F("before rotation = [", mx, 7);
|
|
|
|
DEBUG_ECHOPAIR_F("before rotation = [", mx, 7);
|
|
|
|
DEBUG_CHAR(',');
|
|
|
|
DEBUG_CHAR(',');
|
|
|
|
DEBUG_ECHO_F(my, 7);
|
|
|
|
DEBUG_ECHO_F(my, 7);
|
|
|
|
DEBUG_CHAR(',');
|
|
|
|
DEBUG_CHAR(',');
|
|
|
|
DEBUG_ECHO_F(mz, 7);
|
|
|
|
DEBUG_ECHO_F(mz, 7);
|
|
|
|
DEBUG_ECHOPGM("] ---> ");
|
|
|
|
DEBUG_ECHOPGM("] ---> ");
|
|
|
|
DEBUG_DELAY(20);
|
|
|
|
DEBUG_DELAY(20);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
apply_rotation_xyz(rotation, mx, my, mz);
|
|
|
|
apply_rotation_xyz(rotation, mx, my, mz);
|
|
|
|
|
|
|
|
|
|
|
|
if (DEBUGGING(LEVELING)) {
|
|
|
|
if (DEBUGGING(LEVELING)) {
|
|
|
|
DEBUG_ECHOPAIR_F("after rotation = [", mx, 7);
|
|
|
|
DEBUG_ECHOPAIR_F("after rotation = [", mx, 7);
|
|
|
|
DEBUG_CHAR(',');
|
|
|
|
DEBUG_CHAR(',');
|
|
|
|
DEBUG_ECHO_F(my, 7);
|
|
|
|
DEBUG_ECHO_F(my, 7);
|
|
|
|
DEBUG_CHAR(',');
|
|
|
|
DEBUG_CHAR(',');
|
|
|
|
DEBUG_ECHO_F(mz, 7);
|
|
|
|
DEBUG_ECHO_F(mz, 7);
|
|
|
|
DEBUG_ECHOLNPGM("]");
|
|
|
|
DEBUG_ECHOLNPGM("]");
|
|
|
|
DEBUG_DELAY(20);
|
|
|
|
DEBUG_DELAY(20);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
z_values[i][j] = mz - lsf_results.D;
|
|
|
|
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
|
|
|
|
ExtUI::onMeshUpdate(i, j, z_values[i][j]);
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
z_values[i][j] = mz - lsf_results.D;
|
|
|
|
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
|
|
|
|
ExtUI::onMeshUpdate(i, j, z_values[i][j]);
|
|
|
|
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if (DEBUGGING(LEVELING)) {
|
|
|
|
if (DEBUGGING(LEVELING)) {
|
|
|
|
@ -1661,7 +1644,7 @@
|
|
|
|
// being extrapolated so that nearby points will have greater influence on
|
|
|
|
// being extrapolated so that nearby points will have greater influence on
|
|
|
|
// the point being extrapolated. Then extrapolate the mesh point from WLSF.
|
|
|
|
// the point being extrapolated. Then extrapolate the mesh point from WLSF.
|
|
|
|
|
|
|
|
|
|
|
|
static_assert(GRID_MAX_POINTS_Y <= 16, "GRID_MAX_POINTS_Y too big");
|
|
|
|
static_assert((GRID_MAX_POINTS_Y) <= 16, "GRID_MAX_POINTS_Y too big");
|
|
|
|
uint16_t bitmap[GRID_MAX_POINTS_X] = { 0 };
|
|
|
|
uint16_t bitmap[GRID_MAX_POINTS_X] = { 0 };
|
|
|
|
struct linear_fit_data lsf_results;
|
|
|
|
struct linear_fit_data lsf_results;
|
|
|
|
|
|
|
|
|
|
|
|
@ -1669,23 +1652,20 @@
|
|
|
|
|
|
|
|
|
|
|
|
const float weight_scaled = weight_factor * _MAX(MESH_X_DIST, MESH_Y_DIST);
|
|
|
|
const float weight_scaled = weight_factor * _MAX(MESH_X_DIST, MESH_Y_DIST);
|
|
|
|
|
|
|
|
|
|
|
|
for (uint8_t jx = 0; jx < GRID_MAX_POINTS_X; jx++)
|
|
|
|
GRID_LOOP(jx, jy) if (!isnan(z_values[jx][jy])) SBI(bitmap[jx], jy);
|
|
|
|
for (uint8_t jy = 0; jy < GRID_MAX_POINTS_Y; jy++)
|
|
|
|
|
|
|
|
if (!isnan(z_values[jx][jy]))
|
|
|
|
|
|
|
|
SBI(bitmap[jx], jy);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
xy_pos_t ppos;
|
|
|
|
xy_pos_t ppos;
|
|
|
|
for (uint8_t ix = 0; ix < GRID_MAX_POINTS_X; ix++) {
|
|
|
|
LOOP_L_N(ix, GRID_MAX_POINTS_X) {
|
|
|
|
ppos.x = mesh_index_to_xpos(ix);
|
|
|
|
ppos.x = mesh_index_to_xpos(ix);
|
|
|
|
for (uint8_t iy = 0; iy < GRID_MAX_POINTS_Y; iy++) {
|
|
|
|
LOOP_L_N(iy, GRID_MAX_POINTS_Y) {
|
|
|
|
ppos.y = mesh_index_to_ypos(iy);
|
|
|
|
ppos.y = mesh_index_to_ypos(iy);
|
|
|
|
if (isnan(z_values[ix][iy])) {
|
|
|
|
if (isnan(z_values[ix][iy])) {
|
|
|
|
// undefined mesh point at (ppos.x,ppos.y), compute weighted LSF from original valid mesh points.
|
|
|
|
// undefined mesh point at (ppos.x,ppos.y), compute weighted LSF from original valid mesh points.
|
|
|
|
incremental_LSF_reset(&lsf_results);
|
|
|
|
incremental_LSF_reset(&lsf_results);
|
|
|
|
xy_pos_t rpos;
|
|
|
|
xy_pos_t rpos;
|
|
|
|
for (uint8_t jx = 0; jx < GRID_MAX_POINTS_X; jx++) {
|
|
|
|
LOOP_L_N(jx, GRID_MAX_POINTS_X) {
|
|
|
|
rpos.x = mesh_index_to_xpos(jx);
|
|
|
|
rpos.x = mesh_index_to_xpos(jx);
|
|
|
|
for (uint8_t jy = 0; jy < GRID_MAX_POINTS_Y; jy++) {
|
|
|
|
LOOP_L_N(jy, GRID_MAX_POINTS_Y) {
|
|
|
|
if (TEST(bitmap[jx], jy)) {
|
|
|
|
if (TEST(bitmap[jx], jy)) {
|
|
|
|
rpos.y = mesh_index_to_ypos(jy);
|
|
|
|
rpos.y = mesh_index_to_ypos(jy);
|
|
|
|
const float rz = z_values[jx][jy],
|
|
|
|
const float rz = z_values[jx][jy],
|
|
|
|
@ -1747,7 +1727,7 @@
|
|
|
|
SERIAL_ECHOLNPAIR("MESH_Y_DIST ", MESH_Y_DIST); serial_delay(50);
|
|
|
|
SERIAL_ECHOLNPAIR("MESH_Y_DIST ", MESH_Y_DIST); serial_delay(50);
|
|
|
|
|
|
|
|
|
|
|
|
SERIAL_ECHOPGM("X-Axis Mesh Points at: ");
|
|
|
|
SERIAL_ECHOPGM("X-Axis Mesh Points at: ");
|
|
|
|
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
|
|
|
|
LOOP_L_N(i, GRID_MAX_POINTS_X) {
|
|
|
|
SERIAL_ECHO_F(LOGICAL_X_POSITION(mesh_index_to_xpos(i)), 3);
|
|
|
|
SERIAL_ECHO_F(LOGICAL_X_POSITION(mesh_index_to_xpos(i)), 3);
|
|
|
|
SERIAL_ECHOPGM(" ");
|
|
|
|
SERIAL_ECHOPGM(" ");
|
|
|
|
serial_delay(25);
|
|
|
|
serial_delay(25);
|
|
|
|
@ -1755,7 +1735,7 @@
|
|
|
|
SERIAL_EOL();
|
|
|
|
SERIAL_EOL();
|
|
|
|
|
|
|
|
|
|
|
|
SERIAL_ECHOPGM("Y-Axis Mesh Points at: ");
|
|
|
|
SERIAL_ECHOPGM("Y-Axis Mesh Points at: ");
|
|
|
|
for (uint8_t i = 0; i < GRID_MAX_POINTS_Y; i++) {
|
|
|
|
LOOP_L_N(i, GRID_MAX_POINTS_Y) {
|
|
|
|
SERIAL_ECHO_F(LOGICAL_Y_POSITION(mesh_index_to_ypos(i)), 3);
|
|
|
|
SERIAL_ECHO_F(LOGICAL_Y_POSITION(mesh_index_to_ypos(i)), 3);
|
|
|
|
SERIAL_ECHOPGM(" ");
|
|
|
|
SERIAL_ECHOPGM(" ");
|
|
|
|
serial_delay(25);
|
|
|
|
serial_delay(25);
|
|
|
|
@ -1840,13 +1820,12 @@
|
|
|
|
|
|
|
|
|
|
|
|
SERIAL_ECHOLNPAIR("Subtracting mesh in slot ", g29_storage_slot, " from current mesh.");
|
|
|
|
SERIAL_ECHOLNPAIR("Subtracting mesh in slot ", g29_storage_slot, " from current mesh.");
|
|
|
|
|
|
|
|
|
|
|
|
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
|
|
|
|
GRID_LOOP(x, y) {
|
|
|
|
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
|
|
|
|
z_values[x][y] -= tmp_z_values[x][y];
|
|
|
|
z_values[x][y] -= tmp_z_values[x][y];
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
#if ENABLED(EXTENSIBLE_UI)
|
|
|
|
ExtUI::onMeshUpdate(x, y, z_values[x][y]);
|
|
|
|
ExtUI::onMeshUpdate(x, y, z_values[x][y]);
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
#endif // UBL_DEVEL_DEBUGGING
|
|
|
|
#endif // UBL_DEVEL_DEBUGGING
|
|
|
|
|
Scott Lahteine
5 years ago
committed by