添加ref

修
新轮腿
2026-03-12 00:51:15 +08:00 · 2026-03-12 00:14:36 +08:00 · 2026-03-09 14:36:04 +08:00 · 2026-03-03 23:19:23 +08:00 · 2026-03-03 00:16:50 +08:00 · 2026-02-25 17:57:05 +08:00
19 changed files with 7993 additions and 7565 deletions
--- a/.DS_Store
+++ b/.DS_Store
--- a/.github/copilot-instructions.md
+++ b/.github/copilot-instructions.md
@ -0,0 +1,6 @@
+---
+description: 当助手已完成当前请求的工作、准备结束回复或不再有明确下一步可执行内容时，必须先调用提问工具 askQuestions 获取进一步指示；不得直接结束对话。 / When the assistant has completed the current task and is about to end the reply or has no clear next action, it must call the askQuestions tool to obtain further instructions instead of ending the conversation.
+applyTo: ‘当检测到回复将以收尾语句结束或无后续动作时 / When a reply is about to end with a closing statement or has no follow-up action’
+---
+当你完成工作时，请调用提问工具（askQuestions）来获取进一步的指示，而不是直接结束对话。
+When you finish the task, call the askQuestions tool to request further instructions instead of ending the conversation.
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -69,6 +69,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
    User/component/pid.c
    User/component/user_math.c
    User/component/vmc.c
+    User/component/ui.c

    # User/device sources
    User/device/bmi088.c
@ -83,6 +84,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
    User/device/vision_bridge.c
    User/device/vofa.c
    User/device/mrobot.c
+    User/device/referee.c
+    User/device/supercap.c

    # User/module sources
    User/module/balance_chassis.c
--- a/Core/Inc/stm32h7xx_it.h
+++ b/Core/Inc/stm32h7xx_it.h
@ -58,6 +58,7 @@ void DMA1_Stream2_IRQHandler(void);
 void DMA1_Stream3_IRQHandler(void);
 void DMA1_Stream4_IRQHandler(void);
 void DMA1_Stream5_IRQHandler(void);
+void DMA1_Stream6_IRQHandler(void);
 void FDCAN1_IT0_IRQHandler(void);
 void FDCAN2_IT0_IRQHandler(void);
 void FDCAN1_IT1_IRQHandler(void);
@ -67,6 +68,7 @@ void USART1_IRQHandler(void);
 void USART2_IRQHandler(void);
 void USART3_IRQHandler(void);
 void EXTI15_10_IRQHandler(void);
+void DMA1_Stream7_IRQHandler(void);
 void UART5_IRQHandler(void);
 void UART7_IRQHandler(void);
 void ADC3_IRQHandler(void);
--- a/Core/Src/dma.c
+++ b/Core/Src/dma.c
@ -61,6 +61,12 @@ void MX_DMA_Init(void)
  /* DMA1_Stream5_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 5, 0);
  HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn);
+  /* DMA1_Stream6_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 5, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream6_IRQn);
+  /* DMA1_Stream7_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream7_IRQn, 5, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream7_IRQn);

 }

--- a/Core/Src/stm32h7xx_it.c
+++ b/Core/Src/stm32h7xx_it.c
@ -68,6 +68,8 @@ extern SPI_HandleTypeDef hspi2;
 extern DMA_HandleTypeDef hdma_uart5_rx;
 extern DMA_HandleTypeDef hdma_usart1_tx;
 extern DMA_HandleTypeDef hdma_usart1_rx;
+extern DMA_HandleTypeDef hdma_usart10_rx;
+extern DMA_HandleTypeDef hdma_usart10_tx;
 extern UART_HandleTypeDef huart5;
 extern UART_HandleTypeDef huart7;
 extern UART_HandleTypeDef huart1;
@ -262,6 +264,20 @@ void DMA1_Stream5_IRQHandler(void)
  /* USER CODE END DMA1_Stream5_IRQn 1 */
 }

+/**
+  * @brief This function handles DMA1 stream6 global interrupt.
+  */
+void DMA1_Stream6_IRQHandler(void)
+{
+  /* USER CODE BEGIN DMA1_Stream6_IRQn 0 */
+
+  /* USER CODE END DMA1_Stream6_IRQn 0 */
+  HAL_DMA_IRQHandler(&hdma_usart10_rx);
+  /* USER CODE BEGIN DMA1_Stream6_IRQn 1 */
+
+  /* USER CODE END DMA1_Stream6_IRQn 1 */
+}
+
 /**
  * @brief This function handles FDCAN1 interrupt 0.
  */
@ -390,6 +406,20 @@ void EXTI15_10_IRQHandler(void)
  /* USER CODE END EXTI15_10_IRQn 1 */
 }

+/**
+  * @brief This function handles DMA1 stream7 global interrupt.
+  */
+void DMA1_Stream7_IRQHandler(void)
+{
+  /* USER CODE BEGIN DMA1_Stream7_IRQn 0 */
+
+  /* USER CODE END DMA1_Stream7_IRQn 0 */
+  HAL_DMA_IRQHandler(&hdma_usart10_tx);
+  /* USER CODE BEGIN DMA1_Stream7_IRQn 1 */
+
+  /* USER CODE END DMA1_Stream7_IRQn 1 */
+}
+
 /**
  * @brief This function handles UART5 global interrupt.
  */
--- a/Core/Src/usart.c
+++ b/Core/Src/usart.c
@ -33,6 +33,8 @@ UART_HandleTypeDef huart10;
 DMA_HandleTypeDef hdma_uart5_rx;
 DMA_HandleTypeDef hdma_usart1_tx;
 DMA_HandleTypeDef hdma_usart1_rx;
+DMA_HandleTypeDef hdma_usart10_rx;
+DMA_HandleTypeDef hdma_usart10_tx;

 /* UART5 init function */
 void MX_UART5_Init(void)
@ -265,7 +267,7 @@ void MX_USART10_UART_Init(void)

  /* USER CODE END USART10_Init 1 */
  huart10.Instance = USART10;
-  huart10.Init.BaudRate = 921600;
+  huart10.Init.BaudRate = 115200;
  huart10.Init.WordLength = UART_WORDLENGTH_8B;
  huart10.Init.StopBits = UART_STOPBITS_1;
  huart10.Init.Parity = UART_PARITY_NONE;
@ -598,6 +600,43 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
    GPIO_InitStruct.Alternate = GPIO_AF11_USART10;
    HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);

