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

⚡️ Improve Sensorless homing/probing accuracy for G28, G33, M48 (#24220)

Browse Source 
Co-Authored-By: Robby Candra <robbycandra.mail@gmail.com>
Co-Authored-By: ellensp <530024+ellensp@users.noreply.github.com>
This commit is contained in:
lujios
2022-06-03 07:19:25 +02:00
committed by Scott Lahteine
parent 0253500ccd
commit fcef8d946c
12 changed files with 212 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Marlin/src/module/delta.cpp
View File

@ -60,6 +60,10 @@ xy_float_t delta_tower[ABC];
abc_float_t delta_diagonal_rod_2_tower;
float delta_clip_start_height = Z_MAX_POS;
abc_float_t delta_diagonal_rod_trim;
#if HAS_DELTA_SENSORLESS_PROBING
abc_float_t offset_sensorless_adj{0};
float largest_sensorless_adj = 0;
#endif
float delta_safe_distance_from_top();
@ -233,6 +237,9 @@ void home_delta() {
TERN_(I_SENSORLESS, sensorless_t stealth_states_i = start_sensorless_homing_per_axis(I_AXIS));
TERN_(J_SENSORLESS, sensorless_t stealth_states_j = start_sensorless_homing_per_axis(J_AXIS));
TERN_(K_SENSORLESS, sensorless_t stealth_states_k = start_sensorless_homing_per_axis(K_AXIS));
#if SENSORLESS_STALLGUARD_DELAY
safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
#endif
#endif
// Move all carriages together linearly until an endstop is hit.
@ -249,6 +256,9 @@ void home_delta() {
TERN_(I_SENSORLESS, end_sensorless_homing_per_axis(I_AXIS, stealth_states_i));
TERN_(J_SENSORLESS, end_sensorless_homing_per_axis(J_AXIS, stealth_states_j));
TERN_(K_SENSORLESS, end_sensorless_homing_per_axis(K_AXIS, stealth_states_k));
#if SENSORLESS_STALLGUARD_DELAY
safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
#endif
#endif
endstops.validate_homing_move();

11
Marlin/src/module/delta.h
View File

@ -38,6 +38,10 @@ extern xy_float_t delta_tower[ABC];
extern abc_float_t delta_diagonal_rod_2_tower;
extern float delta_clip_start_height;
extern abc_float_t delta_diagonal_rod_trim;
#if HAS_DELTA_SENSORLESS_PROBING
extern abc_float_t offset_sensorless_adj;
extern float largest_sensorless_adj;
#endif
/**
* Recalculate factors used for delta kinematics whenever
@ -45,6 +49,13 @@ extern abc_float_t delta_diagonal_rod_trim;
*/
void recalc_delta_settings();
/**
* Get a safe radius for calibration
*/
#if HAS_DELTA_SENSORLESS_PROBING
static constexpr float sensorless_radius_factor = 0.7f;
#endif
/**
* Delta Inverse Kinematics
*

66
Marlin/src/module/endstops.cpp
View File

@ -31,6 +31,9 @@
#include "temperature.h"
#include "../lcd/marlinui.h"
#define DEBUG_OUT BOTH(USE_SENSORLESS, DEBUG_LEVELING_FEATURE)
#include "../core/debug_out.h"
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
#include HAL_PATH(../HAL, endstop_interrupts.h)
#endif
@ -1355,3 +1358,66 @@ void Endstops::update() {
}
#endif // PINS_DEBUGGING
#if USE_SENSORLESS
/**
* Change TMC driver currents to N##_CURRENT_HOME, saving the current configuration of each.
*/
void Endstops::set_homing_current(const bool onoff) {
#define HAS_CURRENT_HOME(N) (defined(N##_CURRENT_HOME) && N##_CURRENT_HOME != N##_CURRENT)
#define HAS_DELTA_X_CURRENT (ENABLED(DELTA) && HAS_CURRENT_HOME(X))
#define HAS_DELTA_Y_CURRENT (ENABLED(DELTA) && HAS_CURRENT_HOME(Y))
#if HAS_DELTA_X_CURRENT || HAS_DELTA_Y_CURRENT || HAS_CURRENT_HOME(Z)
#if HAS_DELTA_X_CURRENT
static int16_t saved_current_x;
#endif
#if HAS_DELTA_Y_CURRENT
static int16_t saved_current_y;
#endif
#if HAS_CURRENT_HOME(Z)
static int16_t saved_current_z;
#endif
auto debug_current_on = [](PGM_P const s, const int16_t a, const int16_t b) {
if (DEBUGGING(LEVELING)) { DEBUG_ECHOPGM_P(s); DEBUG_ECHOLNPGM(" current: ", a, " -> ", b); }
};
if (onoff) {
#if HAS_DELTA_X_CURRENT
saved_current_x = stepperX.getMilliamps();
stepperX.rms_current(X_CURRENT_HOME);
debug_current_on(PSTR("X"), saved_current_x, X_CURRENT_HOME);
#endif
#if HAS_DELTA_Y_CURRENT
saved_current_y = stepperY.getMilliamps();
stepperY.rms_current(Y_CURRENT_HOME);
debug_current_on(PSTR("Y"), saved_current_y, Y_CURRENT_HOME);
#endif
#if HAS_CURRENT_HOME(Z)
saved_current_z = stepperZ.getMilliamps();
stepperZ.rms_current(Z_CURRENT_HOME);
debug_current_on(PSTR("Z"), saved_current_z, Z_CURRENT_HOME);
#endif
}
else {
#if HAS_DELTA_X_CURRENT
stepperX.rms_current(saved_current_x);
debug_current_on(PSTR("X"), X_CURRENT_HOME, saved_current_x);
#endif
#if HAS_DELTA_Y_CURRENT
stepperY.rms_current(saved_current_y);
debug_current_on(PSTR("Y"), Y_CURRENT_HOME, saved_current_y);
#endif
#if HAS_CURRENT_HOME(Z)
stepperZ.rms_current(saved_current_z);
debug_current_on(PSTR("Z"), Z_CURRENT_HOME, saved_current_z);
#endif
}
TERN_(IMPROVE_HOMING_RELIABILITY, planner.enable_stall_prevention(onoff));
#if SENSORLESS_STALLGUARD_DELAY
safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
#endif
#endif // XYZ
}
#endif

5
Marlin/src/module/endstops.h
View File

@ -241,6 +241,11 @@ class Endstops {
static void clear_endstop_state();
static bool tmc_spi_homing_check();
#endif
public:
// Basic functions for Sensorless Homing
#if USE_SENSORLESS
static void set_homing_current(const bool onoff);
#endif
};
extern Endstops endstops;

14
Marlin/src/module/motion.cpp
View File

@ -1541,7 +1541,12 @@ void prepare_line_to_destination() {
}
// Disable stealthChop if used. Enable diag1 pin on driver.
TERN_(SENSORLESS_HOMING, stealth_states = start_sensorless_homing_per_axis(axis));
#if ENABLED(SENSORLESS_HOMING)
stealth_states = start_sensorless_homing_per_axis(axis);
#if SENSORLESS_STALLGUARD_DELAY
safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
#endif
#endif
}
#if EITHER(MORGAN_SCARA, MP_SCARA)
@ -1577,7 +1582,12 @@ void prepare_line_to_destination() {
endstops.validate_homing_move();
// Re-enable stealthChop if used. Disable diag1 pin on driver.
TERN_(SENSORLESS_HOMING, end_sensorless_homing_per_axis(axis, stealth_states));
#if ENABLED(SENSORLESS_HOMING)
end_sensorless_homing_per_axis(axis, stealth_states);
#if SENSORLESS_STALLGUARD_DELAY
safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
#endif
#endif
}
}

5
Marlin/src/module/planner.cpp
View File

@ -1657,10 +1657,7 @@ void Planner::quick_stop() {
// forced to empty, there's no risk the ISR will touch this.
delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
#if HAS_WIRED_LCD
// Clear the accumulated runtime
clear_block_buffer_runtime();
#endif
TERN_(HAS_WIRED_LCD, clear_block_buffer_runtime()); // Clear the accumulated runtime
// Make sure to drop any attempt of queuing moves for 1 second
cleaning_buffer_counter = TEMP_TIMER_FREQUENCY;

103
Marlin/src/module/probe.cpp
View File

@ -103,7 +103,7 @@ xyz_pos_t Probe::offset; // Initialized by settings.load()
#endif
#if ENABLED(SENSORLESS_PROBING)
Probe::sense_bool_t Probe::test_sensitivity;
Probe::sense_bool_t Probe::test_sensitivity = { true, true, true };
#endif
#if ENABLED(Z_PROBE_SLED)
@ -531,12 +531,12 @@ bool Probe::probe_down_to_z(const_float_t z, const_feedRate_t fr_mm_s) {
#if ENABLED(SENSORLESS_PROBING)
sensorless_t stealth_states { false };
#if HAS_DELTA_SENSORLESS_PROBING
if (test_sensitivity.x) stealth_states.x = tmc_enable_stallguard(stepperX); // Delta watches all DIAG pins for a stall
if (test_sensitivity.x) stealth_states.x = tmc_enable_stallguard(stepperX); // Delta watches all DIAG pins for a stall
if (test_sensitivity.y) stealth_states.y = tmc_enable_stallguard(stepperY);
#endif
if (test_sensitivity.z) stealth_states.z = tmc_enable_stallguard(stepperZ); // All machines will check Z-DIAG for stall
if (test_sensitivity.z) stealth_states.z = tmc_enable_stallguard(stepperZ); // All machines will check Z-DIAG for stall
endstops.set_homing_current(true); // The "homing" current also applies to probing
endstops.enable(true);
set_homing_current(true); // The "homing" current also applies to probing
#endif
TERN_(HAS_QUIET_PROBING, set_probing_paused(true));
@ -553,6 +553,11 @@ bool Probe::probe_down_to_z(const_float_t z, const_feedRate_t fr_mm_s) {
#endif
;
// Offset sensorless probing
#if HAS_DELTA_SENSORLESS_PROBING
if (probe_triggered) probe.refresh_largest_sensorless_adj();
#endif
TERN_(HAS_QUIET_PROBING, set_probing_paused(false));
// Re-enable stealthChop if used. Disable diag1 pin on driver.
@ -563,7 +568,7 @@ bool Probe::probe_down_to_z(const_float_t z, const_feedRate_t fr_mm_s) {
if (test_sensitivity.y) tmc_disable_stallguard(stepperY, stealth_states.y);
#endif
if (test_sensitivity.z) tmc_disable_stallguard(stepperZ, stealth_states.z);
set_homing_current(false);
endstops.set_homing_current(false);
#endif
#if ENABLED(BLTOUCH)
@ -666,8 +671,7 @@ float Probe::run_z_probe(const bool sanity_check/*=true*/) {
if (try_to_probe(PSTR("FAST"), z_probe_low_point, z_probe_fast_mm_s,
sanity_check, Z_CLEARANCE_BETWEEN_PROBES) ) return NAN;
const float first_probe_z = current_position.z;
const float first_probe_z = DIFF_TERN(HAS_DELTA_SENSORLESS_PROBING, current_position.z, largest_sensorless_adj);
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("1st Probe Z:", first_probe_z);
// Raise to give the probe clearance
@ -709,7 +713,7 @@ float Probe::run_z_probe(const bool sanity_check/*=true*/) {
TERN_(MEASURE_BACKLASH_WHEN_PROBING, backlash.measure_with_probe());
const float z = current_position.z;
const float z = DIFF_TERN(HAS_DELTA_SENSORLESS_PROBING, current_position.z, largest_sensorless_adj);
#if EXTRA_PROBING > 0
// Insert Z measurement into probes[]. Keep it sorted ascending.
@ -760,7 +764,7 @@ float Probe::run_z_probe(const bool sanity_check/*=true*/) {
#elif TOTAL_PROBING == 2
const float z2 = current_position.z;
const float z2 = DIFF_TERN(HAS_DELTA_SENSORLESS_PROBING, current_position.z, largest_sensorless_adj);
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("2nd Probe Z:", z2, " Discrepancy:", first_probe_z - z2);
@ -842,7 +846,7 @@ float Probe::probe_at_point(const_float_t rx, const_float_t ry, const ProbePtRai
SERIAL_ERROR_MSG(STR_ERR_PROBING_FAILED);
#endif
}
DEBUG_ECHOLNPGM("measured_z: ", measured_z);
return measured_z;
}
@ -895,58 +899,39 @@ float Probe::probe_at_point(const_float_t rx, const_float_t ry, const ProbePtRai
}
/**
* Change the current in the TMC drivers to N##_CURRENT_HOME. And we save the current configuration of each TMC driver.
* Set the sensorless Z offset
*/
void Probe::set_homing_current(const bool onoff) {
#define HAS_CURRENT_HOME(N) (defined(N##_CURRENT_HOME) && N##_CURRENT_HOME != N##_CURRENT)
#if HAS_CURRENT_HOME(X) || HAS_CURRENT_HOME(Y) || HAS_CURRENT_HOME(Z)
#if ENABLED(DELTA)
static int16_t saved_current_X, saved_current_Y;
void Probe::set_offset_sensorless_adj(const_float_t sz) {
#if ENABLED(SENSORLESS_PROBING)
DEBUG_SECTION(pso, "Probe::set_offset_sensorless_adj", true);
#if HAS_DELTA_SENSORLESS_PROBING
if (test_sensitivity.x) offset_sensorless_adj.a = sz;
if (test_sensitivity.y) offset_sensorless_adj.b = sz;
#endif
#if HAS_CURRENT_HOME(Z)
static int16_t saved_current_Z;
if (test_sensitivity.z) offset_sensorless_adj.c = sz;
#endif
}
/**
* Refresh largest_sensorless_adj based on triggered endstops
*/
void Probe::refresh_largest_sensorless_adj() {
#if ENABLED(SENSORLESS_PROBING)
DEBUG_SECTION(rso, "Probe::refresh_largest_sensorless_adj", true);
largest_sensorless_adj = -3; // A reference away from any real probe height
#if HAS_DELTA_SENSORLESS_PROBING
if (TEST(endstops.state(), X_MAX)) {
NOLESS(largest_sensorless_adj, offset_sensorless_adj.a);
DEBUG_ECHOLNPGM("Endstop_X: ", largest_sensorless_adj, " TowerX");
}
if (TEST(endstops.state(), Y_MAX)) {
NOLESS(largest_sensorless_adj, offset_sensorless_adj.b);
DEBUG_ECHOLNPGM("Endstop_Y: ", largest_sensorless_adj, " TowerY");
}
#endif
#if ((ENABLED(DELTA) && (HAS_CURRENT_HOME(X) || HAS_CURRENT_HOME(Y))) || HAS_CURRENT_HOME(Z))
auto debug_current_on = [](PGM_P const s, const int16_t a, const int16_t b) {
if (DEBUGGING(LEVELING)) { DEBUG_ECHOPGM_P(s); DEBUG_ECHOLNPGM(" current: ", a, " -> ", b); }
};
#endif
if (onoff) {
#if ENABLED(DELTA)
#if HAS_CURRENT_HOME(X)
saved_current_X = stepperX.getMilliamps();
stepperX.rms_current(X_CURRENT_HOME);
debug_current_on(PSTR("X"), saved_current_X, X_CURRENT_HOME);
#endif
#if HAS_CURRENT_HOME(Y)
saved_current_Y = stepperY.getMilliamps();
stepperY.rms_current(Y_CURRENT_HOME);
debug_current_on(PSTR("Y"), saved_current_Y, Y_CURRENT_HOME);
#endif
#endif
#if HAS_CURRENT_HOME(Z)
saved_current_Z = stepperZ.getMilliamps();
stepperZ.rms_current(Z_CURRENT_HOME);
debug_current_on(PSTR("Z"), saved_current_Z, Z_CURRENT_HOME);
#endif
TERN_(IMPROVE_HOMING_RELIABILITY, planner.enable_stall_prevention(true));
}
else {
#if ENABLED(DELTA)
#if HAS_CURRENT_HOME(X)
stepperX.rms_current(saved_current_X);
debug_current_on(PSTR("X"), X_CURRENT_HOME, saved_current_X);
#endif
#if HAS_CURRENT_HOME(Y)
stepperY.rms_current(saved_current_Y);
debug_current_on(PSTR("Y"), Y_CURRENT_HOME, saved_current_Y);
#endif
#endif
#if HAS_CURRENT_HOME(Z)
stepperZ.rms_current(saved_current_Z);
debug_current_on(PSTR("Z"), Z_CURRENT_HOME, saved_current_Z);
#endif
TERN_(IMPROVE_HOMING_RELIABILITY, planner.enable_stall_prevention(false));
if (TEST(endstops.state(), Z_MAX)) {
NOLESS(largest_sensorless_adj, offset_sensorless_adj.c);
DEBUG_ECHOLNPGM("Endstop_Z: ", largest_sensorless_adj, " TowerZ");
}
#endif
}

11
Marlin/src/module/probe.h
View File

@ -66,7 +66,13 @@ class Probe {
public:
#if ENABLED(SENSORLESS_PROBING)
typedef struct { bool x:1, y:1, z:1; } sense_bool_t;
typedef struct {
#if HAS_DELTA_SENSORLESS_PROBING
bool x:1, y:1, z:1;
#else
bool z;
#endif
} sense_bool_t;
static sense_bool_t test_sensitivity;
#endif
@ -299,7 +305,8 @@ public:
#if USE_SENSORLESS
static void enable_stallguard_diag1();
static void disable_stallguard_diag1();
static void set_homing_current(const bool onoff);
static void set_offset_sensorless_adj(const_float_t sz);
static void refresh_largest_sensorless_adj();
#endif
private: