fix gimbal

CMakeLists.txt

@@ -89,6 +89,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/module/config.c
     User/module/gimbal.c
     User/module/shoot.c
+    User/module/cap.c
     User/module/cmd/cmd.c
     User/module/cmd/cmd_adapter.c
     User/module/cmd/cmd_behavior.c
@@ -107,6 +108,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/task/monitor.c
     User/task/rc.c
     User/task/cli.c
+    User/task/cap.c
     User/task/user_task.c
     User/task/vofa.c
     User/task/cmd.c

User/device/supercap.h

@@ -8,8 +8,8 @@ extern "C" {
 #include "device/device.h"
 //#include "referee.h"
 
-#define SUPERCAP_CAN  BSP_FDCAN_1
+#define SUPERCAP_CAN  BSP_FDCAN_2
-//#define SUPERCAP_CAN  BSP_FDCAN_2
+// #define SUPERCAP_CAN  BSP_FDCAN_3
 
 //#define SUPERCAP_TX_ID 0x301  //C板发给超级电容的ID
 //#define SUPERCAP_RX_ID 0x100  //超级电容发给C板的ID

User/module/config.c

@@ -501,7 +501,7 @@ Config_RobotParam_t robot_config = {
         .rc_mode_map = {
 #if CMD_ENABLE_MODULE_GIMBAL
             .gimbal_sw_up   = GIMBAL_MODE_RELAX,
-            .gimbal_sw_mid  = GIMBAL_MODE_RECOVER,
+            .gimbal_sw_mid  = GIMBAL_MODE_RELATIVE,
             .gimbal_sw_down = GIMBAL_MODE_RELATIVE,
 #endif
 #if CMD_ENABLE_MODULE_BALANCE_CHASSIS

User/module/gimbal.c

@@ -219,7 +219,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
     g->out.yaw = 0.0f;
     g->out.pit = 0.0f;
     /* fallthrough - AI控制模式也需要执行PID计算 */
-    
+    break;
   case GIMBAL_MODE_ABSOLUTE:
   case GIMBAL_MODE_RELATIVE:
     yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,

User/module/gimbal.h

@@ -28,10 +28,10 @@ extern "C" {
 
 typedef enum {
   GIMBAL_MODE_RELAX,    /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
-  GIMBAL_MODE_RECOVER,  /* 回中模式，使用编码器双环PID控制yaw回零点，pitch锁定限位中心 */
   GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
   GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
-  GIMBAL_MODE_AI_CONTROL /* AI控制模式，直接接受AI下发的目标角度 */
+  GIMBAL_MODE_AI_CONTROL, /* AI控制模式，直接接受AI下发的目标角度 */
+  GIMBAL_MODE_RECOVER   /* 回中模式，使用编码器双环PID控制yaw回零点，pitch锁定限位中心 */
 } Gimbal_Mode_t;
 
 /* UI 导出结构（供 referee 系统绘制） */

User/task/cap.c

@@ -28,14 +28,14 @@ Cap_RefereeUI_t cap_ui;
 
 /* USER PRIVATE CODE END */
 /* Exported functions ------------------------------------------------------- */
-void Task_super_cap(void *argument) {
+void Task_cap(void *argument) {
   (void)argument; /* 未使用argument，消除警告 */
 
   
   /* 计算任务运行到指定频率需要等待的tick数 */
-  const uint32_t delay_tick = osKernelGetTickFreq() / SUPER_CAP_FREQ;
+  const uint32_t delay_tick = osKernelGetTickFreq() / CAP_FREQ;
 
-  osDelay(SUPER_CAP_INIT_DELAY); /* 延时一段时间再开启任务 */
+  osDelay(CAP_INIT_DELAY); /* 延时一段时间再开启任务 */
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
@@ -53,13 +53,13 @@ void Task_super_cap(void *argument) {
 			
 			cap_online = get_supercap_online_state();
 			
-			osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
+			// osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
       /* 根据裁判系统数据计算输出功率 */
 			SuperCap_CanTX.Powerlimit = power_limit;
       Cap_Control(&CAN_SuperCapRXData, &referee_cap);
 			SuperCap_Update();
       CAN_TX_SuperCapData(&SuperCap_CanTX);
-      osKernelUnlock();
+      // osKernelUnlock();
 		  /* 将电容状态发送到Chassis */
 //      osMessageQueueReset(task_runtime.msgq.cap.for_chassis);
 //      osMessageQueuePut(task_runtime.msgq.cap.for_chassis, &CAN_SuperCapRXData, 0, 0);

User/task/ctrl_chassis.c

@@ -106,7 +106,7 @@ void Task_ctrl_chassis(void *argument) {
     // float power_limit = (chassis_ref.ref_status == REF_STATUS_RUNNING)
     //                     ? chassis_ref.chassis_power_limit
     //                     : 500.0f;
-    // Chassis_Power_Control(&chassis, power_limit);
+     //(&chassis, power_limit);
 
 
     osMessageQueueReset(task_runtime.msgq.chassis.vofa); // 重置消息队列，防止阻塞

User/task/init.c

@@ -43,6 +43,7 @@ void Task_Init(void *argument) {
   task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
   task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);
   task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
+  task_runtime.thread.cap = osThreadNew(Task_cap, NULL, &attr_cap);
   // task_runtime.thread.vofa = osThreadNew(Task_vofa, NULL, &attr_vofa);
   task_runtime.thread.cli = osThreadNew(Task_cli, NULL, &attr_cli);
   // task_runtime.thread.debug = osThreadNew(Task_debug, NULL, &attr_debug);

User/task/rc.c

@@ -80,14 +80,18 @@ void Task_rc(void *argument) {
 
     /* 检测左拨杆切换到UP位置时触发软件复位 */
     if (dr16.header.online) {
-      /* 拨杆从非UP状态切换到UP状态，且复位功能已使能，触发系统复位 */
+      if (last_sw_l == DR16_SW_ERR) {
-      if (
+        /* 首次上线，仅同步状态，不触发动作 */
-         dr16.data.sw_l == DR16_SW_UP &&
+        last_sw_l = dr16.data.sw_l;
-          last_sw_l != DR16_SW_UP) {
+      } else {
-          reset=!reset; 
+        if (dr16.data.sw_l == DR16_SW_UP && last_sw_l != DR16_SW_UP) {
+          reset = !reset;
+        }
+        last_sw_l = dr16.data.sw_l;
       }
-
+    } else {
-      last_sw_l = dr16.data.sw_l;  /* 更新拨杆状态 */
+      /* 离线时重置为ERR，下次上线重新同步 */
+      last_sw_l = DR16_SW_ERR;
     }
 
     /* USER CODE END */

User/task/user_task.c

@@ -73,4 +73,9 @@ const osThreadAttr_t attr_referee = {
     .name = "referee",
     .priority = osPriorityNormal,
     .stack_size = 512 * 4, 
+};
+const osThreadAttr_t attr_cap = {
+    .name = "cap",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
 };

User/task/user_task.h

@@ -24,7 +24,7 @@ extern "C" {
 #define DEBUG_FREQ (10.0)
 #define CMD_FREQ (500.0)
 #define REFEREE_FREQ (500.0)
-#define SUPER_CAP_FREQ (500.0)
+#define CAP_FREQ (500.0)
 
 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
@@ -41,7 +41,7 @@ extern "C" {
 #define DEBUG_INIT_DELAY (0)
 #define CMD_INIT_DELAY (0)
 #define REFEREE_INIT_DELAY (0)
-#define SUPER_CAP_INIT_DELAY (0)
+#define CAP_INIT_DELAY (0)
 
 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@@ -64,7 +64,7 @@ typedef struct {
         osThreadId_t debug;
         osThreadId_t cmd;
         osThreadId_t referee; 
-
+        osThreadId_t cap;
     } thread;
 
     /* USER MESSAGE BEGIN */
@@ -144,6 +144,7 @@ typedef struct {
         UBaseType_t ai;
         UBaseType_t vofa;
         UBaseType_t cli;
+        UBaseType_t cap;
         UBaseType_t debug;
         UBaseType_t cmd;
         UBaseType_t referee;
@@ -163,6 +164,7 @@ typedef struct {
         float debug;
         float cmd;
         float referee;
+            float cap;
     } freq;
 
     /* 任务最近运行时间 */
@@ -178,6 +180,7 @@ typedef struct {
         float debug;
         float cmd;  
         float referee;
+        float cap;
     } last_up_time;
 
 } Task_Runtime_t;
@@ -199,6 +202,7 @@ extern const osThreadAttr_t attr_vofa;
 extern const osThreadAttr_t attr_cli;
 extern const osThreadAttr_t attr_debug;
 extern const osThreadAttr_t attr_cmd;
+extern const osThreadAttr_t attr_cap;
 extern const osThreadAttr_t attr_referee;
 
 /* 任务函数声明 */
@@ -216,7 +220,7 @@ void Task_cli(void *argument);
 void Task_debug(void *argument);
 void Task_cmd(void *argument);
 void Task_referee(void *argument);
-
+void Task_cap(void *argument);
 #ifdef __cplusplus
 }
 #endif