2017-03-18 15:14:31 +00:00
|
|
|
/**
|
|
|
|
* Marlin 3D Printer Firmware
|
|
|
|
* Copyright (C) 2016 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 <http://www.gnu.org/licenses/>.
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "Marlin.h"
|
|
|
|
#include "math.h"
|
|
|
|
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_UBL)
|
2017-03-24 05:53:37 +00:00
|
|
|
|
2017-03-18 15:14:31 +00:00
|
|
|
#include "UBL.h"
|
|
|
|
#include "hex_print_routines.h"
|
2017-03-21 06:30:26 +00:00
|
|
|
extern int ubl_eeprom_start;
|
2017-03-18 15:14:31 +00:00
|
|
|
|
2017-03-24 05:53:37 +00:00
|
|
|
/**
|
|
|
|
* These support functions allow the use of large bit arrays of flags that take very
|
|
|
|
* little RAM. Currently they are limited to being 16x16 in size. Changing the declaration
|
|
|
|
* to unsigned long will allow us to go to 32x32 if higher resolution Mesh's are needed
|
|
|
|
* in the future.
|
|
|
|
*/
|
|
|
|
void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y) { CBI(bits[y], x); }
|
|
|
|
void bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { SBI(bits[y], x); }
|
|
|
|
bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { return TEST(bits[y], x); }
|
|
|
|
|
2017-03-29 00:45:54 +00:00
|
|
|
static void serial_echo_xy(const uint16_t x, const uint16_t y) {
|
|
|
|
SERIAL_CHAR('(');
|
|
|
|
SERIAL_ECHO(x);
|
|
|
|
SERIAL_CHAR(',');
|
|
|
|
SERIAL_ECHO(y);
|
|
|
|
SERIAL_CHAR(')');
|
|
|
|
safe_delay(10);
|
|
|
|
}
|
|
|
|
|
|
|
|
static void serial_echo_10x_spaces() {
|
|
|
|
for (uint8_t i = UBL_MESH_NUM_X_POINTS - 1; --i;) {
|
|
|
|
SERIAL_ECHOPGM(" ");
|
|
|
|
#if TX_BUFFER_SIZE > 0
|
|
|
|
MYSERIAL.flushTX();
|
|
|
|
#endif
|
|
|
|
safe_delay(10);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-03-18 15:14:31 +00:00
|
|
|
/**
|
2017-03-20 06:42:41 +00:00
|
|
|
* These variables used to be declared inside the unified_bed_leveling class. We are going to
|
|
|
|
* still declare them within the .cpp file for bed leveling. But there is only one instance of
|
|
|
|
* the bed leveling object and we can get rid of a level of inderection by not making them
|
|
|
|
* 'member data'. So, in the interest of speed, we do it this way. On a 32-bit CPU they can be
|
|
|
|
* moved back inside the bed leveling class.
|
2017-03-18 15:14:31 +00:00
|
|
|
*/
|
|
|
|
float last_specified_z,
|
|
|
|
fade_scaling_factor_for_current_height,
|
|
|
|
z_values[UBL_MESH_NUM_X_POINTS][UBL_MESH_NUM_Y_POINTS],
|
2017-03-20 06:42:41 +00:00
|
|
|
mesh_index_to_x_location[UBL_MESH_NUM_X_POINTS + 1], // +1 just because of paranoia that we might end up on the
|
|
|
|
mesh_index_to_y_location[UBL_MESH_NUM_Y_POINTS + 1]; // the last Mesh Line and that is the start of a whole new cell
|
2017-03-18 15:14:31 +00:00
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
unified_bed_leveling::unified_bed_leveling() {
|
2017-03-18 15:14:31 +00:00
|
|
|
for (uint8_t i = 0; i <= UBL_MESH_NUM_X_POINTS; i++) // We go one past what we expect to ever need for safety
|
2017-03-20 06:42:41 +00:00
|
|
|
mesh_index_to_x_location[i] = double(UBL_MESH_MIN_X) + double(MESH_X_DIST) * double(i);
|
2017-03-18 15:14:31 +00:00
|
|
|
|
|
|
|
for (uint8_t i = 0; i <= UBL_MESH_NUM_Y_POINTS; i++) // We go one past what we expect to ever need for safety
|
2017-03-20 06:42:41 +00:00
|
|
|
mesh_index_to_y_location[i] = double(UBL_MESH_MIN_Y) + double(MESH_Y_DIST) * double(i);
|
2017-03-18 15:14:31 +00:00
|
|
|
|
|
|
|
reset();
|
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
void unified_bed_leveling::store_state() {
|
2017-03-24 05:53:37 +00:00
|
|
|
const uint16_t i = UBL_LAST_EEPROM_INDEX;
|
|
|
|
eeprom_write_block((void *)&ubl.state, (void *)i, sizeof(state));
|
2017-03-18 15:14:31 +00:00
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
void unified_bed_leveling::load_state() {
|
2017-03-24 05:53:37 +00:00
|
|
|
const uint16_t i = UBL_LAST_EEPROM_INDEX;
|
|
|
|
eeprom_read_block((void *)&ubl.state, (void *)i, sizeof(state));
|
2017-03-18 15:14:31 +00:00
|
|
|
|
|
|
|
if (sanity_check())
|
|
|
|
SERIAL_PROTOCOLLNPGM("?In load_state() sanity_check() failed.\n");
|
|
|
|
|
2017-03-24 05:53:37 +00:00
|
|
|
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
|
|
|
/**
|
|
|
|
* These lines can go away in a few weeks. They are just
|
|
|
|
* to make sure people updating thier firmware won't be using
|
|
|
|
* an incomplete Bed_Leveling.state structure. For speed
|
|
|
|
* we now multiply by the inverse of the Fade Height instead of
|
|
|
|
* dividing by it. Soon... all of the old structures will be
|
|
|
|
* updated, but until then, we try to ease the transition
|
|
|
|
* for our Beta testers.
|
|
|
|
*/
|
|
|
|
if (ubl.state.g29_fade_height_multiplier != 1.0 / ubl.state.g29_correction_fade_height) {
|
|
|
|
ubl.state.g29_fade_height_multiplier = 1.0 / ubl.state.g29_correction_fade_height;
|
|
|
|
store_state();
|
|
|
|
}
|
|
|
|
#endif
|
2017-03-18 15:14:31 +00:00
|
|
|
}
|
|
|
|
|
2017-03-24 05:53:37 +00:00
|
|
|
void unified_bed_leveling::load_mesh(const int16_t m) {
|
|
|
|
int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
|
2017-03-18 15:14:31 +00:00
|
|
|
|
|
|
|
if (m == -1) {
|
|
|
|
SERIAL_PROTOCOLLNPGM("?No mesh saved in EEPROM. Zeroing mesh in memory.\n");
|
|
|
|
reset();
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
if (m < 0 || m >= j || ubl_eeprom_start <= 0) {
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_PROTOCOLLNPGM("?EEPROM storage not available to load mesh.\n");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2017-03-24 05:53:37 +00:00
|
|
|
j = UBL_LAST_EEPROM_INDEX - (m + 1) * sizeof(z_values);
|
2017-03-29 00:45:54 +00:00
|
|
|
eeprom_read_block((void *)&z_values, (void *)j, sizeof(z_values));
|
2017-03-18 15:14:31 +00:00
|
|
|
|
2017-03-29 00:45:54 +00:00
|
|
|
SERIAL_PROTOCOLPAIR("Mesh loaded from slot ", m);
|
|
|
|
SERIAL_PROTOCOLLNPAIR(" at offset 0x", hex_word(j));
|
2017-03-18 15:14:31 +00:00
|
|
|
}
|
|
|
|
|
2017-03-24 05:53:37 +00:00
|
|
|
void unified_bed_leveling::store_mesh(const int16_t m) {
|
|
|
|
int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
|
2017-03-18 15:14:31 +00:00
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
if (m < 0 || m >= j || ubl_eeprom_start <= 0) {
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_PROTOCOLLNPGM("?EEPROM storage not available to load mesh.\n");
|
|
|
|
SERIAL_PROTOCOL(m);
|
|
|
|
SERIAL_PROTOCOLLNPGM(" mesh slots available.\n");
|
|
|
|
SERIAL_PROTOCOLLNPAIR("E2END : ", E2END);
|
2017-03-24 05:53:37 +00:00
|
|
|
SERIAL_PROTOCOLLNPAIR("k : ", (int)UBL_LAST_EEPROM_INDEX);
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_PROTOCOLLNPAIR("j : ", j);
|
|
|
|
SERIAL_PROTOCOLLNPAIR("m : ", m);
|
|
|
|
SERIAL_EOL;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2017-03-24 05:53:37 +00:00
|
|
|
j = UBL_LAST_EEPROM_INDEX - (m + 1) * sizeof(z_values);
|
2017-03-18 15:14:31 +00:00
|
|
|
eeprom_write_block((const void *)&z_values, (void *)j, sizeof(z_values));
|
|
|
|
|
2017-03-29 00:45:54 +00:00
|
|
|
SERIAL_PROTOCOLPAIR("Mesh saved in slot ", m);
|
|
|
|
SERIAL_PROTOCOLLNPAIR(" at offset 0x", hex_word(j));
|
2017-03-18 15:14:31 +00:00
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
void unified_bed_leveling::reset() {
|
2017-03-18 15:14:31 +00:00
|
|
|
state.active = false;
|
|
|
|
state.z_offset = 0;
|
2017-03-20 06:42:41 +00:00
|
|
|
state.eeprom_storage_slot = -1;
|
2017-03-18 15:14:31 +00:00
|
|
|
|
|
|
|
ZERO(z_values);
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
last_specified_z = -999.9;
|
|
|
|
fade_scaling_factor_for_current_height = 0.0;
|
2017-03-18 15:14:31 +00:00
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
void unified_bed_leveling::invalidate() {
|
2017-03-29 00:45:54 +00:00
|
|
|
print_hex_word((uint16_t)this);
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_EOL;
|
|
|
|
|
|
|
|
state.active = false;
|
|
|
|
state.z_offset = 0;
|
|
|
|
for (int x = 0; x < UBL_MESH_NUM_X_POINTS; x++)
|
|
|
|
for (int y = 0; y < UBL_MESH_NUM_Y_POINTS; y++)
|
|
|
|
z_values[x][y] = NAN;
|
|
|
|
}
|
|
|
|
|
2017-03-24 05:53:37 +00:00
|
|
|
void unified_bed_leveling::display_map(const int map_type) {
|
2017-03-18 15:14:31 +00:00
|
|
|
|
2017-03-29 00:45:54 +00:00
|
|
|
const bool map0 = map_type == 0;
|
2017-03-18 15:14:31 +00:00
|
|
|
|
2017-03-29 00:45:54 +00:00
|
|
|
if (map0) {
|
|
|
|
SERIAL_PROTOCOLLNPGM("\nBed Topography Report:\n");
|
|
|
|
serial_echo_xy(0, UBL_MESH_NUM_Y_POINTS - 1);
|
|
|
|
SERIAL_ECHOPGM(" ");
|
2017-03-25 23:37:54 +00:00
|
|
|
}
|
2017-03-18 15:14:31 +00:00
|
|
|
|
2017-03-29 00:45:54 +00:00
|
|
|
if (map0) {
|
|
|
|
serial_echo_10x_spaces();
|
|
|
|
serial_echo_xy(UBL_MESH_NUM_X_POINTS - 1, UBL_MESH_NUM_Y_POINTS - 1);
|
|
|
|
SERIAL_EOL;
|
|
|
|
serial_echo_xy(UBL_MESH_MIN_X, UBL_MESH_MIN_Y);
|
|
|
|
serial_echo_10x_spaces();
|
|
|
|
serial_echo_xy(UBL_MESH_MAX_X, UBL_MESH_MAX_Y);
|
|
|
|
SERIAL_EOL;
|
2017-03-25 23:37:54 +00:00
|
|
|
}
|
2017-03-18 15:14:31 +00:00
|
|
|
|
2017-03-29 00:45:54 +00:00
|
|
|
const float current_xi = ubl.get_cell_index_x(current_position[X_AXIS] + (MESH_X_DIST) / 2.0),
|
|
|
|
current_yi = ubl.get_cell_index_y(current_position[Y_AXIS] + (MESH_Y_DIST) / 2.0);
|
|
|
|
|
|
|
|
for (uint8_t j = UBL_MESH_NUM_Y_POINTS - 1; j >= 0; j--) {
|
|
|
|
for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) {
|
|
|
|
const bool is_current = i == current_xi && j == current_yi;
|
2017-03-18 15:14:31 +00:00
|
|
|
|
|
|
|
// is the nozzle here? if so, mark the number
|
2017-03-29 00:45:54 +00:00
|
|
|
if (map0)
|
|
|
|
SERIAL_CHAR(is_current ? '[' : ' ');
|
|
|
|
|
|
|
|
const float f = z_values[i][j];
|
|
|
|
if (isnan(f)) {
|
|
|
|
serialprintPGM(map0 ? PSTR(" . ") : PSTR("NAN"));
|
|
|
|
}
|
2017-03-18 15:14:31 +00:00
|
|
|
else {
|
|
|
|
// if we don't do this, the columns won't line up nicely
|
2017-03-29 00:45:54 +00:00
|
|
|
if (f >= 0.0 && map0) SERIAL_CHAR(' ');
|
2017-03-24 23:35:10 +00:00
|
|
|
SERIAL_PROTOCOL_F(f, 3);
|
2017-03-18 15:14:31 +00:00
|
|
|
idle();
|
|
|
|
}
|
2017-03-29 00:45:54 +00:00
|
|
|
if (!map0 && i < UBL_MESH_NUM_X_POINTS - 1)
|
|
|
|
SERIAL_CHAR(',');
|
2017-03-25 23:37:54 +00:00
|
|
|
|
2017-03-29 00:45:54 +00:00
|
|
|
#if TX_BUFFER_SIZE > 0
|
2017-03-24 23:35:10 +00:00
|
|
|
MYSERIAL.flushTX();
|
|
|
|
#endif
|
2017-03-27 04:49:56 +00:00
|
|
|
safe_delay(15);
|
2017-03-29 00:45:54 +00:00
|
|
|
if (map0) {
|
|
|
|
SERIAL_CHAR(is_current ? ']' : ' ');
|
2017-03-25 23:37:54 +00:00
|
|
|
SERIAL_CHAR(' ');
|
|
|
|
}
|
2017-03-18 15:14:31 +00:00
|
|
|
}
|
|
|
|
SERIAL_EOL;
|
2017-03-29 00:45:54 +00:00
|
|
|
if (j && map0) { // we want the (0,0) up tight against the block of numbers
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_CHAR(' ');
|
|
|
|
SERIAL_EOL;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-03-29 00:45:54 +00:00
|
|
|
if (map0) {
|
|
|
|
serial_echo_xy(UBL_MESH_MIN_X, UBL_MESH_MIN_Y);
|
|
|
|
SERIAL_ECHOPGM(" ");
|
|
|
|
serial_echo_10x_spaces();
|
|
|
|
serial_echo_xy(UBL_MESH_MAX_X, UBL_MESH_MIN_Y);
|
|
|
|
SERIAL_EOL;
|
|
|
|
serial_echo_xy(0, 0);
|
|
|
|
SERIAL_ECHOPGM(" ");
|
|
|
|
serial_echo_10x_spaces();
|
|
|
|
serial_echo_xy(UBL_MESH_NUM_X_POINTS - 1, 0);
|
2017-03-25 23:37:54 +00:00
|
|
|
SERIAL_EOL;
|
2017-03-24 23:35:10 +00:00
|
|
|
}
|
2017-03-18 15:14:31 +00:00
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
bool unified_bed_leveling::sanity_check() {
|
2017-03-18 15:14:31 +00:00
|
|
|
uint8_t error_flag = 0;
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
if (state.n_x != UBL_MESH_NUM_X_POINTS) {
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_PROTOCOLLNPGM("?UBL_MESH_NUM_X_POINTS set wrong\n");
|
|
|
|
error_flag++;
|
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
if (state.n_y != UBL_MESH_NUM_Y_POINTS) {
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_PROTOCOLLNPGM("?UBL_MESH_NUM_Y_POINTS set wrong\n");
|
|
|
|
error_flag++;
|
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
if (state.mesh_x_min != UBL_MESH_MIN_X) {
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_PROTOCOLLNPGM("?UBL_MESH_MIN_X set wrong\n");
|
|
|
|
error_flag++;
|
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
if (state.mesh_y_min != UBL_MESH_MIN_Y) {
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_PROTOCOLLNPGM("?UBL_MESH_MIN_Y set wrong\n");
|
|
|
|
error_flag++;
|
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
if (state.mesh_x_max != UBL_MESH_MAX_X) {
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_PROTOCOLLNPGM("?UBL_MESH_MAX_X set wrong\n");
|
|
|
|
error_flag++;
|
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
if (state.mesh_y_max != UBL_MESH_MAX_Y) {
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_PROTOCOLLNPGM("?UBL_MESH_MAX_Y set wrong\n");
|
|
|
|
error_flag++;
|
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
if (state.mesh_x_dist != MESH_X_DIST) {
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_PROTOCOLLNPGM("?MESH_X_DIST set wrong\n");
|
|
|
|
error_flag++;
|
|
|
|
}
|
|
|
|
|
2017-03-20 06:42:41 +00:00
|
|
|
if (state.mesh_y_dist != MESH_Y_DIST) {
|
2017-03-18 15:14:31 +00:00
|
|
|
SERIAL_PROTOCOLLNPGM("?MESH_Y_DIST set wrong\n");
|
|
|
|
error_flag++;
|
|
|
|
}
|
|
|
|
|
2017-03-24 05:53:37 +00:00
|
|
|
const int j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
|
2017-03-18 15:14:31 +00:00
|
|
|
if (j < 1) {
|
|
|
|
SERIAL_PROTOCOLLNPGM("?No EEPROM storage available for a mesh of this size.\n");
|
|
|
|
error_flag++;
|
|
|
|
}
|
|
|
|
|
|
|
|
// SERIAL_PROTOCOLPGM("?sanity_check() return value: ");
|
|
|
|
// SERIAL_PROTOCOL(error_flag);
|
|
|
|
// SERIAL_EOL;
|
|
|
|
|
|
|
|
return !!error_flag;
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif // AUTO_BED_LEVELING_UBL
|