优化云台

CMakeLists.txt

@@ -82,6 +82,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/module/balance_chassis.c
     User/module/config.c
     User/module/shoot.c
+    User/module/gimbal.c
 
     # User/task sources
     User/task/atti_esit.c

MDK-ARM/CtrBoard-H7_ALL.uvoptx

@@ -153,7 +153,24 @@
           <Name>-U-O142 -O2254 -S0 -C0 -N00("ARM CoreSight SW-DP") -D00(5BA02477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
         </SetRegEntry>
       </TargetDriverDllRegistry>
-      <Breakpoint/>
+      <Breakpoint>
+        <Bp>
+          <Number>0</Number>
+          <Type>0</Type>
+          <LineNumber>56</LineNumber>
+          <EnabledFlag>1</EnabledFlag>
+          <Address>134292314</Address>
+          <ByteObject>0</ByteObject>
+          <HtxType>0</HtxType>
+          <ManyObjects>0</ManyObjects>
+          <SizeOfObject>0</SizeOfObject>
+          <BreakByAccess>0</BreakByAccess>
+          <BreakIfRCount>1</BreakIfRCount>
+          <Filename>..\User\task\ctrl_gimbal.c</Filename>
+          <ExecCommand></ExecCommand>
+          <Expression>\\CtrBoard_H7_ALL\../User/task/ctrl_gimbal.c\56</Expression>
+        </Bp>
+      </Breakpoint>
       <WatchWindow1>
         <Ww>
           <count>0</count>
@@ -170,6 +187,16 @@
           <WinNumber>1</WinNumber>
           <ItemText>dr16</ItemText>
         </Ww>
+        <Ww>
+          <count>3</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>gimbal</ItemText>
+        </Ww>
+        <Ww>
+          <count>4</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>GIMBAL_IMU_Update</ItemText>
+        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
@@ -1257,6 +1284,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>82</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\device\gimbal_imu.c</PathWithFileName>
+      <FilenameWithoutPath>gimbal_imu.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -1267,7 +1306,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>82</FileNumber>
+      <FileNumber>83</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1279,7 +1318,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>83</FileNumber>
+      <FileNumber>84</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1291,7 +1330,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>84</FileNumber>
+      <FileNumber>85</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1301,6 +1340,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>9</GroupNumber>
+      <FileNumber>86</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\module\gimbal.c</PathWithFileName>
+      <FilenameWithoutPath>gimbal.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -1311,7 +1362,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>85</FileNumber>
+      <FileNumber>87</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1323,7 +1374,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>86</FileNumber>
+      <FileNumber>88</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1335,7 +1386,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>89</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1347,7 +1398,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>90</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1359,7 +1410,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>89</FileNumber>
+      <FileNumber>91</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1371,7 +1422,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>90</FileNumber>
+      <FileNumber>92</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1383,7 +1434,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>93</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1395,7 +1446,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>94</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1407,7 +1458,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>93</FileNumber>
+      <FileNumber>95</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1419,7 +1470,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>94</FileNumber>
+      <FileNumber>96</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

MDK-ARM/CtrBoard-H7_ALL.uvprojx

@@ -824,6 +824,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\device\motor_rm.c</FilePath>
             </File>
+            <File>
+              <FileName>gimbal_imu.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\gimbal_imu.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -844,6 +849,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\module\shoot.c</FilePath>
             </File>
+            <File>
+              <FileName>gimbal.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\module\gimbal.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>

User/bsp/fdcan.c

@@ -27,7 +27,7 @@
   #define FDCAN2_FILTER_CONFIG_TABLE(X) \
     X(0,       FDCAN_STANDARD_ID,   FDCAN_FILTER_MASK,   0x000     ,   0x000     ,   0) \
     X(1,       FDCAN_EXTENDED_ID,   FDCAN_FILTER_MASK,   0x00000000,   0x00000000,   0) 
-    #define FDCAN2_GLOBAL_FILTER FDCAN_REJECT, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
+    #define FDCAN2_GLOBAL_FILTER FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
 #endif
 #ifdef FDCAN3_EN
   #define FDCAN3_FILTER_CONFIG_TABLE(X) \

User/device/gimbal_imu.c

@@ -8,6 +8,7 @@
 #include <string.h>
 
 #include "bsp/can.h"
+#include "bsp/time.h"
 
 /* Private function prototypes ---------------------------------------------- */
 
@@ -34,9 +35,6 @@ static void GIMBAL_IMU_ParseMessage(GIMBAL_IMU_t *gimbal_imu, uint32_t id, const
     return;
   }
   
-  /* 更新在线状态 */
-  gimbal_imu->header.online = true;
-  
   /* 判断是哪个ID的数据 */
   if (id == gimbal_imu->param.accl_id) {
     /* 解析加速度计数据 */
@@ -89,10 +87,15 @@ static void GIMBAL_IMU_ParseMessage(GIMBAL_IMU_t *gimbal_imu, uint32_t id, const
 
 /**
  * @brief 初始化Gimbal IMU设备
+ * @param gimbal_imu Gimbal IMU结构体指针
+ * @param param 参数配置
+ * @return 初始化结果
+ *   - DEVICE_OK: 成功
+ *   - DEVICE_ERR_NULL: 参数为空
  */
 int8_t GIMBAL_IMU_Init(GIMBAL_IMU_t *gimbal_imu, const GIMBAL_IMU_Param_t *param) {
   if (gimbal_imu == NULL || param == NULL) {
-    return -1;
+    return DEVICE_ERR_NULL;
   }
   
   /* 清空数据 */
@@ -110,35 +113,56 @@ int8_t GIMBAL_IMU_Init(GIMBAL_IMU_t *gimbal_imu, const GIMBAL_IMU_Param_t *param
   BSP_CAN_RegisterId(param->can, param->eulr_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
   BSP_CAN_RegisterId(param->can, param->quat_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
   
-  return 0;
+  return DEVICE_OK;
 }
 
 /**
  * @brief 更新Gimbal IMU设备状态 - 从CAN队列读取并解析数据
+ * @param gimbal_imu Gimbal IMU结构体指针
+ * @return 更新结果
+ *   - DEVICE_OK: 成功
+ *   - DEVICE_ERR_NULL: 参数为空
  */
 int8_t GIMBAL_IMU_Update(GIMBAL_IMU_t *gimbal_imu) {
   if (gimbal_imu == NULL) {
-    return -1;
+    return DEVICE_ERR_NULL;
   }
   
   BSP_CAN_Message_t msg;
+  bool received = false;
   
   /* 读取所有可用的CAN消息 */
-  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.accl_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
+  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.accl_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
     GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.accl_id, msg.data);
+    received = true;
   }
   
-  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.gyro_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
+  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.gyro_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
     GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.gyro_id, msg.data);
+    received = true;
   }
   
-  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.eulr_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
+  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.eulr_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
     GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.eulr_id, msg.data);
+    received = true;
   }
   
-  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.quat_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
+  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.quat_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
     GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.quat_id, msg.data);
