bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Outdent UBL code

Browse Source
This commit is contained in:
Scott Lahteine 2021-02-23 16:09:54 -06:00 committed by Scott Lahteine
parent 8fca59f63b
commit dc0247c57e
3 changed files with 2044 additions and 2044 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -24,234 +24,234 @@
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
#include "../bedlevel.h" #include "../bedlevel.h"
unified_bed_leveling ubl; unified_bed_leveling ubl;
#include "../../../MarlinCore.h" #include "../../../MarlinCore.h"
#include "../../../gcode/gcode.h" #include "../../../gcode/gcode.h"
#include "../../../module/settings.h" #include "../../../module/settings.h"
#include "../../../module/planner.h" #include "../../../module/planner.h"
#include "../../../module/motion.h" #include "../../../module/motion.h"
#include "../../../module/probe.h" #include "../../../module/probe.h"
#if ENABLED(EXTENSIBLE_UI)
#include "../../../lcd/extui/ui_api.h"
#endif
#include "math.h"
void unified_bed_leveling::echo_name() { SERIAL_ECHOPGM("Unified Bed Leveling"); }
void unified_bed_leveling::report_current_mesh() {
if (!leveling_is_valid()) return;
SERIAL_ECHO_MSG(" G29 I999");
GRID_LOOP(x, y)
if (!isnan(z_values[x][y])) {
SERIAL_ECHO_START();
SERIAL_ECHOPAIR(" M421 I", x, " J", y);
SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, z_values[x][y], 4);
serial_delay(75); // Prevent Printrun from exploding
}
}
void unified_bed_leveling::report_state() {
echo_name();
SERIAL_ECHO_TERNARY(planner.leveling_active, " System v" UBL_VERSION " ", "", "in", "active\n");
serial_delay(50);
}
int8_t unified_bed_leveling::storage_slot;
float unified_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
#define _GRIDPOS(A,N) (MESH_MIN_##A + N * (MESH_##A##_DIST))
const float
unified_bed_leveling::_mesh_index_to_xpos[GRID_MAX_POINTS_X] PROGMEM = ARRAY_N(GRID_MAX_POINTS_X,
_GRIDPOS(X, 0), _GRIDPOS(X, 1), _GRIDPOS(X, 2), _GRIDPOS(X, 3),
_GRIDPOS(X, 4), _GRIDPOS(X, 5), _GRIDPOS(X, 6), _GRIDPOS(X, 7),
_GRIDPOS(X, 8), _GRIDPOS(X, 9), _GRIDPOS(X, 10), _GRIDPOS(X, 11),
_GRIDPOS(X, 12), _GRIDPOS(X, 13), _GRIDPOS(X, 14), _GRIDPOS(X, 15)
),
unified_bed_leveling::_mesh_index_to_ypos[GRID_MAX_POINTS_Y] PROGMEM = ARRAY_N(GRID_MAX_POINTS_Y,
_GRIDPOS(Y, 0), _GRIDPOS(Y, 1), _GRIDPOS(Y, 2), _GRIDPOS(Y, 3),
_GRIDPOS(Y, 4), _GRIDPOS(Y, 5), _GRIDPOS(Y, 6), _GRIDPOS(Y, 7),
_GRIDPOS(Y, 8), _GRIDPOS(Y, 9), _GRIDPOS(Y, 10), _GRIDPOS(Y, 11),
_GRIDPOS(Y, 12), _GRIDPOS(Y, 13), _GRIDPOS(Y, 14), _GRIDPOS(Y, 15)
);
volatile int16_t unified_bed_leveling::encoder_diff;
unified_bed_leveling::unified_bed_leveling() { reset(); }
void unified_bed_leveling::reset() {
const bool was_enabled = planner.leveling_active;
set_bed_leveling_enabled(false);
storage_slot = -1;
ZERO(z_values);
#if ENABLED(EXTENSIBLE_UI) #if ENABLED(EXTENSIBLE_UI)
#include "../../../lcd/extui/ui_api.h" GRID_LOOP(x, y) ExtUI::onMeshUpdate(x, y, 0);
#endif #endif
if (was_enabled) report_current_position();
}
#include "math.h" void unified_bed_leveling::invalidate() {
set_bed_leveling_enabled(false);
set_all_mesh_points_to_value(NAN);
}
void unified_bed_leveling::echo_name() { SERIAL_ECHOPGM("Unified Bed Leveling"); } void unified_bed_leveling::set_all_mesh_points_to_value(const float value) {
GRID_LOOP(x, y) {
z_values[x][y] = value;
TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, value));
}
}
void unified_bed_leveling::report_current_mesh() { #if ENABLED(OPTIMIZED_MESH_STORAGE)
if (!leveling_is_valid()) return;
SERIAL_ECHO_MSG(" G29 I999"); constexpr float mesh_store_scaling = 1000;
GRID_LOOP(x, y) constexpr int16_t Z_STEPS_NAN = INT16_MAX;
if (!isnan(z_values[x][y])) {
SERIAL_ECHO_START(); void unified_bed_leveling::set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values) {
SERIAL_ECHOPAIR(" M421 I", x, " J", y); auto z_to_store = [](const float &z) {
SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, z_values[x][y], 4); if (isnan(z)) return Z_STEPS_NAN;
serial_delay(75); // Prevent Printrun from exploding const int32_t z_scaled = TRUNC(z * mesh_store_scaling);
} if (z_scaled == Z_STEPS_NAN || !WITHIN(z_scaled, INT16_MIN, INT16_MAX))
return Z_STEPS_NAN; // If Z is out of range, return our custom 'NaN'
return int16_t(z_scaled);
};
GRID_LOOP(x, y) stored_values[x][y] = z_to_store(in_values[x][y]);
} }
void unified_bed_leveling::report_state() { void unified_bed_leveling::set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values) {
echo_name(); auto store_to_z = [](const int16_t z_scaled) {
SERIAL_ECHO_TERNARY(planner.leveling_active, " System v" UBL_VERSION " ", "", "in", "active\n"); return z_scaled == Z_STEPS_NAN ? NAN : z_scaled / mesh_store_scaling;
serial_delay(50); };
GRID_LOOP(x, y) out_values[x][y] = store_to_z(stored_values[x][y]);
} }
int8_t unified_bed_leveling::storage_slot; #endif // OPTIMIZED_MESH_STORAGE
float unified_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; static void serial_echo_xy(const uint8_t sp, const int16_t x, const int16_t y) {
SERIAL_ECHO_SP(sp);
SERIAL_CHAR('(');
if (x < 100) { SERIAL_CHAR(' '); if (x < 10) SERIAL_CHAR(' '); }
SERIAL_ECHO(x);
SERIAL_CHAR(',');
if (y < 100) { SERIAL_CHAR(' '); if (y < 10) SERIAL_CHAR(' '); }
SERIAL_ECHO(y);
SERIAL_CHAR(')');
serial_delay(5);
}
#define _GRIDPOS(A,N) (MESH_MIN_##A + N * (MESH_##A##_DIST)) static void serial_echo_column_labels(const uint8_t sp) {
SERIAL_ECHO_SP(7);
const float LOOP_L_N(i, GRID_MAX_POINTS_X) {
unified_bed_leveling::_mesh_index_to_xpos[GRID_MAX_POINTS_X] PROGMEM = ARRAY_N(GRID_MAX_POINTS_X, if (i < 10) SERIAL_CHAR(' ');
_GRIDPOS(X, 0), _GRIDPOS(X, 1), _GRIDPOS(X, 2), _GRIDPOS(X, 3), SERIAL_ECHO(i);
_GRIDPOS(X, 4), _GRIDPOS(X, 5), _GRIDPOS(X, 6), _GRIDPOS(X, 7),
_GRIDPOS(X, 8), _GRIDPOS(X, 9), _GRIDPOS(X, 10), _GRIDPOS(X, 11),
_GRIDPOS(X, 12), _GRIDPOS(X, 13), _GRIDPOS(X, 14), _GRIDPOS(X, 15)
),
unified_bed_leveling::_mesh_index_to_ypos[GRID_MAX_POINTS_Y] PROGMEM = ARRAY_N(GRID_MAX_POINTS_Y,
_GRIDPOS(Y, 0), _GRIDPOS(Y, 1), _GRIDPOS(Y, 2), _GRIDPOS(Y, 3),
_GRIDPOS(Y, 4), _GRIDPOS(Y, 5), _GRIDPOS(Y, 6), _GRIDPOS(Y, 7),
_GRIDPOS(Y, 8), _GRIDPOS(Y, 9), _GRIDPOS(Y, 10), _GRIDPOS(Y, 11),
_GRIDPOS(Y, 12), _GRIDPOS(Y, 13), _GRIDPOS(Y, 14), _GRIDPOS(Y, 15)
);
volatile int16_t unified_bed_leveling::encoder_diff;
unified_bed_leveling::unified_bed_leveling() { reset(); }
void unified_bed_leveling::reset() {
const bool was_enabled = planner.leveling_active;
set_bed_leveling_enabled(false);
storage_slot = -1;
ZERO(z_values);
#if ENABLED(EXTENSIBLE_UI)
GRID_LOOP(x, y) ExtUI::onMeshUpdate(x, y, 0);
#endif
if (was_enabled) report_current_position();
}
void unified_bed_leveling::invalidate() {
set_bed_leveling_enabled(false);
set_all_mesh_points_to_value(NAN);
}
void unified_bed_leveling::set_all_mesh_points_to_value(const float value) {
GRID_LOOP(x, y) {
z_values[x][y] = value;
TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, value));
}
}
#if ENABLED(OPTIMIZED_MESH_STORAGE)
constexpr float mesh_store_scaling = 1000;
constexpr int16_t Z_STEPS_NAN = INT16_MAX;
void unified_bed_leveling::set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values) {
auto z_to_store = [](const float &z) {
if (isnan(z)) return Z_STEPS_NAN;
const int32_t z_scaled = TRUNC(z * mesh_store_scaling);
if (z_scaled == Z_STEPS_NAN || !WITHIN(z_scaled, INT16_MIN, INT16_MAX))
return Z_STEPS_NAN; // If Z is out of range, return our custom 'NaN'
return int16_t(z_scaled);
};
GRID_LOOP(x, y) stored_values[x][y] = z_to_store(in_values[x][y]);
}
void unified_bed_leveling::set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values) {
auto store_to_z = [](const int16_t z_scaled) {
return z_scaled == Z_STEPS_NAN ? NAN : z_scaled / mesh_store_scaling;
};
GRID_LOOP(x, y) out_values[x][y] = store_to_z(stored_values[x][y]);
}
#endif // OPTIMIZED_MESH_STORAGE
static void serial_echo_xy(const uint8_t sp, const int16_t x, const int16_t y) {
SERIAL_ECHO_SP(sp); SERIAL_ECHO_SP(sp);
SERIAL_CHAR('('); }
if (x < 100) { SERIAL_CHAR(' '); if (x < 10) SERIAL_CHAR(' '); } serial_delay(10);
SERIAL_ECHO(x); }
SERIAL_CHAR(',');
if (y < 100) { SERIAL_CHAR(' '); if (y < 10) SERIAL_CHAR(' '); } /**
SERIAL_ECHO(y); * Produce one of these mesh maps:
SERIAL_CHAR(')'); * 0: Human-readable
serial_delay(5); * 1: CSV format for spreadsheet import
* 2: TODO: Display on Graphical LCD
* 4: Compact Human-Readable
*/
void unified_bed_leveling::display_map(const int map_type) {
const bool was = gcode.set_autoreport_paused(true);
constexpr uint8_t eachsp = 1 + 6 + 1, // [-3.567]
twixt = eachsp * (GRID_MAX_POINTS_X) - 9 * 2; // Leading 4sp, Coordinates 9sp each
const bool human = !(map_type & 0x3), csv = map_type == 1, lcd = map_type == 2, comp = map_type & 0x4;
SERIAL_ECHOPGM("\nBed Topography Report");
if (human) {
SERIAL_ECHOLNPGM(":\n");
serial_echo_xy(4, MESH_MIN_X, MESH_MAX_Y);
serial_echo_xy(twixt, MESH_MAX_X, MESH_MAX_Y);
SERIAL_EOL();
serial_echo_column_labels(eachsp - 2);
}
else {
SERIAL_ECHOPGM(" for ");
serialprintPGM(csv ? PSTR("CSV:\n") : PSTR("LCD:\n"));
} }
static void serial_echo_column_labels(const uint8_t sp) { // Add XY probe offset from extruder because probe.probe_at_point() subtracts them when
SERIAL_ECHO_SP(7); // moving to the XY position to be measured. This ensures better agreement between
// the current Z position after G28 and the mesh values.
const xy_int8_t curr = closest_indexes(xy_pos_t(current_position) + probe.offset_xy);
if (!lcd) SERIAL_EOL();
for (int8_t j = GRID_MAX_POINTS_Y - 1; j >= 0; j--) {
// Row Label (J index)
if (human) {
if (j < 10) SERIAL_CHAR(' ');
SERIAL_ECHO(j);
SERIAL_ECHOPGM(" |");
}
// Row Values (I indexes)
LOOP_L_N(i, GRID_MAX_POINTS_X) { LOOP_L_N(i, GRID_MAX_POINTS_X) {
if (i < 10) SERIAL_CHAR(' ');
SERIAL_ECHO(i); // Opening Brace or Space
SERIAL_ECHO_SP(sp); const bool is_current = i == curr.x && j == curr.y;
if (human) SERIAL_CHAR(is_current ? '[' : ' ');
// Z Value at current I, J
const float f = z_values[i][j];
if (lcd) {
// TODO: Display on Graphical LCD
}
else if (isnan(f))
serialprintPGM(human ? PSTR(" . ") : PSTR("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 (csv && i < GRID_MAX_POINTS_X - 1) SERIAL_CHAR('\t');
// Closing Brace or Space
if (human) SERIAL_CHAR(is_current ? ']' : ' ');
SERIAL_FLUSHTX();
idle_no_sleep();
} }
serial_delay(10);
}
/**
* Produce one of these mesh maps:
* 0: Human-readable
* 1: CSV format for spreadsheet import
* 2: TODO: Display on Graphical LCD
* 4: Compact Human-Readable
*/
void unified_bed_leveling::display_map(const int map_type) {
const bool was = gcode.set_autoreport_paused(true);
constexpr uint8_t eachsp = 1 + 6 + 1, // [-3.567]
twixt = eachsp * (GRID_MAX_POINTS_X) - 9 * 2; // Leading 4sp, Coordinates 9sp each
const bool human = !(map_type & 0x3), csv = map_type == 1, lcd = map_type == 2, comp = map_type & 0x4;
SERIAL_ECHOPGM("\nBed Topography Report");
if (human) {
SERIAL_ECHOLNPGM(":\n");
serial_echo_xy(4, MESH_MIN_X, MESH_MAX_Y);
serial_echo_xy(twixt, MESH_MAX_X, MESH_MAX_Y);
SERIAL_EOL();
serial_echo_column_labels(eachsp - 2);
}
else {
SERIAL_ECHOPGM(" for ");
serialprintPGM(csv ? PSTR("CSV:\n") : PSTR("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
// the current Z position after G28 and the mesh values.
const xy_int8_t curr = closest_indexes(xy_pos_t(current_position) + probe.offset_xy);
if (!lcd) SERIAL_EOL(); if (!lcd) SERIAL_EOL();
for (int8_t j = GRID_MAX_POINTS_Y - 1; j >= 0; j--) {
// Row Label (J index) // A blank line between rows (unless compact)
if (human) { if (j && human && !comp) SERIAL_ECHOLNPGM(" |");
if (j < 10) SERIAL_CHAR(' ');
SERIAL_ECHO(j);
SERIAL_ECHOPGM(" |");
}
// Row Values (I indexes)
LOOP_L_N(i, GRID_MAX_POINTS_X) {
// Opening Brace or Space
const bool is_current = i == curr.x && j == curr.y;
if (human) SERIAL_CHAR(is_current ? '[' : ' ');
// Z Value at current I, J
const float f = z_values[i][j];
if (lcd) {
// TODO: Display on Graphical LCD
}
else if (isnan(f))
serialprintPGM(human ? PSTR(" . ") : PSTR("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 (csv && i < GRID_MAX_POINTS_X - 1) SERIAL_CHAR('\t');
// Closing Brace or Space
if (human) SERIAL_CHAR(is_current ? ']' : ' ');
SERIAL_FLUSHTX();
idle_no_sleep();
}
if (!lcd) SERIAL_EOL();
// A blank line between rows (unless compact)
if (j && human && !comp) SERIAL_ECHOLNPGM(" |");
}
if (human) {
serial_echo_column_labels(eachsp - 2);
SERIAL_EOL();
serial_echo_xy(4, MESH_MIN_X, MESH_MIN_Y);
serial_echo_xy(twixt, MESH_MAX_X, MESH_MIN_Y);
SERIAL_EOL();
SERIAL_EOL();
}
gcode.set_autoreport_paused(was);
} }
bool unified_bed_leveling::sanity_check() { if (human) {
uint8_t error_flag = 0; serial_echo_column_labels(eachsp - 2);
SERIAL_EOL();
if (settings.calc_num_meshes() < 1) { serial_echo_xy(4, MESH_MIN_X, MESH_MIN_Y);
SERIAL_ECHOLNPGM("?Mesh too big for EEPROM."); serial_echo_xy(twixt, MESH_MAX_X, MESH_MIN_Y);
error_flag++; SERIAL_EOL();
} SERIAL_EOL();
return !!error_flag;
} }
gcode.set_autoreport_paused(was);
}
bool unified_bed_leveling::sanity_check() {
uint8_t error_flag = 0;
if (settings.calc_num_meshes() < 1) {
SERIAL_ECHOLNPGM("?Mesh too big for EEPROM.");
error_flag++;
}
return !!error_flag;
}
#endif // AUTO_BED_LEVELING_UBL #endif // AUTO_BED_LEVELING_UBL

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

@ -46,275 +46,275 @@ struct mesh_index_pair;
#endif #endif
class unified_bed_leveling { class unified_bed_leveling {
private: private:
static int g29_verbose_level, static int g29_verbose_level,
g29_phase_value, g29_phase_value,
g29_repetition_cnt, g29_repetition_cnt,
g29_storage_slot, g29_storage_slot,
g29_map_type; g29_map_type;
static bool g29_c_flag; static bool g29_c_flag;
static float g29_card_thickness, static float g29_card_thickness,
g29_constant; g29_constant;
static xy_pos_t g29_pos; static xy_pos_t g29_pos;
static xy_bool_t xy_seen; static xy_bool_t xy_seen;
#if HAS_BED_PROBE #if HAS_BED_PROBE
static int g29_grid_size; static int g29_grid_size;
#endif #endif
#if IS_NEWPANEL #if IS_NEWPANEL
static void move_z_with_encoder(const float &multiplier); static void move_z_with_encoder(const float &multiplier);
static float measure_point_with_encoder(); static float measure_point_with_encoder();
static float measure_business_card_thickness(); static float measure_business_card_thickness();
static void manually_probe_remaining_mesh(const xy_pos_t&, const float&, const float&, const bool) _O0; static void manually_probe_remaining_mesh(const xy_pos_t&, const float&, const float&, const bool) _O0;
static void fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) _O0; static void fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) _O0;
#endif #endif
static bool g29_parameter_parsing() _O0; static bool g29_parameter_parsing() _O0;
static void shift_mesh_height(); 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 &z1, const float &z2, const float &z3); static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map); 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 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 inline 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); return smart_fill_one(pos.x, pos.y, dir.x, dir.y);
} }
static void smart_fill_mesh(); static void smart_fill_mesh();
#if ENABLED(UBL_DEVEL_DEBUGGING) #if ENABLED(UBL_DEVEL_DEBUGGING)
static void g29_what_command(); static void g29_what_command();
static void g29_eeprom_dump(); static void g29_eeprom_dump();
static void g29_compare_current_mesh_to_stored_mesh(); static void g29_compare_current_mesh_to_stored_mesh();
#endif #endif
public: public:
static void echo_name(); static void echo_name();
static void report_current_mesh(); static void report_current_mesh();
static void report_state(); static void report_state();
static void save_ubl_active_state_and_disable(); static void save_ubl_active_state_and_disable();
static void restore_ubl_active_state_and_leave(); static void restore_ubl_active_state_and_leave();
static void display_map(const int) _O0; static void display_map(const int) _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_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 mesh_index_pair find_furthest_invalid_mesh_point() _O0;
static void reset(); static void reset();
static void invalidate(); static void invalidate();
static void set_all_mesh_points_to_value(const float value); static void set_all_mesh_points_to_value(const float value);
static void adjust_mesh_to_mean(const bool cflag, const float value); static void adjust_mesh_to_mean(const bool cflag, const float value);
static bool sanity_check(); static bool sanity_check();
static void G29() _O0; // O0 for no optimization static void G29() _O0; // O0 for no optimization
static void smart_fill_wlsf(const float &) _O2; // O2 gives smaller code than Os on A2560 static void smart_fill_wlsf(const float &) _O2; // O2 gives smaller code than Os on A2560
static int8_t storage_slot; static int8_t storage_slot;
static bed_mesh_t z_values; static bed_mesh_t z_values;
#if ENABLED(OPTIMIZED_MESH_STORAGE) #if ENABLED(OPTIMIZED_MESH_STORAGE)
static void set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values); static void set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values);
static void set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values); static void set_mesh_from_store(const mesh_store_t &stored_values, bed_mesh_t &out_values);
#endif #endif
static const float _mesh_index_to_xpos[GRID_MAX_POINTS_X], static const float _mesh_index_to_xpos[GRID_MAX_POINTS_X],
_mesh_index_to_ypos[GRID_MAX_POINTS_Y]; _mesh_index_to_ypos[GRID_MAX_POINTS_Y];
#if HAS_LCD_MENU #if HAS_LCD_MENU
static bool lcd_map_control; static bool lcd_map_control;
static void steppers_were_disabled(); static void steppers_were_disabled();
#else #else
static inline void steppers_were_disabled() {} static inline void steppers_were_disabled() {}
#endif #endif
static volatile int16_t encoder_diff; // Volatile because buttons may changed it at interrupt time static volatile int16_t encoder_diff; // Volatile because buttons may changed it at interrupt time
unified_bed_leveling(); unified_bed_leveling();
FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; } FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
static int8_t cell_index_x_raw(const float &x) { static int8_t cell_index_x_raw(const float &x) {
return FLOOR((x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST)); return FLOOR((x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST));
}
static int8_t cell_index_y_raw(const float &y) {
return FLOOR((y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST));
}
static int8_t cell_index_x_valid(const float &x) {
return WITHIN(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X - 2));
}
static int8_t cell_index_y_valid(const float &y) {
return WITHIN(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y - 2));
}
static int8_t cell_index_x(const float &x) {
return constrain(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X) - 2);
}
static int8_t cell_index_y(const float &y) {
return constrain(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y) - 2);
}
static inline xy_int8_t cell_indexes(const float &x, const float &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 int8_t closest_x_index(const float &x) {
const int8_t px = (x - (MESH_MIN_X) + (MESH_X_DIST) * 0.5) * RECIPROCAL(MESH_X_DIST);
return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
}
static int8_t closest_y_index(const float &y) {
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) {
return { closest_x_index(xy.x), closest_y_index(xy.y) };
}
/**
* z2 --|
* z0 | |
* | | + (z2-z1)
* z1 | | |
* ---+-------------+--------+-- --|
* a1 a0 a2
* |<---delta_a---------->|
*
* calc_z0 is the basis for all the Mesh Based correction. It is used to
* find the expected Z Height at a position between two known Z-Height locations.
*
* It is fairly expensive with its 4 floating point additions and 2 floating point
* multiplications.
*/
FORCE_INLINE static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
}
#ifdef UBL_Z_RAISE_WHEN_OFF_MESH
#define _UBL_OUTER_Z_RAISE UBL_Z_RAISE_WHEN_OFF_MESH
#else
#define _UBL_OUTER_Z_RAISE NAN
#endif
/**
* 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 &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)) {
if (WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("yi"); else DEBUG_ECHOPGM("x1_i");
DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(rx0=", rx0, ",x1_i=", x1_i, ",yi=", yi, ")");
}
// The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN.
return _UBL_OUTER_Z_RAISE;
} }
static int8_t cell_index_y_raw(const float &y) { const float xratio = (rx0 - mesh_index_to_xpos(x1_i)) * RECIPROCAL(MESH_X_DIST),
return FLOOR((y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_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
// If it is, it is clamped to the last element of the
// z_values[][] array and no correction is applied.
}
//
// 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 &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)) {
if (WITHIN(xi, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("y1_i"); else DEBUG_ECHOPGM("xi");
DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ry0=", ry0, ", xi=", xi, ", y1_i=", y1_i, ")");
}
// The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN.
return _UBL_OUTER_Z_RAISE;
} }
static int8_t cell_index_x_valid(const float &x) { const float yratio = (ry0 - mesh_index_to_ypos(y1_i)) * RECIPROCAL(MESH_Y_DIST),
return WITHIN(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X - 2)); z1 = z_values[xi][y1_i];
}
static int8_t cell_index_y_valid(const float &y) { 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
return WITHIN(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y - 2)); // If it is, it is clamped to the last element of the
} // z_values[][] array and no correction is applied.
}
static int8_t cell_index_x(const float &x) { /**
return constrain(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X) - 2); * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first
} * does a linear interpolation along both of the bounding X-Mesh-Lines to find the
* Z-Height at both ends. Then it does a linear interpolation of these heights based
static int8_t cell_index_y(const float &y) { * on the Y position within the cell.
return constrain(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y) - 2); */
} static float get_z_correction(const float &rx0, const float &ry0) {
const int8_t cx = cell_index_x(rx0), cy = cell_index_y(ry0); // return values are clamped
static inline xy_int8_t cell_indexes(const float &x, const float &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 int8_t closest_x_index(const float &x) {
const int8_t px = (x - (MESH_MIN_X) + (MESH_X_DIST) * 0.5) * RECIPROCAL(MESH_X_DIST);
return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
}
static int8_t closest_y_index(const float &y) {
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) {
return { closest_x_index(xy.x), closest_y_index(xy.y) };
}
/** /**
* z2 --| * Check if the requested location is off the mesh. If so, and
* z0 | | * UBL_Z_RAISE_WHEN_OFF_MESH is specified, that value is returned.
* | | + (z2-z1)
* z1 | | |
* ---+-------------+--------+-- --|
* a1 a0 a2
* |<---delta_a---------->|
*
* calc_z0 is the basis for all the Mesh Based correction. It is used to
* find the expected Z Height at a position between two known Z-Height locations.
*
* It is fairly expensive with its 4 floating point additions and 2 floating point
* multiplications.
*/ */
FORCE_INLINE static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
}
#ifdef UBL_Z_RAISE_WHEN_OFF_MESH #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
#define _UBL_OUTER_Z_RAISE UBL_Z_RAISE_WHEN_OFF_MESH if (!WITHIN(rx0, MESH_MIN_X, MESH_MAX_X) || !WITHIN(ry0, MESH_MIN_Y, MESH_MAX_Y))
#else return UBL_Z_RAISE_WHEN_OFF_MESH;
#define _UBL_OUTER_Z_RAISE NAN
#endif #endif
/** const float z1 = calc_z0(rx0,
* z_correction_for_x_on_horizontal_mesh_line is an optimization for mesh_index_to_xpos(cx), z_values[cx][cy],
* the case where the printer is making a vertical line that only crosses horizontal mesh lines. mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][cy]);
*/
static inline float z_correction_for_x_on_horizontal_mesh_line(const float &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)) { const float z2 = calc_z0(rx0,
if (WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("yi"); else DEBUG_ECHOPGM("x1_i"); mesh_index_to_xpos(cx), z_values[cx][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1],
DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(rx0=", rx0, ",x1_i=", x1_i, ",yi=", yi, ")"); mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1]);
}
// The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN. float z0 = calc_z0(ry0,
return _UBL_OUTER_Z_RAISE; mesh_index_to_ypos(cy), z1,
} mesh_index_to_ypos(cy + 1), z2);
const float xratio = (rx0 - mesh_index_to_xpos(x1_i)) * RECIPROCAL(MESH_X_DIST), if (DEBUGGING(MESH_ADJUST)) {
z1 = z_values[x1_i][yi]; DEBUG_ECHOPAIR(" raw get_z_correction(", rx0);
DEBUG_CHAR(','); DEBUG_ECHO(ry0);
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 DEBUG_ECHOPAIR_F(") = ", z0, 6);
// If it is, it is clamped to the last element of the DEBUG_ECHOLNPAIR_F(" >>>---> ", z0, 6);
// z_values[][] array and no correction is applied.
} }
// if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN
// See comments above for z_correction_for_x_on_horizontal_mesh_line z0 = 0.0; // in ubl.z_values[][] and propagate through the
// // calculations. If our correction is NAN, we throw it out
static inline float z_correction_for_y_on_vertical_mesh_line(const float &ry0, const int xi, const int y1_i) { // because part of the Mesh is undefined and we don't have the
if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 1)) { // information we need to complete the height correction.
if (DEBUGGING(LEVELING)) {
if (WITHIN(xi, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("y1_i"); else DEBUG_ECHOPGM("xi");
DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ry0=", ry0, ", xi=", xi, ", y1_i=", y1_i, ")");
}
// The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN.
return _UBL_OUTER_Z_RAISE;
}
const float yratio = (ry0 - mesh_index_to_ypos(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
// If it is, it is clamped to the last element of the
// z_values[][] array and no correction is applied.
}
/**
* This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first
* does a linear interpolation along both of the bounding X-Mesh-Lines to find the
* Z-Height at both ends. Then it does a linear interpolation of these heights based
* on the Y position within the cell.
*/
static float get_z_correction(const float &rx0, const float &ry0) {
const int8_t cx = cell_index_x(rx0), cy = cell_index_y(ry0); // return values are clamped
/**
* Check if the requested location is off the mesh. If so, and
* UBL_Z_RAISE_WHEN_OFF_MESH is specified, that value is returned.
*/
#ifdef UBL_Z_RAISE_WHEN_OFF_MESH
if (!WITHIN(rx0, MESH_MIN_X, MESH_MAX_X) || !WITHIN(ry0, MESH_MIN_Y, MESH_MAX_Y))
return UBL_Z_RAISE_WHEN_OFF_MESH;
#endif
const float z1 = calc_z0(rx0,
mesh_index_to_xpos(cx), z_values[cx][cy],
mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][cy]);
const float z2 = calc_z0(rx0,
mesh_index_to_xpos(cx), z_values[cx][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1],
mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1]);
float z0 = calc_z0(ry0,
mesh_index_to_ypos(cy), z1,
mesh_index_to_ypos(cy + 1), z2);
if (DEBUGGING(MESH_ADJUST)) { if (DEBUGGING(MESH_ADJUST)) {
DEBUG_ECHOPAIR(" raw get_z_correction(", rx0); DEBUG_ECHOPAIR("??? Yikes! NAN in get_z_correction(", rx0);
DEBUG_CHAR(','); DEBUG_ECHO(ry0); DEBUG_CHAR(',');
DEBUG_ECHOPAIR_F(") = ", z0, 6); DEBUG_ECHO(ry0);
DEBUG_ECHOLNPAIR_F(" >>>---> ", z0, 6); DEBUG_CHAR(')');
DEBUG_EOL();
} }
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 (DEBUGGING(MESH_ADJUST)) {
DEBUG_ECHOPAIR("??? Yikes! NAN in get_z_correction(", rx0);
DEBUG_CHAR(',');
DEBUG_ECHO(ry0);
DEBUG_CHAR(')');
DEBUG_EOL();
}
}
return z0;
} }
static inline float get_z_correction(const xy_pos_t &pos) { return get_z_correction(pos.x, pos.y); } return z0;
}
static inline 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 inline float mesh_index_to_xpos(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); 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 inline float mesh_index_to_ypos(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); return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
} }
#if UBL_SEGMENTED #if UBL_SEGMENTED
static bool line_to_destination_segmented(const feedRate_t &scaled_fr_mm_s); static bool line_to_destination_segmented(const feedRate_t &scaled_fr_mm_s);
#else #else
static void line_to_destination_cartesian(const feedRate_t &scaled_fr_mm_s, const uint8_t e); static void line_to_destination_cartesian(const feedRate_t &scaled_fr_mm_s, const uint8_t e);
#endif #endif
static inline bool mesh_is_valid() { static inline bool mesh_is_valid() {
GRID_LOOP(x, y) if (isnan(z_values[x][y])) return false; GRID_LOOP(x, y) if (isnan(z_values[x][y])) return false;
return true; return true;
} }
}; // class unified_bed_leveling }; // class unified_bed_leveling

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

File diff suppressed because it is too large Load Diff