+    /* USART10 DMA Init */
+    /* USART10_RX Init */
+    hdma_usart10_rx.Instance = DMA1_Stream6;
+    hdma_usart10_rx.Init.Request = DMA_REQUEST_USART10_RX;
+    hdma_usart10_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
+    hdma_usart10_rx.Init.PeriphInc = DMA_PINC_DISABLE;
+    hdma_usart10_rx.Init.MemInc = DMA_MINC_ENABLE;
+    hdma_usart10_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+    hdma_usart10_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+    hdma_usart10_rx.Init.Mode = DMA_NORMAL;
+    hdma_usart10_rx.Init.Priority = DMA_PRIORITY_LOW;
+    hdma_usart10_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+    if (HAL_DMA_Init(&hdma_usart10_rx) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    __HAL_LINKDMA(uartHandle,hdmarx,hdma_usart10_rx);
+
+    /* USART10_TX Init */
+    hdma_usart10_tx.Instance = DMA1_Stream7;
+    hdma_usart10_tx.Init.Request = DMA_REQUEST_USART10_TX;
+    hdma_usart10_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
+    hdma_usart10_tx.Init.PeriphInc = DMA_PINC_DISABLE;
+    hdma_usart10_tx.Init.MemInc = DMA_MINC_ENABLE;
+    hdma_usart10_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+    hdma_usart10_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+    hdma_usart10_tx.Init.Mode = DMA_NORMAL;
+    hdma_usart10_tx.Init.Priority = DMA_PRIORITY_LOW;
+    hdma_usart10_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+    if (HAL_DMA_Init(&hdma_usart10_tx) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    __HAL_LINKDMA(uartHandle,hdmatx,hdma_usart10_tx);
+
    /* USART10 interrupt Init */
    HAL_NVIC_SetPriority(USART10_IRQn, 5, 0);
    HAL_NVIC_EnableIRQ(USART10_IRQn);
@ -737,6 +776,10 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
    */
    HAL_GPIO_DeInit(GPIOE, GPIO_PIN_2|GPIO_PIN_3);

+    /* USART10 DMA DeInit */
+    HAL_DMA_DeInit(uartHandle->hdmarx);
+    HAL_DMA_DeInit(uartHandle->hdmatx);
+
    /* USART10 interrupt Deinit */
    HAL_NVIC_DisableIRQ(USART10_IRQn);
  /* USER CODE BEGIN USART10_MspDeInit 1 */
--- a/CtrBoard-H7_ALL.ioc
+++ b/CtrBoard-H7_ALL.ioc
@ -80,7 +80,9 @@ Dma.Request2=SPI2_TX
 Dma.Request3=UART5_RX
 Dma.Request4=USART1_TX
 Dma.Request5=USART1_RX
-Dma.RequestsNb=6
+Dma.Request6=USART10_RX
+Dma.Request7=USART10_TX
+Dma.RequestsNb=8
 Dma.SPI2_RX.1.Direction=DMA_PERIPH_TO_MEMORY
 Dma.SPI2_RX.1.EventEnable=DISABLE
 Dma.SPI2_RX.1.FIFOMode=DMA_FIFOMODE_DISABLE
@ -135,6 +137,42 @@ Dma.UART5_RX.3.SyncEnable=DISABLE
 Dma.UART5_RX.3.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
 Dma.UART5_RX.3.SyncRequestNumber=1
 Dma.UART5_RX.3.SyncSignalID=NONE
+Dma.USART10_RX.6.Direction=DMA_PERIPH_TO_MEMORY
+Dma.USART10_RX.6.EventEnable=DISABLE
+Dma.USART10_RX.6.FIFOMode=DMA_FIFOMODE_DISABLE
+Dma.USART10_RX.6.Instance=DMA1_Stream6
+Dma.USART10_RX.6.MemDataAlignment=DMA_MDATAALIGN_BYTE
+Dma.USART10_RX.6.MemInc=DMA_MINC_ENABLE
+Dma.USART10_RX.6.Mode=DMA_NORMAL
+Dma.USART10_RX.6.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
+Dma.USART10_RX.6.PeriphInc=DMA_PINC_DISABLE
+Dma.USART10_RX.6.Polarity=HAL_DMAMUX_REQ_GEN_RISING
+Dma.USART10_RX.6.Priority=DMA_PRIORITY_LOW
+Dma.USART10_RX.6.RequestNumber=1
+Dma.USART10_RX.6.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
+Dma.USART10_RX.6.SignalID=NONE
+Dma.USART10_RX.6.SyncEnable=DISABLE
+Dma.USART10_RX.6.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
+Dma.USART10_RX.6.SyncRequestNumber=1
+Dma.USART10_RX.6.SyncSignalID=NONE
+Dma.USART10_TX.7.Direction=DMA_MEMORY_TO_PERIPH
+Dma.USART10_TX.7.EventEnable=DISABLE
+Dma.USART10_TX.7.FIFOMode=DMA_FIFOMODE_DISABLE
+Dma.USART10_TX.7.Instance=DMA1_Stream7
+Dma.USART10_TX.7.MemDataAlignment=DMA_MDATAALIGN_BYTE
+Dma.USART10_TX.7.MemInc=DMA_MINC_ENABLE
+Dma.USART10_TX.7.Mode=DMA_NORMAL
+Dma.USART10_TX.7.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
+Dma.USART10_TX.7.PeriphInc=DMA_PINC_DISABLE
+Dma.USART10_TX.7.Polarity=HAL_DMAMUX_REQ_GEN_RISING
+Dma.USART10_TX.7.Priority=DMA_PRIORITY_LOW
+Dma.USART10_TX.7.RequestNumber=1
+Dma.USART10_TX.7.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
+Dma.USART10_TX.7.SignalID=NONE
+Dma.USART10_TX.7.SyncEnable=DISABLE
+Dma.USART10_TX.7.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
+Dma.USART10_TX.7.SyncRequestNumber=1
+Dma.USART10_TX.7.SyncSignalID=NONE
 Dma.USART1_RX.5.Direction=DMA_PERIPH_TO_MEMORY
 Dma.USART1_RX.5.EventEnable=DISABLE
 Dma.USART1_RX.5.FIFOMode=DMA_FIFOMODE_DISABLE
@ -334,6 +372,8 @@ NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DMA1_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DMA1_Stream4_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DMA1_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DMA1_Stream6_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DMA1_Stream7_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.EXTI15_10_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.FDCAN1_IT0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
@ -758,7 +798,7 @@ UART7.IPParameters=BaudRate
 USART1.BaudRate=921600
 USART1.IPParameters=VirtualMode-Asynchronous,BaudRate
 USART1.VirtualMode-Asynchronous=VM_ASYNC
-USART10.BaudRate=921600
+USART10.BaudRate=115200
 USART10.IPParameters=VirtualMode,BaudRate
 USART10.VirtualMode=VM_ASYNC
 USART2.BaudRate=921600
--- a/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
+++ b/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
--- a/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
+++ b/MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
--- a/User/bsp/uart.c
+++ b/User/bsp/uart.c
@ -27,6 +27,8 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
    return BSP_UART_DR16;
  else if (huart->Instance == USART1)
    return BSP_UART_VOFA;
+  else if (huart->Instance == USART10)
+    return BSP_UART_REF;
  else
    return BSP_UART_ERR;
 }
@ -119,6 +121,8 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
      return &huart5;
    case BSP_UART_VOFA:
      return &huart1;
+    case BSP_UART_REF:
+      return &huart10;
    default:
      return NULL;
  }
--- a/User/bsp/uart.h
+++ b/User/bsp/uart.h
@ -29,6 +29,7 @@ extern "C" {
 typedef enum {
  BSP_UART_DR16,
  BSP_UART_VOFA,
+  BSP_UART_REF,
  BSP_UART_NUM,
  BSP_UART_ERR,
 } BSP_UART_t;
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@ -18,16 +18,8 @@
 /* Private defines ---------------------------------------------------------- */
 #define LIMIT(x, min, max)  ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))

-#define WHEEL_RADIUS        0.068f   /* 轮子半径 (m) */
-#define MAX_ACCELERATION    2.0f     /* 最大加速度 (m/s²) */
-#define WHEEL_GEAR_RATIO    4.5f     /* 轮毂电机扭矩 */
-#define BASE_LEG_LENGTH     0.16f    /* 基础腿长 (m) */
-#define LEG_LENGTH_RANGE    0.16f    /* 腿长调节范围 (m) */
-#define MIN_LEG_LENGTH      0.10f    /* 最小腿长 (m) */
-#define MAX_LEG_LENGTH      0.33f    /* 最大腿长 (m) */
-#define NON_CONTACT_THETA   0.0f    /* 离地时的摆角目标 (rad) */
-#define LEFT_BASE_FORCE     60.0f    /* 左腿基础支撑力 (N) */
-#define RIGHT_BASE_FORCE    60.0f    /* 右腿基础支撑力 (N) */
+/* 物理常量（不可调） */
+#define WHEEL_RADIUS        0.068f   /* 轮子半径 (m)，固定机械尺寸 */


 // float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f,LF =0.0f,RF=0.0f;
@ -39,6 +31,7 @@ static void Chassis_UpdateChassisState(Chassis_t *c);
 static void Chassis_ResetControllers(Chassis_t *c);
 static void Chassis_SelectYawZeroPoint(Chassis_t *c);
 static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force);
