From 1661902e2f74e266025128b0d10f8e16f28b3ae9 Mon Sep 17 00:00:00 2001 From: bubnikv Date: Mon, 14 May 2018 11:32:19 +0200 Subject: [PATCH] ENABLE_LEVELING_FADE_HEIGHT --- Firmware/Configuration_prusa.h | 2 ++ Firmware/Marlin_main.cpp | 8 ++++-- Firmware/mesh_bed_leveling.cpp | 16 +++++++++++ Firmware/mesh_bed_leveling.h | 50 ++++++++++++++++++++++++++++++++-- Firmware/planner.cpp | 6 ++-- 5 files changed, 74 insertions(+), 8 deletions(-) diff --git a/Firmware/Configuration_prusa.h b/Firmware/Configuration_prusa.h index a7579d063..9ed148983 100644 --- a/Firmware/Configuration_prusa.h +++ b/Firmware/Configuration_prusa.h @@ -23,6 +23,8 @@ GENERAL SETTINGS #define MK3 #define EINSY_HIGH_SAMPLE_RATE +#define ENABLE_LEVELING_FADE_HEIGHT + // Uncomment the below for the E3D PT100 temperature sensor (with or without PT100 Amplifier) //#define E3D_PT100_EXTRUDER_WITH_AMP //#define E3D_PT100_EXTRUDER_NO_AMP diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index 999f34c6b..aa7b2f462 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -4929,9 +4929,11 @@ case 404: //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp break; #endif - - - + #ifdef ENABLE_LEVELING_FADE_HEIGHT + case 420: // M420 Z[height] Sets the Z fade height (0 or none to disable) + if (code_seen('Z')) mbl.set_z_fade_height(code_value()); + break; + #endif case 500: // M500 Store settings in EEPROM { diff --git a/Firmware/mesh_bed_leveling.cpp b/Firmware/mesh_bed_leveling.cpp index 506d3746a..b096d2394 100644 --- a/Firmware/mesh_bed_leveling.cpp +++ b/Firmware/mesh_bed_leveling.cpp @@ -13,6 +13,14 @@ void mesh_bed_leveling::reset() { for (int y = 0; y < MESH_NUM_Y_POINTS; y++) for (int x = 0; x < MESH_NUM_X_POINTS; x++) z_values[y][x] = 0; + +#ifdef ENABLE_LEVELING_FADE_HEIGHT + z_avg = 0.f; + z_fade = 0.; + z_prev = FLT_MAX; + // Fade factor, calculated for z_prev. + fade_factor = 0.; +#endif } static inline bool vec_undef(const float v[2]) @@ -173,6 +181,14 @@ void mesh_bed_leveling::upsample_3x3() } } */ + +#ifdef ENABLE_LEVELING_FADE_HEIGHT + z_avg = 0; + for (int j = 0; j < MESH_NUM_Y_POINTS; ++ j) + for (int i = 0; i < MESH_NUM_X_POINTS; ++ i) + z_avg += z_values[j][i]; + z_avg /= float(MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS); +#endif } #endif diff --git a/Firmware/mesh_bed_leveling.h b/Firmware/mesh_bed_leveling.h index bf3729558..8bdbdfdac 100644 --- a/Firmware/mesh_bed_leveling.h +++ b/Firmware/mesh_bed_leveling.h @@ -1,5 +1,7 @@ #include "Marlin.h" +#include "float.h" + #ifdef MESH_BED_LEVELING #define MEAS_NUM_X_DIST (float(MESH_MAX_X - MESH_MIN_X)/float(MESH_MEAS_NUM_X_POINTS - 1)) @@ -12,10 +14,25 @@ class mesh_bed_leveling { public: uint8_t active; float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS]; + +#ifdef ENABLE_LEVELING_FADE_HEIGHT + // Average Z height of z_values. + float z_avg; + // Z height at which to fade to zero. + float z_fade; + // Previous z, at which fade_factor was calculated. + float z_prev; + // Fade factor, calculated for z_prev. + float fade_factor; +#endif mesh_bed_leveling(); void reset(); + +#ifdef ENABLE_LEVELING_FADE_HEIGHT + void set_z_fade_height(float z) { z_fade = (z > 0.f) ? z : 0.f; z_prev = - FLT_MAX; } +#endif #if MESH_NUM_X_POINTS>=5 && MESH_NUM_Y_POINTS>=5 && (MESH_NUM_X_POINTS&1)==1 && (MESH_NUM_Y_POINTS&1)==1 void upsample_3x3(); @@ -43,9 +60,33 @@ public: return i - 1; } - float get_z(float x, float y) { + float get_z(float x, float y, float z) { int i, j; float s, t; + +#ifdef ENABLE_LEVELING_FADE_HEIGHT + if (z != z_prev) { + if (z > z_fade || z_fade == 0.) + fade_factor = 0.f; + else if (z <= 0.f) + fade_factor = 1.f; + else { + fade_factor = (z_fade - z) / z_fade; + } +#if 0 + SERIAL_ECHOPGM("fading z: "); + MYSERIAL.print(z); + SERIAL_ECHOPGM(", fade: "); + MYSERIAL.print(z_fade); + SERIAL_ECHOLNPGM(", fade_factor: "); + MYSERIAL.print(fade_factor); + SERIAL_ECHOLNPGM(""); +#endif + z_prev = z; + } + if (fade_factor == 0.f) + return z_avg; +#endif #if MESH_NUM_X_POINTS==3 && MESH_NUM_Y_POINTS==3 #define MESH_MID_X (0.5f*(MESH_MIN_X+MESH_MAX_X)) @@ -115,7 +156,12 @@ public: float si = 1.f-s; float z0 = si * z_values[j ][i] + s * z_values[j ][i+1]; float z1 = si * z_values[j+1][i] + s * z_values[j+1][i+1]; - return (1.f-t) * z0 + t * z1; + return +#ifdef ENABLE_LEVELING_FADE_HEIGHT + z_avg + fade_factor * ((1.f-t) * z0 + t * z1 - z_avg); +#else + ((1.f-t) * z0 + t * z1); +#endif } }; diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index bdfc4ca21..966fd1d56 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -563,7 +563,7 @@ void planner_abort_hard() // Apply the mesh bed leveling correction to the Z axis. #ifdef MESH_BED_LEVELING if (mbl.active) - current_position[Z_AXIS] -= mbl.get_z(current_position[X_AXIS], current_position[Y_AXIS]); + current_position[Z_AXIS] -= mbl.get_z(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]); #endif // Apply inverse world correction matrix. machine2world(current_position[X_AXIS], current_position[Y_AXIS]); @@ -668,7 +668,7 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); #ifdef MESH_BED_LEVELING if (mbl.active){ - target[Z_AXIS] = lround((z+mbl.get_z(x, y))*axis_steps_per_unit[Z_AXIS]); + target[Z_AXIS] = lround((z+mbl.get_z(x, y, z))*axis_steps_per_unit[Z_AXIS]); }else{ target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); } @@ -1171,7 +1171,7 @@ void plan_set_position(float x, float y, float z, const float &e) position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); #ifdef MESH_BED_LEVELING if (mbl.active){ - position[Z_AXIS] = lround((z+mbl.get_z(x, y))*axis_steps_per_unit[Z_AXIS]); + position[Z_AXIS] = lround((z+mbl.get_z(x, y, z))*axis_steps_per_unit[Z_AXIS]); }else{ position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); }