+    received = true;
   }
   
-  return 0;
+  /* 更新在线状态和时间戳 */
+  if (received) {
+    gimbal_imu->header.online = true;
+    gimbal_imu->header.last_online_time = BSP_TIME_Get();
+  } else {
+    /* 超时检测 - 100ms */
+    uint64_t now_time = BSP_TIME_Get();
+    if (now_time - gimbal_imu->header.last_online_time > 100000) {  /* 100ms超时，单位微秒 */
+      gimbal_imu->header.online = false;
+    }
+  }
+  
+  return DEVICE_OK;
 }

User/device/gimbal_imu.h

@@ -65,14 +65,21 @@ typedef struct {
 
 /**
  * @brief 初始化Gimbal IMU设备
- * 
  * @param gimbal_imu Gimbal IMU结构体指针
  * @param param 参数配置
- * @return int8_t 0:成功 -1:失败
+ * @return 初始化结果
+ *   - DEVICE_OK: 成功
+ *   - DEVICE_ERR_NULL: 参数为空
  */
 int8_t GIMBAL_IMU_Init(GIMBAL_IMU_t *gimbal_imu, const GIMBAL_IMU_Param_t *param);
 
-
+/**
+ * @brief 更新Gimbal IMU设备状态
+ * @param gimbal_imu Gimbal IMU结构体指针
+ * @return 更新结果
+ *   - DEVICE_OK: 成功
+ *   - DEVICE_ERR_NULL: 参数为空
+ */
 int8_t GIMBAL_IMU_Update(GIMBAL_IMU_t *gimbal_imu);
 
 

User/module/config.c

@@ -20,9 +20,91 @@
  */
 Config_RobotParam_t robot_config = {
     /* USER CODE BEGIN robot_config */
+
+    .gimbal_param = {
+        .pid = {
+            .yaw_omega = {
+                .k = 1.0f,
+                .p = 1.0f,
+                .i = 0.3f,
+                .d = 0.0f,
+                .i_limit = 1.0f,
+                .out_limit = 1.0f,
+                .d_cutoff_freq = -1.0f,
+                .range = -1.0f,
+            },
+            .yaw_angle = {
+                .k = 8.0f,
+                .p = 3.0f,
+                .i = 0.0f,
+                .d = 0.0f,
+                .i_limit = 0.0f,
+                .out_limit = 10.0f,
+                .d_cutoff_freq = -1.0f,
+                .range = M_2PI,
+            },
+            .pit_omega = {
+                .k = 0.25f,
+                .p = 1.0f,
+                .i = 0.0f,
+                .d = 0.0f,
+                .i_limit = 1.0f,
+                .out_limit = 1.0f,
+                .d_cutoff_freq = -1.0f,
+                .range = -1.0f,
+            },
+            .pit_angle = {
+                .k = 5.0f,
+                .p = 5.0f,
+                .i = 2.5f,
+                .d = 0.0f,
+                .i_limit = 0.0f,
+                .out_limit = 10.0f,
+                .d_cutoff_freq = -1.0f,
+                .range = M_2PI,
+            },
+        },
+        .mech_zero = {
+            .yaw = 0.0f,
+            .pit = 1.77f,
+        },
+        .travel = {
+            .yaw = -1.0f,
+            .pit = 0.8f,
+        },
+        .low_pass_cutoff_freq = {
+            .out = -1.0f,
+            .gyro = 1000.0f,
+        },
+        .pit_motor ={
+            .can = BSP_CAN_1,
+            .id = 0x206,
+            .gear = false,
+            .module = MOTOR_GM6020,
+            .reverse = true,
+        },
+        .yaw_motor = {
+            .can = BSP_CAN_1,
+            .can_id = 0x1,
+            .master_id = 0x11,
+            .module = MOTOR_DM_J4310,
+            .reverse = false,
+        },
+        .imu = {
+            .can = BSP_CAN_2,
+            .accl_id = 0x64,  // 100
+            .gyro_id = 0x65,  // 101
+            .eulr_id = 0x66,  // 102
+            .quat_id = 0x67   // 103
+        },
+        
+    },
+
+
+
     .shoot_param = {
             .trig_step_angle=M_2PI/8,
-            .shot_delay_time=0.5f,
+            .shot_delay_time=1.0f,
             .shot_burst_num=20,
             .fric_motor_param[0] = {
                 .can = BSP_CAN_1,
@@ -47,7 +129,7 @@ Config_RobotParam_t robot_config = {
             },
             .fric_follow = {
                 .k=1.0f,
-                .p=1.8f,
+                .p=1.5f,
                 .i=0.0f,
                 .d=0.0f,
                 .i_limit=0.0f,

User/module/config.h

@@ -12,6 +12,7 @@ extern "C" {
 #include <stdbool.h>
 #include "module/shoot.h"
 #include "module/balance_chassis.h"
+#include "module/gimbal.h"
 /**
  * @brief 机器人参数配置结构体
  * @note 在此添加您的配置参数
@@ -20,6 +21,7 @@ typedef struct {
     /* USER CODE BEGIN Config_RobotParam */
     Shoot_Params_t shoot_param;
     Chassis_Params_t chassis_param;
+    Gimbal_Params_t gimbal_param;
     /* USER CODE END Config_RobotParam */
 } Config_RobotParam_t;
 

User/module/gimbal.c

@@ -9,6 +9,7 @@
 #include <math.h> 
 #include "component/filter.h"
 #include "component/pid.h"
+#include "device/gimbal_imu.h"
 #include "device/motor_dm.h"
 #include "device/motor_rm.h"
 
@@ -72,9 +73,12 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
   if (g == NULL)
     return -1;
 
-  g->param = param;
+  g->param = (Gimbal_Params_t *)param;
   g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
   
+  /* 先初始化CAN */
+  BSP_CAN_Init();
+  
   /* 初始化云台电机控制PID和LPF */
   PID_Init(&(g->pid.yaw_angle), KPID_MODE_NO_D, target_freq,
            &(g->param->pid.yaw_angle));
@@ -93,12 +97,15 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
   g->limit.yaw.min = g->param->mech_zero.yaw;
   g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit;
   g->limit.pit.min = g->param->mech_zero.pit;
-  BSP_CAN_Init();
   
   MOTOR_RM_Register(&(g->param->pit_motor));
   MOTOR_DM_Register(&(g->param->yaw_motor));
 
   MOTOR_DM_Enable(&(g->param->yaw_motor));
+  
+  /* 最后初始化IMU，确保CAN和电机都已经设置好 */
+  GIMBAL_IMU_Init(&g->imu, &(g->param->imu));
+  
   return 0;
 }
 
@@ -136,7 +143,14 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal){
   if (gimbal == NULL) {
     return -1;
   }
+  GIMBAL_IMU_Update(&gimbal->imu);
   
+  /* 将IMU数据复制到feedback中供控制使用 */
+  gimbal->feedback.imu.eulr = gimbal->imu.data.eulr;
+  gimbal->feedback.imu.gyro = gimbal->imu.data.gyro;
+  gimbal->feedback.imu.accl = gimbal->imu.data.accl;
+  
+  return 0;
 }
 
 /**
@@ -208,6 +222,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
     break;
 
   case GIMBAL_MODE_ABSOLUTE:
+  case GIMBAL_MODE_RELATIVE:
     yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
                                    g->feedback.imu.eulr.yaw, 0.0f, g->dt);
     g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
@@ -220,13 +235,13 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
 
               
     break;
+  }
   
   /* 输出滤波 */
   g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
   g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
 
   return 0;
-  }
 }
 
   /**

User/module/gimbal.h

@@ -76,7 +76,11 @@ typedef struct {
 
 /* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
 typedef struct {
-  GIMBAL_IMU_t imu;
+  struct {
+    AHRS_Eulr_t eulr; /* 云台姿态欧拉角 */
+    AHRS_Gyro_t gyro; /* 云台陀螺仪数据 */
+    AHRS_Accl_t accl; /* 云台加速度计数据 */
+  }imu;
   struct {
     MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */
     MOTOR_Feedback_t pit; /* pitch轴电机反馈 */
@@ -100,6 +104,7 @@ typedef struct {
 
   /* 模块通用 */
   Gimbal_Mode_t mode; /* 云台模式 */
+  GIMBAL_IMU_t imu; /* 云台IMU数据 */
 
   /* PID计算的目标值 */
   struct {

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=5000.0f;  /* 归一化目标转速 */
+    s->target_variable.target_rpm=6000.0f;  /* 归一化目标转速 */
 
 	return 0;
 }

User/task/ctrl_gimbal.c

@@ -6,9 +6,10 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
+#include "component/ahrs.h"
+#include "module/gimbal.h"
+#include "module/config.h"
 #include "bsp/can.h"
-#include "device/motor.h"
-#include "device/motor_dm.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -16,13 +17,9 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
-MOTOR_DM_Param_t motor_dm_yaw = {
+Gimbal_t gimbal;
-    .can = BSP_CAN_1,
+Gimbal_CMD_t gimbal_cmd;
-    .can_id = 0x1,
+BSP_FDCAN_StdDataFrame_t can_frame;
-    .master_id = 0x11,
-    .module = MOTOR_DM_J4310,
-    .reverse = false,
-};
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -38,14 +35,30 @@ void Task_ctrl_gimbal(void *argument) {
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
-  BSP_CAN_Init();
+  Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal_param, CTRL_GIMBAL_FREQ);
-  MOTOR_DM_Register(&motor_dm_yaw);
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-    MOTOR_DM_Enable(&motor_dm_yaw);
+
+    can_frame.id = 0x200;
+    can_frame.dlc = 8;
+    BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
+    
+    Gimbal_UpdateIMU(&gimbal);
+    osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
+
+
+    Gimbal_UpdateFeedback(&gimbal);
+
+    // osMessageQueueReset(task_runtime.msgq.chassis_yaw);
+    osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
+    
+    Gimbal_Control(&gimbal, &gimbal_cmd);
+
+    Gimbal_Output(&gimbal);
+
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/user_task.c

@@ -22,12 +22,12 @@ const osThreadAttr_t attr_atti_esit = {
 const osThreadAttr_t attr_ctrl_chassis = {
     .name = "ctrl_chassis",
     .priority = osPriorityNormal,
-    .stack_size = 256 * 4,
+    .stack_size = 256 * 8,
 };
 const osThreadAttr_t attr_ctrl_gimbal = {
     .name = "ctrl_gimbal",
     .priority = osPriorityNormal,
-    .stack_size = 256 * 4,
+    .stack_size = 256 * 8,
 };
 const osThreadAttr_t attr_monitor = {
     .name = "monitor",