+static void Chassis_RecoverControl(Chassis_t *c);

 /* Private functions -------------------------------------------------------- */

@ -120,7 +113,7 @@ static void Chassis_ResetControllers(Chassis_t *c) {
 */
 static void Chassis_SelectYawZeroPoint(Chassis_t *c) {
  float current_yaw = c->feedback.yaw.rotor_abs_angle;
-  float zero_point_1 = c->param->mech_zero_yaw;
+  float zero_point_1 = c->param->mech.mech_zero_yaw;
  float zero_point_2 = zero_point_1 + M_PI;

  float error_to_zero1 = CircleError(zero_point_1, current_yaw, M_2PI);
@ -145,10 +138,40 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
  if (c == NULL) return CHASSIS_ERR_NULL;
  if (mode == c->mode) return CHASSIS_OK;

+  /* RECOVER 进行中时，忽略 BALANCE 指令，等自起完成后自动切换 */
+  if (c->mode == CHASSIS_MODE_RECOVER &&
+      mode == CHASSIS_MODE_WHELL_LEG_BALANCE) {
+    return CHASSIS_OK;
+  }
+
  Chassis_MotorEnable(c);
  Chassis_ResetControllers(c);
  Chassis_SelectYawZeroPoint(c);

+  /* RELAX -> BALANCE：根据 pitch 角度判断是否需要自起 */
+  if (mode == CHASSIS_MODE_WHELL_LEG_BALANCE && c->mode == CHASSIS_MODE_RELAX) {
+    float pitch = c->feedback.imu.euler.pit;
+    /* pitch 绝对值大于 0.8rad（约46度）认为是倒地，需要自起 */
+    if (fabsf(pitch) > 0.2f) {
+      /* 根据 theta 判断腿朝上还是朝下 */
+      float theta_l = c->vmc_[0].leg.theta;
+      float theta_r = c->vmc_[1].leg.theta;
+      c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
+      c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
+      c->mode = CHASSIS_MODE_RECOVER;
+      return CHASSIS_OK;
+    }
+    /* 否则直接进入平衡模式 */
+  }
+
+  /* 直接进入 RECOVER 时也重置状态机 */
+  if (mode == CHASSIS_MODE_RECOVER) {
+    float theta_l = c->vmc_[0].leg.theta;
+    float theta_r = c->vmc_[1].leg.theta;
+    c->recover.state[0] = (fabsf(theta_l) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
+    c->recover.state[1] = (fabsf(theta_r) > (float)M_PI_2) ? RECOVER_FLIP : RECOVER_STRETCH;
+  }
+
  c->mode = mode;
  return CHASSIS_OK;
 }
@ -289,88 +312,169 @@ static float Chassis_CalcSpringForce(float leg_length)
 }

 /**
- * @brief 跳跃控制状态机
+ * @brief 跳跃控制
 * @param c 底盘结构体指针
 * @param c_cmd 控制指令
 * @param target_L0 输出的目标腿长数组[2]
 * @param extra_force 输出的额外力数组[2]
+ *
+ * @note 跳跃流程：
+ *   触发后直接进入 LAUNCH(蹬地起跳) -> RETRACT(收腿) -> LAND(落地缓冲) -> IDLE
 */
 static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force) {
-  uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000);  /* 当前时间 ms */
-  uint32_t elapsed_time = current_time - c->jump.state_start_time;
-  
+  uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000);
+
  /* 初始化额外力为0 */
  extra_force[0] = 0.0f;
  extra_force[1] = 0.0f;
-  
-  /* 检测跳跃触发 */
+
+  /* ==================== 跳跃触发检测 ==================== */
+  /* 触发后直接进入LAUNCH */
  if (c->jump.trigger && c->jump.state == JUMP_IDLE) {
-    c->jump.state = JUMP_CROUCH;
+    c->jump.state = JUMP_LAUNCH;  /* 直接进入起跳阶段 */
    c->jump.state_start_time = current_time;
-    c->jump.trigger = false;  /* 清除触发标志 */
+    c->jump.trigger = false;
  }
