diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index b597465..05ba90e 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -137,6 +137,14 @@ static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
  */
 static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
   if (c == NULL) return CHASSIS_ERR_NULL;
+
+  /* 自动退回BALANCE后，只要用户切到非上台阶模式，就允许下次重新触发 */
+  if (mode != CHASSIS_MODE_CLIMB_STEP && c->climb.completed) {
+    c->climb.completed = false;
+    c->climb.state = CLIMB_IDLE;
+    c->climb.state_start_time = 0;
+  }
+
   if (mode == c->mode) return CHASSIS_OK;
 
   /* 上台阶已完成，忽略重复的CLIMB_STEP请求，需先切别的模式再回来 */
@@ -197,6 +205,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
   if (c->mode == CHASSIS_MODE_CLIMB_STEP && mode != CHASSIS_MODE_CLIMB_STEP) {
     c->climb.completed = false;
     c->climb.state = CLIMB_IDLE;
+    c->climb.state_start_time = 0;
   }
 
   c->mode = mode;
@@ -469,9 +478,10 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       break;
 
     case CLIMB_EXTEND: {
-      /* 伸腿前进：腿长最大，给定前进速度，削弱摆力矩让腿自由后摆 */
-      climb_cmd.move_vec.vx = c->param->climb.forward_speed / 2.0f;
-      climb_cmd.height = 1.0f;  /* 最大腿长 */
+      /* 伸腿前进采用斜坡，避免切换模式时瞬间猛站 */
+      float extend_ratio = LIMIT((float)elapsed_time / 500.0f, 0.0f, 1.0f);
+      climb_cmd.move_vec.vx = (c->param->climb.forward_speed / 2.0f) * extend_ratio;
+      climb_cmd.height = extend_ratio;
 
       /* 运行LQR和VMC */
       Chassis_LQRControl(c, &climb_cmd);
@@ -483,9 +493,12 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       Chassis_VMCControl(c, &climb_cmd);
       Chassis_Output(c);
 
-      /* 检查腿后摆角度：任一腿theta超过阈值则收腿 */
+      /* 先伸腿到位，再根据摆角触发收腿，避免刚进入模式就立刻缩回 */
+      bool legs_extended = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) &&
+                           (c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f);
       float avg_theta = (c->vmc_[0].leg.theta + c->vmc_[1].leg.theta) / 2.0f;
-      if (fabsf(avg_theta) > c->param->climb.theta_retract_threshold) {
+      if (elapsed_time > 300 && legs_extended &&
+          fabsf(avg_theta) > c->param->climb.theta_retract_threshold) {
         c->climb.state = CLIMB_RETRACT;
         c->climb.state_start_time = current_time;
       }
@@ -672,6 +685,11 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   c->rotor_state.current_spin_speed = 0.0f;
   c->rotor_state.exiting = false;
 
+  /* 初始化上台阶状态 */
+  c->climb.state = CLIMB_IDLE;
+  c->climb.completed = false;
+  c->climb.state_start_time = 0;
+
   return CHASSIS_OK;
 }