ref离线了

User/module/balance_chassis.c

@@ -1042,7 +1042,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     c->chassis_state.target_velocity_x = -world_vx * sinf(gimbal_offset) + world_vy * cosf(gimbal_offset);
     c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
   } else {
-    c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 2.0f;
+    c->chassis_state.target_velocity_x = c_cmd->move_vec.vx * 3.0f;
     c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
   }
 

User/module/config.c

@@ -73,9 +73,31 @@ Config_RobotParam_t robot_config = {
                 .d_cutoff_freq = -1.0f,
                 .range = M_2PI,
             },
+            .recover = {
+                .yaw_angle = {
+                    .k = 2.0f,
+                    .p = 1.0f,
+                    .i = 0.0f,
+                    .d = 0.0f,
+                    .i_limit = 0.0f,
+                    .out_limit = 5.0f,
+                    .d_cutoff_freq = -1.0f,
+                    .range = M_2PI,
+                },
+                .pit_angle = {
+                    .k = 1.5f,
+                    .p = 1.0f,
+                    .i = 0.0f,
+                    .d = 0.0f,
+                    .i_limit = 0.0f,
+                    .out_limit = 5.0f,
+                    .d_cutoff_freq = -1.0f,
+                    .range = M_2PI,
+                },
+            },
         },
         .mech_zero = {
-            .yaw = 0.0f,
+            .yaw = 2.96925735f,  /* 机械零点 */
             .pit = 3.23056364f,
         },
         .travel = {
@@ -500,7 +522,8 @@ Config_RobotParam_t robot_config = {
         .rc_mode_map = {
 #if CMD_ENABLE_MODULE_GIMBAL
             .gimbal_sw_up   = GIMBAL_MODE_RELAX,
-            .gimbal_sw_mid  = GIMBAL_MODE_ABSOLUTE,
+            // .gimbal_sw_mid  = GIMBAL_MODE_ABSOLUTE,
+            .gimbal_sw_mid = GIMBAL_MODE_RECOVER,
             .gimbal_sw_down = GIMBAL_MODE_RELATIVE,
 #endif
 #if CMD_ENABLE_MODULE_BALANCE_CHASSIS

User/module/gimbal.c

@@ -46,6 +46,8 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
   PID_Reset(&g->pid.aimbot.yaw_omega);
   PID_Reset(&g->pid.aimbot.pit_angle);
   PID_Reset(&g->pid.aimbot.pit_omega);
+  PID_Reset(&g->pid.recover.yaw_angle);
+  PID_Reset(&g->pid.recover.pit_angle);
   LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
   LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
 
@@ -105,6 +107,12 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
   PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq,
            &(g->param->pid.aimbot.pit_omega));
 
+  /* Recover模式PID（单环，编码器反馈） */
+  PID_Init(&(g->pid.recover.yaw_angle), KPID_MODE_NO_D, target_freq,
+           &(g->param->pid.recover.yaw_angle));
+  PID_Init(&(g->pid.recover.pit_angle), KPID_MODE_NO_D, target_freq,
+           &(g->param->pid.recover.pit_angle));
+
   LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
                        g->param->low_pass_cutoff_freq.out);
   LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
@@ -211,7 +219,18 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
     /* AI 模式：直接从指令获取角度设定值（每周期刷新） */
     g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
     g->setpoint.eulr.pit = g_cmd->setpoint_pit;
-    }
+  } else if (g_cmd->mode == GIMBAL_MODE_RECOVER) {
+    /* 双零点选近：mech_zero 和 mech_zero+π，选离当前编码器角度最近的 */
+    float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
+    float zero0 = g->param->mech_zero.yaw;
+    float zero1 = zero0 + M_PI;
+    if (zero1 >= M_2PI) zero1 -= M_2PI;
+    float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
+    float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
+    g->setpoint.eulr.yaw = (err0 <= err1) ? zero0 : zero1;
+    g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
+  }
+
   /* 控制相关逻辑 */
   float yaw_omega_set_point, pit_omega_set_point;
   switch (g->mode) {
@@ -253,6 +272,12 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
                           g->feedback.imu.gyro.y, 0.0f, g->dt)
                + g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA;  // 加速度前馈: J * acc
     break;
+  case GIMBAL_MODE_RECOVER:
+    g->out.yaw = PID_Calc(&(g->pid.recover.yaw_angle), g->setpoint.eulr.yaw,
+                          g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt);
+    g->out.pit = PID_Calc(&(g->pid.recover.pit_angle), g->setpoint.eulr.pit,
+                          g->feedback.motor.pit.rotor_abs_angle, 0.0f, g->dt);
+    break;
   }
   
   /* 输出滤波 */

User/module/gimbal.h

@@ -30,7 +30,8 @@ typedef enum {
   GIMBAL_MODE_RELAX,    /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
   GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
   GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
-  GIMBAL_MODE_AI_CONTROL  /* AI控制模式，直接接受AI下发的目标角度 */
+  GIMBAL_MODE_AI_CONTROL,  /* AI控制模式，直接接受AI下发的目标角度 */
+  GIMBAL_MODE_RECOVER  /* 自起模式，底盘翻倒后自动恢复到正常姿态 */
 } Gimbal_Mode_t;
 
 /* UI 导出结构（供 referee 系统绘制） */
@@ -79,6 +80,10 @@ typedef struct {
       KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
       KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
     } aimbot;
+    struct {
+      KPID_Params_t yaw_angle; /* recover yaw位置环PID参数 */
+      KPID_Params_t pit_angle; /* recover pitch位置环PID参数 */
+    } recover;
   } pid;
 
   /* 低通滤波器截止频率 */
@@ -141,12 +146,16 @@ typedef struct {
     KPID_t yaw_omega; /* yaw轴角速度环PID */
     KPID_t pit_angle; /* pitch轴角位置环PID */
     KPID_t pit_omega; /* pitch轴角速度环PID */
-            struct {
+    struct {
       KPID_t yaw_angle; /* yaw轴角位置环PID */
       KPID_t yaw_omega; /* yaw轴角速度环PID */
       KPID_t pit_angle; /* pitch轴角位置环PID */
       KPID_t pit_omega; /* pitch轴角速度环PID */
     } aimbot;
+    struct {
+      KPID_t yaw_angle; /* recover yaw位置环PID */
+      KPID_t pit_angle; /* recover pitch位置环PID */
+    } recover;
   } pid;
 
   struct {