-  
-  /* 跳跃状态机 */
+
+  /* elapsed_time 必须在触发逻辑之后计算 */
+  uint32_t elapsed_time = current_time - c->jump.state_start_time;
+
+  /* ==================== 跳跃状态机 ==================== */
  switch (c->jump.state) {
    case JUMP_IDLE:
-      /* 待机状态，使用正常腿长控制 */
+      /* 待机状态，预收腿逻辑已在上面处理 */
      break;
-      
+
    case JUMP_CROUCH:
-      /* 蓄力阶段：缩短腿长 */
-      target_L0[0] = c->param->jump_params.crouch_leg_length;
-      target_L0[1] = c->param->jump_params.crouch_leg_length;
-      
-      if (elapsed_time >= c->param->jump_params.crouch_time_ms) {
-        c->jump.state = JUMP_LAUNCH;
-        c->jump.state_start_time = current_time;
-      }
+      /* CROUCH状态已废弃，直接跳到LAUNCH */
+      c->jump.state = JUMP_LAUNCH;
+      c->jump.state_start_time = current_time;
      break;
-      
-    case JUMP_LAUNCH:
-      /* 起跳阶段：发力向下推地面 */
-      target_L0[0] = MAX_LEG_LENGTH;  /* 腿伸长 */
-      target_L0[1] = MAX_LEG_LENGTH;
-      extra_force[0] = c->param->jump_params.launch_force;  /* 额外向下的力 */
-      extra_force[1] = c->param->jump_params.launch_force;
-      
-      if (elapsed_time >= c->param->jump_params.launch_time_ms) {
+
+    case JUMP_LAUNCH: {
+      /* 起跳阶段：腿伸长 + 大推力 */
+      target_L0[0] = c->param->leg.max_length;
+      target_L0[1] = c->param->leg.max_length;
+
+      /* 参考Chassis项目：直接给大推力，不依赖PID增益 */
+      /* 腿长误差约0.17m，Kp=70000时，力=70000*0.17=11900N */
+      /* 考虑到机器人重量和跳跃高度，给5000-8000N的推力 */
+      extra_force[0] = 6000.0f;  /* 直接给推力(N) */
+      extra_force[1] = 6000.0f;
+
+      /* 位置驱动：腿伸到位就收腿（差值<3cm） */
+      bool leg_ext = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) &&
+                     (c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f);
+      if (leg_ext) {
        c->jump.state = JUMP_RETRACT;
        c->jump.state_start_time = current_time;
      }
-      break;
-      
-    case JUMP_RETRACT:
-      /* 收腿阶段：腿收缩准备落地 */
-      target_L0[0] = c->param->jump_params.retract_leg_length;
-      target_L0[1] = c->param->jump_params.retract_leg_length;
-      extra_force[0] = c->param->jump_params.retract_force;  /* 前馈力帮助收腿 */
-      extra_force[1] = c->param->jump_params.retract_force;
-      
-      /* 检测落地或超时 */
-      if ( elapsed_time >= c->param->jump_params.retract_time_ms) {
+      /* 超时保护 */
+      if (elapsed_time >= c->param->jump.launch_time_ms) {
+        c->jump.state = JUMP_RETRACT;
+        c->jump.state_start_time = current_time;
+      }
+    } break;
+
+    case JUMP_RETRACT: {
+      /* 收腿阶段 */
+      target_L0[0] = c->param->jump.retract_leg_length;
+      target_L0[1] = c->param->jump.retract_leg_length;
+      extra_force[0] = c->param->jump.retract_force;
+      extra_force[1] = c->param->jump.retract_force;
+
+      /* 位置驱动：腿缩到位就进落地缓冲 */
+      float rt = c->param->jump.retract_leg_length;
+      bool leg_retracted = (c->vmc_[0].leg.L0 < rt + 0.02f) &&
+                           (c->vmc_[1].leg.L0 < rt + 0.02f);
+      if (leg_retracted) {
        c->jump.state = JUMP_LAND;
        c->jump.state_start_time = current_time;
      }
-      break;
-      
-    case JUMP_LAND:
-      /* 落地阶段：缓冲并恢复正常 */
-      /* 使用正常腿长，让PID自动恢复 */
-      
-      if (elapsed_time >= c->param->jump_params.land_time_ms) {
-        c->jump.state = JUMP_IDLE;
+      /* 超时保护 */
+      if (elapsed_time >= c->param->jump.retract_time_ms) {
+        c->jump.state = JUMP_LAND;
        c->jump.state_start_time = current_time;
      }
+    } break;
+
+    case JUMP_LAND:
+      /* 落地缓冲：时间到就回 IDLE */
+      if (elapsed_time >= c->param->jump.land_time_ms) {
+        c->jump.state = JUMP_IDLE;
+      }
      break;
-      
+
    default:
      c->jump.state = JUMP_IDLE;
      break;
  }
 }

-/* Public functions --------------------------------------------------------- */
+/**
+ * @brief 倒地自起控制
+ * @note 自起流程（左右腿独立状态机）：
+ *   STRETCH(伸腿) -> BACKLEG(后甩腿) -> COMPLETE(完成)
+ *   两腿均 COMPLETE 后自动切换到 WHELL_LEG_BALANCE 模式
+ *
+ * @param c 底盘结构体指针
+ */
+static void Chassis_RecoverControl(Chassis_t *c) {
+  float theta_l = c->vmc_[0].leg.theta;
+  float theta_r = c->vmc_[1].leg.theta;
+
+  /* ===== 左腿状态机 ===== */
+  switch (c->recover.state[0]) {
+    case RECOVER_FLIP:
+      /* 翻身阶段：theta朝上（|theta| > π/2），收腿慢慢翻转 */
+      /* 等待 theta 转到腿朝下（|theta| < π/2） */
+      if (fabsf(theta_l) < (float)M_PI_2) {
+        c->recover.state[0] = RECOVER_STRETCH;
+      }
+      break;
+
+    case RECOVER_STRETCH:
+      /* 伸腿：等待腿伸到位 */
+      if (c->vmc_[0].leg.L0 > 0.28f) {
+        c->recover.state[0] = RECOVER_BACKLEG;
+      }
+      break;
+
+    case RECOVER_BACKLEG:
+      /* 后甩腿：等待 phi0 趋近目标角度 */
+      if (fabsf(c->vmc_[0].leg.phi0 - 0.5f) < 0.1f) {
+        c->recover.state[0] = RECOVER_COMPLETE;
+      }
+      break;
+
+    case RECOVER_COMPLETE:
+      break;
+  }
+
+  /* ===== 右腿状态机 ===== */
+  switch (c->recover.state[1]) {
+    case RECOVER_FLIP:
+      if (fabsf(theta_r) < (float)M_PI_2) {
+        c->recover.state[1] = RECOVER_STRETCH;
+      }
+      break;
+
+    case RECOVER_STRETCH:
+      if (c->vmc_[1].leg.L0 > 0.28f) {
+        c->recover.state[1] = RECOVER_BACKLEG;
+      }
+      break;
+
+    case RECOVER_BACKLEG:
+      if (fabsf(c->vmc_[1].leg.phi0 - 0.5f) < 0.1f) {
+        c->recover.state[1] = RECOVER_COMPLETE;
+      }
+      break;
+
+    case RECOVER_COMPLETE:
+      break;
+  }
+}

 /**
 * @brief 初始化底盘
@ -499,9 +603,9 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
  c->jump.state = JUMP_IDLE;
  c->jump.trigger = false;
  c->jump.state_start_time = 0;
-  c->jump.crouch_leg_length = c->param->jump_params.crouch_leg_length;
-  c->jump.launch_force = c->param->jump_params.launch_force;
-  c->jump.retract_leg_length = c->param->jump_params.retract_leg_length;
+  c->jump.crouch_leg_length = c->param->jump.crouch_leg_length;
+  c->jump.launch_force = c->param->jump.launch_force;
+  c->jump.retract_leg_length = c->param->jump.retract_leg_length;

  return CHASSIS_OK;
 }
@ -532,7 +636,7 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
    c->feedback.joint[i].rotor_abs_angle = -c->feedback.joint[i].rotor_abs_angle;
    
    /* 应用零点偏移 */
-    c->feedback.joint[i].rotor_abs_angle -= c->param->joint_zero[i];
+    c->feedback.joint[i].rotor_abs_angle -= c->param->mech.joint_zero[i];
  }

  /* 更新轮子电机反馈 */
@ -582,7 +686,7 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {

  /* 跳跃触发入口统一放在底盘控制主流程：由chassis_cmd直接驱动 */
  if (c_cmd->jump_trigger &&
-      c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE &&
+      (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE || c->mode == CHASSIS_MODE_BALANCE_ROTOR) &&
      c->jump.state == JUMP_IDLE) {
    c->jump.trigger = true;
  }
@ -615,23 +719,53 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
      break;

    case CHASSIS_MODE_RECOVER: {
-      float fn = -20.0f, tp = 0.0f;
-      Chassis_LQRControl(c, c_cmd);
+      /* 运行自起状态机 */
+      Chassis_RecoverControl(c);

-      /* 计算弹簧力补偿 */
+      /* 根据状态决定输出力 */
+      float fn_left, fn_right;
      float spring_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
      float spring_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
-      float fn_left = fn - (isnan(spring_left) ? 0.0f : spring_left);
-      float fn_right = fn - (isnan(spring_right) ? 0.0f : spring_right);
+      if (isnan(spring_left)) spring_left = 0.0f;
+      if (isnan(spring_right)) spring_right = 0.0f;

-      VMC_InverseSolve(&c->vmc_[0], fn_left, tp);
+      /* 左腿 */
+      if (c->recover.state[0] == RECOVER_FLIP) {
+        fn_left = 30.0f - spring_left;   /* 收腿，借重力慢慢翻转 */
+      } else if (c->recover.state[0] == RECOVER_STRETCH) {
+        fn_left = -80.0f - spring_left;  /* 大力伸腿 */
+      } else if (c->recover.state[0] == RECOVER_BACKLEG) {
+        fn_left = -40.0f - spring_left;  /* 维持伸腿 */
+      } else {
+        fn_left = -20.0f - spring_left;
+      }
+
+      /* 右腿 */
+      if (c->recover.state[1] == RECOVER_FLIP) {
+        fn_right = 30.0f - spring_right;
+      } else if (c->recover.state[1] == RECOVER_STRETCH) {
+        fn_right = -80.0f - spring_right;
+      } else if (c->recover.state[1] == RECOVER_BACKLEG) {
+        fn_right = -40.0f - spring_right;
+      } else {
+        fn_right = -20.0f - spring_right;
+      }
+
+      VMC_InverseSolve(&c->vmc_[0], fn_left, 0.0f);
      VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
-      VMC_InverseSolve(&c->vmc_[1], fn_right, tp);
+      VMC_InverseSolve(&c->vmc_[1], fn_right, 0.0f);
      VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
-
-      c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f;
-      c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f;
+      c->output.wheel[0] = 0.0f;
+      c->output.wheel[1] = 0.0f;
      Chassis_Output(c);
+
+      /* 两腿均完成，自动切换到平衡模式 */
+      if (c->recover.state[0] == RECOVER_COMPLETE &&
+          c->recover.state[1] == RECOVER_COMPLETE) {
+        Chassis_ResetControllers(c);
+        Chassis_SelectYawZeroPoint(c);
+        c->mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+      }
    } break;
    
    // case CHASSIS_MODE_CALIBRATE: {
@ -716,7 +850,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {

  // /* 加速度限制 */
  // float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
-  // float max_velocity_change = MAX_ACCELERATION * c->dt;
+  // float max_velocity_change = c->param->motion.max_acceleration * c->dt;
  // velocity_change = LIMIT(velocity_change, -max_velocity_change, max_velocity_change);

  // float target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
@ -750,9 +884,9 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {

  /* ==================== 目标状态 ==================== */
  LQR_State_t target_state = {
-      .theta = 0.1f,
+      .theta = 0.0f,
      .d_theta = 0.0f,
-      .phi = -0.1f,
+      .phi = 0.0f,
      .d_phi = 0.0f,
      .x = c->chassis_state.target_x,
      .d_x = c->chassis_state.target_velocity_x,
@ -763,8 +897,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
    c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;

    float manual_offset = c_cmd->move_vec.vy * M_PI_2;
-    float base_target_1 = c->param->mech_zero_yaw + manual_offset;
-    float base_target_2 = c->param->mech_zero_yaw + M_PI + manual_offset;
+    float base_target_1 = c->param->mech.mech_zero_yaw + manual_offset;
+    float base_target_2 = c->param->mech.mech_zero_yaw + M_PI + manual_offset;

    float error_to_target1 = CircleError(base_target_1, c->yaw_control.current_yaw, M_2PI);
    float error_to_target2 = CircleError(base_target_2, c->yaw_control.current_yaw, M_2PI);
@ -786,11 +920,11 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
    LQR_SetTargetState(&c->lqr[0], &target_state);
    LQR_Control(&c->lqr[0]);
  } else {
-    target_state.theta = NON_CONTACT_THETA;
+    target_state.theta = c->param->motion.non_contact_theta;
    LQR_SetTargetState(&c->lqr[0], &target_state);
    c->lqr[0].control_output.T = 0.0f;
    c->lqr[0].control_output.Tp =
-        c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + NON_CONTACT_THETA) +
+        c->lqr[0].K_matrix[1][0] * (-c->vmc_[0].leg.theta + c->param->motion.non_contact_theta) +
        c->lqr[0].K_matrix[1][1] * (-c->vmc_[0].leg.d_theta);
    c->yaw_control.yaw_force = 0.0f;
  }
@ -800,22 +934,27 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
    LQR_SetTargetState(&c->lqr[1], &target_state);
    LQR_Control(&c->lqr[1]);
  } else {
-    target_state.theta = NON_CONTACT_THETA;
+    target_state.theta = c->param->motion.non_contact_theta;
    LQR_SetTargetState(&c->lqr[1], &target_state);
    c->lqr[1].control_output.T = 0.0f;
    c->lqr[1].control_output.Tp =
-        c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + NON_CONTACT_THETA) +
+        c->lqr[1].K_matrix[1][0] * (-c->vmc_[1].leg.theta + c->param->motion.non_contact_theta) +
        c->lqr[1].K_matrix[1][1] * (-c->vmc_[1].leg.d_theta);
    c->yaw_control.yaw_force = 0.0f;
  }

  /* ==================== 轮毂输出 ==================== */
