diff --git a/klippy/extras/c_axis_align.py b/klippy/extras/c_axis_align.py
index fb580505a12b..ae188d8ff325 100644
--- a/klippy/extras/c_axis_align.py
+++ b/klippy/extras/c_axis_align.py
@@ -1,6 +1,8 @@
 # Helper script to automate a axis offset calculation
 
 import math
+import logging
+
 
 RAD_TO_DEG = 57.295779513
 
@@ -10,16 +12,23 @@ def __init__(self, config):
         self.printer = config.get_printer()
         self.gcode_move = self.printer.load_object(config, 'gcode_move')
         self.gcode = self.printer.lookup_object('gcode')
+        self.max_repeat_probe = config.getint('max_repeat_probe', 5)
+        self.threshold_value = config.getfloat('threshold_value', 0.02)
         self.point_coords = [
             [0., 0., 0., 0., 0., 0.],
             [0., 0., 0., 0., 0., 0.]
         ]
-        self.gcode.register_command('CALC_C_AXIS_ALIGN',
-                                    self.cmd_CALC_C_AXIS_ALIGN,
-                                    desc=self.cmd_CALC_C_AXIS_ALIGN_help)
+        self.gcode.register_command(
+            'ALIGN_C_AXIS', self.cmd_ALIGN_C_AXIS,
+            desc=self.cmd_ALIGN_C_AXIS_help)
         self.gcode.register_command(
             'SAVE_C_AXIS_POINT', self.cmd_SAVE_C_AXIS_POINT,
             desc=self.cmd_SAVE_C_AXIS_POINT_help)
+        self.gcode.register_command(
+            'MOVE_ALIGN_C_AXIS', self.cmd_MOVE_ALIGN_C_AXIS,
+            desc=self.cmd_MOVE_ALIGN_C_AXIS_help)
+
+    cmd_SAVE_C_AXIS_POINT_help = "Save point for C axis align"
 
     def cmd_SAVE_C_AXIS_POINT(self, gcmd):
         point_idx = gcmd.get_int('POINT', 0)
@@ -37,23 +46,37 @@ def cmd_SAVE_C_AXIS_POINT(self, gcmd):
             for axis, coord in enumerate(coords):
                 self.point_coords[point_idx][axis] = coord
 
-    cmd_SAVE_C_AXIS_POINT_help = "Save point for C axis align"
+    cmd_ALIGN_C_AXIS_help = "Calculate and apply correction for align the C axis"
+
+    def cmd_ALIGN_C_AXIS(self, gcmd):
+        for i in range(self.max_repeat_probe):
+            self.gcode.run_script_from_command("MOVE_ALIGN_C_AXIS")
+            offset = self._calc_c_axis_align(
+                self.point_coords[0], self.point_coords[1])
+            if abs(offset) <= self.threshold_value:
+                logging.info("align the axis C completed")
+                break
+            else:
+                self._apply_offset_c(offset)
+
+    cmd_MOVE_ALIGN_C_AXIS_help = "By default, it returns the offset of the\
+        axis, if necessary, perform rename_existing of this command in the macro"
+    def cmd_MOVE_ALIGN_C_AXIS(self, gcmd):
+        gcmd.respond_info("the MOVE_ALIGN_C_AXIS command is not supported")
 
     def _calc_c_axis_align(self, point_0, point_1):
         offset = math.atan((point_1[1] - point_0[1]) / 90) * RAD_TO_DEG / 3
+        logging.info("calculate offset for the axis C: %f." % offset)
         return offset
 
-    def cmd_CALC_C_AXIS_ALIGN(self, gcmd):
-        offset = self._calc_c_axis_align(
-            self.point_coords[0], self.point_coords[1])
+    def _apply_offset_c(self, offset):
+        logging.info("an offset has been applied to correct the C axis")
         align_gcmd = self.gcode.create_gcode_command(
             'G0', 'G0', {'C': offset})
         self.gcode_move.cmd_G1(align_gcmd)
         self.gcode_move.cmd_G92(self.gcode.create_gcode_command(
             'G92', 'G92', {'C': 0}))
 
-    cmd_CALC_C_AXIS_ALIGN_help = "Calculate C axis align"
-
 
 def load_config(config):
     CAxisAlignCalculation(config)
diff --git a/stereotech_config/calibrate/probe_5d_template.cfg b/stereotech_config/calibrate/probe_5d_template.cfg
index ac4f80c5076a..c8276986e1d8 100644
--- a/stereotech_config/calibrate/probe_5d_template.cfg
+++ b/stereotech_config/calibrate/probe_5d_template.cfg
@@ -31,18 +31,14 @@ gcode:
     {% set v = 'v_' if a == 90 else '' %}
     SET_GCODE_VARIABLE MACRO=PROBE_TEMPLATE_POINT VARIABLE={"a_" ~ v ~"probe_z"} VALUE={printer.probe.last_result[2] - printer.gcode_move.homing_origin.z}
 
-[gcode_macro ALIGN_C_AXIS]
-description: align template along axis x
-variable_repeat: 2
+[gcode_macro MOVE_ALIGN_C_AXIS]
+description: moves for align template along axis X
+rename_existing: MOVE_ALIGN_C_AXIS_OLD
 gcode:
-    {% set repeat = printer["gcode_macro ALIGN_C_AXIS"].repeat %}
-    {% for idx in range(repeat) %}
-        PROBE_TEMPLATE_POINT POINT=D_Y
-        SET_POINT MACRO=SAVE_C_AXIS_POINT POINT=1
-        PROBE_TEMPLATE_POINT POINT=C_Y
-        SET_POINT MACRO=SAVE_C_AXIS_POINT POINT=0
-        CALC_C_AXIS_ALIGN
-    {% endfor %}
+    PROBE_TEMPLATE_POINT POINT=D_Y
+    SET_POINT MACRO=SAVE_C_AXIS_POINT POINT=1
+    PROBE_TEMPLATE_POINT POINT=C_Y
+    SET_POINT MACRO=SAVE_C_AXIS_POINT POINT=0
 
 [gcode_macro ALIGN_A_AXIS]
 description: align template along horizontal plane