Add G29_RETRY_AND_RECOVER feature

- Add an option to retry G29, optionally executing a G-code procedure after each failed probe.

Marlin/Configuration_adv.h

   //#define MESH_MAX_Y Y_BED_SIZE - (MESH_INSET)
 #endif
 
+/**
+ * Repeatedly attempt G29 leveling until it succeeds.
+ * Stop after G29_MAX_RETRIES attempts.
+ */
+//#define G29_RETRY_AND_RECOVER
+#if ENABLED(G29_RETRY_AND_RECOVER)
+  #define G29_MAX_RETRIES 3
+  #define G29_HALT_ON_FAILURE
+  /**
+   * Specify the GCODE commands that will be executed when leveling succeeds,
+   * between attempts, and after the maximum number of retries have been tried.
+   */
+  #define G29_SUCCESS_COMMANDS "M117 Bed leveling done."
+  #define G29_RECOVER_COMMANDS "M117 Probe failed. Rewiping.\nG28\nG12 P0 S12 T0"
+  #define G29_FAILURE_COMMANDS "M117 Bed leveling failed.\nG0 Z10\nM300 P25 S880\nM300 P50 S0\nM300 P25 S880\nM300 P50 S0\nM300 P25 S880\nM300 P50 S0\nG4 S1"
+  /**
+   * Specify an action command to send to the host on a recovery attempt or failure.
+   * Will be sent in the form '//action:ACTION_ON_G29_FAILURE', e.g. '//action:probe_failed'.
+   * The host must be configured to handle the action command.
+   */
+  #define G29_ACTION_ON_RECOVER "probe_rewipe"
+  #define G29_ACTION_ON_FAILURE "probe_failed"
+#endif
+
 // @section extras
 
 //

Marlin/src/config/default/Configuration_adv.h

   //#define MESH_MAX_Y Y_BED_SIZE - (MESH_INSET)
 #endif
 
+/**
+ * Repeatedly attempt G29 leveling until it succeeds.
+ * Stop after G29_MAX_RETRIES attempts.
+ */
+//#define G29_RETRY_AND_RECOVER
+#if ENABLED(G29_RETRY_AND_RECOVER)
+  #define G29_MAX_RETRIES 3
+  #define G29_HALT_ON_FAILURE
+  /**
+   * Specify the GCODE commands that will be executed when leveling succeeds,
+   * between attempts, and after the maximum number of retries have been tried.
+   */
+  #define G29_SUCCESS_COMMANDS "M117 Bed leveling done."
+  #define G29_RECOVER_COMMANDS "M117 Probe failed. Rewiping.\nG28\nG12 P0 S12 T0"
+  #define G29_FAILURE_COMMANDS "M117 Bed leveling failed.\nG0 Z10\nM300 P25 S880\nM300 P50 S0\nM300 P25 S880\nM300 P50 S0\nM300 P25 S880\nM300 P50 S0\nG4 S1"
+  /**
+   * Specify an action command to send to the host on a recovery attempt or failure.
+   * Will be sent in the form '//action:ACTION_ON_G29_FAILURE', e.g. '//action:probe_failed'.
+   * The host must be configured to handle the action command.
+   */
+  #define G29_ACTION_ON_RECOVER "probe_rewipe"
+  #define G29_ACTION_ON_FAILURE "probe_failed"
+#endif
+
 // @section extras
 
 //

Marlin/src/gcode/gcode.cpp

   float GcodeSuite::coordinate_system[MAX_COORDINATE_SYSTEMS][XYZ];
 #endif
 
+#if HAS_LEVELING && ENABLED(G29_RETRY_AND_RECOVER)
+  #include "../feature/bedlevel/bedlevel.h"
+  #include "../module/planner.h"
+#endif
+
 /**
  * Set target_extruder from the T parameter or the active_extruder
  *
   while (PENDING(millis(), time)) idle();
 }
 
+/**
+ * When G29_RETRY_AND_RECOVER is enabled, call G29() in
+ * a loop with recovery and retry handling.
+ */
+#if HAS_LEVELING && ENABLED(G29_RETRY_AND_RECOVER)
+
+  void GcodeSuite::G29_with_retry() {
+    set_bed_leveling_enabled(false);
+    for (uint8_t i = G29_MAX_RETRIES; i--;) {
+      G29();
+      if (planner.leveling_active) break;
+      #ifdef G29_ACTION_ON_RECOVER
+        SERIAL_ECHOLNPGM("//action:" G29_ACTION_ON_RECOVER);
+      #endif
+      #ifdef G29_RECOVERY_COMMANDS
+        process_subcommands_now_P(PSTR(G29_RECOVER_COMMANDS));
+      #endif
+    }
+    if (planner.leveling_active) {
+      #ifdef G29_SUCCESS_COMMANDS
+        process_subcommands_now_P(PSTR(G29_SUCCESS_COMMANDS));
+      #endif
+    }
+    else {
+      #ifdef G29_FAILURE_COMMANDS
+        process_subcommands_now_P(PSTR(G29_FAILURE_COMMANDS));
+      #endif
+      #ifdef G29_ACTION_ON_FAILURE
+        SERIAL_ECHOLNPGM("//action:" G29_ACTION_ON_FAILURE);
+      #endif
+      #if ENABLED(G29_HALT_ON_FAILURE)
+        kill(PSTR(MSG_ERR_PROBING_FAILED));
+      #endif
+    }
+  }
+
+#endif // HAS_LEVELING && G29_RETRY_AND_RECOVER
+
 //
 // Placeholders for non-migrated codes
 //
 /**
  * Process the parsed command and dispatch it to its handler
  */