-  c->output.wheel[0] = c->lqr[0].control_output.T / WHEEL_GEAR_RATIO + c->yaw_control.yaw_force;
-  c->output.wheel[1] = c->lqr[1].control_output.T / WHEEL_GEAR_RATIO - c->yaw_control.yaw_force;
+  /* 腿长增加时有效轮距增大，等比例缩小yaw_force防止过冲 */
+  float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f;
+  float yaw_scale = c->param->leg.base_length / avg_L0;  /* 以基础腿长为基准归一化 */
+  float scaled_yaw_force = c->yaw_control.yaw_force * yaw_scale;
+
+  c->output.wheel[0] = c->lqr[0].control_output.T / c->param->mech.wheel_gear_ratio + scaled_yaw_force;
+  c->output.wheel[1] = c->lqr[1].control_output.T / c->param->mech.wheel_gear_ratio - scaled_yaw_force;

  /* ==================== 横滚角补偿 ==================== */
  /* 方法1: 几何前馈腿长补偿 (参考底盘文件夹算法) */
-  float Rl = c->param->hip_width / 2.0f;  /* 髋宽的一半 */
+  float Rl = c->param->mech.hip_width / 2.0f;  /* 髋宽的一半 */
  float roll_error = c->feedback.imu.euler.rol - 0.0f;  /* Roll角误差 */
  float Delta_L0 = c->vmc_[1].leg.L0 - c->vmc_[0].leg.L0;  /* 右腿减左腿腿长差 */
  
@ -844,9 +983,10 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
  /* ==================== 腿长控制 ==================== */
  float target_L0[2];
  float jump_extra_force[2] = {0.0f, 0.0f};
-  
+
  /* 基础腿长目标 (应用几何前馈补偿) */
-  float base_leg_length = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE;
+  /* height 可以为负值用于收腿（预蹲），正值用于伸腿 */
+  float base_leg_length = c->param->leg.base_length + c_cmd->height * c->param->leg.length_range;
  target_L0[0] = base_leg_length + roll_leg_compensation_left;
  target_L0[1] = base_leg_length + roll_leg_compensation_right;

