⚡️ Improve Sensorless homing/probing accuracy for G28, G33, M48 (Marl…

…inFirmware#24220) Co-authored-by: Robby Candra <[email protected]> Co-authored-by: ellensp <[email protected]>
LCh-77 · Jun 15, 2022 · a06a0b2 · a06a0b2
1 parent 78d64ff
commit a06a0b2
Show file tree

Hide file tree

Showing 13 changed files with 202 additions and 79 deletions.
diff --git a/Marlin/src/gcode/calibrate/G28.cpp b/Marlin/src/gcode/calibrate/G28.cpp
@@ -321,6 +321,7 @@ void GcodeSuite::G28() {
       stepperW.rms_current(W_CURRENT_HOME);
       if (DEBUGGING(LEVELING)) debug_current(F(STR_W), tmc_save_current_W, W_CURRENT_HOME);
     #endif
+    safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
   #endif
 
   #if ENABLED(IMPROVE_HOMING_RELIABILITY)
@@ -576,6 +577,7 @@ void GcodeSuite::G28() {
     #if HAS_CURRENT_HOME(W)
       stepperW.rms_current(tmc_save_current_W);
     #endif
+    safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
   #endif // HAS_HOMING_CURRENT
 
   ui.refresh();

diff --git a/Marlin/src/gcode/calibrate/G33.cpp b/Marlin/src/gcode/calibrate/G33.cpp
@@ -71,9 +71,9 @@ float lcd_probe_pt(const xy_pos_t &xy);
 
 void ac_home() {
   endstops.enable(true);
-  TERN_(HAS_DELTA_SENSORLESS_PROBING, probe.set_homing_current(true));
+  TERN_(SENSORLESS_HOMING, endstops.set_homing_current(true));
   home_delta();
-  TERN_(HAS_DELTA_SENSORLESS_PROBING, probe.set_homing_current(false));
+  TERN_(SENSORLESS_HOMING, endstops.set_homing_current(false));
   endstops.not_homing();
 }
 
@@ -390,6 +390,8 @@ static float auto_tune_a(const float dcr) {
  *   X   Don't activate stallguard on X.
  *   Y   Don't activate stallguard on Y.
  *   Z   Don't activate stallguard on Z.
+ *
+ *   S   Save offset_sensorless_adj
  */
 void GcodeSuite::G33() {
 
@@ -411,7 +413,8 @@ void GcodeSuite::G33() {
     dcr -= probe_at_offset ? _MAX(total_offset, PROBING_MARGIN) : total_offset;
   #endif
   NOMORE(dcr, DELTA_PRINTABLE_RADIUS);
-  if (parser.seenval('R')) dcr -= _MAX(parser.value_float(),0);
+  if (parser.seenval('R')) dcr -= _MAX(parser.value_float(), 0.0f);
+  TERN_(HAS_DELTA_SENSORLESS_PROBING, dcr *= sensorless_radius_factor);
 
   const float calibration_precision = parser.floatval('C', 0.0f);
   if (calibration_precision < 0) {
@@ -434,9 +437,8 @@ void GcodeSuite::G33() {
   const bool stow_after_each = parser.seen_test('E');
 
   #if HAS_DELTA_SENSORLESS_PROBING
-    probe.test_sensitivity.x = !parser.seen_test('X');
-    TERN_(HAS_Y_AXIS, probe.test_sensitivity.y = !parser.seen_test('Y'));
-    TERN_(HAS_Z_AXIS, probe.test_sensitivity.z = !parser.seen_test('Z'));
+    probe.test_sensitivity.set(!parser.seen_test('X'), !parser.seen_test('Y'), !parser.seen_test('Z'));
+    const bool do_save_offset_adj = parser.seen_test('S');
   #endif
 
   const bool _0p_calibration      = probe_points == 0,
@@ -475,6 +477,25 @@ void GcodeSuite::G33() {
 
   if (!_0p_calibration) ac_home();
 
+  #if HAS_DELTA_SENSORLESS_PROBING
+    if (verbose_level > 0 && do_save_offset_adj) {
+      offset_sensorless_adj.reset();
+
+      auto caltower = [&](Probe::sense_bool_t s){
+        float z_at_pt[NPP + 1];
+        LOOP_CAL_ALL(rad) z_at_pt[rad] = 0.0f;
+        probe.test_sensitivity = s;
+        if (probe_calibration_points(z_at_pt, 1, dcr, false, false, probe_at_offset))
+          probe.set_offset_sensorless_adj(z_at_pt[CEN]);
+      };
+      caltower({ true, false, false }); // A
+      caltower({ false, true, false }); // B
+      caltower({ false, false, true }); // C
+
+      probe.test_sensitivity = { true, true, true }; // reset to all
+    }
+  #endif
+
   do { // start iterations
 
     float z_at_pt[NPP + 1] = { 0.0f };
@@ -598,8 +619,17 @@ void GcodeSuite::G33() {
 
     // print report
 
-    if (verbose_level == 3 || verbose_level == 0)
+    if (verbose_level == 3 || verbose_level == 0) {
       print_calibration_results(z_at_pt, _tower_results, _opposite_results);
+      #if HAS_DELTA_SENSORLESS_PROBING
+        if (verbose_level == 0 && probe_points == 1) {
+          if (do_save_offset_adj)
+            probe.set_offset_sensorless_adj(z_at_pt[CEN]);
+          else
+            probe.refresh_largest_sensorless_adj();
+        }
+      #endif
+    }
 
     if (verbose_level != 0) { // !dry run
       if ((zero_std_dev >= test_precision && iterations > force_iterations) || zero_std_dev <= calibration_precision) { // end iterations
@@ -660,6 +690,9 @@ void GcodeSuite::G33() {
   ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index));
 
   TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE));
+  #if HAS_DELTA_SENSORLESS_PROBING
+    probe.test_sensitivity = { true, true, true };
+  #endif
 }
 
 #endif // DELTA_AUTO_CALIBRATION
diff --git a/Marlin/src/inc/Conditionals_adv.h b/Marlin/src/inc/Conditionals_adv.h
@@ -1048,3 +1048,8 @@
 #if ANY(DISABLE_INACTIVE_X, DISABLE_INACTIVE_Y, DISABLE_INACTIVE_Z, DISABLE_INACTIVE_I, DISABLE_INACTIVE_J, DISABLE_INACTIVE_K, DISABLE_INACTIVE_U, DISABLE_INACTIVE_V, DISABLE_INACTIVE_W, DISABLE_INACTIVE_E)
   #define HAS_DISABLE_INACTIVE_AXIS 1
 #endif
+
+// Delay Sensorless Homing/Probing
+#if EITHER(SENSORLESS_HOMING, SENSORLESS_PROBING) && !defined(SENSORLESS_STALLGUARD_DELAY)
+  #define SENSORLESS_STALLGUARD_DELAY 0
+#endif
diff --git a/Marlin/src/lcd/extui/dgus/mks/DGUSScreenHandler.cpp b/Marlin/src/lcd/extui/dgus/mks/DGUSScreenHandler.cpp
@@ -193,7 +193,6 @@ void DGUSScreenHandlerMKS::DGUSLCD_SendTMCStepValue(DGUS_VP_Variable &var) {
     if (!ExtUI::isPrintingFromMedia()) return; // avoid race condition when user stays in this menu and printer finishes.
     switch (swap16(*(uint16_t*)val_ptr)) {
       case 0: { // Resume
-
         auto cs = getCurrentScreen();
         if (runout_mks.runout_status != RUNOUT_WAITING_STATUS && runout_mks.runout_status != UNRUNOUT_STATUS) {
           if (cs == MKSLCD_SCREEN_PRINT || cs == MKSLCD_SCREEN_PAUSE)
@@ -213,7 +212,6 @@ void DGUSScreenHandlerMKS::DGUSLCD_SendTMCStepValue(DGUS_VP_Variable &var) {
       } break;
 
       case 1: // Pause
-
         GotoScreen(MKSLCD_SCREEN_PAUSE);
         if (!ExtUI::isPrintingFromMediaPaused()) {
           nozzle_park_mks.print_pause_start_flag = 1;
@@ -222,6 +220,7 @@ void DGUSScreenHandlerMKS::DGUSLCD_SendTMCStepValue(DGUS_VP_Variable &var) {
           //ExtUI::mks_pausePrint();
         }
         break;
+
       case 2: // Abort
         HandleUserConfirmationPopUp(VP_SD_AbortPrintConfirmed, nullptr, PSTR("Abort printing"), filelist.filename(), PSTR("?"), true, true, false, true);
         break;

diff --git a/Marlin/src/lcd/menu/menu_delta_calibrate.cpp b/Marlin/src/lcd/menu/menu_delta_calibrate.cpp
@@ -41,6 +41,10 @@
   #include "../extui/ui_api.h"
 #endif
 
+#if HAS_PROBE_XY_OFFSET
+  #include "../../module/probe.h"
+#endif
+
 void _man_probe_pt(const xy_pos_t &xy) {
   if (!ui.wait_for_move) {
     ui.wait_for_move = true;
@@ -88,7 +92,9 @@ void _man_probe_pt(const xy_pos_t &xy) {
   }
 
   void _goto_tower_a(const_float_t a) {
-    constexpr float dcr = DELTA_PRINTABLE_RADIUS;
+    float dcr = DELTA_PRINTABLE_RADIUS - PROBING_MARGIN;
+    TERN_(HAS_PROBE_XY_OFFSET, dcr -= HYPOT(probe.offset_xy.x, probe.offset_xy.y));
+    TERN_(HAS_DELTA_SENSORLESS_PROBING, dcr *= sensorless_radius_factor);
     xy_pos_t tower_vec = { cos(RADIANS(a)), sin(RADIANS(a)) };
     _man_probe_pt(tower_vec * dcr);
   }

diff --git a/Marlin/src/module/delta.cpp b/Marlin/src/module/delta.cpp
@@ -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();
 
@@ -236,6 +240,7 @@ void home_delta() {
     TERN_(U_SENSORLESS, sensorless_t stealth_states_u = start_sensorless_homing_per_axis(U_AXIS));
     TERN_(V_SENSORLESS, sensorless_t stealth_states_v = start_sensorless_homing_per_axis(V_AXIS));
     TERN_(W_SENSORLESS, sensorless_t stealth_states_w = start_sensorless_homing_per_axis(W_AXIS));
+    safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
   #endif
 
   // Move all carriages together linearly until an endstop is hit.
@@ -255,6 +260,7 @@ void home_delta() {
     TERN_(U_SENSORLESS, end_sensorless_homing_per_axis(U_AXIS, stealth_states_u));
     TERN_(V_SENSORLESS, end_sensorless_homing_per_axis(V_AXIS, stealth_states_v));
     TERN_(W_SENSORLESS, end_sensorless_homing_per_axis(W_AXIS, stealth_states_w));
+    safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
   #endif
 
   endstops.validate_homing_move();

diff --git a/Marlin/src/module/delta.h b/Marlin/src/module/delta.h
@@ -38,13 +38,24 @@ 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
  * settings have been changed (e.g., by M665).
  */
 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
  *

diff --git a/Marlin/src/module/endstops.cpp b/Marlin/src/module/endstops.cpp
@@ -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
@@ -1621,3 +1624,63 @@ 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));
+      safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
+
+    #endif // XYZ
+  }
+#endif
diff --git a/Marlin/src/module/endstops.h b/Marlin/src/module/endstops.h
@@ -247,6 +247,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;

diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
@@ -1663,7 +1663,10 @@ 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);
+        safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
+      #endif
     }
 
     #if EITHER(MORGAN_SCARA, MP_SCARA)
@@ -1699,7 +1702,10 @@ 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);
+        safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle
+      #endif
     }
   }
 

diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp
@@ -1663,10 +1663,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;