-void GcodeSuite::process_parsed_command() {
+void GcodeSuite::process_parsed_command(
+  #if ENABLED(USE_EXECUTE_COMMANDS_IMMEDIATE)
+    const bool no_ok
+  #endif
+) {
   KEEPALIVE_STATE(IN_HANDLER);
 
   // Handle a known G, M, or T
       case 28: G28(false); break;                                 // G28: Home all axes, one at a time
 
       #if HAS_LEVELING
-        case 29: G29(); break;                                    // G29: Bed leveling calibration
-      #endif
+        case 29:                                                  // G29: Bed leveling calibration
+          #if ENABLED(G29_RETRY_AND_RECOVER)
+            G29_with_retry();
+          #else
+            G29();
+          #endif
+          break;
+      #endif // HAS_LEVELING
 
       #if HAS_BED_PROBE
         case 30: G30(); break;                                    // G30: Single Z probe
 
   KEEPALIVE_STATE(NOT_BUSY);
 
-  ok_to_send();
+  #if ENABLED(USE_EXECUTE_COMMANDS_IMMEDIATE)
+    if (!no_ok)
+  #endif
+      ok_to_send();
 }
 
 /**
   process_parsed_command();
 }
 
+#if ENABLED(USE_EXECUTE_COMMANDS_IMMEDIATE)
+  /**
+   * Run a series of commands, bypassing the command queue to allow
+   * G-code "macros" to be called from within other G-code handlers.
+   */
+  void GcodeSuite::process_subcommands_now_P(const char *pgcode) {
+    // Save the parser state
+    char saved_cmd[strlen(parser.command_ptr) + 1];
+    strcpy(saved_cmd, parser.command_ptr);
+
+    // Process individual commands in string
+    while (pgm_read_byte_near(pgcode)) {
+      // Break up string at '\n' delimiters
+      const char *delim = strchr_P(pgcode, '\n');
+      size_t len = delim ? delim - pgcode : strlen_P(pgcode);
+      char cmd[len + 1];
+      strncpy_P(cmd, pgcode, len);
+      cmd[len] = '\0';
+      pgcode += len;
+      if (delim) pgcode++;
+
+      // Parse the next command in the string
+      parser.parse(cmd);
+      process_parsed_command(true);
+    }
+
+    // Restore the parser state
+    parser.parse(saved_cmd);
+  }
+#endif
+
 #if ENABLED(HOST_KEEPALIVE_FEATURE)
 
   /**

Marlin/src/gcode/gcode.h

 
   static bool get_target_extruder_from_command();
   static void get_destination_from_command();
-  static void process_parsed_command();
+  static void process_parsed_command(
+    #if ENABLED(USE_EXECUTE_COMMANDS_IMMEDIATE)
+      const bool no_ok = false
+    #endif
+  );
   static void process_next_command();
 
+  #if ENABLED(USE_EXECUTE_COMMANDS_IMMEDIATE)
+    static void process_subcommands_now_P(const char *pgcode);
+  #endif
+
   FORCE_INLINE static void home_all_axes() { G28(true); }
 
   /**
 
   #if HAS_LEVELING
     static void G29();
+    #if ENABLED(G29_RETRY_AND_RECOVER)
+      static void G29_with_retry();
+    #endif
   #endif
 
   #if HAS_BED_PROBE

Marlin/src/inc/Conditionals_post.h

 // If platform requires early initialization of watchdog to properly boot
 #define EARLY_WATCHDOG (ENABLED(USE_WATCHDOG) && defined(ARDUINO_ARCH_SAM))
 
+#if ENABLED(G29_RETRY_AND_RECOVER)
+  #define USE_EXECUTE_COMMANDS_IMMEDIATE
+#endif
+
 #endif // CONDITIONALS_POST_H