@ -859,23 +999,23 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
  /* 检查哪条腿更短（需要缩短的那条） */
  if (target_L0[0] < target_L0[1]) {
    /* 左腿更短，检查是否触碰下限 */
-    if (target_L0[0] < MIN_LEG_LENGTH) {
-      compensation_transfer = MIN_LEG_LENGTH - target_L0[0];  /* 计算短缺量 */
-      target_L0[0] = MIN_LEG_LENGTH;  /* 左腿限制在最小值 */
+    if (target_L0[0] < c->param->leg.min_length) {
+      compensation_transfer = c->param->leg.min_length - target_L0[0];  /* 计算短缺量 */
+      target_L0[0] = c->param->leg.min_length;  /* 左腿限制在最小值 */
      target_L0[1] += compensation_transfer;  /* 右腿补偿增加 */
    }
  } else if (target_L0[1] < target_L0[0]) {
    /* 右腿更短，检查是否触碰下限 */
-    if (target_L0[1] < MIN_LEG_LENGTH) {
-      compensation_transfer = MIN_LEG_LENGTH - target_L0[1];  /* 计算短缺量 */
-      target_L0[1] = MIN_LEG_LENGTH;  /* 右腿限制在最小值 */
+    if (target_L0[1] < c->param->leg.min_length) {
+      compensation_transfer = c->param->leg.min_length - target_L0[1];  /* 计算短缺量 */
+      target_L0[1] = c->param->leg.min_length;  /* 右腿限制在最小值 */
      target_L0[0] += compensation_transfer;  /* 左腿补偿增加 */
    }
  }
  
  /* 最终安全限幅 */
-  target_L0[0] = LIMIT(target_L0[0], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
-  target_L0[1] = LIMIT(target_L0[1], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
+  target_L0[0] = LIMIT(target_L0[0], c->param->leg.min_length, c->param->leg.max_length);
+  target_L0[1] = LIMIT(target_L0[1], c->param->leg.min_length, c->param->leg.max_length);

  /* 获取腿长变化率 */
  float leg_d_length[2];
@ -895,18 +1035,21 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
  float pid_output_left = PID_Calc(&c->pid.leg_length[0], target_L0[0],
                                    c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
  float spring_force_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
-  if (isnan(spring_force_left)) spring_force_left = 0.0f;  /* 处理无效值 */
-  
-  /* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
+  if (isnan(spring_force_left)) spring_force_left = 0.0f;
+
  float virtual_force_left, virtual_torque_left;
-  if (c->jump.state == JUMP_RETRACT) {
-    /* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
-    virtual_force_left = pid_output_left - spring_force_left + jump_extra_force[0];
-    virtual_torque_left = 0.0f;  /* 收腿时不控制摆角 */
+  if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) {
+    /* 跳跃阶段：完全接管力控 */
+    /* 所有跳跃阶段统一处理：PID + 额外力 */
+    virtual_force_left = jump_extra_force[0] + pid_output_left;
+    virtual_torque_left = 0.0f;
+    /* 跳跃阶段同时清零轮子，防止平衡控制干扰 */
+    c->output.wheel[0] = 0.0f;
+    c->output.wheel[1] = 0.0f;
  } else {
    /* 正常状态：添加Roll力补偿 (低增益避免抖动) */
    virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
-                          pid_output_left + LEFT_BASE_FORCE - spring_force_left + 
+                          pid_output_left + c->param->leg.left_base_force - spring_force_left +
                          jump_extra_force[0] + roll_force_compensation;
    virtual_torque_left = c->lqr[0].control_output.Tp + Delta_Tp[0];
  }
@ -922,18 +1065,18 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
  float pid_output_right = PID_Calc(&c->pid.leg_length[1], target_L0[1],
                                     c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
  float spring_force_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
-  if (isnan(spring_force_right)) spring_force_right = 0.0f;  /* 处理无效值 */
-  
-  /* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
+  if (isnan(spring_force_right)) spring_force_right = 0.0f;
+
  float virtual_force_right, virtual_torque_right;
-  if (c->jump.state == JUMP_RETRACT) {
-    /* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
-    virtual_force_right = pid_output_right - spring_force_right + jump_extra_force[1];
-    virtual_torque_right = 0.0f;  /* 收腿时不控制摆角 */
+  if (c->jump.state == JUMP_CROUCH || c->jump.state == JUMP_LAUNCH || c->jump.state == JUMP_RETRACT) {
+    /* 跳跃阶段：完全接管力控 */
+    /* 所有跳跃阶段统一处理：PID + 额外力 */
+    virtual_force_right = jump_extra_force[1] + pid_output_right;
+    virtual_torque_right = 0.0f;
  } else {
    /* 正常状态：添加反向Roll力补偿 (低增益避免抖动) */
    virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
-                           pid_output_right + RIGHT_BASE_FORCE - spring_force_right + 
+                           pid_output_right + c->param->leg.right_base_force - spring_force_right +
                           jump_extra_force[1] - roll_force_compensation;
    virtual_torque_right = c->lqr[1].control_output.Tp + Delta_Tp[1];
  }
--- a/User/module/balance_chassis.h
+++ b/User/module/balance_chassis.h
@ -41,7 +41,7 @@ typedef enum {
  CHASSIS_MODE_RELAX,            /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
  CHASSIS_MODE_RECOVER,          /* 复位模式 */
  // CHASSIS_MODE_CALIBRATE,        /* 校准模式 */
-  CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
+  CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡，支持跳跃 */
  CHASSIS_MODE_BALANCE_ROTOR /*小陀螺*/
 } Chassis_Mode_t;

@ -54,6 +54,14 @@ typedef enum {
  JUMP_LAND       /* 落地阶段：恢复正常控制 */
 } Jump_State_t;

+/* 自起状态枚举 */
+typedef enum {
+  RECOVER_FLIP,      /* 翻身阶段：theta朝上时，先慢慢翻转到腿朝下 */
+  RECOVER_STRETCH,   /* 伸腿阶段：腿伸到最大长度 */
+  RECOVER_BACKLEG,   /* 后甩腿阶段：phi0 趋近目标角度 */
+  RECOVER_COMPLETE   /* 完成：切换到平衡模式 */
+} Recover_State_t;
+
 typedef struct {
  Chassis_Mode_t mode;           /* 底盘模式 */
  MoveVector_t move_vec;         /* 底盘运动向量 */
@ -89,37 +97,61 @@ typedef struct {
  VMC_Param_t vmc_param[2];        /* VMC参数 */
  LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
  
-  KPID_Params_t yaw;               /* yaw轴PID控制参数，用于控制底盘朝向 */
-  KPID_Params_t roll;              /* roll轴腿长补偿PID控制参数 */
-  KPID_Params_t roll_force;        /* roll轴力补偿PID控制参数 */
-  KPID_Params_t tp;          /* 摆力矩PID控制参数，用于控制摆力矩 */
-  KPID_Params_t leg_length;         /* 腿长PID控制参数，用于控制虚拟腿长度 */
-  KPID_Params_t leg_theta;          /* 摆角PID控制参数，用于控制虚拟腿摆角 */
+  KPID_Params_t yaw;               /* yaw轴PID控制参数 */
+  KPID_Params_t roll;              /* roll轴腿长补偿PID */
+  KPID_Params_t roll_force;        /* roll轴力补偿PID */
+  KPID_Params_t tp;                /* 摆力矩PID */
+  KPID_Params_t leg_length;        /* 腿长PID */
+  KPID_Params_t leg_theta;         /* 摆角PID */

+  /* 机械参数 */
  struct {
-    uint32_t crouch_time_ms;      /* 蓄力时间 (ms) */
-    uint32_t launch_time_ms;      /* 起跳发力时间 (ms) */
-    uint32_t retract_time_ms;     /* 收腿时间 (ms) */
-    uint32_t land_time_ms;        /* 落地缓冲时间 (ms) */
-    float crouch_leg_length;      /* 蓄力时腿长 (m) */
-    float launch_force;           /* 起跳力 (N) */
-    float retract_leg_length;     /* 收腿时腿长 (m) */
-    float retract_force;          /* 收腿前馈力 (N)，负值表示向上收 */
-  } jump_params;
+    float wheel_radius;            /* 轮子半径 (m) */
+    float wheel_gear_ratio;        /* 轮毂电机减速比 */
+    float hip_width;               /* 髋宽，两腿间距 (m) */
+    float joint_zero[4];           /* 关节电机零点偏移 */
+    float mech_zero_yaw;           /* yaw轴机械零点 */
+  } mech;

-  float joint_zero[4]; /* 关节电机零点偏移位置 */
-  float hip_width;     /* 髋宽，两腿间距 (m) */
+  /* 腿长控制参数 */
+  struct {
+    float base_length;             /* 基础腿长 (m) */
+    float length_range;            /* 腿长调节范围 (m)，波轮控制 */
+    float min_length;              /* 最小腿长 (m) */
+    float max_length;              /* 最大腿长 (m) */
+    float left_base_force;         /* 左腿基础支撑力 (N) */
+    float right_base_force;        /* 右腿基础支撑力 (N) */
+  } leg;

-  float mech_zero_yaw; /* 机械零点 */
+  /* 跳跃参数 */
+  struct {
+    uint32_t crouch_time_ms;       /* 蓄力超时 (ms) */
+    uint32_t launch_time_ms;       /* 起跳超时 (ms) */
+    uint32_t retract_time_ms;      /* 收腿超时 (ms) */
+    uint32_t land_time_ms;         /* 落地缓冲时间 (ms) */
+    float crouch_leg_length;       /* 蓄力时腿长 (m) */
+    float launch_force;            /* 起跳额外力 (N) */
+    float retract_leg_length;      /* 收腿时腿长 (m) */
+    float retract_force;           /* 收腿前馈力 (N)，负值=向上 */
+  } jump;

-  float theta;
-  float x;
-  float phi;
+  /* 运动控制参数 */
+  struct {
+    float max_acceleration;        /* 最大加速度 (m/s²) */
+    float non_contact_theta;       /* 离地时摆角目标 (rad) */
+  } motion;
+
+  /* LQR 目标状态偏移 */
+  struct {
+    float theta;
+    float x;
+    float phi;
+  } lqr_offset;

  /* 低通滤波器截止频率 */
  struct {
-    float in;  /* 输入 */
-    float out; /* 输出 */
+    float in;
+    float out;
  } low_pass_cutoff_freq;
 } Chassis_Params_t;

@ -173,6 +205,11 @@ typedef struct {
    float retract_leg_length; /* 收腿时的腿长 */
  } jump;

+  /* 自起控制相关 */
+  struct {
+    Recover_State_t state[2]; /* 左右腿自起状态 */
+  } recover;
+
  /* PID计算的目标值 */
  struct {
    AHRS_Eulr_t chassis;
--- a/User/module/config.c
+++ b/User/module/config.c
@ -66,7 +66,7 @@ Config_RobotParam_t robot_config = {
        },
        .mech_zero = {
            .yaw = 0.0f,
-            .pit = 2.2f,
+            .pit = 3.23056364f,
        },
        .travel = {
            .yaw = -1.0f,
@ -214,11 +214,11 @@ Config_RobotParam_t robot_config = {

        .leg_length={
            .k = 40.0f,
-            .p = 20.0f,
+            .p = 50.0f,
            .i = 0.01f,
            .d = 2.0f,
            .i_limit = 0.0f,
-            .out_limit = 200.0f,
+            .out_limit = 500.0f,
            .d_cutoff_freq = -1.0f,
            .range = -1.0f,
        },
@ -308,10 +308,29 @@ Config_RobotParam_t robot_config = {
            },
        },

-        .mech_zero_yaw = 1.78040409f, /* 机械零点 */
-        .joint_zero = {0.0f, 0.0f, 3.84780002f, -1.16999996f}, /* 关节电机零点偏移位置 */
-        .hip_width = 0.423f,  /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */
-                .vmc_param = {
+        .mech = {
+            .wheel_radius = 0.068f,
+            .wheel_gear_ratio = 4.5f,
+            .hip_width = 0.423f,           /* 髋宽，即两腿间距 (m)，用于Roll几何补偿 */
+            .joint_zero = {0.0f, 0.0f, 1.33022018f, 1.13195965f}, /* 关节电机零点偏移 */
+            .mech_zero_yaw = 2.96925735f,  /* 机械零点 */
+        },
+
+        .leg = {
+            .base_length = 0.16f,          /* 基础腿长 (m) */
+            .length_range = 0.20f,         /* 腿长调节范围 (m) */
+            .min_length = 0.14f,           /* 最小腿长 (m) */
+            .max_length = 0.36f,           /* 最大腿长 (m) */
+            .left_base_force = 65.0f,      /* 左腿基础支撑力 (N) */
+            .right_base_force = 65.0f,     /* 右腿基础支撑力 (N) */
+        },
+
+        .motion = {
+            .max_acceleration = 5.0f,      /* 最大加速度 (m/s^2) */
+            .non_contact_theta = -0.01f,    /* 离地摆角目标 (rad) */
+        },
+
+        .vmc_param = {
            { // 左腿
                .leg_1 = 0.215f,      // 后髋连杆长度 (L1) (m)
                .leg_2 = 0.258f,      // 后膝连杆长度 (L2) (m)
@ -327,36 +346,38 @@ Config_RobotParam_t robot_config = {
                .hip_length = 0.0f    // 髋宽 (L5) (m)
            }
        },
-
    .lqr_gains = {
-        .k11_coeff = { -2.110325959941390e+02f, 2.474876302869424e+02f, -1.259312069344584e+02f, -3.555067587081389e+00f }, // theta
-        .k12_coeff = { 1.729035713826111e+00f, -1.094205267383853e+00f, -8.235618497594484e+00f, -2.949091603597677e-01f },  // d_theta
-        .k13_coeff = { -3.661055595462458e+01f, 4.066046385752247e+01f, -1.618786894025212e+01f, -1.618742454043447e+00f },  // x
-        .k14_coeff = { -2.988926501197955e+01f, 3.445642428071118e+01f, -1.547117906530474e+01f, -2.039186188549067e+00f },  // d_x
-        .k15_coeff = { -4.094585161782607e+01f, 6.119228618474273e+01f, -3.603009625243965e+01f, 1.037078067693317e+01f }, // phi
-        .k16_coeff = { -7.625020535837323e+00f, 1.097226354374976e+01f, -6.302244199082423e+00f, 1.920720114282383e+00f },  // d_phi
-        .k21_coeff = { 3.113860878856309e+02f, -2.577426343951217e+02f, 4.982404130107013e+01f, 1.415180465415879e+01f }, // theta
-        .k22_coeff = { 1.126082466887103e+01f, -9.812804977178935e+00f, 1.814407629267238e+00f, 1.941377111901043e+00f },  // d_theta
-        .k23_coeff = { -1.771607516002724e+01f, 4.047388400543465e+01f, -2.962942748901807e+01f, 8.971209797230671e+00f },  // x
-        .k24_coeff = { -3.391460592939657e+01f, 5.575858169617614e+01f, -3.466996436706782e+01f, 9.833893476570065e+00f },  // d_x
-        .k25_coeff = { 2.392778926806863e+02f, -2.574664846375947e+02f, 9.953632837845201e+01f, 2.579297039170335e+00f }, // phi
-        .k26_coeff = { 4.297556375859175e+01f, -4.657509218314731e+01f, 1.826212630358702e+01f, 1.257410509642398e-01f },  // d_phi
+        .k11_coeff = { -1.997928012118380e+02f, 2.358970856133577e+02f, -1.221432656066952e+02f, -3.660884265594530e+00f }, // theta
+        .k12_coeff = { 1.144642140244768e+00f, -6.781143989723293e-01f, -8.286867905694857e+00f, -3.233743818654922e-01f },  // d_theta
+        .k13_coeff = { -2.920908857554099e+01f, 3.294053235814564e+01f, -1.331127106026891e+01f, -2.036633181450379e+00f },  // x
+        .k14_coeff = { -2.202395151966686e+01f, 2.624258394298977e+01f, -1.238392622003398e+01f, -2.516594294742349e+00f },  // d_x
+        .k15_coeff = { -5.531234648285231e+01f, 7.666196018790744e+01f, -4.254301644877548e+01f, 1.191958597928930e+01f }, // phi
+        .k16_coeff = { -8.603393557876432e+00f, 1.181868681749069e+01f, -6.519358011847959e+00f, 2.002836931783026e+00f },  // d_phi
+        .k21_coeff = { 3.121841964126664e+02f, -2.644789946321499e+02f, 5.653973783920610e+01f, 1.306067415926613e+01f }, // theta
+        .k22_coeff = { 9.751578045310433e+00f, -8.174866581419979e+00f, 1.060040492386880e+00f, 1.945460420856203e+00f },  // d_theta
+        .k23_coeff = { -1.589159892148102e+01f, 3.771040781828523e+01f, -2.789168930865428e+01f, 8.354369470743295e+00f },  // x
+        .k24_coeff = { -3.304075197263637e+01f, 5.411818719230526e+01f, -3.342861600971858e+01f, 9.256855471266778e+00f },  // d_x
+        .k25_coeff = { 2.574491871362636e+02f, -2.771545105998671e+02f, 1.074894289176479e+02f, 3.130550754680395e+00f }, // phi
+        .k26_coeff = { 4.162978395880715e+01f, -4.533986766238992e+01f, 1.794589550749570e+01f, 2.032213740678163e-01f },  // d_phi
    },


-    .jump_params = {
-        .crouch_time_ms = 100,
-        .launch_time_ms = 120,
-        .retract_time_ms = 80,
-        .land_time_ms = 300,
-        .crouch_leg_length = 0.14f,
-        .launch_force = 200.0f,
-        .retract_leg_length = 0.1f,   /* 收腿目标更短 */
-        .retract_force = -120.0f,      /* 收腿前馈力加大 */
+    .jump = {
+        .crouch_time_ms = 500,
+        .launch_time_ms = 300,
+        .retract_time_ms = 500,
+        .land_time_ms = 500,
+        .crouch_leg_length = 0.13f,    /* 不用蹲太低 */
+        .launch_force = 20.0f,         /* 起跳力减小 */
+        .retract_leg_length = 0.12f,   /* 收腿目标 */
+        .retract_force = -20.0f,       /* 收腿前馈力减小 */
    },
+
+    .lqr_offset = {
        .theta = 0.0f,
        .x = 0.0f,
        .phi = 0.0f,
+    },
    
    },

--- a/User/module/shoot.c
+++ b/User/module/shoot.c
@ -134,7 +134,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
 	
    memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
    memset(&s->output,0,sizeof(s->output));
-    s->target_variable.target_rpm=2000.0f;  /* 归一化目标转速 */
+    s->target_variable.target_rpm=6800.0f;  /* 归一化目标转速 */

 	return 0;
 }
--- a/User/task/ctrl_chassis.c
+++ b/User/task/ctrl_chassis.c
@ -41,13 +41,21 @@ Chassis_IMU_t chassis_imu;
 static int print_chassis(const void *data, char *buf, size_t size) {
  const Chassis_t *chassis = (const Chassis_t *)data;
  return MRobot_Snprintf(buf, size,
-      "  Mode    : %d\r\n"
+      "  Mode    : %d  Recover: L=%d R=%d\r\n"
+      "  Jump    : state=%d trigger=%d\r\n"
+      "  LegLen  : L=%.3f R=%.3f (m)\r\n"
+      "  Theta   : L=%.3f R=%.3f (rad)\r\n"
+      "  Phi0    : L=%.3f R=%.3f (rad)\r\n"
      "  IMU     : Roll=%.2f Pitch=%.2f Yaw=%.2f (deg)\r\n"
      "  Motors  : J0=%.1f J1=%.1f J2=%.1f J3=%.1f (rpm)\r\n"
      "  Wheels  : W0=%.1f W1=%.1f (rpm)\r\n"
      "  Output  : J0=%.1f J1=%.1f J2=%.1f J3=%.1f\r\n"
      "  VelX    : %.3f m/s\r\n",
-      chassis->mode,
+      chassis->mode, chassis->recover.state[0], chassis->recover.state[1],
+      chassis->jump.state, chassis->jump.trigger,
+      chassis->vmc_[0].leg.L0, chassis->vmc_[1].leg.L0,
+      chassis->vmc_[0].leg.theta, chassis->vmc_[1].leg.theta,
+      chassis->vmc_[0].leg.phi0, chassis->vmc_[1].leg.phi0,
      chassis->feedback.imu.euler.rol, chassis->feedback.imu.euler.pit, chassis->feedback.imu.euler.yaw,
      chassis->feedback.joint[0].rotor_speed, chassis->feedback.joint[1].rotor_speed,
      chassis->feedback.joint[2].rotor_speed, chassis->feedback.joint[3].rotor_speed,
--- a/User/task/rc.c
+++ b/User/task/rc.c
@ -141,18 +141,37 @@ void Task_rc(void *argument) {
    osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);

 /************************* 底盘命令 **************************************/
-    /* 跳跃触发检测：ch_res 从 -1.0f 松开回 0 时触发 */
-    static bool ch_res_was_min = false;  /* 记录上次是否在最低位置 */
-    static uint8_t jump_trigger_hold_cnt = 0;  /* 触发保持计数，避免单拍脉冲被队列重置吞掉 */
-    const float CH_RES_MIN_THRESHOLD = -0.9f;  /* 判定为最低位置的阈值 */
-    const float CH_RES_RELEASE_THRESHOLD = -0.3f;  /* 判定为松开的阈值 */
+    /* 蓄力-释放跳跃逻辑：向上推蓄力收腿，松开回弹时触发跳跃 */
+    static float last_ch_res = 0.0f;  /* 上一次拨杆位置 */
+    static float min_ch_res = 0.0f;   /* 记录最小值（最大蓄力） */
+    static uint8_t jump_trigger_hold_cnt = 0;  /* 触发保持计数 */
+    static bool in_charge_state = false;  /* 是否处于蓄力状态 */

-    if (dr16.data.ch_res < CH_RES_MIN_THRESHOLD) {
-      ch_res_was_min = true;  /* 记录已到达最低位置 */
-    } else if (ch_res_was_min && dr16.data.ch_res > CH_RES_RELEASE_THRESHOLD) {
-      /* 从最低位置松开，触发跳跃 */
-      jump_trigger_hold_cnt = 5;  /* 保持5个RC周期，确保ctrl任务一定能收到 */
-      ch_res_was_min = false;  /* 重置状态 */
+    const float CHARGE_THRESHOLD = -0.4f;  /* 蓄力阈值：低于此值开始蓄力 */
+    const float RELEASE_THRESHOLD = -0.2f; /* 释放阈值：回到此值以上触发跳跃 */
+    const float MIN_CHARGE = -0.5f;        /* 最小蓄力量：至少要推到此值才能触发 */
+
+    /* 检测蓄力：拨杆向上推 */
+    if (dr16.data.ch_res < CHARGE_THRESHOLD) {
+      in_charge_state = true;
+      if (dr16.data.ch_res < min_ch_res) {
+        min_ch_res = dr16.data.ch_res;  /* 更新最小值 */
+      }
+    }
+
+    /* 检测释放：拨杆回弹到阈值以上 && 之前有足够蓄力 */
+    if (in_charge_state &&
+        dr16.data.ch_res > RELEASE_THRESHOLD &&
+        min_ch_res < MIN_CHARGE) {
+      jump_trigger_hold_cnt = 5;  /* 触发跳跃，保持5个RC周期 */
+      in_charge_state = false;    /* 退出蓄力状态 */
+      min_ch_res = 0.0f;          /* 重置蓄力状态 */
+    }
+
+    /* 拨杆回到中位，重置蓄力 */
+    if (dr16.data.ch_res > -0.1f) {
+      in_charge_state = false;
+      min_ch_res = 0.0f;
    }

    cmd_for_chassis.jump_trigger = (jump_trigger_hold_cnt > 0);
@ -160,6 +179,8 @@ void Task_rc(void *argument) {
      jump_trigger_hold_cnt--;
    }

+    last_ch_res = dr16.data.ch_res;  /* 保存当前值 */
+
 switch (dr16.data.sw_l) {
    case DR16_SW_UP:
      cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
@ -169,10 +190,7 @@ switch (dr16.data.sw_l) {
      cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
      break;
    case DR16_SW_DOWN:
-    // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
      cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
-      // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
-      // cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
      break;
    default:
      cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
@ -181,7 +199,8 @@ switch (dr16.data.sw_l) {
    cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y;
    cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x;
    cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x;
-    cmd_for_chassis.height = max(dr16.data.ch_res, 0.0f);
+    /* height 传递拨杆位置，负值表示下推收腿，正值表示上推伸腿 */
+    cmd_for_chassis.height = dr16.data.ch_res;

    osMessageQueueReset(
        task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞
--- a/utils/Simulation-master/balance/series_legs/get_k_length.m
+++ b/utils/Simulation-master/balance/series_legs/get_k_length.m
@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
    B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
    B=double(B);
    
-    Q=diag([1  1  20  5  200 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
-    R=[1.5 0;0 0.25];                %T Tp
+    Q=diag([4000  200  500  50  50000 10]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[150 0;0 5];                %T Tp
    
    K=lqr(A,B,Q,R);
Author	SHA1	Message	Date
Robofish	66e6d29808	添加ref	2026-03-12 00:51:15 +08:00
Robofish	527d2e9be4	修	2026-03-12 00:14:36 +08:00
Robofish	6480f59e1e	新轮腿	2026-03-09 14:36:04 +08:00
Robofish	5ca466dd45	准备重构	2026-03-03 23:19:23 +08:00
Robofish	2c5c77645d	跳跃	2026-03-03 00:16:50 +08:00
Robofish	2a51286ca3	测试	2026-02-25 17:57:05 +08:00