wp/Marlin-2-0-x-Anycubic-i3-MEGA-S
1
0
Fork 0
Code Issues 28 Pull Requests 2 Actions Packages Projects Releases 30 Wiki Activity

Merge upstream changes from Marlin 2.1.1

Browse Source
This commit is contained in:
Stefan Kalscheuer
2022-09-03 09:23:32 +02:00
parent 626283aadb
commit 986e416c7f
1610 changed files with 73839 additions and 40857 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
Marlin/src/feature/bedlevel/abl/abl.h
View File

@@ -1,45 +0,0 @@
/**
* Marlin 3D Printer Firmware
* Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#include "../../../inc/MarlinConfigPre.h"
extern xy_pos_t bilinear_grid_spacing, bilinear_start;
extern xy_float_t bilinear_grid_factor;
extern bed_mesh_t z_values;
float bilinear_z_offset(const xy_pos_t &raw);
void extrapolate_unprobed_bed_level();
void print_bilinear_leveling_grid();
void refresh_bed_level();
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
void print_bilinear_leveling_grid_virt();
void bed_level_virt_interpolate();
#endif
#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)
void bilinear_line_to_destination(const_feedRate_t scaled_fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF);
#endif
#define _GET_MESH_X(I) float(bilinear_start.x + (I) * bilinear_grid_spacing.x)
#define _GET_MESH_Y(J) float(bilinear_start.y + (J) * bilinear_grid_spacing.y)
#define Z_VALUES_ARR z_values

124
Marlin/src/feature/bedlevel/abl/abl.cpp → Marlin/src/feature/bedlevel/abl/bbl.cpp
View File

@@ -35,14 +35,19 @@
#include "../../../lcd/extui/ui_api.h"
#endif
xy_pos_t bilinear_grid_spacing, bilinear_start;
xy_float_t bilinear_grid_factor;
bed_mesh_t z_values;
LevelingBilinear bedlevel;
xy_pos_t LevelingBilinear::grid_spacing,
LevelingBilinear::grid_start;
xy_float_t LevelingBilinear::grid_factor;
bed_mesh_t LevelingBilinear::z_values;
xy_pos_t LevelingBilinear::cached_rel;
xy_int8_t LevelingBilinear::cached_g;
/**
* Extrapolate a single point from its neighbors
*/
static void extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) {
void LevelingBilinear::extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) {
if (!isnan(z_values[x][y])) return;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPGM("Extrapolate [");
@@ -92,11 +97,26 @@ static void extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t
#endif
#endif
void LevelingBilinear::reset() {
grid_start.reset();
grid_spacing.reset();
GRID_LOOP(x, y) {
z_values[x][y] = NAN;
TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, 0));
}
}
void LevelingBilinear::set_grid(const xy_pos_t& _grid_spacing, const xy_pos_t& _grid_start) {
grid_spacing = _grid_spacing;
grid_start = _grid_start;
grid_factor = grid_spacing.reciprocal();
}
/**
* Fill in the unprobed points (corners of circular print surface)
* using linear extrapolation, away from the center.
*/
void extrapolate_unprobed_bed_level() {
void LevelingBilinear::extrapolate_unprobed_bed_level() {
#ifdef HALF_IN_X
constexpr uint8_t ctrx2 = 0, xend = GRID_MAX_POINTS_X - 1;
#else
@@ -131,35 +151,31 @@ void extrapolate_unprobed_bed_level() {
#endif
extrapolate_one_point(x2, y2, -1, -1); // right-above - -
}
}
void print_bilinear_leveling_grid() {
void LevelingBilinear::print_leveling_grid(const bed_mesh_t* _z_values /*= NULL*/) {
// print internal grid(s) or just the one passed as a parameter
SERIAL_ECHOLNPGM("Bilinear Leveling Grid:");
print_2d_array(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y, 3,
[](const uint8_t ix, const uint8_t iy) { return z_values[ix][iy]; }
);
print_2d_array(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y, 3, _z_values ? *_z_values[0] : z_values[0]);
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
if (!_z_values) {
SERIAL_ECHOLNPGM("Subdivided with CATMULL ROM Leveling Grid:");
print_2d_array(ABL_GRID_POINTS_VIRT_X, ABL_GRID_POINTS_VIRT_Y, 5, z_values_virt[0]);
}
#endif
}
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
#define ABL_GRID_POINTS_VIRT_X GRID_MAX_CELLS_X * (BILINEAR_SUBDIVISIONS) + 1
#define ABL_GRID_POINTS_VIRT_Y GRID_MAX_CELLS_Y * (BILINEAR_SUBDIVISIONS) + 1
#define ABL_TEMP_POINTS_X (GRID_MAX_POINTS_X + 2)
#define ABL_TEMP_POINTS_Y (GRID_MAX_POINTS_Y + 2)
float z_values_virt[ABL_GRID_POINTS_VIRT_X][ABL_GRID_POINTS_VIRT_Y];
xy_pos_t bilinear_grid_spacing_virt;
xy_float_t bilinear_grid_factor_virt;
void print_bilinear_leveling_grid_virt() {
SERIAL_ECHOLNPGM("Subdivided with CATMULL ROM Leveling Grid:");
print_2d_array(ABL_GRID_POINTS_VIRT_X, ABL_GRID_POINTS_VIRT_Y, 5,
[](const uint8_t ix, const uint8_t iy) { return z_values_virt[ix][iy]; }
);
}
float LevelingBilinear::z_values_virt[ABL_GRID_POINTS_VIRT_X][ABL_GRID_POINTS_VIRT_Y];
xy_pos_t LevelingBilinear::grid_spacing_virt;
xy_float_t LevelingBilinear::grid_factor_virt;
#define LINEAR_EXTRAPOLATION(E, I) ((E) * 2 - (I))
float bed_level_virt_coord(const uint8_t x, const uint8_t y) {
float LevelingBilinear::bed_level_virt_coord(const uint8_t x, const uint8_t y) {
uint8_t ep = 0, ip = 1;
if (x > (GRID_MAX_POINTS_X) + 1 || y > (GRID_MAX_POINTS_Y) + 1) {
// The requested point requires extrapolating two points beyond the mesh.
@@ -204,7 +220,7 @@ void print_bilinear_leveling_grid() {
return z_values[x - 1][y - 1];
}
static float bed_level_virt_cmr(const float p[4], const uint8_t i, const float t) {
float LevelingBilinear::bed_level_virt_cmr(const float p[4], const uint8_t i, const float t) {
return (
p[i-1] * -t * sq(1 - t)
+ p[i] * (2 - 5 * sq(t) + 3 * t * sq(t))
@@ -213,7 +229,7 @@ void print_bilinear_leveling_grid() {
) * 0.5f;
}
static float bed_level_virt_2cmr(const uint8_t x, const uint8_t y, const_float_t tx, const_float_t ty) {
float LevelingBilinear::bed_level_virt_2cmr(const uint8_t x, const uint8_t y, const_float_t tx, const_float_t ty) {
float row[4], column[4];
LOOP_L_N(i, 4) {
LOOP_L_N(j, 4) {
@@ -224,9 +240,9 @@ void print_bilinear_leveling_grid() {
return bed_level_virt_cmr(row, 1, tx);
}
void bed_level_virt_interpolate() {
bilinear_grid_spacing_virt = bilinear_grid_spacing / (BILINEAR_SUBDIVISIONS);
bilinear_grid_factor_virt = bilinear_grid_spacing_virt.reciprocal();
void LevelingBilinear::bed_level_virt_interpolate() {
grid_spacing_virt = grid_spacing / (BILINEAR_SUBDIVISIONS);
grid_factor_virt = grid_spacing_virt.reciprocal();
LOOP_L_N(y, GRID_MAX_POINTS_Y)
LOOP_L_N(x, GRID_MAX_POINTS_X)
LOOP_L_N(ty, BILINEAR_SUBDIVISIONS)
@@ -242,40 +258,42 @@ void print_bilinear_leveling_grid() {
);
}
}
#endif // ABL_BILINEAR_SUBDIVISION
// Refresh after other values have been updated
void refresh_bed_level() {
bilinear_grid_factor = bilinear_grid_spacing.reciprocal();
void LevelingBilinear::refresh_bed_level() {
TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate());
cached_rel.x = cached_rel.y = -999.999;
cached_g.x = cached_g.y = -99;
}
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
#define ABL_BG_SPACING(A) bilinear_grid_spacing_virt.A
#define ABL_BG_FACTOR(A) bilinear_grid_factor_virt.A
#define ABL_BG_SPACING(A) grid_spacing_virt.A
#define ABL_BG_FACTOR(A) grid_factor_virt.A
#define ABL_BG_POINTS_X ABL_GRID_POINTS_VIRT_X
#define ABL_BG_POINTS_Y ABL_GRID_POINTS_VIRT_Y
#define ABL_BG_GRID(X,Y) z_values_virt[X][Y]
#else
#define ABL_BG_SPACING(A) bilinear_grid_spacing.A
#define ABL_BG_FACTOR(A) bilinear_grid_factor.A
#define ABL_BG_SPACING(A) grid_spacing.A
#define ABL_BG_FACTOR(A) grid_factor.A
#define ABL_BG_POINTS_X GRID_MAX_POINTS_X
#define ABL_BG_POINTS_Y GRID_MAX_POINTS_Y
#define ABL_BG_GRID(X,Y) z_values[X][Y]
#endif
// Get the Z adjustment for non-linear bed leveling
float bilinear_z_offset(const xy_pos_t &raw) {
float LevelingBilinear::get_z_correction(const xy_pos_t &raw) {
static float z1, d2, z3, d4, L, D;
static xy_pos_t prev { -999.999, -999.999 }, ratio;
static xy_pos_t ratio;
// Whole units for the grid line indices. Constrained within bounds.
static xy_int8_t thisg, nextg, lastg { -99, -99 };
static xy_int8_t thisg, nextg;
// XY relative to the probed area
xy_pos_t rel = raw - bilinear_start.asFloat();
xy_pos_t rel = raw - grid_start.asFloat();
#if ENABLED(EXTRAPOLATE_BEYOND_GRID)
#define FAR_EDGE_OR_BOX 2 // Keep using the last grid box
@@ -283,8 +301,8 @@ float bilinear_z_offset(const xy_pos_t &raw) {
#define FAR_EDGE_OR_BOX 1 // Just use the grid far edge
#endif
if (prev.x != rel.x) {
prev.x = rel.x;
if (cached_rel.x != rel.x) {
cached_rel.x = rel.x;
ratio.x = rel.x * ABL_BG_FACTOR(x);
const float gx = constrain(FLOOR(ratio.x), 0, ABL_BG_POINTS_X - (FAR_EDGE_OR_BOX));
ratio.x -= gx; // Subtract whole to get the ratio within the grid box
@@ -298,10 +316,10 @@ float bilinear_z_offset(const xy_pos_t &raw) {
nextg.x = _MIN(thisg.x + 1, ABL_BG_POINTS_X - 1);
}
if (prev.y != rel.y || lastg.x != thisg.x) {
if (cached_rel.y != rel.y || cached_g.x != thisg.x) {
if (prev.y != rel.y) {
prev.y = rel.y;
if (cached_rel.y != rel.y) {
cached_rel.y = rel.y;
ratio.y = rel.y * ABL_BG_FACTOR(y);
const float gy = constrain(FLOOR(ratio.y), 0, ABL_BG_POINTS_Y - (FAR_EDGE_OR_BOX));
ratio.y -= gy;
@@ -315,8 +333,8 @@ float bilinear_z_offset(const xy_pos_t &raw) {
nextg.y = _MIN(thisg.y + 1, ABL_BG_POINTS_Y - 1);
}
if (lastg != thisg) {
lastg = thisg;
if (cached_g != thisg) {
cached_g = thisg;
// Z at the box corners
z1 = ABL_BG_GRID(thisg.x, thisg.y); // left-front
d2 = ABL_BG_GRID(thisg.x, nextg.y) - z1; // left-back (delta)
@@ -336,8 +354,8 @@ float bilinear_z_offset(const xy_pos_t &raw) {
/*
static float last_offset = 0;
if (ABS(last_offset - offset) > 0.2) {
SERIAL_ECHOLNPGM("Sudden Shift at x=", rel.x, " / ", bilinear_grid_spacing.x, " -> thisg.x=", thisg.x);
SERIAL_ECHOLNPGM(" y=", rel.y, " / ", bilinear_grid_spacing.y, " -> thisg.y=", thisg.y);
SERIAL_ECHOLNPGM("Sudden Shift at x=", rel.x, " / ", grid_spacing.x, " -> thisg.x=", thisg.x);
SERIAL_ECHOLNPGM(" y=", rel.y, " / ", grid_spacing.y, " -> thisg.y=", thisg.y);
SERIAL_ECHOLNPGM(" ratio.x=", ratio.x, " ratio.y=", ratio.y);
SERIAL_ECHOLNPGM(" z1=", z1, " z2=", z2, " z3=", z3, " z4=", z4);
SERIAL_ECHOLNPGM(" L=", L, " R=", R, " offset=", offset);
@@ -350,13 +368,13 @@ float bilinear_z_offset(const xy_pos_t &raw) {
#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)
#define CELL_INDEX(A,V) ((V - bilinear_start.A) * ABL_BG_FACTOR(A))
#define CELL_INDEX(A,V) ((V - grid_start.A) * ABL_BG_FACTOR(A))
/**
* Prepare a bilinear-leveled linear move on Cartesian,
* splitting the move where it crosses grid borders.
*/
void bilinear_line_to_destination(const_feedRate_t scaled_fr_mm_s, uint16_t x_splits, uint16_t y_splits) {
void LevelingBilinear::line_to_destination(const_feedRate_t scaled_fr_mm_s, uint16_t x_splits, uint16_t y_splits) {
// Get current and destination cells for this line
xy_int_t c1 { CELL_INDEX(x, current_position.x), CELL_INDEX(y, current_position.y) },
c2 { CELL_INDEX(x, destination.x), CELL_INDEX(y, destination.y) };
@@ -384,7 +402,7 @@ float bilinear_z_offset(const xy_pos_t &raw) {
// Split on the X grid line
CBI(x_splits, gc.x);
end = destination;
destination.x = bilinear_start.x + ABL_BG_SPACING(x) * gc.x;
destination.x = grid_start.x + ABL_BG_SPACING(x) * gc.x;
normalized_dist = (destination.x - current_position.x) / (end.x - current_position.x);
destination.y = LINE_SEGMENT_END(y);
}
@@ -393,7 +411,7 @@ float bilinear_z_offset(const xy_pos_t &raw) {
// Split on the Y grid line
CBI(y_splits, gc.y);
end = destination;
destination.y = bilinear_start.y + ABL_BG_SPACING(y) * gc.y;
destination.y = grid_start.y + ABL_BG_SPACING(y) * gc.y;
normalized_dist = (destination.y - current_position.y) / (end.y - current_position.y);
destination.x = LINE_SEGMENT_END(x);
}
@@ -409,11 +427,11 @@ float bilinear_z_offset(const xy_pos_t &raw) {
destination.e = LINE_SEGMENT_END(e);
// Do the split and look for more borders
bilinear_line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
// Restore destination from stack
destination = end;
bilinear_line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
}
#endif // IS_CARTESIAN && !SEGMENT_LEVELED_MOVES

70
Marlin/src/feature/bedlevel/abl/bbl.h Normal file
View File

@@ -0,0 +1,70 @@
/**
* Marlin 3D Printer Firmware
* Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#include "../../../inc/MarlinConfigPre.h"
class LevelingBilinear {
public:
static bed_mesh_t z_values;
static xy_pos_t grid_spacing, grid_start;
private:
static xy_float_t grid_factor;
static xy_pos_t cached_rel;
static xy_int8_t cached_g;
static void extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir);
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
#define ABL_GRID_POINTS_VIRT_X (GRID_MAX_CELLS_X * (BILINEAR_SUBDIVISIONS) + 1)
#define ABL_GRID_POINTS_VIRT_Y (GRID_MAX_CELLS_Y * (BILINEAR_SUBDIVISIONS) + 1)
static float z_values_virt[ABL_GRID_POINTS_VIRT_X][ABL_GRID_POINTS_VIRT_Y];
static xy_pos_t grid_spacing_virt;
static xy_float_t grid_factor_virt;
static float bed_level_virt_coord(const uint8_t x, const uint8_t y);
static float bed_level_virt_cmr(const float p[4], const uint8_t i, const float t);
static float bed_level_virt_2cmr(const uint8_t x, const uint8_t y, const_float_t tx, const_float_t ty);
static void bed_level_virt_interpolate();
#endif
public:
static void reset();
static void set_grid(const xy_pos_t& _grid_spacing, const xy_pos_t& _grid_start);
static void extrapolate_unprobed_bed_level();
static void print_leveling_grid(const bed_mesh_t* _z_values = NULL);
static void refresh_bed_level();
static bool has_mesh() { return !!grid_spacing.x; }
static bool mesh_is_valid() { return has_mesh(); }
static float get_mesh_x(const uint8_t i) { return grid_start.x + i * grid_spacing.x; }
static float get_mesh_y(const uint8_t j) { return grid_start.y + j * grid_spacing.y; }
static float get_z_correction(const xy_pos_t &raw);
static constexpr float get_z_offset() { return 0.0f; }
#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)
static void line_to_destination(const_feedRate_t scaled_fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF);
#endif
};
extern LevelingBilinear bedlevel;

195
Marlin/src/feature/bedlevel/bdl/bdl.cpp Normal file
View File

@@ -0,0 +1,195 @@
/**
* Marlin 3D Printer Firmware
* Copyright (c) 2022 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
*/
#include "../../../inc/MarlinConfig.h"
#if ENABLED(BD_SENSOR)
#include "../../../MarlinCore.h"
#include "../../../gcode/gcode.h"
#include "../../../module/settings.h"
#include "../../../module/motion.h"
#include "../../../module/planner.h"
#include "../../../module/stepper.h"
#include "../../../module/probe.h"
#include "../../../module/temperature.h"
#include "../../../module/endstops.h"
#include "../../babystep.h"
// I2C software Master library for segment bed heating and bed distance sensor
#include <Panda_segmentBed_I2C.h>
#include "bdl.h"
BDS_Leveling bdl;
//#define DEBUG_OUT_BD
// M102 S-5 Read raw Calibrate data
// M102 S-6 Start Calibrate
// M102 S4 Set the adjustable Z height value (e.g., 'M102 S4' means it will do adjusting while the Z height <= 0.4mm , disable with 'M102 S0'.)
// M102 S-1 Read sensor information
#define MAX_BD_HEIGHT 4.0f
#define CMD_START_READ_CALIBRATE_DATA 1017
#define CMD_END_READ_CALIBRATE_DATA 1018
#define CMD_START_CALIBRATE 1019
#define CMD_END_CALIBRATE 1021
#define CMD_READ_VERSION 1016
I2C_SegmentBED BD_I2C_SENSOR;
#define BD_SENSOR_I2C_ADDR 0x3C
int8_t BDS_Leveling::config_state;
uint8_t BDS_Leveling::homing;
void BDS_Leveling::echo_name() { SERIAL_ECHOPGM("Bed Distance Leveling"); }
void BDS_Leveling::init(uint8_t _sda, uint8_t _scl, uint16_t delay_s) {
int ret = BD_I2C_SENSOR.i2c_init(_sda, _scl, BD_SENSOR_I2C_ADDR, delay_s);
if (ret != 1) SERIAL_ECHOLNPGM("BD_I2C_SENSOR Init Fail return code:", ret);
config_state = 0;
}
float BDS_Leveling::read() {
const uint16_t tmp = BD_I2C_SENSOR.BD_i2c_read();
float BD_z = NAN;
if (BD_I2C_SENSOR.BD_Check_OddEven(tmp) && (tmp & 0x3FF) < 1020)
BD_z = (tmp & 0x3FF) / 100.0f;
return BD_z;
}
void BDS_Leveling::process() {
//if (config_state == 0) return;
static millis_t next_check_ms = 0; // starting at T=0
static float z_pose = 0.0f;
const millis_t ms = millis();
if (ELAPSED(ms, next_check_ms)) { // timed out (or first run)
next_check_ms = ms + (config_state < 0 ? 1000 : 100); // check at 1Hz or 10Hz
unsigned short tmp = 0;
const float cur_z = planner.get_axis_position_mm(Z_AXIS); //current_position.z
static float old_cur_z = cur_z,
old_buf_z = current_position.z;
tmp = BD_I2C_SENSOR.BD_i2c_read();
if (BD_I2C_SENSOR.BD_Check_OddEven(tmp) && (tmp & 0x3FF) < 1020) {
const float z_sensor = (tmp & 0x3FF) / 100.0f;
if (cur_z < 0) config_state = 0;
//float abs_z = current_position.z > cur_z ? (current_position.z - cur_z) : (cur_z - current_position.z);
if ( cur_z < config_state * 0.1f
&& config_state > 0
&& old_cur_z == cur_z
&& old_buf_z == current_position.z
&& z_sensor < (MAX_BD_HEIGHT)
) {
babystep.set_mm(Z_AXIS, cur_z - z_sensor);
#if ENABLED(DEBUG_OUT_BD)
SERIAL_ECHOLNPGM("BD:", z_sensor, ", Z:", cur_z, "|", current_position.z);
#endif
}
else {
babystep.set_mm(Z_AXIS, 0);
//if (old_cur_z <= cur_z) Z_DIR_WRITE(!INVERT_Z_DIR);
stepper.set_directions();
}
old_cur_z = cur_z;
old_buf_z = current_position.z;
endstops.bdp_state_update(z_sensor <= 0.01f);
//endstops.update();
}
else
stepper.set_directions();
#if ENABLED(DEBUG_OUT_BD)
SERIAL_ECHOLNPGM("BD:", tmp & 0x3FF, ", Z:", cur_z, "|", current_position.z);
if (BD_I2C_SENSOR.BD_Check_OddEven(tmp) == 0) SERIAL_ECHOLNPGM("errorCRC");
#endif
if ((tmp & 0x3FF) > 1020) {
BD_I2C_SENSOR.BD_i2c_stop();
safe_delay(10);
}
// read raw calibrate data
if (config_state == -5) {
BD_I2C_SENSOR.BD_i2c_write(CMD_START_READ_CALIBRATE_DATA);
safe_delay(1000);
for (int i = 0; i < MAX_BD_HEIGHT * 10; i++) {
tmp = BD_I2C_SENSOR.BD_i2c_read();
SERIAL_ECHOLNPGM("Calibrate data:", i, ",", tmp & 0x3FF, ", check:", BD_I2C_SENSOR.BD_Check_OddEven(tmp));
safe_delay(500);
}
config_state = 0;
BD_I2C_SENSOR.BD_i2c_write(CMD_END_READ_CALIBRATE_DATA);
safe_delay(500);
}
else if (config_state <= -6) { // Start Calibrate
safe_delay(100);
if (config_state == -6) {
//BD_I2C_SENSOR.BD_i2c_write(1019); // begin calibrate
//delay(1000);
gcode.stepper_inactive_time = SEC_TO_MS(60 * 5);
gcode.process_subcommands_now(F("M17 Z"));
gcode.process_subcommands_now(F("G1 Z0.0"));
z_pose = 0;
safe_delay(1000);
BD_I2C_SENSOR.BD_i2c_write(CMD_START_CALIBRATE); // Begin calibrate
SERIAL_ECHOLNPGM("Begin calibrate");
safe_delay(2000);
config_state = -7;
}
else if (planner.get_axis_position_mm(Z_AXIS) < 10.0f) {
if (z_pose >= MAX_BD_HEIGHT) {
BD_I2C_SENSOR.BD_i2c_write(CMD_END_CALIBRATE); // End calibrate
SERIAL_ECHOLNPGM("End calibrate data");
z_pose = 7;
config_state = 0;
safe_delay(1000);
}
else {
float tmp_k = 0;
char tmp_1[30];
sprintf_P(tmp_1, PSTR("G1 Z%d.%d"), int(z_pose), int(int(z_pose * 10) % 10));
gcode.process_subcommands_now(tmp_1);
SERIAL_ECHO(tmp_1);
SERIAL_ECHOLNPGM(" ,Z:", current_position.z);
while (tmp_k < (z_pose - 0.1f)) {
tmp_k = planner.get_axis_position_mm(Z_AXIS);
safe_delay(1);
}
safe_delay(800);
tmp = (z_pose + 0.0001f) * 10;
BD_I2C_SENSOR.BD_i2c_write(tmp);
SERIAL_ECHOLNPGM("w:", tmp, ",Zpose:", z_pose);
z_pose += 0.1001f;
//queue.enqueue_now_P(PSTR("G90"));
}
}
}
}
}
#endif // BD_SENSOR

36
Marlin/src/feature/bedlevel/bdl/bdl.h Normal file
View File

@@ -0,0 +1,36 @@
/**
* Marlin 3D Printer Firmware
* Copyright (c) 2022 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#include <stdint.h>
class BDS_Leveling {
public:
static int8_t config_state;
static uint8_t homing;
static void echo_name();
static void init(uint8_t _sda, uint8_t _scl, uint16_t delay_s);
static void process();
static float read();
};
extern BDS_Leveling bdl;

66
Marlin/src/feature/bedlevel/bedlevel.cpp
View File

@@ -47,14 +47,11 @@
#endif
bool leveling_is_valid() {
return TERN1(MESH_BED_LEVELING, mbl.has_mesh())
&& TERN1(AUTO_BED_LEVELING_BILINEAR, !!bilinear_grid_spacing.x)
&& TERN1(AUTO_BED_LEVELING_UBL, ubl.mesh_is_valid());
return TERN1(HAS_MESH, bedlevel.mesh_is_valid());
}
/**
* Turn bed leveling on or off, fixing the current
* position as-needed.
* Turn bed leveling on or off, correcting the current position.
*
* Disable: Current position = physical position
* Enable: Current position = "unleveled" physical position
@@ -65,30 +62,25 @@ void set_bed_leveling_enabled(const bool enable/*=true*/) {
if (can_change && enable != planner.leveling_active) {
auto _report_leveling = []{
if (DEBUGGING(LEVELING)) {
if (planner.leveling_active)
DEBUG_POS("Leveling ON", current_position);
else
DEBUG_POS("Leveling OFF", current_position);
}
};
_report_leveling();
planner.synchronize();
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
// Force bilinear_z_offset to re-calculate next time
const xyz_pos_t reset { -9999.999, -9999.999, 0 };
(void)bilinear_z_offset(reset);
#endif
if (planner.leveling_active) { // leveling from on to off
if (DEBUGGING(LEVELING)) DEBUG_POS("Leveling ON", current_position);
// change unleveled current_position to physical current_position without moving steppers.
planner.apply_leveling(current_position);
planner.leveling_active = false; // disable only AFTER calling apply_leveling
if (DEBUGGING(LEVELING)) DEBUG_POS("...Now OFF", current_position);
}
else { // leveling from off to on
if (DEBUGGING(LEVELING)) DEBUG_POS("Leveling OFF", current_position);
planner.leveling_active = true; // enable BEFORE calling unapply_leveling, otherwise ignored
// change physical current_position to unleveled current_position without moving steppers.
planner.unapply_leveling(current_position);
if (DEBUGGING(LEVELING)) DEBUG_POS("...Now ON", current_position);
}
// Get the corrected leveled / unleveled position
planner.apply_modifiers(current_position); // Physical position with all modifiers
planner.leveling_active ^= true; // Toggle leveling between apply and unapply
planner.unapply_modifiers(current_position); // Logical position with modifiers removed
sync_plan_position();
_report_leveling();
}
}
@@ -122,23 +114,9 @@ TemporaryBedLevelingState::TemporaryBedLevelingState(const bool enable) : saved(
*/
void reset_bed_level() {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("reset_bed_level");
#if ENABLED(AUTO_BED_LEVELING_UBL)
ubl.reset();
#else
set_bed_leveling_enabled(false);
#if ENABLED(MESH_BED_LEVELING)
mbl.reset();
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
bilinear_start.reset();
bilinear_grid_spacing.reset();
GRID_LOOP(x, y) {
z_values[x][y] = NAN;
TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, 0));
}
#elif ABL_PLANAR
planner.bed_level_matrix.set_to_identity();
#endif
#endif
IF_DISABLED(AUTO_BED_LEVELING_UBL, set_bed_leveling_enabled(false));
TERN_(HAS_MESH, bedlevel.reset());
TERN_(ABL_PLANAR, planner.bed_level_matrix.set_to_identity());
}
#if EITHER(AUTO_BED_LEVELING_BILINEAR, MESH_BED_LEVELING)
@@ -156,7 +134,7 @@ void reset_bed_level() {
/**
* Print calibration results for plotting or manual frame adjustment.
*/
void print_2d_array(const uint8_t sx, const uint8_t sy, const uint8_t precision, element_2d_fn fn) {
void print_2d_array(const uint8_t sx, const uint8_t sy, const uint8_t precision, const float *values) {
#ifndef SCAD_MESH_OUTPUT
LOOP_L_N(x, sx) {
serial_spaces(precision + (x < 10 ? 3 : 2));
@@ -176,7 +154,7 @@ void reset_bed_level() {
#endif
LOOP_L_N(x, sx) {
SERIAL_CHAR(' ');
const float offset = fn(x, y);
const float offset = values[x * sy + y];
if (!isnan(offset)) {
if (offset >= 0) SERIAL_CHAR('+');
SERIAL_ECHO_F(offset, int(precision));

9
Marlin/src/feature/bedlevel/bedlevel.h
View File

@@ -62,16 +62,13 @@ class TemporaryBedLevelingState {
typedef float bed_mesh_t[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
#include "abl/abl.h"
#include "abl/bbl.h"
#elif ENABLED(AUTO_BED_LEVELING_UBL)
#include "ubl/ubl.h"
#elif ENABLED(MESH_BED_LEVELING)
#include "mbl/mesh_bed_leveling.h"
#endif
#define Z_VALUES(X,Y) Z_VALUES_ARR[X][Y]
#define _GET_MESH_POS(M) { _GET_MESH_X(M.a), _GET_MESH_Y(M.b) }
#if EITHER(AUTO_BED_LEVELING_BILINEAR, MESH_BED_LEVELING)
#include <stdint.h>
@@ -81,7 +78,7 @@ class TemporaryBedLevelingState {
/**
* Print calibration results for plotting or manual frame adjustment.
*/
void print_2d_array(const uint8_t sx, const uint8_t sy, const uint8_t precision, element_2d_fn fn);
void print_2d_array(const uint8_t sx, const uint8_t sy, const uint8_t precision, const float *values);
#endif
@@ -92,7 +89,7 @@ class TemporaryBedLevelingState {
bool valid() const { return pos.x >= 0 && pos.y >= 0; }
#if ENABLED(AUTO_BED_LEVELING_UBL)
xy_pos_t meshpos() {
return { ubl.mesh_index_to_xpos(pos.x), ubl.mesh_index_to_ypos(pos.y) };
return { bedlevel.get_mesh_x(pos.x), bedlevel.get_mesh_y(pos.y) };
}
#endif
operator xy_int8_t&() { return pos; }

6
Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp
View File

@@ -32,7 +32,7 @@
#include "../../../lcd/extui/ui_api.h"
#endif
mesh_bed_leveling mbl;
mesh_bed_leveling bedlevel;
float mesh_bed_leveling::z_offset,
mesh_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y],
@@ -125,9 +125,7 @@
void mesh_bed_leveling::report_mesh() {
SERIAL_ECHOPAIR_F(STRINGIFY(GRID_MAX_POINTS_X) "x" STRINGIFY(GRID_MAX_POINTS_Y) " mesh. Z offset: ", z_offset, 5);
SERIAL_ECHOLNPGM("\nMeasured points:");
print_2d_array(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y, 5,
[](const uint8_t ix, const uint8_t iy) { return z_values[ix][iy]; }
);
print_2d_array(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y, 5, z_values[0]);
}
#endif // MESH_BED_LEVELING

31
Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h
View File

@@ -34,9 +34,6 @@ enum MeshLevelingState : char {
#define MESH_X_DIST (float(MESH_MAX_X - (MESH_MIN_X)) / (GRID_MAX_CELLS_X))
#define MESH_Y_DIST (float(MESH_MAX_Y - (MESH_MIN_Y)) / (GRID_MAX_CELLS_Y))
#define _GET_MESH_X(I) mbl.index_to_xpos[I]
#define _GET_MESH_Y(J) mbl.index_to_ypos[J]
#define Z_VALUES_ARR mbl.z_values
class mesh_bed_leveling {
public:
@@ -56,9 +53,11 @@ public:
return false;
}
static bool mesh_is_valid() { return has_mesh(); }
static void set_z(const int8_t px, const int8_t py, const_float_t z) { z_values[px][py] = z; }
static inline void zigzag(const int8_t index, int8_t &px, int8_t &py) {
static void zigzag(const int8_t index, int8_t &px, int8_t &py) {
px = index % (GRID_MAX_POINTS_X);
py = index / (GRID_MAX_POINTS_X);
if (py & 1) px = (GRID_MAX_POINTS_X) - 1 - px; // Zig zag
@@ -70,6 +69,9 @@ public:
set_z(px, py, z);
}
static float get_mesh_x(const uint8_t i) { return index_to_xpos[i]; }
static float get_mesh_y(const uint8_t i) { return index_to_ypos[i]; }
static int8_t cell_index_x(const_float_t x) {
int8_t cx = (x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST);
return constrain(cx, 0, GRID_MAX_CELLS_X - 1);
@@ -78,10 +80,10 @@ public:
int8_t cy = (y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST);
return constrain(cy, 0, GRID_MAX_CELLS_Y - 1);
}
static inline xy_int8_t cell_indexes(const_float_t x, const_float_t y) {
static xy_int8_t cell_indexes(const_float_t x, const_float_t y) {
return { cell_index_x(x), cell_index_y(y) };
}
static inline xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); }
static xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); }
static int8_t probe_index_x(const_float_t x) {
int8_t px = (x - (MESH_MIN_X) + 0.5f * (MESH_X_DIST)) * RECIPROCAL(MESH_X_DIST);
@@ -91,10 +93,10 @@ public:
int8_t py = (y - (MESH_MIN_Y) + 0.5f * (MESH_Y_DIST)) * RECIPROCAL(MESH_Y_DIST);
return WITHIN(py, 0, (GRID_MAX_POINTS_Y) - 1) ? py : -1;
}
static inline xy_int8_t probe_indexes(const_float_t x, const_float_t y) {
static xy_int8_t probe_indexes(const_float_t x, const_float_t y) {
return { probe_index_x(x), probe_index_y(y) };
}
static inline xy_int8_t probe_indexes(const xy_pos_t &xy) { return probe_indexes(xy.x, xy.y); }
static xy_int8_t probe_indexes(const xy_pos_t &xy) { return probe_indexes(xy.x, xy.y); }
static float calc_z0(const_float_t a0, const_float_t a1, const_float_t z1, const_float_t a2, const_float_t z2) {
const float delta_z = (z2 - z1) / (a2 - a1),
@@ -102,12 +104,9 @@ public:
return z1 + delta_a * delta_z;
}
static float get_z(const xy_pos_t &pos
OPTARG(ENABLE_LEVELING_FADE_HEIGHT, const_float_t factor=1.0f)
) {
#if DISABLED(ENABLE_LEVELING_FADE_HEIGHT)
constexpr float factor = 1.0f;
#endif
static float get_z_offset() { return z_offset; }
static float get_z_correction(const xy_pos_t &pos) {
const xy_int8_t ind = cell_indexes(pos);
const float x1 = index_to_xpos[ind.x], x2 = index_to_xpos[ind.x+1],
y1 = index_to_xpos[ind.y], y2 = index_to_xpos[ind.y+1],
@@ -115,7 +114,7 @@ public:
z2 = calc_z0(pos.x, x1, z_values[ind.x][ind.y+1], x2, z_values[ind.x+1][ind.y+1]),
zf = calc_z0(pos.y, y1, z1, y2, z2);
return z_offset + zf * factor;
return zf;
}
#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)
@@ -123,4 +122,4 @@ public:
#endif
};
extern mesh_bed_leveling mbl;
extern mesh_bed_leveling bedlevel;

28
Marlin/src/feature/bedlevel/ubl/ubl.cpp
View File

@@ -26,7 +26,7 @@
#include "../bedlevel.h"
unified_bed_leveling ubl;
unified_bed_leveling bedlevel;
#include "../../../MarlinCore.h"
#include "../../../gcode/gcode.h"
@@ -180,10 +180,8 @@ void unified_bed_leveling::display_map(const uint8_t map_type) {
SERIAL_EOL();
serial_echo_column_labels(eachsp - 2);
}
else {
SERIAL_ECHOPGM(" for ");
SERIAL_ECHOPGM_P(csv ? PSTR("CSV:\n") : PSTR("LCD:\n"));
}
else
SERIAL_ECHOPGM(" for ", csv ? F("CSV:\n") : F("LCD:\n"));
// Add XY probe offset from extruder because probe.probe_at_point() subtracts them when
// moving to the XY position to be measured. This ensures better agreement between
@@ -213,10 +211,10 @@ void unified_bed_leveling::display_map(const uint8_t map_type) {
// TODO: Display on Graphical LCD
}
else if (isnan(f))
SERIAL_ECHOPGM_P(human ? PSTR(" . ") : PSTR("NAN"));
SERIAL_ECHOF(human ? F(" . ") : F("NAN"));
else if (human || csv) {
if (human && f >= 0.0) SERIAL_CHAR(f > 0 ? '+' : ' '); // Display sign also for positive numbers (' ' for 0)
SERIAL_ECHO_F(f, 3); // Positive: 5 digits, Negative: 6 digits
if (human && f >= 0) SERIAL_CHAR(f > 0 ? '+' : ' '); // Display sign also for positive numbers (' ' for 0)
SERIAL_DECIMAL(f); // Positive: 5 digits, Negative: 6 digits
}
if (csv && i < (GRID_MAX_POINTS_X) - 1) SERIAL_CHAR('\t');
@@ -281,10 +279,10 @@ bool unified_bed_leveling::sanity_check() {
}
#endif
process_subcommands_now_P(G28_STR); // Home
process_subcommands_now_P(PSTR(ALIGN_GCODE "\n" // Align multi z axis if available
PROBE_GCODE "\n" // Build mesh with available hardware
"G29P3\nG29P3")); // Ensure mesh is complete by running smart fill twice
process_subcommands_now(FPSTR(G28_STR)); // Home
process_subcommands_now(F(ALIGN_GCODE "\n" // Align multi z axis if available
PROBE_GCODE "\n" // Build mesh with available hardware
"G29P3\nG29P3")); // Ensure mesh is complete by running smart fill twice
if (parser.seenval('S')) {
char umw_gcode[32];
@@ -292,9 +290,9 @@ bool unified_bed_leveling::sanity_check() {
queue.inject(umw_gcode);
}
process_subcommands_now_P(PSTR("G29A\nG29F10\n" // Set UBL Active & Fade 10
"M140S0\nM104S0\n" // Turn off heaters
"M500")); // Store settings
process_subcommands_now(F("G29A\nG29F10\n" // Set UBL Active & Fade 10
"M140S0\nM104S0\n" // Turn off heaters
"M500")); // Store settings
}
#endif // UBL_MESH_WIZARD

75
Marlin/src/feature/bedlevel/ubl/ubl.h
View File

@@ -70,20 +70,19 @@ private:
static void move_z_with_encoder(const_float_t multiplier);
static float measure_point_with_encoder();
static float measure_business_card_thickness();
static void manually_probe_remaining_mesh(const xy_pos_t&, const_float_t , const_float_t , const bool) _O0;
static void fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) _O0;
static void manually_probe_remaining_mesh(const xy_pos_t&, const_float_t , const_float_t , const bool) __O0;
static void fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) __O0;
#endif
static bool G29_parse_parameters() _O0;
static bool G29_parse_parameters() __O0;
static void shift_mesh_height();
static void probe_entire_mesh(const xy_pos_t &near, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) _O0;
static void probe_entire_mesh(const xy_pos_t &near, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) __O0;
static void tilt_mesh_based_on_3pts(const_float_t z1, const_float_t z2, const_float_t z3);
static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map);
static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir);
static inline bool smart_fill_one(const xy_uint8_t &pos, const xy_uint8_t &dir) {
static bool smart_fill_one(const xy_uint8_t &pos, const xy_uint8_t &dir) {
return smart_fill_one(pos.x, pos.y, dir.x, dir.y);
}
static void smart_fill_mesh();
#if ENABLED(UBL_DEVEL_DEBUGGING)
static void g29_what_command();
@@ -98,17 +97,18 @@ public:
static void report_state();
static void save_ubl_active_state_and_disable();
static void restore_ubl_active_state_and_leave();
static void display_map(const uint8_t) _O0;
static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const xy_pos_t&, const bool=false, MeshFlags *done_flags=nullptr) _O0;
static mesh_index_pair find_furthest_invalid_mesh_point() _O0;
static void display_map(const uint8_t) __O0;
static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const xy_pos_t&, const bool=false, MeshFlags *done_flags=nullptr) __O0;
static mesh_index_pair find_furthest_invalid_mesh_point() __O0;
static void reset();
static void invalidate();
static void set_all_mesh_points_to_value(const_float_t value);
static void adjust_mesh_to_mean(const bool cflag, const_float_t value);
static bool sanity_check();
static void smart_fill_mesh();
static void G29() _O0; // O0 for no optimization
static void smart_fill_wlsf(const_float_t ) _O2; // O2 gives smaller code than Os on A2560
static void G29() __O0; // O0 for no optimization
static void smart_fill_wlsf(const_float_t ) __O2; // O2 gives smaller code than Os on A2560
static int8_t storage_slot;
@@ -120,11 +120,11 @@ public:
static const float _mesh_index_to_xpos[GRID_MAX_POINTS_X],
_mesh_index_to_ypos[GRID_MAX_POINTS_Y];
#if HAS_LCD_MENU
#if HAS_MARLINUI_MENU
static bool lcd_map_control;
static void steppers_were_disabled();
#else
static inline void steppers_were_disabled() {}
static void steppers_were_disabled() {}
#endif
static volatile int16_t encoder_diff; // Volatile because buttons may change it at interrupt time
@@ -157,10 +157,10 @@ public:
return constrain(cell_index_y_raw(y), 0, GRID_MAX_CELLS_Y - 1);
}
static inline xy_int8_t cell_indexes(const_float_t x, const_float_t y) {
static xy_int8_t cell_indexes(const_float_t x, const_float_t y) {
return { cell_index_x(x), cell_index_y(y) };
}
static inline xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); }
static xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); }
static int8_t closest_x_index(const_float_t x) {
const int8_t px = (x - (MESH_MIN_X) + (MESH_X_DIST) * 0.5) * RECIPROCAL(MESH_X_DIST);
@@ -170,7 +170,7 @@ public:
const int8_t py = (y - (MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * RECIPROCAL(MESH_Y_DIST);
return WITHIN(py, 0, (GRID_MAX_POINTS_Y) - 1) ? py : -1;
}
static inline xy_int8_t closest_indexes(const xy_pos_t &xy) {
static xy_int8_t closest_indexes(const xy_pos_t &xy) {
return { closest_x_index(xy.x), closest_y_index(xy.y) };
}
@@ -203,7 +203,7 @@ public:
* z_correction_for_x_on_horizontal_mesh_line is an optimization for
* the case where the printer is making a vertical line that only crosses horizontal mesh lines.
*/
static inline float z_correction_for_x_on_horizontal_mesh_line(const_float_t rx0, const int x1_i, const int yi) {
static float z_correction_for_x_on_horizontal_mesh_line(const_float_t rx0, const int x1_i, const int yi) {
if (!WITHIN(x1_i, 0, (GRID_MAX_POINTS_X) - 1) || !WITHIN(yi, 0, (GRID_MAX_POINTS_Y) - 1)) {
if (DEBUGGING(LEVELING)) {
@@ -215,7 +215,7 @@ public:
return _UBL_OUTER_Z_RAISE;
}
const float xratio = (rx0 - mesh_index_to_xpos(x1_i)) * RECIPROCAL(MESH_X_DIST),
const float xratio = (rx0 - get_mesh_x(x1_i)) * RECIPROCAL(MESH_X_DIST),
z1 = z_values[x1_i][yi];
return z1 + xratio * (z_values[_MIN(x1_i, (GRID_MAX_POINTS_X) - 2) + 1][yi] - z1); // Don't allow x1_i+1 to be past the end of the array
@@ -226,7 +226,7 @@ public:
//
// See comments above for z_correction_for_x_on_horizontal_mesh_line
//
static inline float z_correction_for_y_on_vertical_mesh_line(const_float_t ry0, const int xi, const int y1_i) {
static float z_correction_for_y_on_vertical_mesh_line(const_float_t ry0, const int xi, const int y1_i) {
if (!WITHIN(xi, 0, (GRID_MAX_POINTS_X) - 1) || !WITHIN(y1_i, 0, (GRID_MAX_POINTS_Y) - 1)) {
if (DEBUGGING(LEVELING)) {
@@ -238,7 +238,7 @@ public:
return _UBL_OUTER_Z_RAISE;
}
const float yratio = (ry0 - mesh_index_to_ypos(y1_i)) * RECIPROCAL(MESH_Y_DIST),
const float yratio = (ry0 - get_mesh_y(y1_i)) * RECIPROCAL(MESH_Y_DIST),
z1 = z_values[xi][y1_i];
return z1 + yratio * (z_values[xi][_MIN(y1_i, (GRID_MAX_POINTS_Y) - 2) + 1] - z1); // Don't allow y1_i+1 to be past the end of the array
@@ -264,16 +264,17 @@ public:
return UBL_Z_RAISE_WHEN_OFF_MESH;
#endif
const uint8_t mx = _MIN(cx, (GRID_MAX_POINTS_X) - 2) + 1, my = _MIN(cy, (GRID_MAX_POINTS_Y) - 2) + 1;
const float z1 = calc_z0(rx0, mesh_index_to_xpos(cx), z_values[cx][cy], mesh_index_to_xpos(cx + 1), z_values[mx][cy]);
const float z2 = calc_z0(rx0, mesh_index_to_xpos(cx), z_values[cx][my], mesh_index_to_xpos(cx + 1), z_values[mx][my]);
float z0 = calc_z0(ry0, mesh_index_to_ypos(cy), z1, mesh_index_to_ypos(cy + 1), z2);
const uint8_t mx = _MIN(cx, (GRID_MAX_POINTS_X) - 2) + 1, my = _MIN(cy, (GRID_MAX_POINTS_Y) - 2) + 1,
x0 = get_mesh_x(cx), x1 = get_mesh_x(cx + 1);
const float z1 = calc_z0(rx0, x0, z_values[cx][cy], x1, z_values[mx][cy]),
z2 = calc_z0(rx0, x0, z_values[cx][my], x1, z_values[mx][my]);
float z0 = calc_z0(ry0, get_mesh_y(cy), z1, get_mesh_y(cy + 1), z2);
if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN
z0 = 0.0; // in ubl.z_values[][] and propagate through the
// calculations. If our correction is NAN, we throw it out
// because part of the Mesh is undefined and we don't have the
// information we need to complete the height correction.
if (isnan(z0)) { // If part of the Mesh is undefined, it will show up as NAN
z0 = 0.0; // in z_values[][] and propagate through the calculations.
// If our correction is NAN, we throw it out because part of
// the Mesh is undefined and we don't have the information
// needed to complete the height correction.
if (DEBUGGING(MESH_ADJUST)) DEBUG_ECHOLNPGM("??? Yikes! NAN in ");
}
@@ -285,12 +286,14 @@ public:
return z0;
}
static inline float get_z_correction(const xy_pos_t &pos) { return get_z_correction(pos.x, pos.y); }
static float get_z_correction(const xy_pos_t &pos) { return get_z_correction(pos.x, pos.y); }
static inline float mesh_index_to_xpos(const uint8_t i) {
static constexpr float get_z_offset() { return 0.0f; }
static float get_mesh_x(const uint8_t i) {
return i < (GRID_MAX_POINTS_X) ? pgm_read_float(&_mesh_index_to_xpos[i]) : MESH_MIN_X + i * (MESH_X_DIST);
}
static inline float mesh_index_to_ypos(const uint8_t i) {
static float get_mesh_y(const uint8_t i) {
return i < (GRID_MAX_POINTS_Y) ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
}
@@ -300,18 +303,14 @@ public:
static void line_to_destination_cartesian(const_feedRate_t scaled_fr_mm_s, const uint8_t e);
#endif
static inline bool mesh_is_valid() {
static bool mesh_is_valid() {
GRID_LOOP(x, y) if (isnan(z_values[x][y])) return false;
return true;
}
}; // class unified_bed_leveling
extern unified_bed_leveling ubl;
#define _GET_MESH_X(I) ubl.mesh_index_to_xpos(I)
#define _GET_MESH_Y(J) ubl.mesh_index_to_ypos(J)
#define Z_VALUES_ARR ubl.z_values
extern unified_bed_leveling bedlevel;
// Prevent debugging propagating to other files
#include "../../../core/debug_out.h"

184
Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
View File

@@ -31,7 +31,6 @@
#include "../../../libs/hex_print.h"
#include "../../../module/settings.h"
#include "../../../lcd/marlinui.h"
#include "../../../module/stepper.h"
#include "../../../module/planner.h"
#include "../../../module/motion.h"
#include "../../../module/probe.h"
@@ -57,7 +56,7 @@
#define UBL_G29_P31
#if HAS_LCD_MENU
#if HAS_MARLINUI_MENU
bool unified_bed_leveling::lcd_map_control = false;
@@ -316,7 +315,43 @@ void unified_bed_leveling::G29() {
planner.synchronize();
// Send 'N' to force homing before G29 (internal only)
if (axes_should_home() || parser.seen_test('N')) gcode.home_all_axes();
TERN_(HAS_MULTI_HOTEND, if (active_extruder) tool_change(0));
TERN_(HAS_MULTI_HOTEND, if (active_extruder != 0) tool_change(0, true));
// Position bed horizontally and Z probe vertically.
#if defined(SAFE_BED_LEVELING_START_X) || defined(SAFE_BED_LEVELING_START_Y) || defined(SAFE_BED_LEVELING_START_Z) \
|| defined(SAFE_BED_LEVELING_START_I) || defined(SAFE_BED_LEVELING_START_J) || defined(SAFE_BED_LEVELING_START_K) \
|| defined(SAFE_BED_LEVELING_START_U) || defined(SAFE_BED_LEVELING_START_V) || defined(SAFE_BED_LEVELING_START_W)
xyze_pos_t safe_position = current_position;
#ifdef SAFE_BED_LEVELING_START_X
safe_position.x = SAFE_BED_LEVELING_START_X;
#endif
#ifdef SAFE_BED_LEVELING_START_Y
safe_position.y = SAFE_BED_LEVELING_START_Y;
#endif
#ifdef SAFE_BED_LEVELING_START_Z
safe_position.z = SAFE_BED_LEVELING_START_Z;
#endif
#ifdef SAFE_BED_LEVELING_START_I
safe_position.i = SAFE_BED_LEVELING_START_I;
#endif
#ifdef SAFE_BED_LEVELING_START_J
safe_position.j = SAFE_BED_LEVELING_START_J;
#endif
#ifdef SAFE_BED_LEVELING_START_K
safe_position.k = SAFE_BED_LEVELING_START_K;
#endif
#ifdef SAFE_BED_LEVELING_START_U
safe_position.u = SAFE_BED_LEVELING_START_U;
#endif
#ifdef SAFE_BED_LEVELING_START_V
safe_position.v = SAFE_BED_LEVELING_START_V;
#endif
#ifdef SAFE_BED_LEVELING_START_W
safe_position.w = SAFE_BED_LEVELING_START_W;
#endif
do_blocking_move_to(safe_position);
#endif
}
// Invalidate one or more nearby mesh points, possibly all.
@@ -346,13 +381,14 @@ void unified_bed_leveling::G29() {
if (parser.seen('Q')) {
const int16_t test_pattern = parser.has_value() ? parser.value_int() : -99;
if (!WITHIN(test_pattern, -1, 2)) {
SERIAL_ECHOLNPGM("Invalid test_pattern value. (-1 to 2)\n");
if (!WITHIN(test_pattern, TERN0(UBL_DEVEL_DEBUGGING, -1), 2)) {
SERIAL_ECHOLNPGM("?Invalid (Q) test pattern. (" TERN(UBL_DEVEL_DEBUGGING, "-1", "0") " to 2)\n");
return;
}
SERIAL_ECHOLNPGM("Loading test_pattern values.\n");
SERIAL_ECHOLNPGM("Applying test pattern.\n");
switch (test_pattern) {
default:
case -1: TERN_(UBL_DEVEL_DEBUGGING, g29_eeprom_dump()); break;
case 0:
@@ -366,13 +402,13 @@ void unified_bed_leveling::G29() {
case 1:
LOOP_L_N(x, GRID_MAX_POINTS_X) { // Create a diagonal line several Mesh cells thick that is raised
const uint8_t x2 = x + (x < (GRID_MAX_POINTS_Y) - 1 ? 1 : -1);
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][x2] += 9.999f; // We want the altered line several mesh points thick
#if ENABLED(EXTENSIBLE_UI)
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, (x2), z_values[x][x2]);
#endif
}
break;
@@ -442,7 +478,7 @@ void unified_bed_leveling::G29() {
#endif // HAS_BED_PROBE
case 2: {
#if HAS_LCD_MENU
#if HAS_MARLINUI_MENU
//
// Manually Probe Mesh in areas that can't be reached by the probe
//
@@ -554,7 +590,7 @@ void unified_bed_leveling::G29() {
}
case 4: // Fine Tune (i.e., Edit) the Mesh
#if HAS_LCD_MENU
#if HAS_MARLINUI_MENU
fine_tune_mesh(param.XY_pos, parser.seen_test('T'));
#else
SERIAL_ECHOLNPGM("?P4 is only available when an LCD is present.");
@@ -645,7 +681,7 @@ void unified_bed_leveling::G29() {
LEAVE:
#if HAS_LCD_MENU
#if HAS_MARLINUI_MENU
ui.reset_alert_level();
ui.quick_feedback();
ui.reset_status();
@@ -656,13 +692,13 @@ void unified_bed_leveling::G29() {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
if (probe_deployed) {
planner.synchronize();
gcode.process_subcommands_now_P(PSTR(Z_PROBE_END_SCRIPT));
gcode.process_subcommands_now(F(Z_PROBE_END_SCRIPT));
}
#else
UNUSED(probe_deployed);
#endif
TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index));
TERN_(HAS_MULTI_HOTEND, if (old_tool_index != 0) tool_change(old_tool_index));
return;
}
@@ -724,7 +760,9 @@ void unified_bed_leveling::shift_mesh_height() {
void unified_bed_leveling::probe_entire_mesh(const xy_pos_t &nearby, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) {
probe.deploy(); // Deploy before ui.capture() to allow for PAUSE_BEFORE_DEPLOY_STOW
TERN_(HAS_LCD_MENU, ui.capture());
TERN_(HAS_MARLINUI_MENU, ui.capture());
TERN_(EXTENSIBLE_UI, ExtUI::onLevelingStart());
TERN_(DWIN_LCD_PROUI, DWIN_LevelingStart());
save_ubl_active_state_and_disable(); // No bed level correction so only raw data is obtained
uint8_t count = GRID_MAX_POINTS;
@@ -736,9 +774,9 @@ void unified_bed_leveling::shift_mesh_height() {
const uint8_t point_num = (GRID_MAX_POINTS - count) + 1;
SERIAL_ECHOLNPGM("Probing mesh point ", point_num, "/", GRID_MAX_POINTS, ".");
TERN_(HAS_STATUS_MESSAGE, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_POINT), point_num, int(GRID_MAX_POINTS)));
TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_POINT), point_num, int(GRID_MAX_POINTS)));
#if HAS_LCD_MENU
#if HAS_MARLINUI_MENU
if (ui.button_pressed()) {
ui.quick_feedback(false); // Preserve button state for click-and-hold
SERIAL_ECHOLNPGM("\nMesh only partially populated.\n");
@@ -746,6 +784,7 @@ void unified_bed_leveling::shift_mesh_height() {
ui.quick_feedback();
ui.release();
probe.stow(); // Release UI before stow to allow for PAUSE_BEFORE_DEPLOY_STOW
TERN_(EXTENSIBLE_UI, ExtUI::onLevelingDone());
return restore_ubl_active_state_and_leave();
}
#endif
@@ -773,9 +812,9 @@ void unified_bed_leveling::shift_mesh_height() {
TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(best.pos, ExtUI::G29_FINISH));
// Release UI during stow to allow for PAUSE_BEFORE_DEPLOY_STOW
TERN_(HAS_LCD_MENU, ui.release());
TERN_(HAS_MARLINUI_MENU, ui.release());
probe.stow();
TERN_(HAS_LCD_MENU, ui.capture());
TERN_(HAS_MARLINUI_MENU, ui.capture());
probe.move_z_after_probing();
@@ -785,20 +824,23 @@ void unified_bed_leveling::shift_mesh_height() {
constrain(nearby.x - probe.offset_xy.x, MESH_MIN_X, MESH_MAX_X),
constrain(nearby.y - probe.offset_xy.y, MESH_MIN_Y, MESH_MAX_Y)
);
TERN_(EXTENSIBLE_UI, ExtUI::onLevelingDone());
TERN_(DWIN_LCD_PROUI, DWIN_LevelingDone());
}
#endif // HAS_BED_PROBE
void set_message_with_feedback(PGM_P const msg_P) {
#if HAS_LCD_MENU
ui.set_status_P(msg_P);
void set_message_with_feedback(FSTR_P const fstr) {
#if HAS_MARLINUI_MENU
ui.set_status(fstr);
ui.quick_feedback();
#else
UNUSED(msg_P);
UNUSED(fstr);
#endif
}
#if HAS_LCD_MENU
#if HAS_MARLINUI_MENU
typedef void (*clickFunc_t)();
@@ -850,7 +892,7 @@ void set_message_with_feedback(PGM_P const msg_P) {
planner.synchronize();
SERIAL_ECHOPGM("Place shim under nozzle");
LCD_MESSAGEPGM(MSG_UBL_BC_INSERT);
LCD_MESSAGE(MSG_UBL_BC_INSERT);
ui.return_to_status();
echo_and_take_a_measurement();
@@ -859,7 +901,7 @@ void set_message_with_feedback(PGM_P const msg_P) {
planner.synchronize();
SERIAL_ECHOPGM("Remove shim");
LCD_MESSAGEPGM(MSG_UBL_BC_REMOVE);
LCD_MESSAGE(MSG_UBL_BC_REMOVE);
echo_and_take_a_measurement();
const float z2 = measure_point_with_encoder();
@@ -884,6 +926,7 @@ void set_message_with_feedback(PGM_P const msg_P) {
*/
void unified_bed_leveling::manually_probe_remaining_mesh(const xy_pos_t &pos, const_float_t z_clearance, const_float_t thick, const bool do_ubl_mesh_map) {
ui.capture();
TERN_(EXTENSIBLE_UI, ExtUI::onLevelingStart());
save_ubl_active_state_and_disable(); // No bed level correction so only raw data is obtained
do_blocking_move_to_xy_z(current_position, z_clearance);
@@ -897,15 +940,11 @@ void set_message_with_feedback(PGM_P const msg_P) {
// It doesn't matter if the probe can't reach the NAN location. This is a manual probe.
if (!location.valid()) continue;
const xyz_pos_t ppos = {
mesh_index_to_xpos(lpos.x),
mesh_index_to_ypos(lpos.y),
z_clearance
};
const xyz_pos_t ppos = { get_mesh_x(lpos.x), get_mesh_y(lpos.y), z_clearance };
if (!position_is_reachable(ppos)) break; // SHOULD NOT OCCUR (find_closest_mesh_point only returns reachable points)
LCD_MESSAGEPGM(MSG_UBL_MOVING_TO_NEXT);
LCD_MESSAGE(MSG_UBL_MOVING_TO_NEXT);
do_blocking_move_to(ppos);
do_z_clearance(z_clearance);
@@ -917,11 +956,11 @@ void set_message_with_feedback(PGM_P const msg_P) {
if (parser.seen_test('B')) {
SERIAL_ECHOPGM("Place Shim & Measure");
LCD_MESSAGEPGM(MSG_UBL_BC_INSERT);
LCD_MESSAGE(MSG_UBL_BC_INSERT);
}
else {
SERIAL_ECHOPGM("Measure");
LCD_MESSAGEPGM(MSG_UBL_BC_INSERT2);
LCD_MESSAGE(MSG_UBL_BC_INSERT2);
}
const float z_step = 0.01f; // 0.01mm per encoder tick, occasionally step
@@ -947,6 +986,8 @@ void set_message_with_feedback(PGM_P const msg_P) {
restore_ubl_active_state_and_leave();
do_blocking_move_to_xy_z(pos, Z_CLEARANCE_DEPLOY_PROBE);
TERN_(EXTENSIBLE_UI, ExtUI::onLevelingDone());
}
/**
@@ -974,7 +1015,7 @@ void set_message_with_feedback(PGM_P const msg_P) {
save_ubl_active_state_and_disable();
LCD_MESSAGEPGM(MSG_UBL_FINE_TUNE_MESH);
LCD_MESSAGE(MSG_UBL_FINE_TUNE_MESH);
ui.capture(); // Take over control of the LCD encoder
do_blocking_move_to_xy_z(pos, Z_CLEARANCE_BETWEEN_PROBES); // Move to the given XY with probe clearance
@@ -994,11 +1035,7 @@ void set_message_with_feedback(PGM_P const msg_P) {
done_flags.mark(lpos); // Mark this location as 'adjusted' so a new
// location is used on the next loop
const xyz_pos_t raw = {
mesh_index_to_xpos(lpos.x),
mesh_index_to_ypos(lpos.y),
Z_CLEARANCE_BETWEEN_PROBES
};
const xyz_pos_t raw = { get_mesh_x(lpos.x), get_mesh_y(lpos.y), Z_CLEARANCE_BETWEEN_PROBES };
if (!position_is_reachable(raw)) break; // SHOULD NOT OCCUR (find_closest_mesh_point_of_type only returns reachable)
@@ -1039,7 +1076,7 @@ void set_message_with_feedback(PGM_P const msg_P) {
if (_click_and_hold([]{
ui.return_to_status();
do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES);
set_message_with_feedback(GET_TEXT(MSG_EDITING_STOPPED));
set_message_with_feedback(GET_TEXT_F(MSG_EDITING_STOPPED));
})) break;
// TODO: Disable leveling here so the Z value becomes the 'native' Z value.
@@ -1060,7 +1097,7 @@ void set_message_with_feedback(PGM_P const msg_P) {
do_blocking_move_to_xy_z(pos, Z_CLEARANCE_BETWEEN_PROBES);
LCD_MESSAGEPGM(MSG_UBL_DONE_EDITING_MESH);
LCD_MESSAGE(MSG_UBL_DONE_EDITING_MESH);
SERIAL_ECHOLNPGM("Done Editing Mesh");
if (lcd_map_control)
@@ -1069,7 +1106,7 @@ void set_message_with_feedback(PGM_P const msg_P) {
ui.return_to_status();
}
#endif // HAS_LCD_MENU
#endif // HAS_MARLINUI_MENU
/**
* Parse and validate most G29 parameters, store for use by G29 functions.
@@ -1077,7 +1114,7 @@ void set_message_with_feedback(PGM_P const msg_P) {
bool unified_bed_leveling::G29_parse_parameters() {
bool err_flag = false;
set_message_with_feedback(GET_TEXT(MSG_UBL_DOING_G29));
set_message_with_feedback(GET_TEXT_F(MSG_UBL_DOING_G29));
param.C_constant = 0;
param.R_repetition = 0;
@@ -1200,7 +1237,7 @@ void unified_bed_leveling::save_ubl_active_state_and_disable() {
ubl_state_recursion_chk++;
if (ubl_state_recursion_chk != 1) {
SERIAL_ECHOLNPGM("save_ubl_active_state_and_disabled() called multiple times in a row.");
set_message_with_feedback(GET_TEXT(MSG_UBL_SAVE_ERROR));
set_message_with_feedback(GET_TEXT_F(MSG_UBL_SAVE_ERROR));
return;
}
#endif
@@ -1209,15 +1246,16 @@ void unified_bed_leveling::save_ubl_active_state_and_disable() {
}
void unified_bed_leveling::restore_ubl_active_state_and_leave() {
TERN_(HAS_LCD_MENU, ui.release());
TERN_(HAS_MARLINUI_MENU, ui.release());
#if ENABLED(UBL_DEVEL_DEBUGGING)
if (--ubl_state_recursion_chk) {
SERIAL_ECHOLNPGM("restore_ubl_active_state_and_leave() called too many times.");
set_message_with_feedback(GET_TEXT(MSG_UBL_RESTORE_ERROR));
set_message_with_feedback(GET_TEXT_F(MSG_UBL_RESTORE_ERROR));
return;
}
#endif
set_bed_leveling_enabled(ubl_state_at_invocation);
TERN_(EXTENSIBLE_UI, ExtUI::onLevelingDone());
}
mesh_index_pair unified_bed_leveling::find_furthest_invalid_mesh_point() {
@@ -1230,7 +1268,7 @@ mesh_index_pair unified_bed_leveling::find_furthest_invalid_mesh_point() {
if (!isnan(z_values[i][j])) continue; // Skip valid mesh points
// Skip unreachable points
if (!probe.can_reach(mesh_index_to_xpos(i), mesh_index_to_ypos(j)))
if (!probe.can_reach(get_mesh_x(i), get_mesh_y(j)))
continue;
found_a_NAN = true;
@@ -1282,11 +1320,11 @@ mesh_index_pair unified_bed_leveling::find_furthest_invalid_mesh_point() {
static bool test_func(uint8_t i, uint8_t j, void *data) {
find_closest_t *d = (find_closest_t*)data;
if ( d->type == CLOSEST || d->type == (isnan(ubl.z_values[i][j]) ? INVALID : REAL)
if ( d->type == CLOSEST || d->type == (isnan(bedlevel.z_values[i][j]) ? INVALID : REAL)
|| (d->type == SET_IN_BITMAP && !d->done_flags->marked(i, j))
) {
// Found a Mesh Point of the specified type!
const xy_pos_t mpos = { ubl.mesh_index_to_xpos(i), ubl.mesh_index_to_ypos(j) };
const xy_pos_t mpos = { bedlevel.get_mesh_x(i), bedlevel.get_mesh_y(j) };
// 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.
@@ -1330,7 +1368,7 @@ mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const Mesh
|| (type == SET_IN_BITMAP && !done_flags->marked(i, j))
) {
// 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 = { get_mesh_x(i), get_mesh_y(j) };
// 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.
@@ -1386,10 +1424,10 @@ typedef struct { uint8_t sx, ex, sy, ey; bool yfirst; } smart_fill_info;
void unified_bed_leveling::smart_fill_mesh() {
static const smart_fill_info
info0 PROGMEM = { 0, GRID_MAX_POINTS_X, 0, GRID_MAX_POINTS_Y - 2, false }, // Bottom of the mesh looking up
info1 PROGMEM = { 0, GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y - 1, 0, false }, // Top of the mesh looking down
info2 PROGMEM = { 0, GRID_MAX_POINTS_X - 2, 0, GRID_MAX_POINTS_Y, true }, // Left side of the mesh looking right
info3 PROGMEM = { GRID_MAX_POINTS_X - 1, 0, 0, GRID_MAX_POINTS_Y, true }; // Right side of the mesh looking left
info0 PROGMEM = { 0, GRID_MAX_POINTS_X, 0, (GRID_MAX_POINTS_Y) - 2, false }, // Bottom of the mesh looking up
info1 PROGMEM = { 0, GRID_MAX_POINTS_X, (GRID_MAX_POINTS_Y) - 1, 0, false }, // Top of the mesh looking down
info2 PROGMEM = { 0, (GRID_MAX_POINTS_X) - 2, 0, GRID_MAX_POINTS_Y, true }, // Left side of the mesh looking right
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 };
LOOP_L_N(i, COUNT(info)) {
@@ -1438,7 +1476,7 @@ void unified_bed_leveling::smart_fill_mesh() {
if (do_3_pt_leveling) {
SERIAL_ECHOLNPGM("Tilting mesh (1/3)");
TERN_(HAS_STATUS_MESSAGE, ui.status_printf_P(0, PSTR(S_FMT " 1/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " 1/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
measured_z = probe.probe_at_point(points[0], PROBE_PT_RAISE, param.V_verbosity);
if (isnan(measured_z))
@@ -1457,7 +1495,7 @@ void unified_bed_leveling::smart_fill_mesh() {
if (!abort_flag) {
SERIAL_ECHOLNPGM("Tilting mesh (2/3)");
TERN_(HAS_STATUS_MESSAGE, ui.status_printf_P(0, PSTR(S_FMT " 2/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " 2/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
measured_z = probe.probe_at_point(points[1], PROBE_PT_RAISE, param.V_verbosity);
#ifdef VALIDATE_MESH_TILT
@@ -1477,7 +1515,7 @@ void unified_bed_leveling::smart_fill_mesh() {
if (!abort_flag) {
SERIAL_ECHOLNPGM("Tilting mesh (3/3)");
TERN_(HAS_STATUS_MESSAGE, ui.status_printf_P(0, PSTR(S_FMT " 3/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " 3/3"), GET_TEXT(MSG_LCD_TILTING_MESH)));
measured_z = probe.probe_at_point(points[2], PROBE_PT_LAST_STOW, param.V_verbosity);
#ifdef VALIDATE_MESH_TILT
@@ -1518,7 +1556,7 @@ void unified_bed_leveling::smart_fill_mesh() {
if (!abort_flag) {
SERIAL_ECHOLNPGM("Tilting mesh point ", point_num, "/", total_points, "\n");
TERN_(HAS_STATUS_MESSAGE, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_LCD_TILTING_MESH), point_num, total_points));
TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/%i"), GET_TEXT(MSG_LCD_TILTING_MESH), point_num, total_points));
measured_z = probe.probe_at_point(rpos, parser.seen_test('E') ? PROBE_PT_STOW : PROBE_PT_RAISE, param.V_verbosity); // TODO: Needs error handling
@@ -1578,9 +1616,7 @@ void unified_bed_leveling::smart_fill_mesh() {
matrix_3x3 rotation = matrix_3x3::create_look_at(vector_3(lsf_results.A, lsf_results.B, 1));
GRID_LOOP(i, j) {
float mx = mesh_index_to_xpos(i),
my = mesh_index_to_ypos(j),
mz = z_values[i][j];
float mx = get_mesh_x(i), my = get_mesh_y(j), mz = z_values[i][j];
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR_F("before rotation = [", mx, 7);
@@ -1609,7 +1645,7 @@ void unified_bed_leveling::smart_fill_mesh() {
}
if (DEBUGGING(LEVELING)) {
rotation.debug(PSTR("rotation matrix:\n"));
rotation.debug(F("rotation matrix:\n"));
DEBUG_ECHOPAIR_F("LSF Results A=", lsf_results.A, 7);
DEBUG_ECHOPAIR_F(" B=", lsf_results.B, 7);
DEBUG_ECHOLNPAIR_F(" D=", lsf_results.D, 7);
@@ -1636,14 +1672,14 @@ void unified_bed_leveling::smart_fill_mesh() {
auto normed = [&](const xy_pos_t &pos, const_float_t zadd) {
return normal.x * pos.x + normal.y * pos.y + zadd;
};
auto debug_pt = [](PGM_P const pre, const xy_pos_t &pos, const_float_t zadd) {
d_from(); SERIAL_ECHOPGM_P(pre);
auto debug_pt = [](FSTR_P const pre, const xy_pos_t &pos, const_float_t zadd) {
d_from(); SERIAL_ECHOF(pre);
DEBUG_ECHO_F(normed(pos, zadd), 6);
DEBUG_ECHOLNPAIR_F(" Z error = ", zadd - get_z_correction(pos), 6);
};
debug_pt(PSTR("1st point: "), probe_pt[0], normal.z * z1);
debug_pt(PSTR("2nd point: "), probe_pt[1], normal.z * z2);
debug_pt(PSTR("3rd point: "), probe_pt[2], normal.z * z3);
debug_pt(F("1st point: "), probe_pt[0], normal.z * z1);
debug_pt(F("2nd point: "), probe_pt[1], normal.z * z2);
debug_pt(F("3rd point: "), probe_pt[2], normal.z * z3);
d_from(); DEBUG_ECHOPGM("safe home with Z=");
DEBUG_ECHOLNPAIR_F("0 : ", normed(safe_homing_xy, 0), 6);
d_from(); DEBUG_ECHOPGM("safe home with Z=");
@@ -1677,18 +1713,18 @@ void unified_bed_leveling::smart_fill_mesh() {
xy_pos_t ppos;
LOOP_L_N(ix, GRID_MAX_POINTS_X) {
ppos.x = mesh_index_to_xpos(ix);
ppos.x = get_mesh_x(ix);
LOOP_L_N(iy, GRID_MAX_POINTS_Y) {
ppos.y = mesh_index_to_ypos(iy);
ppos.y = get_mesh_y(iy);
if (isnan(z_values[ix][iy])) {
// undefined mesh point at (ppos.x,ppos.y), compute weighted LSF from original valid mesh points.
incremental_LSF_reset(&lsf_results);
xy_pos_t rpos;
LOOP_L_N(jx, GRID_MAX_POINTS_X) {
rpos.x = mesh_index_to_xpos(jx);
rpos.x = get_mesh_x(jx);
LOOP_L_N(jy, GRID_MAX_POINTS_Y) {
if (TEST(bitmap[jx], jy)) {
rpos.y = mesh_index_to_ypos(jy);
rpos.y = get_mesh_y(jy);
const float rz = z_values[jx][jy],
w = 1.0f + weight_scaled / (rpos - ppos).magnitude();
incremental_WLSF(&lsf_results, rpos, rz, w);
@@ -1747,7 +1783,7 @@ void unified_bed_leveling::smart_fill_mesh() {
SERIAL_ECHOPGM("X-Axis Mesh Points at: ");
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(get_mesh_x(i)), 3);
SERIAL_ECHOPGM(" ");
serial_delay(25);
}
@@ -1755,7 +1791,7 @@ void unified_bed_leveling::smart_fill_mesh() {
SERIAL_ECHOPGM("Y-Axis Mesh Points at: ");
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(get_mesh_y(i)), 3);
SERIAL_ECHOPGM(" ");
serial_delay(25);
}

59
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
View File

@@ -26,7 +26,6 @@
#include "../bedlevel.h"
#include "../../../module/planner.h"
#include "../../../module/stepper.h"
#include "../../../module/motion.h"
#if ENABLED(DELTA)
@@ -36,8 +35,18 @@
#include "../../../MarlinCore.h"
#include <math.h>
//#define DEBUG_UBL_MOTION
#define DEBUG_OUT ENABLED(DEBUG_UBL_MOTION)
#include "../../../core/debug_out.h"
#if !UBL_SEGMENTED
// TODO: The first and last parts of a move might result in very short segment(s)
// after getting split on the cell boundary, so moves like that should not
// get split. This will be most common for moves that start/end near the
// corners of cells. To fix the issue, simply check if the start/end of the line
// is very close to a cell boundary in advance and don't split the line there.
void unified_bed_leveling::line_to_destination_cartesian(const_feedRate_t scaled_fr_mm_s, const uint8_t extruder) {
/**
* Much of the nozzle movement will be within the same cell. So we will do as little computation
@@ -76,8 +85,8 @@
#endif
// The distance is always MESH_X_DIST so multiply by the constant reciprocal.
const float xratio = (end.x - mesh_index_to_xpos(iend.x)) * RECIPROCAL(MESH_X_DIST),
yratio = (end.y - mesh_index_to_ypos(iend.y)) * RECIPROCAL(MESH_Y_DIST),
const float xratio = (end.x - get_mesh_x(iend.x)) * RECIPROCAL(MESH_X_DIST),
yratio = (end.y - get_mesh_y(iend.y)) * RECIPROCAL(MESH_Y_DIST),
z1 = z_values[iend.x][iend.y ] + xratio * (z_values[iend.x + 1][iend.y ] - z_values[iend.x][iend.y ]),
z2 = z_values[iend.x][iend.y + 1] + xratio * (z_values[iend.x + 1][iend.y + 1] - z_values[iend.x][iend.y + 1]);
@@ -139,7 +148,7 @@
icell.y += ineg.y; // Line going down? Just go to the bottom.
while (icell.y != iend.y + ineg.y) {
icell.y += iadd.y;
const float next_mesh_line_y = mesh_index_to_ypos(icell.y);
const float next_mesh_line_y = get_mesh_y(icell.y);
/**
* Skip the calculations for an infinite slope.
@@ -155,7 +164,7 @@
// Replace NAN corrections with 0.0 to prevent NAN propagation.
if (isnan(z0)) z0 = 0.0;
dest.y = mesh_index_to_ypos(icell.y);
dest.y = get_mesh_y(icell.y);
/**
* Without this check, it's possible to generate a zero length move, as in the case where
@@ -176,7 +185,9 @@
dest.z += z0;
planner.buffer_segment(dest, scaled_fr_mm_s, extruder);
} //else printf("FIRST MOVE PRUNED ");
}
else
DEBUG_ECHOLNPGM("[ubl] skip Y segment");
}
// At the final destination? Usually not, but when on a Y Mesh Line it's completed.
@@ -196,7 +207,7 @@
while (icell.x != iend.x + ineg.x) {
icell.x += iadd.x;
dest.x = mesh_index_to_xpos(icell.x);
dest.x = get_mesh_x(icell.x);
dest.y = ratio * dest.x + c; // Calculate Y at the next X mesh line
float z0 = z_correction_for_y_on_vertical_mesh_line(dest.y, icell.x, icell.y)
@@ -225,7 +236,9 @@
dest.z += z0;
if (!planner.buffer_segment(dest, scaled_fr_mm_s, extruder)) break;
} //else printf("FIRST MOVE PRUNED ");
}
else
DEBUG_ECHOLNPGM("[ubl] skip Y segment");
}
if (xy_pos_t(current_position) != xy_pos_t(end))
@@ -245,8 +258,8 @@
while (cnt) {
const float next_mesh_line_x = mesh_index_to_xpos(icell.x + iadd.x),
next_mesh_line_y = mesh_index_to_ypos(icell.y + iadd.y);
const float next_mesh_line_x = get_mesh_x(icell.x + iadd.x),
next_mesh_line_y = get_mesh_y(icell.y + iadd.y);
dest.y = ratio * next_mesh_line_x + c; // Calculate Y at the next X mesh line
dest.x = (next_mesh_line_y - c) / ratio; // Calculate X at the next Y mesh line
@@ -340,7 +353,7 @@
* Returns true if did NOT move, false if moved (requires current_position update).
*/
bool _O2 unified_bed_leveling::line_to_destination_segmented(const_feedRate_t scaled_fr_mm_s) {
bool __O2 unified_bed_leveling::line_to_destination_segmented(const_feedRate_t scaled_fr_mm_s) {
if (!position_is_reachable(destination)) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate
@@ -360,11 +373,12 @@
#endif
NOLESS(segments, 1U); // Must have at least one segment
const float inv_segments = 1.0f / segments, // Reciprocal to save calculation
segment_xyz_mm = SQRT(cart_xy_mm_2 + sq(total.z)) * inv_segments; // Length of each segment
const float inv_segments = 1.0f / segments; // Reciprocal to save calculation
// Add hints to help optimize the move
PlannerHints hints(SQRT(cart_xy_mm_2 + sq(total.z)) * inv_segments); // Length of each segment
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = scaled_fr_mm_s / segment_xyz_mm;
hints.inv_duration = scaled_fr_mm_s / hints.millimeters;
#endif
xyze_float_t diff = total * inv_segments;
@@ -378,13 +392,9 @@
if (!planner.leveling_active || !planner.leveling_active_at_z(destination.z)) {
while (--segments) {
raw += diff;
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm
OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
);
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
}
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, segment_xyz_mm
OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
);
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, hints);
return false; // Did not set current from destination
}
@@ -423,7 +433,7 @@
if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell,
if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points
const xy_pos_t pos = { mesh_index_to_xpos(icell.x), mesh_index_to_ypos(icell.y) };
const xy_pos_t pos = { get_mesh_x(icell.x), get_mesh_y(icell.y) };
xy_pos_t cell = raw - pos;
const float z_xmy0 = (z_x1y0 - z_x0y0) * RECIPROCAL(MESH_X_DIST), // z slope per x along y0 (lower left to lower right)
@@ -450,13 +460,10 @@
if (--segments == 0) raw = destination; // if this is last segment, use destination for exact
const float z_cxcy = (z_cxy0 + z_cxym * cell.y) // interpolated mesh z height along cell.x at cell.y
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
* fade_scaling_factor // apply fade factor to interpolated mesh height
#endif
;
TERN_(ENABLE_LEVELING_FADE_HEIGHT, * fade_scaling_factor); // apply fade factor to interpolated height
const float oldz = raw.z; raw.z += z_cxcy;
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration) );
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
raw.z = oldz;
if (segments == 0) // done with last segment